Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-06 03:31: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;
}

11
wpilibc/src/main/native/include/wpi/hardware/expansionhub/ExpansionHubMotor.hpp
View File

@@ -5,7 +5,8 @@
#pragma once
#include "wpi/hardware/expansionhub/ExpansionHub.hpp"
#include "wpi/hardware/expansionhub/ExpansionHubPidConstants.hpp"
#include "wpi/hardware/expansionhub/ExpansionHubPositionConstants.hpp"
#include "wpi/hardware/expansionhub/ExpansionHubVelocityConstants.hpp"
#include "wpi/nt/BooleanTopic.hpp"
#include "wpi/nt/DoubleTopic.hpp"
#include "wpi/nt/IntegerTopic.hpp"
@@ -120,14 +121,14 @@ class ExpansionHubMotor {
*
* @return Velocity PID constants object
*/
ExpansionHubPidConstants& GetVelocityPidConstants();
ExpansionHubVelocityConstants& GetVelocityConstants();
/**
* Gets the PID constants object for position PID.
*
* @return Position PID constants object
*/
ExpansionHubPidConstants& GetPositionPidConstants();
ExpansionHubPositionConstants& GetPositionConstants();
/**
* Gets if the underlying ExpansionHub is connected.
@@ -165,7 +166,7 @@ class ExpansionHubMotor {
wpi::nt::DoublePublisher m_distancePerCountPublisher;
ExpansionHubPidConstants m_velocityPidConstants;
ExpansionHubPidConstants m_positionPidConstants;
ExpansionHubVelocityConstants m_velocityConstants;
ExpansionHubPositionConstants m_positionConstants;
};
} // namespace wpi

49
wpilibc/src/main/native/include/wpi/hardware/expansionhub/ExpansionHubPidConstants.hpp → wpilibc/src/main/native/include/wpi/hardware/expansionhub/ExpansionHubPositionConstants.hpp
View File

@@ -6,24 +6,25 @@
#include "wpi/nt/BooleanTopic.hpp"
#include "wpi/nt/DoubleTopic.hpp"
#include "wpi/nt/IntegerTopic.hpp"
namespace wpi {
class ExpansionHubMotor;
/** This class contains PID constants for an ExpansionHub motor. */
class ExpansionHubPidConstants {
/** This class contains feedback and feedforward constants for an ExpansionHub
* motor. */
class ExpansionHubPositionConstants {
public:
/**
* Sets the PID Controller gain parameters.
*
* Sets the proportional, integral, and differential coefficients.
* Set the proportional, integral, and differential coefficients.
*
* @param p The proportional coefficient. Must be >= 0.
* @param i The integral coefficient. Must be >= 0.
* @param d The differential coefficient. Must be >= 0.
* @param p The proportional coefficient.
* @param i The integral coefficient.
* @param d The derivative coefficient.
* @return This object, for method chaining.
*/
void SetPID(double p, double i, double d);
ExpansionHubPositionConstants& SetPID(double p, double i, double d);
/**
* Sets the feed forward gains to the specified values.
@@ -31,13 +32,12 @@ class ExpansionHubPidConstants {
* The units should be radians for angular systems and meters for linear
* systems.
*
* The PID control period is 10ms
* The motor control period is 10ms
*
* @param s The static gain in volts.
* @param v The velocity gain in V/(units/s).
* @param a The acceleration gain in V/(units/s²).
* @return This object, for method chaining.
*/
void SetFF(double s, double v, double a);
ExpansionHubPositionConstants& SetS(double s);
/**
* Enables continuous input.
@@ -48,31 +48,36 @@ class ExpansionHubPidConstants {
*
* @param minimumInput The minimum value expected from the input.
* @param maximumInput The maximum value expected from the input.
* @return This object, for method chaining.
*/
void EnableContinuousInput(double minimumInput, double maximumInput);
ExpansionHubPositionConstants& EnableContinuousInput(double minimumInput,
double maximumInput);
/**
* Disables continuous input.
* Disable continuous input mode.
*
* @return This object, for method chaining.
*/
void DisableContinuousInput();
ExpansionHubPositionConstants& DisableContinuousInput();
ExpansionHubPidConstants(ExpansionHubPidConstants&) = delete;
ExpansionHubPidConstants& operator=(ExpansionHubPidConstants&) = delete;
ExpansionHubPositionConstants(ExpansionHubPositionConstants&) = delete;
ExpansionHubPositionConstants& operator=(ExpansionHubPositionConstants&) =
delete;
ExpansionHubPidConstants(ExpansionHubPidConstants&&) = default;
ExpansionHubPidConstants& operator=(ExpansionHubPidConstants&&) = default;
ExpansionHubPositionConstants(ExpansionHubPositionConstants&&) = default;
ExpansionHubPositionConstants& operator=(ExpansionHubPositionConstants&&) =
default;
friend class ExpansionHubMotor;
private:
ExpansionHubPidConstants(int usbId, int channel, bool isVelocityPid);
ExpansionHubPositionConstants(int hubNumber, int motorNumber);
wpi::nt::DoublePublisher m_pPublisher;
wpi::nt::DoublePublisher m_iPublisher;
wpi::nt::DoublePublisher m_dPublisher;
wpi::nt::DoublePublisher m_sPublisher;
wpi::nt::DoublePublisher m_vPublisher;
wpi::nt::DoublePublisher m_aPublisher;
wpi::nt::BooleanPublisher m_continuousPublisher;
wpi::nt::DoublePublisher m_continuousMinimumPublisher;

64
wpilibc/src/main/native/include/wpi/hardware/expansionhub/ExpansionHubVelocityConstants.hpp Normal file
View File

@@ -0,0 +1,64 @@
// 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.
#pragma once
#include "wpi/nt/DoubleTopic.hpp"
namespace wpi {
class ExpansionHubMotor;
/** This class contains feedback and feedforward constants for an ExpansionHub
* motor. */
class ExpansionHubVelocityConstants {
public:
/**
* Sets the PID Controller gain parameters.
*
* Set the proportional, integral, and differential coefficients.
*
* @param p The proportional coefficient.
* @param i The integral coefficient.
* @param d The derivative coefficient.
* @return This object, for method chaining.
*/
ExpansionHubVelocityConstants& SetPID(double p, double i, double d);
/**
* Sets the feed forward gains to the specified values.
*
* The units should be radians for angular systems and meters for linear
* systems.
*
* The motor control period is 10ms
*
* @param s The static gain in volts.
* @param v The velocity gain in volts per unit per second.
* @param a The acceleration gain in volts per unit per second squared.
* @return This object, for method chaining.
*/
ExpansionHubVelocityConstants& SetFF(double s, double v, double a);
ExpansionHubVelocityConstants(ExpansionHubVelocityConstants&) = delete;
ExpansionHubVelocityConstants& operator=(ExpansionHubVelocityConstants&) =
delete;
ExpansionHubVelocityConstants(ExpansionHubVelocityConstants&&) = default;
ExpansionHubVelocityConstants& operator=(ExpansionHubVelocityConstants&&) =
default;
friend class ExpansionHubMotor;
private:
ExpansionHubVelocityConstants(int hubNumber, int motorNumber);
wpi::nt::DoublePublisher m_pPublisher;
wpi::nt::DoublePublisher m_iPublisher;
wpi::nt::DoublePublisher m_dPublisher;
wpi::nt::DoublePublisher m_sPublisher;
wpi::nt::DoublePublisher m_vPublisher;
wpi::nt::DoublePublisher m_aPublisher;
};
} // namespace wpi