Fix built in PID calculation (#11)

Author: ThadHouse

expansionhub/MotorNtState.cpp

@@ -34,14 +34,12 @@ double MotorNtState::ComputeMotorPower(double batteryVoltage) {
             return (setpoint / batteryVoltage) * reversed;
 
         case POSITION_PID_MODE:
-            return (positionPid.ComputePosition(setpoint, lastEncoderPosition,
-                                                lastEncoderVelocity) /
+            return (positionPid.Compute(setpoint, lastEncoderPosition) /
                     batteryVoltage) *
                    reversed;
 
         case VELOCITY_PID_MODE:
-            return (velocityPid.ComputeVelocity(setpoint, lastEncoderPosition,
-                                                lastEncoderVelocity) /
+            return (velocityPid.Compute(setpoint, lastEncoderVelocity) /
                     batteryVoltage) *
                    reversed;
 
@@ -51,8 +49,8 @@ double MotorNtState::ComputeMotorPower(double batteryVoltage) {
 }
 
 void MotorNtState::Initialize(const nt::NetworkTableInstance& instance,
-                            int motorNum, const std::string& busIdStr,
-                            nt::PubSubOptions options) {
+                              int motorNum, const std::string& busIdStr,
+                              nt::PubSubOptions options) {
     auto motorNumStr = std::to_string(motorNum);
     encoderPublisher = instance
                            .GetDoubleTopic("/rhsp/" + busIdStr + "/motor" +

expansionhub/PidConstants.cpp

@@ -4,8 +4,7 @@
 
 using namespace eh;
 
-double PidConstants::ComputeVelocity(double setpoint, double currentPosition,
-                                     double currentVelocity) {
+double PidConstants::Compute(double setpoint, double measurement) {
     pidController.SetPID(pSubscriber.Get(0), iSubscriber.Get(0),
                          dSubscriber.Get(0));
     if (continuousSubscriber.Get(false)) {
@@ -19,28 +18,8 @@ double PidConstants::ComputeVelocity(double setpoint, double currentPosition,
     feedForward.SetKv(units::volt_t{vSubscriber.Get(0)} / 1_mps);
     feedForward.SetKa(units::volt_t{aSubscriber.Get(0)} / 1_mps_sq);
 
-    return (feedForward.Calculate(units::meters_per_second_t{currentVelocity}) +
-            units::volt_t{pidController.Calculate(currentVelocity, setpoint)})
-        .value();
-}
-
-double PidConstants::ComputePosition(double setpoint, double currentPosition,
-                                     double currentVelocity) {
-    pidController.SetPID(pSubscriber.Get(0), iSubscriber.Get(0),
-                         dSubscriber.Get(0));
-    if (continuousSubscriber.Get(false)) {
-        pidController.EnableContinuousInput(continuousMinimumSubscriber.Get(0),
-                                            continuousMaximumSubscriber.Get(0));
-    } else {
-        pidController.DisableContinuousInput();
-    }
-
-    feedForward.SetKs(units::volt_t{sSubscriber.Get(0)});
-    feedForward.SetKv(units::volt_t{vSubscriber.Get(0)} / 1_mps);
-    feedForward.SetKa(units::volt_t{aSubscriber.Get(0)} / 1_mps_sq);
-
-    return (feedForward.Calculate(units::meters_per_second_t{currentVelocity}) +
-            units::volt_t{pidController.Calculate(currentPosition, setpoint)})
+    return (feedForward.Calculate(units::meters_per_second_t{setpoint}) +
+            units::volt_t{pidController.Calculate(measurement, setpoint)})
         .value();
 }
 

expansionhub/PidConstants.h

@@ -41,11 +41,7 @@ struct PidConstants {
                     const std::string& motorNum, const std::string& busIdStr,
                     const std::string& pidType, nt::PubSubOptions options);
 
-    double ComputeVelocity(double setpoint, double currentPosition,
-                           double currentVelocity);
-
-    double ComputePosition(double setpoint, double currentPosition,
-                           double currentVelocity);
+    double Compute(double setpoint, double measurement);
 };
 
 }  // namespace eh