From 7fc884542412f9d93e0231cff3d443568700176c Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Thu, 23 Apr 2026 20:40:52 -0400 Subject: [PATCH] [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 --- wpilibc/robotpy_pybind_build_info.bzl | 18 ++- .../expansionhub/ExpansionHubMotor.cpp | 14 ++- .../expansionhub/ExpansionHubPidConstants.cpp | 97 --------------- .../ExpansionHubPositionConstants.cpp | 94 +++++++++++++++ .../ExpansionHubVelocityConstants.cpp | 74 ++++++++++++ .../expansionhub/ExpansionHubMotor.hpp | 11 +- ....hpp => ExpansionHubPositionConstants.hpp} | 49 ++++---- .../ExpansionHubVelocityConstants.hpp | 64 ++++++++++ wpilibc/src/main/python/pyproject.toml | 3 +- .../python/semiwrap/ExpansionHubMotor.yml | 4 +- ....yml => ExpansionHubPositionConstants.yml} | 4 +- .../ExpansionHubVelocityConstants.yml | 5 + wpilibc/src/main/python/wpilib/__init__.py | 6 +- .../expansionhub/ExpansionHubMotor.java | 20 ++-- ...ava => ExpansionHubPositionConstants.java} | 75 ++++++------ .../ExpansionHubVelocityConstants.java | 111 ++++++++++++++++++ 16 files changed, 458 insertions(+), 191 deletions(-) delete mode 100644 wpilibc/src/main/native/cpp/hardware/expansionhub/ExpansionHubPidConstants.cpp create mode 100644 wpilibc/src/main/native/cpp/hardware/expansionhub/ExpansionHubPositionConstants.cpp create mode 100644 wpilibc/src/main/native/cpp/hardware/expansionhub/ExpansionHubVelocityConstants.cpp rename wpilibc/src/main/native/include/wpi/hardware/expansionhub/{ExpansionHubPidConstants.hpp => ExpansionHubPositionConstants.hpp} (51%) create mode 100644 wpilibc/src/main/native/include/wpi/hardware/expansionhub/ExpansionHubVelocityConstants.hpp rename wpilibc/src/main/python/semiwrap/{ExpansionHubPidConstants.yml => ExpansionHubPositionConstants.yml} (65%) create mode 100644 wpilibc/src/main/python/semiwrap/ExpansionHubVelocityConstants.yml rename wpilibj/src/main/java/org/wpilib/hardware/expansionhub/{ExpansionHubPidConstants.java => ExpansionHubPositionConstants.java} (70%) create mode 100644 wpilibj/src/main/java/org/wpilib/hardware/expansionhub/ExpansionHubVelocityConstants.java diff --git a/wpilibc/robotpy_pybind_build_info.bzl b/wpilibc/robotpy_pybind_build_info.bzl index 3c4f7f512c..96b991a630 100644 --- a/wpilibc/robotpy_pybind_build_info.bzl +++ b/wpilibc/robotpy_pybind_build_info.bzl @@ -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", diff --git a/wpilibc/src/main/native/cpp/hardware/expansionhub/ExpansionHubMotor.cpp b/wpilibc/src/main/native/cpp/hardware/expansionhub/ExpansionHubMotor.cpp index f856356699..5913ea75cc 100644 --- a/wpilibc/src/main/native/cpp/hardware/expansionhub/ExpansionHubMotor.cpp +++ b/wpilibc/src/main/native/cpp/hardware/expansionhub/ExpansionHubMotor.cpp @@ -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) { diff --git a/wpilibc/src/main/native/cpp/hardware/expansionhub/ExpansionHubPidConstants.cpp b/wpilibc/src/main/native/cpp/hardware/expansionhub/ExpansionHubPidConstants.cpp deleted file mode 100644 index f67e3c0718..0000000000 --- a/wpilibc/src/main/native/cpp/hardware/expansionhub/ExpansionHubPidConstants.cpp +++ /dev/null @@ -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 - -#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); -} diff --git a/wpilibc/src/main/native/cpp/hardware/expansionhub/ExpansionHubPositionConstants.cpp b/wpilibc/src/main/native/cpp/hardware/expansionhub/ExpansionHubPositionConstants.cpp new file mode 100644 index 0000000000..8a31912b1d --- /dev/null +++ b/wpilibc/src/main/native/cpp/hardware/expansionhub/ExpansionHubPositionConstants.cpp @@ -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; +} diff --git a/wpilibc/src/main/native/cpp/hardware/expansionhub/ExpansionHubVelocityConstants.cpp b/wpilibc/src/main/native/cpp/hardware/expansionhub/ExpansionHubVelocityConstants.cpp new file mode 100644 index 0000000000..289746510d --- /dev/null +++ b/wpilibc/src/main/native/cpp/hardware/expansionhub/ExpansionHubVelocityConstants.cpp @@ -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; +} diff --git a/wpilibc/src/main/native/include/wpi/hardware/expansionhub/ExpansionHubMotor.hpp b/wpilibc/src/main/native/include/wpi/hardware/expansionhub/ExpansionHubMotor.hpp index c4a382839b..58b15c1662 100644 --- a/wpilibc/src/main/native/include/wpi/hardware/expansionhub/ExpansionHubMotor.hpp +++ b/wpilibc/src/main/native/include/wpi/hardware/expansionhub/ExpansionHubMotor.hpp @@ -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 diff --git a/wpilibc/src/main/native/include/wpi/hardware/expansionhub/ExpansionHubPidConstants.hpp b/wpilibc/src/main/native/include/wpi/hardware/expansionhub/ExpansionHubPositionConstants.hpp similarity index 51% rename from wpilibc/src/main/native/include/wpi/hardware/expansionhub/ExpansionHubPidConstants.hpp rename to wpilibc/src/main/native/include/wpi/hardware/expansionhub/ExpansionHubPositionConstants.hpp index aec05d5fb6..1dc01733f8 100644 --- a/wpilibc/src/main/native/include/wpi/hardware/expansionhub/ExpansionHubPidConstants.hpp +++ b/wpilibc/src/main/native/include/wpi/hardware/expansionhub/ExpansionHubPositionConstants.hpp @@ -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; diff --git a/wpilibc/src/main/native/include/wpi/hardware/expansionhub/ExpansionHubVelocityConstants.hpp b/wpilibc/src/main/native/include/wpi/hardware/expansionhub/ExpansionHubVelocityConstants.hpp new file mode 100644 index 0000000000..1eb96d3a88 --- /dev/null +++ b/wpilibc/src/main/native/include/wpi/hardware/expansionhub/ExpansionHubVelocityConstants.hpp @@ -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 diff --git a/wpilibc/src/main/python/pyproject.toml b/wpilibc/src/main/python/pyproject.toml index 73c81e69d6..d32ccdaaca 100644 --- a/wpilibc/src/main/python/pyproject.toml +++ b/wpilibc/src/main/python/pyproject.toml @@ -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" diff --git a/wpilibc/src/main/python/semiwrap/ExpansionHubMotor.yml b/wpilibc/src/main/python/semiwrap/ExpansionHubMotor.yml index cdc2f67712..e9f030494f 100644 --- a/wpilibc/src/main/python/semiwrap/ExpansionHubMotor.yml +++ b/wpilibc/src/main/python/semiwrap/ExpansionHubMotor.yml @@ -14,7 +14,7 @@ classes: GetEncoderPosition: SetReversed: ResetEncoder: - GetVelocityPidConstants: - GetPositionPidConstants: IsHubConnected: Follow: + GetVelocityConstants: + GetPositionConstants: diff --git a/wpilibc/src/main/python/semiwrap/ExpansionHubPidConstants.yml b/wpilibc/src/main/python/semiwrap/ExpansionHubPositionConstants.yml similarity index 65% rename from wpilibc/src/main/python/semiwrap/ExpansionHubPidConstants.yml rename to wpilibc/src/main/python/semiwrap/ExpansionHubPositionConstants.yml index 9c7f18ec01..73860f6661 100644 --- a/wpilibc/src/main/python/semiwrap/ExpansionHubPidConstants.yml +++ b/wpilibc/src/main/python/semiwrap/ExpansionHubPositionConstants.yml @@ -1,7 +1,7 @@ classes: - wpi::ExpansionHubPidConstants: + wpi::ExpansionHubPositionConstants: methods: SetPID: - SetFF: + SetS: EnableContinuousInput: DisableContinuousInput: diff --git a/wpilibc/src/main/python/semiwrap/ExpansionHubVelocityConstants.yml b/wpilibc/src/main/python/semiwrap/ExpansionHubVelocityConstants.yml new file mode 100644 index 0000000000..9099843150 --- /dev/null +++ b/wpilibc/src/main/python/semiwrap/ExpansionHubVelocityConstants.yml @@ -0,0 +1,5 @@ +classes: + wpi::ExpansionHubVelocityConstants: + methods: + SetPID: + SetFF: diff --git a/wpilibc/src/main/python/wpilib/__init__.py b/wpilibc/src/main/python/wpilib/__init__.py index 755f2b8f66..0badc582da 100644 --- a/wpilibc/src/main/python/wpilib/__init__.py +++ b/wpilibc/src/main/python/wpilib/__init__.py @@ -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", diff --git a/wpilibj/src/main/java/org/wpilib/hardware/expansionhub/ExpansionHubMotor.java b/wpilibj/src/main/java/org/wpilib/hardware/expansionhub/ExpansionHubMotor.java index 1b98df3a0a..911b26c1fe 100644 --- a/wpilibj/src/main/java/org/wpilib/hardware/expansionhub/ExpansionHubMotor.java +++ b/wpilibj/src/main/java/org/wpilib/hardware/expansionhub/ExpansionHubMotor.java @@ -45,8 +45,8 @@ public class ExpansionHubMotor implements AutoCloseable { private final DoublePublisher m_distancePerCountPublisher; - private final ExpansionHubPidConstants m_velocityPidConstants; - private final ExpansionHubPidConstants m_positionPidConstants; + private final ExpansionHubVelocityConstants m_velocityPidConstants; + private final ExpansionHubPositionConstants m_positionPidConstants; /** * Constructs a servo at the requested channel on a specific USB port. @@ -124,8 +124,8 @@ public class ExpansionHubMotor implements AutoCloseable { .getBooleanTopic("/rhsp/" + usbId + "/motor" + channel + "/resetEncoder") .publish(options); - m_velocityPidConstants = new ExpansionHubPidConstants(usbId, channel, true); - m_positionPidConstants = new ExpansionHubPidConstants(usbId, channel, false); + m_velocityPidConstants = new ExpansionHubVelocityConstants(usbId, channel); + m_positionPidConstants = new ExpansionHubPositionConstants(usbId, channel); } /** Closes a motor so another instance can be constructed. */ @@ -275,20 +275,20 @@ public class ExpansionHubMotor implements AutoCloseable { } /** - * Gets the PID constants object for velocity PID. + * Gets the motor constants object for velocity PID. * - * @return Velocity PID constants object + * @return Velocity motor constants object */ - public ExpansionHubPidConstants getVelocityPidConstants() { + public ExpansionHubVelocityConstants getVelocityConstants() { return m_velocityPidConstants; } /** - * Gets the PID constants object for position PID. + * Gets the motor constants object for position PID. * - * @return Position PID constants object + * @return Position motor constants object */ - public ExpansionHubPidConstants getPositionPidConstants() { + public ExpansionHubPositionConstants getPositionConstants() { return m_positionPidConstants; } diff --git a/wpilibj/src/main/java/org/wpilib/hardware/expansionhub/ExpansionHubPidConstants.java b/wpilibj/src/main/java/org/wpilib/hardware/expansionhub/ExpansionHubPositionConstants.java similarity index 70% rename from wpilibj/src/main/java/org/wpilib/hardware/expansionhub/ExpansionHubPidConstants.java rename to wpilibj/src/main/java/org/wpilib/hardware/expansionhub/ExpansionHubPositionConstants.java index f0f493cd71..f10c6dd9ba 100644 --- a/wpilibj/src/main/java/org/wpilib/hardware/expansionhub/ExpansionHubPidConstants.java +++ b/wpilibj/src/main/java/org/wpilib/hardware/expansionhub/ExpansionHubPositionConstants.java @@ -10,20 +10,19 @@ import org.wpilib.networktables.NetworkTableInstance; import org.wpilib.networktables.PubSubOption; import org.wpilib.system.SystemServer; -/** This class contains PID constants for an ExpansionHub motor. */ -public class ExpansionHubPidConstants { +/** This class contains feedback and feedforward constants for an ExpansionHub motor. */ +public class ExpansionHubPositionConstants { private final DoublePublisher m_pPublisher; private final DoublePublisher m_iPublisher; private final DoublePublisher m_dPublisher; + private final DoublePublisher m_sPublisher; - private final DoublePublisher m_vPublisher; - private final DoublePublisher m_aPublisher; private final BooleanPublisher m_continuousPublisher; private final DoublePublisher m_continuousMinimumPublisher; private final DoublePublisher m_continuousMaximumPublisher; - ExpansionHubPidConstants(int hubNumber, int motorNumber, boolean isVelocityPid) { + ExpansionHubPositionConstants(int hubNumber, int motorNumber) { NetworkTableInstance systemServer = SystemServer.getSystemServer(); PubSubOption[] options = @@ -31,48 +30,39 @@ public class ExpansionHubPidConstants { PubSubOption.SEND_ALL, PubSubOption.KEEP_DUPLICATES, PubSubOption.periodic(0.005) }; - String pidType = isVelocityPid ? "velocity" : "position"; - m_pPublisher = systemServer .getDoubleTopic( - "/rhsp/" + hubNumber + "/motor" + motorNumber + "/pid/" + pidType + "/kp") + "/rhsp/" + hubNumber + "/motor" + motorNumber + "/constants/position" + "/kp") .publish(options); m_iPublisher = systemServer .getDoubleTopic( - "/rhsp/" + hubNumber + "/motor" + motorNumber + "/pid/" + pidType + "/ki") + "/rhsp/" + hubNumber + "/motor" + motorNumber + "/constants/position" + "/ki") .publish(options); m_dPublisher = systemServer .getDoubleTopic( - "/rhsp/" + hubNumber + "/motor" + motorNumber + "/pid/" + pidType + "/kd") - .publish(options); - - m_aPublisher = - systemServer - .getDoubleTopic( - "/rhsp/" + hubNumber + "/motor" + motorNumber + "/pid/" + pidType + "/ka") - .publish(options); - - m_vPublisher = - systemServer - .getDoubleTopic( - "/rhsp/" + hubNumber + "/motor" + motorNumber + "/pid/" + pidType + "/kv") + "/rhsp/" + hubNumber + "/motor" + motorNumber + "/constants/position" + "/kd") .publish(options); m_sPublisher = systemServer .getDoubleTopic( - "/rhsp/" + hubNumber + "/motor" + motorNumber + "/pid/" + pidType + "/ks") + "/rhsp/" + hubNumber + "/motor" + motorNumber + "/constants/position" + "/ks") .publish(options); m_continuousPublisher = systemServer .getBooleanTopic( - "/rhsp/" + hubNumber + "/motor" + motorNumber + "/pid/" + pidType + "/continuous") + "/rhsp/" + + hubNumber + + "/motor" + + motorNumber + + "/constants/position" + + "/continuous") .publish(options); m_continuousMinimumPublisher = @@ -82,8 +72,7 @@ public class ExpansionHubPidConstants { + hubNumber + "/motor" + motorNumber - + "/pid/" - + pidType + + "/constants/position" + "/continuousMinimum") .publish(options); @@ -94,8 +83,7 @@ public class ExpansionHubPidConstants { + hubNumber + "/motor" + motorNumber - + "/pid/" - + pidType + + "/constants/position" + "/continuousMaximum") .publish(options); } @@ -108,11 +96,13 @@ public class ExpansionHubPidConstants { * @param p The proportional coefficient. * @param i The integral coefficient. * @param d The derivative coefficient. + * @return This object, for method chaining. */ - public void setPID(double p, double i, double d) { + public ExpansionHubPositionConstants setPID(double p, double i, double d) { m_pPublisher.set(p); m_iPublisher.set(i); m_dPublisher.set(d); + return this; } /** @@ -120,16 +110,14 @@ public 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. */ - public void setFF(double s, double v, double a) { + public ExpansionHubPositionConstants setS(double s) { m_sPublisher.set(s); - m_vPublisher.set(v); - m_aPublisher.set(a); + return this; } /** @@ -140,24 +128,31 @@ public 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. */ - public void enableContinuousInput(double minimumInput, double maximumInput) { + public ExpansionHubPositionConstants enableContinuousInput( + double minimumInput, double maximumInput) { m_continuousMaximumPublisher.set(maximumInput); m_continuousMinimumPublisher.set(minimumInput); m_continuousPublisher.set(true); + return this; } - /** Disable continuous input mode. */ - public void disableContinuousInput() { + /** + * Disable continuous input mode. + * + * @return This object, for method chaining. + */ + public ExpansionHubPositionConstants disableContinuousInput() { m_continuousPublisher.set(false); + return this; } void close() { + m_pPublisher.close(); m_iPublisher.close(); m_dPublisher.close(); m_sPublisher.close(); - m_vPublisher.close(); - m_aPublisher.close(); m_continuousPublisher.close(); m_continuousMinimumPublisher.close(); m_continuousMaximumPublisher.close(); diff --git a/wpilibj/src/main/java/org/wpilib/hardware/expansionhub/ExpansionHubVelocityConstants.java b/wpilibj/src/main/java/org/wpilib/hardware/expansionhub/ExpansionHubVelocityConstants.java new file mode 100644 index 0000000000..5b65e4c8c6 --- /dev/null +++ b/wpilibj/src/main/java/org/wpilib/hardware/expansionhub/ExpansionHubVelocityConstants.java @@ -0,0 +1,111 @@ +// 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. + +package org.wpilib.hardware.expansionhub; + +import org.wpilib.networktables.DoublePublisher; +import org.wpilib.networktables.NetworkTableInstance; +import org.wpilib.networktables.PubSubOption; +import org.wpilib.system.SystemServer; + +/** This class contains feedback and feedforward constants for an ExpansionHub motor. */ +public class ExpansionHubVelocityConstants { + private final DoublePublisher m_pPublisher; + private final DoublePublisher m_iPublisher; + private final DoublePublisher m_dPublisher; + + private final DoublePublisher m_sPublisher; + private final DoublePublisher m_vPublisher; + private final DoublePublisher m_aPublisher; + + ExpansionHubVelocityConstants(int hubNumber, int motorNumber) { + NetworkTableInstance systemServer = SystemServer.getSystemServer(); + + PubSubOption[] options = + new PubSubOption[] { + PubSubOption.SEND_ALL, PubSubOption.KEEP_DUPLICATES, PubSubOption.periodic(0.005) + }; + + m_pPublisher = + systemServer + .getDoubleTopic( + "/rhsp/" + hubNumber + "/motor" + motorNumber + "/constants/velocity" + "/kp") + .publish(options); + + m_iPublisher = + systemServer + .getDoubleTopic( + "/rhsp/" + hubNumber + "/motor" + motorNumber + "/constants/velocity" + "/ki") + .publish(options); + + m_dPublisher = + systemServer + .getDoubleTopic( + "/rhsp/" + hubNumber + "/motor" + motorNumber + "/constants/velocity" + "/kd") + .publish(options); + + m_sPublisher = + systemServer + .getDoubleTopic( + "/rhsp/" + hubNumber + "/motor" + motorNumber + "/constants/velocity" + "/ks") + .publish(options); + + m_vPublisher = + systemServer + .getDoubleTopic( + "/rhsp/" + hubNumber + "/motor" + motorNumber + "/constants/velocity" + "/kv") + .publish(options); + + m_aPublisher = + systemServer + .getDoubleTopic( + "/rhsp/" + hubNumber + "/motor" + motorNumber + "/constants/velocity" + "/ka") + .publish(options); + } + + /** + * 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. + */ + public ExpansionHubVelocityConstants setPID(double p, double i, double d) { + m_pPublisher.set(p); + m_iPublisher.set(i); + m_dPublisher.set(d); + return this; + } + + /** + * 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. + */ + public ExpansionHubVelocityConstants setFF(double s, double v, double a) { + m_sPublisher.set(s); + m_vPublisher.set(v); + m_aPublisher.set(a); + return this; + } + + void close() { + m_pPublisher.close(); + m_iPublisher.close(); + m_dPublisher.close(); + m_sPublisher.close(); + m_vPublisher.close(); + m_aPublisher.close(); + } +}