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; } 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 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..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); } @@ -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; } 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..db7dd1e9596 100644 --- a/wpilibj/src/main/java/org/wpilib/hardware/expansionhub/ExpansionHubVelocityConstants.java +++ b/wpilibj/src/main/java/org/wpilib/hardware/expansionhub/ExpansionHubVelocityConstants.java @@ -44,6 +44,9 @@ public class ExpansionHubVelocityConstants { .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