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

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

View File

@@ -460,13 +460,13 @@ def wpilib_extension(srcs = [], header_to_dat_deps = [], extra_hdrs = [], includ
],
),
struct(
class_name = "ExpansionHubPidConstants",
yml_file = "semiwrap/ExpansionHubPidConstants.yml",
class_name = "ExpansionHubPositionConstants",
yml_file = "semiwrap/ExpansionHubPositionConstants.yml",
header_root = "$(execpath :robotpy-native-wpilib.copy_headers)",
header_file = "$(execpath :robotpy-native-wpilib.copy_headers)/wpi/hardware/expansionhub/ExpansionHubPidConstants.hpp",
header_file = "$(execpath :robotpy-native-wpilib.copy_headers)/wpi/hardware/expansionhub/ExpansionHubPositionConstants.hpp",
tmpl_class_names = [],
trampolines = [
("wpi::ExpansionHubPidConstants", "wpi__ExpansionHubPidConstants.hpp"),
("wpi::ExpansionHubPositionConstants", "wpi__ExpansionHubPositionConstants.hpp"),
],
),
struct(
@@ -479,6 +479,16 @@ def wpilib_extension(srcs = [], header_to_dat_deps = [], extra_hdrs = [], includ
("wpi::ExpansionHubServo", "wpi__ExpansionHubServo.hpp"),
],
),
struct(
class_name = "ExpansionHubVelocityConstants",
yml_file = "semiwrap/ExpansionHubVelocityConstants.yml",
header_root = "$(execpath :robotpy-native-wpilib.copy_headers)",
header_file = "$(execpath :robotpy-native-wpilib.copy_headers)/wpi/hardware/expansionhub/ExpansionHubVelocityConstants.hpp",
tmpl_class_names = [],
trampolines = [
("wpi::ExpansionHubVelocityConstants", "wpi__ExpansionHubVelocityConstants.hpp"),
],
),
struct(
class_name = "OnboardIMU",
yml_file = "semiwrap/OnboardIMU.yml",

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) {

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);
}

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;
}

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;
}

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

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;

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

View File

@@ -153,8 +153,9 @@ PWM = "wpi/hardware/discrete/PWM.hpp"
# wpi/hardware/expansionhub
ExpansionHub = "wpi/hardware/expansionhub/ExpansionHub.hpp"
ExpansionHubMotor = "wpi/hardware/expansionhub/ExpansionHubMotor.hpp"
ExpansionHubPidConstants = "wpi/hardware/expansionhub/ExpansionHubPidConstants.hpp"
ExpansionHubPositionConstants = "wpi/hardware/expansionhub/ExpansionHubPositionConstants.hpp"
ExpansionHubServo = "wpi/hardware/expansionhub/ExpansionHubServo.hpp"
ExpansionHubVelocityConstants = "wpi/hardware/expansionhub/ExpansionHubVelocityConstants.hpp"
# wpi/hardware/imu
OnboardIMU = "wpi/hardware/imu/OnboardIMU.hpp"

View File

@@ -14,7 +14,7 @@ classes:
GetEncoderPosition:
SetReversed:
ResetEncoder:
GetVelocityPidConstants:
GetPositionPidConstants:
IsHubConnected:
Follow:
GetVelocityConstants:
GetPositionConstants:

View File

@@ -1,7 +1,7 @@
classes:
wpi::ExpansionHubPidConstants:
wpi::ExpansionHubPositionConstants:
methods:
SetPID:
SetFF:
SetS:
EnableContinuousInput:
DisableContinuousInput:

View File

@@ -0,0 +1,5 @@
classes:
wpi::ExpansionHubVelocityConstants:
methods:
SetPID:
SetFF:

View File

@@ -29,8 +29,9 @@ from ._wpilib import (
EventLoop,
ExpansionHub,
ExpansionHubMotor,
ExpansionHubPidConstants,
ExpansionHubPositionConstants,
ExpansionHubServo,
ExpansionHubVelocityConstants,
Field2d,
FieldObject2d,
Gamepad,
@@ -135,8 +136,9 @@ __all__ = [
"EventLoop",
"ExpansionHub",
"ExpansionHubMotor",
"ExpansionHubPidConstants",
"ExpansionHubPositionConstants",
"ExpansionHubServo",
"ExpansionHubVelocityConstants",
"Field2d",
"FieldObject2d",
"Gamepad",