Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-28 02:11:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpilib] separate expansion hub position and velocity constants (#8791)

Browse Source 
this is because position and velocity control follow different rules;
see #8773

Signed-off-by: Zach Harel <zach@zharel.me>
This commit is contained in:
Zach Harel
2026-04-23 20:40:52 -04:00
committed by GitHub
parent 44b9545f30
commit 7fc8845424
16 changed files with 458 additions and 191 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
wpilibc/src/main/native/cpp/hardware/expansionhub/ExpansionHubMotor.cpp
View File

@@ -4,6 +4,8 @@
#include "wpi/hardware/expansionhub/ExpansionHubMotor.hpp"
#include "wpi/hardware/expansionhub/ExpansionHubPositionConstants.hpp"
#include "wpi/hardware/expansionhub/ExpansionHubVelocityConstants.hpp"
#include "wpi/system/Errors.hpp"
#include "wpi/system/SystemServer.hpp"
@@ -18,8 +20,8 @@ using namespace wpi;
ExpansionHubMotor::ExpansionHubMotor(int usbId, int channel)
: m_hub{usbId},
m_channel{channel},
m_velocityPidConstants{usbId, channel, true},
m_positionPidConstants{usbId, channel, false} {
m_velocityConstants{usbId, channel},
m_positionConstants{usbId, channel} {
WPILIB_AssertMessage(channel >= 0 && channel < ExpansionHub::NumMotorPorts,
"ExHub Motor Channel {} out of range", channel);
@@ -148,12 +150,12 @@ void ExpansionHubMotor::ResetEncoder() {
m_resetEncoderPublisher.Set(true);
}
ExpansionHubPidConstants& ExpansionHubMotor::GetVelocityPidConstants() {
return m_velocityPidConstants;
ExpansionHubVelocityConstants& ExpansionHubMotor::GetVelocityConstants() {
return m_velocityConstants;
}
ExpansionHubPidConstants& ExpansionHubMotor::GetPositionPidConstants() {
return m_positionPidConstants;
ExpansionHubPositionConstants& ExpansionHubMotor::GetPositionConstants() {
return m_positionConstants;
}
void ExpansionHubMotor::Follow(const ExpansionHubMotor& leader) {

97
wpilibc/src/main/native/cpp/hardware/expansionhub/ExpansionHubPidConstants.cpp
View File

@@ -1,97 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "wpi/hardware/expansionhub/ExpansionHubPidConstants.hpp"
#include <string>
#include "wpi/system/Errors.hpp"
#include "wpi/system/SystemServer.hpp"
using namespace wpi;
ExpansionHubPidConstants::ExpansionHubPidConstants(int usbId, int channel,
bool isVelocityPid) {
auto systemServer = SystemServer::GetSystemServer();
wpi::nt::PubSubOptions options;
options.sendAll = true;
options.keepDuplicates = true;
options.periodic = 0.005;
std::string pidType = isVelocityPid ? "velocity" : "position";
m_pPublisher = systemServer
.GetDoubleTopic(fmt::format("/rhsp/{}/motor{}/pid/{}/kp",
usbId, channel, pidType))
.Publish(options);
m_iPublisher = systemServer
.GetDoubleTopic(fmt::format("/rhsp/{}/motor{}/pid/{}/ki",
usbId, channel, pidType))
.Publish(options);
m_dPublisher = systemServer
.GetDoubleTopic(fmt::format("/rhsp/{}/motor{}/pid/{}/kd",
usbId, channel, pidType))
.Publish(options);
m_aPublisher = systemServer
.GetDoubleTopic(fmt::format("/rhsp/{}/motor{}/pid/{}/ka",
usbId, channel, pidType))
.Publish(options);
m_vPublisher = systemServer
.GetDoubleTopic(fmt::format("/rhsp/{}/motor{}/pid/{}/kv",
usbId, channel, pidType))
.Publish(options);
m_sPublisher = systemServer
.GetDoubleTopic(fmt::format("/rhsp/{}/motor{}/pid/{}/ks",
usbId, channel, pidType))
.Publish(options);
m_continuousPublisher =
systemServer
.GetBooleanTopic(fmt::format("/rhsp/{}/motor{}/pid/{}/continuous",
usbId, channel, pidType))
.Publish(options);
m_continuousMinimumPublisher =
systemServer
.GetDoubleTopic(
fmt::format("/rhsp/{}/motor{}/pid/{}/continuousMinimum", usbId,
channel, pidType))
.Publish(options);
m_continuousMaximumPublisher =
systemServer
.GetDoubleTopic(
fmt::format("/rhsp/{}/motor{}/pid/{}/continuousMaximum", usbId,
channel, pidType))
.Publish(options);
}
void ExpansionHubPidConstants::SetPID(double p, double i, double d) {
m_pPublisher.Set(p);
m_iPublisher.Set(i);
m_dPublisher.Set(d);
}
void ExpansionHubPidConstants::SetFF(double s, double v, double a) {
m_sPublisher.Set(s);
m_vPublisher.Set(v);
m_aPublisher.Set(a);
}
void ExpansionHubPidConstants::EnableContinuousInput(double minimumInput,
double maximumInput) {
m_continuousMaximumPublisher.Set(maximumInput);
m_continuousMinimumPublisher.Set(minimumInput);
m_continuousPublisher.Set(true);
}
void ExpansionHubPidConstants::DisableContinuousInput() {
m_continuousPublisher.Set(false);
}

94
wpilibc/src/main/native/cpp/hardware/expansionhub/ExpansionHubPositionConstants.cpp Normal file
View File

@@ -0,0 +1,94 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "wpi/hardware/expansionhub/ExpansionHubPositionConstants.hpp"
#include "fmt/core.h"
#include "wpi/system/SystemServer.hpp"
using namespace wpi;
ExpansionHubPositionConstants::ExpansionHubPositionConstants(int hubNumber,
int motorNumber) {
auto systemServer = SystemServer::GetSystemServer();
wpi::nt::PubSubOptions options;
options.sendAll = true;
options.keepDuplicates = true;
options.periodic = 0.005;
m_pPublisher =
systemServer
.GetDoubleTopic(fmt::format("/rhsp/{}/motor{}/constants/position/kp",
hubNumber, motorNumber))
.Publish(options);
m_iPublisher =
systemServer
.GetDoubleTopic(fmt::format("/rhsp/{}/motor{}/constants/position/ki",
hubNumber, motorNumber))
.Publish(options);
m_dPublisher =
systemServer
.GetDoubleTopic(fmt::format("/rhsp/{}/motor{}/constants/position/kd",
hubNumber, motorNumber))
.Publish(options);
m_sPublisher =
systemServer
.GetDoubleTopic(fmt::format("/rhsp/{}/motor{}/constants/position/ks",
hubNumber, motorNumber))
.Publish(options);
m_continuousPublisher =
systemServer
.GetBooleanTopic(
fmt::format("/rhsp/{}/motor{}/constants/position/continuous",
hubNumber, motorNumber))
.Publish(options);
m_continuousMinimumPublisher =
systemServer
.GetDoubleTopic(fmt::format(
"/rhsp/{}/motor{}/constants/position/continuousMinimum",
hubNumber, motorNumber))
.Publish(options);
m_continuousMaximumPublisher =
systemServer
.GetDoubleTopic(fmt::format(
"/rhsp/{}/motor{}/constants/position/continuousMaximum",
hubNumber, motorNumber))
.Publish(options);
}
ExpansionHubPositionConstants& ExpansionHubPositionConstants::SetPID(double p,
double i,
double d) {
m_pPublisher.Set(p);
m_iPublisher.Set(i);
m_dPublisher.Set(d);
return *this;
}
ExpansionHubPositionConstants& ExpansionHubPositionConstants::SetS(double s) {
m_sPublisher.Set(s);
return *this;
}
ExpansionHubPositionConstants&
ExpansionHubPositionConstants::EnableContinuousInput(double minimumInput,
double maximumInput) {
m_continuousMaximumPublisher.Set(maximumInput);
m_continuousMinimumPublisher.Set(minimumInput);
m_continuousPublisher.Set(true);
return *this;
}
ExpansionHubPositionConstants&
ExpansionHubPositionConstants::DisableContinuousInput() {
m_continuousPublisher.Set(false);
return *this;
}

74
wpilibc/src/main/native/cpp/hardware/expansionhub/ExpansionHubVelocityConstants.cpp Normal file
View File

@@ -0,0 +1,74 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "wpi/hardware/expansionhub/ExpansionHubVelocityConstants.hpp"
#include "fmt/core.h"
#include "wpi/system/SystemServer.hpp"
using namespace wpi;
ExpansionHubVelocityConstants::ExpansionHubVelocityConstants(int hubNumber,
int motorNumber) {
auto systemServer = SystemServer::GetSystemServer();
wpi::nt::PubSubOptions options;
options.sendAll = true;
options.keepDuplicates = true;
options.periodic = 0.005;
m_pPublisher =
systemServer
.GetDoubleTopic(fmt::format("/rhsp/{}/motor{}/constants/velocity/kp",
hubNumber, motorNumber))
.Publish(options);
m_iPublisher =
systemServer
.GetDoubleTopic(fmt::format("/rhsp/{}/motor{}/constants/velocity/ki",
hubNumber, motorNumber))
.Publish(options);
m_dPublisher =
systemServer
.GetDoubleTopic(fmt::format("/rhsp/{}/motor{}/constants/velocity/kd",
hubNumber, motorNumber))
.Publish(options);
m_sPublisher =
systemServer
.GetDoubleTopic(fmt::format("/rhsp/{}/motor{}/constants/velocity/ks",
hubNumber, motorNumber))
.Publish(options);
m_vPublisher =
systemServer
.GetDoubleTopic(fmt::format("/rhsp/{}/motor{}/constants/velocity/kv",
hubNumber, motorNumber))
.Publish(options);
m_aPublisher =
systemServer
.GetDoubleTopic(fmt::format("/rhsp/{}/motor{}/constants/velocity/ka",
hubNumber, motorNumber))
.Publish(options);
}
ExpansionHubVelocityConstants& ExpansionHubVelocityConstants::SetPID(double p,
double i,
double d) {
m_pPublisher.Set(p);
m_iPublisher.Set(i);
m_dPublisher.Set(d);
return *this;
}
ExpansionHubVelocityConstants& ExpansionHubVelocityConstants::SetFF(double s,
double v,
double a) {
m_sPublisher.Set(s);
m_vPublisher.Set(v);
m_aPublisher.Set(a);
return *this;
}