Pid Refinement

CHANGELOG.md

@@ -10,6 +10,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 ### Added
 
 ### Changed
+-PID loop for ERG mode instead of P loop.
 
 ### Hardware
 

include/settings.h

@@ -253,6 +253,15 @@ const char* const DEFAULT_PASSWORD = "password";
 
 #define RUNTIMECONFIG_JSON_SIZE 1000 + DEBUG_LOG_BUFFER_SIZE
 
+// Uncomment to use guardrails for ERG mode in the stepper loop.
+// #define ERG_GUARDRAILS
+
+//Uncomment to enable the use of the power table for ERG mode.
+//#define ERG_MODE_USE_POWER_TABLE
+
+// Uncomment to use the PID controller for ERG mode.
+#define ERG_MODE_USE_PID
+
 // PowerTable Version
 #define TABLE_VERSION 5
 

src/ERG_Mode.cpp

@@ -1134,14 +1134,22 @@ void ErgMode::computeErg() {
     return;
   }
 
-  // SetPoint changed
+#ifdef ERG_MODE_USE_POWER_TABLE
+// SetPoint changed
+#ifdef ERG_MODE_USE_PID
   if (abs(this->setPoint - newWatts.getTarget()) > 20) {
+#endif
     _setPointChangeState(newCadence, newWatts);
     return;
+#ifdef ERG_MODE_USE_PID
   }
+#endif
+#endif
 
+#ifdef ERG_MODE_USE_PID
   // Setpoint unchanged
   _inSetpointState(newCadence, newWatts);
+#endif
 }
 
 void ErgMode::_setPointChangeState(int newCadence, Measurement& newWatts) {
@@ -1181,18 +1189,91 @@ void ErgMode::_setPointChangeState(int newCadence, Measurement& newWatts) {
   ergTimer += (ERG_MODE_DELAY * 2);  // Wait for power meter to register new watts
 }
 
+// INTRODUCING PID CONTROL LOOP
+// Error: Difference between TW and Current W
+
+// Proportional term: Directly Proportional to error
+// Integral term: accumulated sum of errors over time
+// Derivative term: rate of change of error
+
+// PrevError
 void ErgMode::_inSetpointState(int newCadence, Measurement& newWatts) {
+  // Setting Gains For PID Loop
+  float Kp = userConfig->getERGSensitivity() * 2;
+  float Ki = 0.1;
+  float Kd = 0.1;
+
+  static float integral  = 0.0;
+  static float prevError = 0.0;
+
+  // retrieves the current Watt output
   int watts = newWatts.getValue();
+  // retrieves target Watt output
+  int target = newWatts.getTarget();
+  // subtracting target from current watts
+  float error = target - watts;
+
+  // Defining proportional term
+  float proportional = Kp * error;
+
+  // Defining integral term
+  integral += error;
+  float integralFinal = Ki * integral;
+
+  // Clamping down integral term
+  float integralMax = 60;
+  float integralMin = -60;
+
+  if (integral > integralMax) {
+    integral = integralMax;
+  } else if (integral < integralMin) {
+    integral = integralMin;
+  }
+
+  // Defining derivative term
+  float derivative     = error - prevError;  // Difference between current and previous errors
+  float derivativeTerm = Kd * derivative;
 
-  int wattChange  = newWatts.getTarget() - watts;  // setpoint_form_trainer - current_torque => Amount to increase or decrease incline
-  float deviation = ((float)wattChange * 100.0) / ((float)newWatts.getTarget());
+  // final PID output
+  float PID_output = proportional + integralFinal + derivativeTerm;
+
+  // log proportional, integral, derivative every five seconds
+  static unsigned long lastTime = 0;
+  if (millis() - lastTime > 5000) {
+    lastTime = millis();
+    SS2K_LOG(ERG_MODE_LOG_TAG, "Proportional: %f, Integral: %f, Derivative: %f", proportional, integralFinal, derivativeTerm);
+  }
 
-  float factor     = abs(deviation) > 10 ? userConfig->getERGSensitivity() : userConfig->getERGSensitivity() / 2;
-  float newIncline = ss2k->getCurrentPosition() + (wattChange * factor);
+  // Calculate new incline
+  float newIncline = ss2k->getCurrentPosition() + PID_output;
 
+  prevError = error;
+
+  // Apply the new values
   _updateValues(newCadence, newWatts, newIncline);
 }
 
+// OLD PI FUNCTION
+//  void ErgMode::_inSetpointState(int newCadence, Measurement& newWatts) {
+//    // retrieves the current Watt output
+//    int watts = newWatts.getValue();
+
+//   // setpoint_form_trainer - current_torque => Amount to increase or decrease incline
+//   int wattChange = newWatts.getTarget() - watts;
+
+//   // Calculates how far current power is from the target power, measred as a percentage of target
+//   float deviation = ((float)wattChange * 100.0) / ((float)newWatts.getTarget());
+
+//   // retrieves the sensitivity from adjustments from userConfig
+//   float factor = abs(deviation) > 10 ? userConfig->getERGSensitivity() : userConfig->getERGSensitivity() / 2;
+
+//   // Adjusts the incline
+//   float newIncline = ss2k->getCurrentPosition() + (wattChange * factor);
+
+//   // passes to apply new cadence, power measurement and incline settings
+//   _updateValues(newCadence, newWatts, newIncline);
+// }
+
 void ErgMode::_updateValues(int newCadence, Measurement& newWatts, float newIncline) {
   rtConfig->setTargetIncline(newIncline);
   _writeLog(ss2k->getCurrentPosition(), newIncline, this->setPoint, newWatts.getTarget(), this->watts.getValue(), newWatts.getValue(), this->cadence, newCadence);

src/Main.cpp

@@ -405,6 +405,7 @@ void SS2K::moveStepper() {
     ss2k->currentPosition  = stepper->getCurrentPosition();
     if (!ss2k->externalControl) {
       if ((rtConfig->getFTMSMode() == FitnessMachineControlPointProcedure::SetTargetPower)) {
+        #ifdef ERG_GUARDRAILS
         // don't drive lower out of bounds. This is a final test that should never happen.
         if ((stepper->getCurrentPosition() > rtConfig->getTargetIncline()) && (rtConfig->watts.getValue() < rtConfig->watts.getTarget())) {
           rtConfig->setTargetIncline(stepper->getCurrentPosition() + 1);
@@ -413,6 +414,7 @@ void SS2K::moveStepper() {
         if ((stepper->getCurrentPosition() < rtConfig->getTargetIncline()) && (rtConfig->watts.getValue() > rtConfig->watts.getTarget())) {
           rtConfig->setTargetIncline(stepper->getCurrentPosition() - 1);
         }
+        #endif
         ss2k->targetPosition = rtConfig->getTargetIncline();
       } else if (rtConfig->getFTMSMode() == FitnessMachineControlPointProcedure::SetTargetResistanceLevel) {
         rtConfig->setTargetIncline(ss2k->currentPosition + ((rtConfig->resistance.getTarget() - rtConfig->resistance.getValue()) * 20));