From 60da9441fcca706b29dfdcdc8cf7e350a29e5be8 Mon Sep 17 00:00:00 2001 From: Sid <114190535+siddharth-shah121@users.noreply.github.com> Date: Wed, 13 May 2026 19:34:32 -0400 Subject: [PATCH 01/12] Update PID coefficients in setPID method --- .../expansionhub/ExpansionHubVelocityConstants.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/wpilibj/src/main/java/org/wpilib/hardware/expansionhub/ExpansionHubVelocityConstants.java b/wpilibj/src/main/java/org/wpilib/hardware/expansionhub/ExpansionHubVelocityConstants.java index 5b65e4c8c64..43a43b242eb 100644 --- a/wpilibj/src/main/java/org/wpilib/hardware/expansionhub/ExpansionHubVelocityConstants.java +++ b/wpilibj/src/main/java/org/wpilib/hardware/expansionhub/ExpansionHubVelocityConstants.java @@ -75,9 +75,9 @@ public class ExpansionHubVelocityConstants { * @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); + m_pPublisher.set(1); + m_iPublisher.set(0); + m_dPublisher.set(0.01); return this; } From 3f38d22fb0ac08c100a02803d72b9d752a4d1102 Mon Sep 17 00:00:00 2001 From: Sid <114190535+siddharth-shah121@users.noreply.github.com> Date: Wed, 13 May 2026 19:35:23 -0400 Subject: [PATCH 02/12] Update PID coefficients in setPID method --- .../expansionhub/ExpansionHubPositionConstants.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/wpilibj/src/main/java/org/wpilib/hardware/expansionhub/ExpansionHubPositionConstants.java b/wpilibj/src/main/java/org/wpilib/hardware/expansionhub/ExpansionHubPositionConstants.java index f10c6dd9baf..14ac15c9e70 100644 --- a/wpilibj/src/main/java/org/wpilib/hardware/expansionhub/ExpansionHubPositionConstants.java +++ b/wpilibj/src/main/java/org/wpilib/hardware/expansionhub/ExpansionHubPositionConstants.java @@ -99,9 +99,9 @@ public class ExpansionHubPositionConstants { * @return This object, for method chaining. */ public ExpansionHubPositionConstants setPID(double p, double i, double d) { - m_pPublisher.set(p); - m_iPublisher.set(i); - m_dPublisher.set(d); + m_pPublisher.set(1); + m_iPublisher.set(0); + m_dPublisher.set(0.01); return this; } From 1401246a39eef0ac9d18ea37ed959cb9a535920f Mon Sep 17 00:00:00 2001 From: Sid <114190535+siddharth-shah121@users.noreply.github.com> Date: Wed, 13 May 2026 19:43:15 -0400 Subject: [PATCH 03/12] Changed pid to go below Update PID coefficients in setPID method. --- .../hardware/expansionhub/ExpansionHubPositionConstants.java | 3 +++ 1 file changed, 3 insertions(+) diff --git a/wpilibj/src/main/java/org/wpilib/hardware/expansionhub/ExpansionHubPositionConstants.java b/wpilibj/src/main/java/org/wpilib/hardware/expansionhub/ExpansionHubPositionConstants.java index 14ac15c9e70..b66ea8bbc51 100644 --- a/wpilibj/src/main/java/org/wpilib/hardware/expansionhub/ExpansionHubPositionConstants.java +++ b/wpilibj/src/main/java/org/wpilib/hardware/expansionhub/ExpansionHubPositionConstants.java @@ -99,6 +99,9 @@ public class ExpansionHubPositionConstants { * @return This object, for method chaining. */ public ExpansionHubPositionConstants setPID(double p, double i, double d) { + m_pPublisher.set(p); + m_iPublisher.set(i); + m_dPublisher.set(d); m_pPublisher.set(1); m_iPublisher.set(0); m_dPublisher.set(0.01); From 0c0c6db97603107ce5ab59cee55607816dc7d7fd Mon Sep 17 00:00:00 2001 From: Sid <114190535+siddharth-shah121@users.noreply.github.com> Date: Wed, 13 May 2026 19:45:03 -0400 Subject: [PATCH 04/12] Update ExpansionHubVelocityConstants.java From 5689072d536ef4c102c9dea422933fce3e89dc11 Mon Sep 17 00:00:00 2001 From: Sid <114190535+siddharth-shah121@users.noreply.github.com> Date: Wed, 13 May 2026 19:45:30 -0400 Subject: [PATCH 05/12] Set fixed PID values in setPID method Update PID coefficients to fixed values. --- .../hardware/expansionhub/ExpansionHubPositionConstants.java | 3 --- 1 file changed, 3 deletions(-) diff --git a/wpilibj/src/main/java/org/wpilib/hardware/expansionhub/ExpansionHubPositionConstants.java b/wpilibj/src/main/java/org/wpilib/hardware/expansionhub/ExpansionHubPositionConstants.java index b66ea8bbc51..14ac15c9e70 100644 --- a/wpilibj/src/main/java/org/wpilib/hardware/expansionhub/ExpansionHubPositionConstants.java +++ b/wpilibj/src/main/java/org/wpilib/hardware/expansionhub/ExpansionHubPositionConstants.java @@ -99,9 +99,6 @@ public class ExpansionHubPositionConstants { * @return This object, for method chaining. */ public ExpansionHubPositionConstants setPID(double p, double i, double d) { - m_pPublisher.set(p); - m_iPublisher.set(i); - m_dPublisher.set(d); m_pPublisher.set(1); m_iPublisher.set(0); m_dPublisher.set(0.01); From 68cf7a812bc0a88afa4ba311e3a25fb3ac6a744c Mon Sep 17 00:00:00 2001 From: Sid <114190535+siddharth-shah121@users.noreply.github.com> Date: Wed, 13 May 2026 19:57:26 -0400 Subject: [PATCH 06/12] Refactor PID settings in ExpansionHubPositionConstants Updated PID default values and modified setPID method to accept parameters. --- .../ExpansionHubPositionConstants.java | 41 +++++++++++-------- 1 file changed, 25 insertions(+), 16 deletions(-) diff --git a/wpilibj/src/main/java/org/wpilib/hardware/expansionhub/ExpansionHubPositionConstants.java b/wpilibj/src/main/java/org/wpilib/hardware/expansionhub/ExpansionHubPositionConstants.java index 14ac15c9e70..d91ee346d7f 100644 --- a/wpilibj/src/main/java/org/wpilib/hardware/expansionhub/ExpansionHubPositionConstants.java +++ b/wpilibj/src/main/java/org/wpilib/hardware/expansionhub/ExpansionHubPositionConstants.java @@ -12,6 +12,7 @@ /** 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; @@ -23,35 +24,43 @@ public class ExpansionHubPositionConstants { private final DoublePublisher m_continuousMaximumPublisher; ExpansionHubPositionConstants(int hubNumber, int motorNumber) { + NetworkTableInstance systemServer = SystemServer.getSystemServer(); PubSubOption[] options = new PubSubOption[] { - PubSubOption.SEND_ALL, PubSubOption.KEEP_DUPLICATES, PubSubOption.periodic(0.005) + PubSubOption.SEND_ALL, + PubSubOption.KEEP_DUPLICATES, + PubSubOption.periodic(0.005) }; m_pPublisher = systemServer .getDoubleTopic( - "/rhsp/" + hubNumber + "/motor" + motorNumber + "/constants/position" + "/kp") + "/rhsp/" + hubNumber + "/motor" + motorNumber + "/constants/position/kp") .publish(options); m_iPublisher = systemServer .getDoubleTopic( - "/rhsp/" + hubNumber + "/motor" + motorNumber + "/constants/position" + "/ki") + "/rhsp/" + hubNumber + "/motor" + motorNumber + "/constants/position/ki") .publish(options); m_dPublisher = systemServer .getDoubleTopic( - "/rhsp/" + hubNumber + "/motor" + motorNumber + "/constants/position" + "/kd") + "/rhsp/" + hubNumber + "/motor" + motorNumber + "/constants/position/kd") .publish(options); + // Default PID values + m_pPublisher.setDefault(1.0); + m_iPublisher.setDefault(0.0); + m_dPublisher.setDefault(0.01); + m_sPublisher = systemServer .getDoubleTopic( - "/rhsp/" + hubNumber + "/motor" + motorNumber + "/constants/position" + "/ks") + "/rhsp/" + hubNumber + "/motor" + motorNumber + "/constants/position/ks") .publish(options); m_continuousPublisher = @@ -61,8 +70,7 @@ public class ExpansionHubPositionConstants { + hubNumber + "/motor" + motorNumber - + "/constants/position" - + "/continuous") + + "/constants/position/continuous") .publish(options); m_continuousMinimumPublisher = @@ -72,8 +80,7 @@ public class ExpansionHubPositionConstants { + hubNumber + "/motor" + motorNumber - + "/constants/position" - + "/continuousMinimum") + + "/constants/position/continuousMinimum") .publish(options); m_continuousMaximumPublisher = @@ -83,8 +90,7 @@ public class ExpansionHubPositionConstants { + hubNumber + "/motor" + motorNumber - + "/constants/position" - + "/continuousMaximum") + + "/constants/position/continuousMaximum") .publish(options); } @@ -99,9 +105,9 @@ public class ExpansionHubPositionConstants { * @return This object, for method chaining. */ public ExpansionHubPositionConstants setPID(double p, double i, double d) { - m_pPublisher.set(1); - m_iPublisher.set(0); - m_dPublisher.set(0.01); + m_pPublisher.set(p); + m_iPublisher.set(i); + m_dPublisher.set(d); return this; } @@ -123,8 +129,9 @@ public ExpansionHubPositionConstants setS(double s) { /** * Enables continuous input. * - *

Rather then using the max and min input range as constraints, it considers them to be the - * same point and automatically calculates the shortest route to the setpoint. + *

Rather then using the max and min input range as constraints, it considers + * them to be the same point and automatically calculates the shortest route to + * the setpoint. * * @param minimumInput The minimum value expected from the input. * @param maximumInput The maximum value expected from the input. @@ -132,9 +139,11 @@ public ExpansionHubPositionConstants setS(double s) { */ public ExpansionHubPositionConstants enableContinuousInput( double minimumInput, double maximumInput) { + m_continuousMaximumPublisher.set(maximumInput); m_continuousMinimumPublisher.set(minimumInput); m_continuousPublisher.set(true); + return this; } From 0301ff7b1b1f97637bf8c45aed8967a0d891f21e Mon Sep 17 00:00:00 2001 From: Sid <114190535+siddharth-shah121@users.noreply.github.com> Date: Wed, 13 May 2026 20:02:59 -0400 Subject: [PATCH 07/12] Refactor ExpansionHubVelocityConstants constructor --- .../ExpansionHubVelocityConstants.java | 159 ++++++------------ 1 file changed, 49 insertions(+), 110 deletions(-) diff --git a/wpilibj/src/main/java/org/wpilib/hardware/expansionhub/ExpansionHubVelocityConstants.java b/wpilibj/src/main/java/org/wpilib/hardware/expansionhub/ExpansionHubVelocityConstants.java index 43a43b242eb..68ac1970654 100644 --- a/wpilibj/src/main/java/org/wpilib/hardware/expansionhub/ExpansionHubVelocityConstants.java +++ b/wpilibj/src/main/java/org/wpilib/hardware/expansionhub/ExpansionHubVelocityConstants.java @@ -1,111 +1,50 @@ -// 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(1); - m_iPublisher.set(0); - m_dPublisher.set(0.01); - 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(); - } +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_pPublisher.setDefault(1.0); + m_iPublisher.setDefault(0.0); + m_dPublisher.setDefault(0.01); + + 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); } From 42af6678cff08042e3fb1f03cfca0679c6edaad1 Mon Sep 17 00:00:00 2001 From: Sid <114190535+siddharth-shah121@users.noreply.github.com> Date: Wed, 13 May 2026 20:05:50 -0400 Subject: [PATCH 08/12] Refactor constructor and method formatting --- .../ExpansionHubPositionConstants.cpp | 53 ++++++++++++------- 1 file changed, 34 insertions(+), 19 deletions(-) diff --git a/wpilibc/src/main/native/cpp/hardware/expansionhub/ExpansionHubPositionConstants.cpp b/wpilibc/src/main/native/cpp/hardware/expansionhub/ExpansionHubPositionConstants.cpp index 8a31912b1d1..9a0ed2f2af1 100644 --- a/wpilibc/src/main/native/cpp/hardware/expansionhub/ExpansionHubPositionConstants.cpp +++ b/wpilibc/src/main/native/cpp/hardware/expansionhub/ExpansionHubPositionConstants.cpp @@ -9,8 +9,9 @@ using namespace wpi; -ExpansionHubPositionConstants::ExpansionHubPositionConstants(int hubNumber, - int motorNumber) { +ExpansionHubPositionConstants::ExpansionHubPositionConstants( + int hubNumber, int motorNumber) { + auto systemServer = SystemServer::GetSystemServer(); wpi::nt::PubSubOptions options; @@ -20,33 +21,42 @@ ExpansionHubPositionConstants::ExpansionHubPositionConstants(int hubNumber, m_pPublisher = systemServer - .GetDoubleTopic(fmt::format("/rhsp/{}/motor{}/constants/position/kp", - hubNumber, motorNumber)) + .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)) + .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)) + .GetDoubleTopic(fmt::format( + "/rhsp/{}/motor{}/constants/position/kd", + hubNumber, motorNumber)) .Publish(options); + // Default PID values + m_pPublisher.SetDefault(1.0); + m_iPublisher.SetDefault(0.0); + m_dPublisher.SetDefault(0.01); + m_sPublisher = systemServer - .GetDoubleTopic(fmt::format("/rhsp/{}/motor{}/constants/position/ks", - hubNumber, motorNumber)) + .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)) + .GetBooleanTopic(fmt::format( + "/rhsp/{}/motor{}/constants/position/continuous", + hubNumber, motorNumber)) .Publish(options); m_continuousMinimumPublisher = @@ -64,26 +74,31 @@ ExpansionHubPositionConstants::ExpansionHubPositionConstants(int hubNumber, .Publish(options); } -ExpansionHubPositionConstants& ExpansionHubPositionConstants::SetPID(double p, - double i, - double d) { +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) { +ExpansionHubPositionConstants& +ExpansionHubPositionConstants::SetS(double s) { m_sPublisher.Set(s); return *this; } ExpansionHubPositionConstants& -ExpansionHubPositionConstants::EnableContinuousInput(double minimumInput, - double maximumInput) { +ExpansionHubPositionConstants::EnableContinuousInput( + double minimumInput, + double maximumInput) { + m_continuousMaximumPublisher.Set(maximumInput); m_continuousMinimumPublisher.Set(minimumInput); m_continuousPublisher.Set(true); + return *this; } From 023c603ce59ebc6ee30b3e774b24b59ed493d608 Mon Sep 17 00:00:00 2001 From: Sid <114190535+siddharth-shah121@users.noreply.github.com> Date: Wed, 13 May 2026 20:06:44 -0400 Subject: [PATCH 09/12] Refactor ExpansionHubVelocityConstants constructor --- .../ExpansionHubVelocityConstants.cpp | 59 ++++++++++++------- 1 file changed, 39 insertions(+), 20 deletions(-) diff --git a/wpilibc/src/main/native/cpp/hardware/expansionhub/ExpansionHubVelocityConstants.cpp b/wpilibc/src/main/native/cpp/hardware/expansionhub/ExpansionHubVelocityConstants.cpp index 289746510d3..f47aab89c08 100644 --- a/wpilibc/src/main/native/cpp/hardware/expansionhub/ExpansionHubVelocityConstants.cpp +++ b/wpilibc/src/main/native/cpp/hardware/expansionhub/ExpansionHubVelocityConstants.cpp @@ -9,8 +9,10 @@ using namespace wpi; -ExpansionHubVelocityConstants::ExpansionHubVelocityConstants(int hubNumber, - int motorNumber) { +ExpansionHubVelocityConstants::ExpansionHubVelocityConstants( + int hubNumber, + int motorNumber) { + auto systemServer = SystemServer::GetSystemServer(); wpi::nt::PubSubOptions options; @@ -20,55 +22,72 @@ ExpansionHubVelocityConstants::ExpansionHubVelocityConstants(int hubNumber, m_pPublisher = systemServer - .GetDoubleTopic(fmt::format("/rhsp/{}/motor{}/constants/velocity/kp", - hubNumber, motorNumber)) + .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)) + .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)) + .GetDoubleTopic(fmt::format( + "/rhsp/{}/motor{}/constants/velocity/kd", + hubNumber, motorNumber)) .Publish(options); + // Default PID values + m_pPublisher.SetDefault(1.0); + m_iPublisher.SetDefault(0.0); + m_dPublisher.SetDefault(0.01); + m_sPublisher = systemServer - .GetDoubleTopic(fmt::format("/rhsp/{}/motor{}/constants/velocity/ks", - hubNumber, motorNumber)) + .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)) + .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)) + .GetDoubleTopic(fmt::format( + "/rhsp/{}/motor{}/constants/velocity/ka", + hubNumber, motorNumber)) .Publish(options); } -ExpansionHubVelocityConstants& ExpansionHubVelocityConstants::SetPID(double p, - double i, - double d) { +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) { +ExpansionHubVelocityConstants& +ExpansionHubVelocityConstants::SetFF(double s, + double v, + double a) { + m_sPublisher.Set(s); m_vPublisher.Set(v); m_aPublisher.Set(a); + return *this; } From 9ea935f989d7ef00825c74c762632ee02cec9ed8 Mon Sep 17 00:00:00 2001 From: Sid <114190535+siddharth-shah121@users.noreply.github.com> Date: Wed, 13 May 2026 20:11:12 -0400 Subject: [PATCH 10/12] Refactor ExpansionHubVelocityConstants constructor and methods --- .../ExpansionHubVelocityConstants.cpp | 59 +++++++------------ 1 file changed, 20 insertions(+), 39 deletions(-) diff --git a/wpilibc/src/main/native/cpp/hardware/expansionhub/ExpansionHubVelocityConstants.cpp b/wpilibc/src/main/native/cpp/hardware/expansionhub/ExpansionHubVelocityConstants.cpp index f47aab89c08..289746510d3 100644 --- a/wpilibc/src/main/native/cpp/hardware/expansionhub/ExpansionHubVelocityConstants.cpp +++ b/wpilibc/src/main/native/cpp/hardware/expansionhub/ExpansionHubVelocityConstants.cpp @@ -9,10 +9,8 @@ using namespace wpi; -ExpansionHubVelocityConstants::ExpansionHubVelocityConstants( - int hubNumber, - int motorNumber) { - +ExpansionHubVelocityConstants::ExpansionHubVelocityConstants(int hubNumber, + int motorNumber) { auto systemServer = SystemServer::GetSystemServer(); wpi::nt::PubSubOptions options; @@ -22,72 +20,55 @@ ExpansionHubVelocityConstants::ExpansionHubVelocityConstants( m_pPublisher = systemServer - .GetDoubleTopic(fmt::format( - "/rhsp/{}/motor{}/constants/velocity/kp", - hubNumber, motorNumber)) + .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)) + .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)) + .GetDoubleTopic(fmt::format("/rhsp/{}/motor{}/constants/velocity/kd", + hubNumber, motorNumber)) .Publish(options); - // Default PID values - m_pPublisher.SetDefault(1.0); - m_iPublisher.SetDefault(0.0); - m_dPublisher.SetDefault(0.01); - m_sPublisher = systemServer - .GetDoubleTopic(fmt::format( - "/rhsp/{}/motor{}/constants/velocity/ks", - hubNumber, motorNumber)) + .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)) + .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)) + .GetDoubleTopic(fmt::format("/rhsp/{}/motor{}/constants/velocity/ka", + hubNumber, motorNumber)) .Publish(options); } -ExpansionHubVelocityConstants& -ExpansionHubVelocityConstants::SetPID(double p, - double i, - double d) { - +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) { - +ExpansionHubVelocityConstants& ExpansionHubVelocityConstants::SetFF(double s, + double v, + double a) { m_sPublisher.Set(s); m_vPublisher.Set(v); m_aPublisher.Set(a); - return *this; } From 04fd1f4a22b4e692a11aafaf0a84c71d938353ad Mon Sep 17 00:00:00 2001 From: Sid <114190535+siddharth-shah121@users.noreply.github.com> Date: Wed, 13 May 2026 20:11:42 -0400 Subject: [PATCH 11/12] Set default values for velocity constants publishers --- .../hardware/expansionhub/ExpansionHubVelocityConstants.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/wpilibc/src/main/native/cpp/hardware/expansionhub/ExpansionHubVelocityConstants.cpp b/wpilibc/src/main/native/cpp/hardware/expansionhub/ExpansionHubVelocityConstants.cpp index 289746510d3..694cc328060 100644 --- a/wpilibc/src/main/native/cpp/hardware/expansionhub/ExpansionHubVelocityConstants.cpp +++ b/wpilibc/src/main/native/cpp/hardware/expansionhub/ExpansionHubVelocityConstants.cpp @@ -35,6 +35,9 @@ ExpansionHubVelocityConstants::ExpansionHubVelocityConstants(int hubNumber, .GetDoubleTopic(fmt::format("/rhsp/{}/motor{}/constants/velocity/kd", hubNumber, motorNumber)) .Publish(options); + m_pPublisher.SetDefault(1.0); + m_iPublisher.SetDefault(0.0); + m_dPublisher.SetDefault(0.01); m_sPublisher = systemServer From cfd9d49629b732f6d152fad5a5ba15785033fc5e Mon Sep 17 00:00:00 2001 From: Sid <114190535+siddharth-shah121@users.noreply.github.com> Date: Wed, 13 May 2026 20:18:18 -0400 Subject: [PATCH 12/12] Refactor ExpansionHubVelocityConstants class This class contains feedback and feedforward constants for an ExpansionHub motor, including methods to set PID and feedforward gains. --- .../ExpansionHubVelocityConstants.java | 162 ++++++++++++------ 1 file changed, 113 insertions(+), 49 deletions(-) diff --git a/wpilibj/src/main/java/org/wpilib/hardware/expansionhub/ExpansionHubVelocityConstants.java b/wpilibj/src/main/java/org/wpilib/hardware/expansionhub/ExpansionHubVelocityConstants.java index 68ac1970654..db7dd1e9596 100644 --- a/wpilibj/src/main/java/org/wpilib/hardware/expansionhub/ExpansionHubVelocityConstants.java +++ b/wpilibj/src/main/java/org/wpilib/hardware/expansionhub/ExpansionHubVelocityConstants.java @@ -1,50 +1,114 @@ -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_pPublisher.setDefault(1.0); - m_iPublisher.setDefault(0.0); - m_dPublisher.setDefault(0.01); - - 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); +// 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_pPublisher.set(1); + m_iPublisher.set(0); + m_dPublisher.set(0.01); + + 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(); + } }