chore: Remove reverse option for PID (useless?)

include/regulator/PID.hpp

@@ -72,14 +72,6 @@ public:
      */
     void reset();
 
-    /**
-     * The PID will either be connected to a DIRECT acting process (+Output leads
-     * to +Input) or a REVERSE acting process(+Output leads to -Input.)  we need to
-     * know which one, because otherwise we may increase the output when we should
-     * be decreasing.
-     */
-    void setReverse(bool reverse);
-
 private:
     const PID::Gains &m_gains;
 
@@ -90,8 +82,6 @@ private:
 
     double m_minOutput;
     double m_maxOutput;
-
-    bool m_reverse;
 };
 
 #endif

src/regulator/PID.cpp

@@ -2,20 +2,14 @@
 
 #include "Math.hpp"
 
-//TODO(cjaverliat): Check that errors is initialized with zeroes
 PID::PID(const PID::Gains &gains, double minOutput, double maxOutput) : m_gains(gains),
                                                                         m_minOutput(minOutput),
-                                                                        m_maxOutput(maxOutput),
-                                                                        m_reverse(false)
+                                                                        m_maxOutput(maxOutput)
 {
 }
 
 double PID::compute(double dt)
 {
-    double kp = m_reverse ? -m_gains.Kp : m_gains.Kp;
-    double ki = m_reverse ? -m_gains.Ki : m_gains.Ki;
-    double kd = m_reverse ? -m_gains.Kd : m_gains.Kd;
-
     double proportional;
     double integral;
     double derivative;
@@ -28,11 +22,11 @@ double PID::compute(double dt)
         m_errors.lastErrorSet = true;
     }
 
-    proportional = kp * m_errors.error;
+    proportional = m_gains.Kp * m_errors.error;
 
     // TODO(cjaverliat): Use trapezoidal integration instead
     m_errors.accumulator += m_errors.error * dt;
-    integral = Math::clamp(ki * m_errors.accumulator, m_minOutput, m_maxOutput);
+    integral = Math::clamp(m_gains.Ki * m_errors.accumulator, m_minOutput, m_maxOutput);
 
     if (dt == 0)
     {
@@ -40,7 +34,7 @@ double PID::compute(double dt)
     }
     else
     {
-        derivative = kd * (m_errors.error - m_errors.lastError) / dt;
+        derivative = m_gains.Kd * (m_errors.error - m_errors.lastError) / dt;
     }
 
     m_errors.lastError = m_errors.error;
@@ -48,11 +42,6 @@ double PID::compute(double dt)
     return Math::clamp(proportional + integral + derivative, m_minOutput, m_maxOutput);
 }
 
-void PID::setReverse(bool reverse)
-{
-    m_reverse = reverse;
-}
-
 void PID::setSetpoint(double setpoint) { m_setpoint = setpoint; }
 
 double PID::getSetpoint() { return m_setpoint; }

src/regulator/SpeedRegulator.cpp

@@ -33,12 +33,6 @@ void SpeedRegulator::update(double dt)
 
 void SpeedRegulator::setSetpoint(double linearSpeed, double angularSpeed)
 {
-    double leftSp = linearSpeed - angularSpeed * (m_odometer.getGeometry().wheelbaseNominal / 2.0);
-    double rightSp = linearSpeed + angularSpeed * (m_odometer.getGeometry().wheelbaseNominal / 2.0);
-
-    m_leftMotorPid.setReverse(leftSp < 0);
-    m_rightMotorPid.setReverse(rightSp < 0);
-
-    m_leftMotorPid.setSetpoint(leftSp);
-    m_rightMotorPid.setSetpoint(rightSp);
+    m_leftMotorPid.setSetpoint(linearSpeed - angularSpeed * (m_odometer.getGeometry().wheelbaseNominal / 2.0));
+    m_rightMotorPid.setSetpoint(linearSpeed + angularSpeed * (m_odometer.getGeometry().wheelbaseNominal / 2.0));
 }