Merge pull request #711 from doudar/Marc_Roy

This PR implements ERG mode tuning and improvements developed in collaboration with Marc Roy. The changes focus on refining PID control, improving power table lookup logic, and adding proper type conversions for sensor data.
Key Changes:

    Refactored ERG mode calculation to return position values instead of directly updating, enabling better control flow
    Added proper rounding for float-to-int conversions in sensor data (cadence and power)
    Improved ERG mode calculations with new PID gain adjustments, better time-to-target calculations, and negative result validation for homed systems

Author: doudar

.vscode/settings.json

@@ -121,4 +121,5 @@
     ],
     "idf.pythonInstallPath": "C:\\Users\\anthony\\.espressif\\tools\\idf-python\\3.11.2\\python.exe",
     "idf.flashType": "UART",
+    "liveServer.settings.port": 5501,
 }

CHANGELOG.md

@@ -9,11 +9,28 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 
 ### Added
 
+### Changed
+- Added feed forward, disabled PowerTable for ERG lookup. 
+- Added tests and removal of duplicates in pt column. 
+- Added removal of negative numbers in pt table. 
+
+### Hardware
+
+
+## [25.12.28]
+
+### Added
+
 ### Changed
 - Removed >0 watts requirement to compute ERG.
 - Filter cadence for crazy values. Only >0 && <250 now accepted.
 - Filter watts for crazy values. Only >0 && <3000 now accepted. 
 - Fixed bug where scans may not happen even when configured devices aren't connected.  
+- Worked with Mark Roy to tune PID.
+- Added proper rounding from float to int for power and cadence.
+- More ERG tweaks for Marc Roy. 
+- If homed, we throw out negative PowerTable returns. 
+- After startup homing, set gear 8.
 
 ### Hardware
 

include/ERG_Mode.h

@@ -35,10 +35,10 @@ class ErgMode {
   bool _userIsSpinning(int cadence, float incline);
 
   // calculate incline if setpoint (from Zwift) changes
-  void _setPointChangeState(int newCadence, Measurement& newWatts);
+  int32_t _setPointChangeState(int newCadence, Measurement& newWatts);
 
   // calculate incline if setpoint is unchanged
-  void _inSetpointState(int newCadence, Measurement& newWatts);
+  int32_t _inSetpointState(int newCadence, Measurement& newWatts);
 
   // update localvalues + incline, creates a log
   void _updateValues(int newCadence, Measurement& newWatts, float newIncline);

include/PowerTable_Helpers.h

@@ -89,6 +89,7 @@ class PTHelpers {
   ptIndex calculateIndex(int watts, int cad);
   void enterData(PTData& ptData,ptIndex index, int pos);
   void clean(PTData& ptData);
+  void fillGaps(PTData& ptData);
   bool fillAllWattColumns(PTData& ptData);
   bool fillAllCadenceLines(PTData& ptData);
   std::pair<std::vector<float>, std::vector<float>> getRow(int row, PTData& ptData);

include/settings.h

@@ -77,7 +77,7 @@ const char* const DEFAULT_PASSWORD = "password";
 // I.E. If the difference between ERG target and Current watts were 30, and the Shift step is defined as 600 steps,
 // and ERG_Sensitivity were 1.0, ERG mode would move the stepper motor 600 steps to compensate. With an ERG_Sensitivity of 2.0, the stepper
 // would move 1200 steps to compensate, however ERG_Sensitivity values much different than 1.0 imply shiftStep has been improperly configured.
-#define ERG_SENSITIVITY 2.0f
+#define ERG_SENSITIVITY 3.0f
 
 // Number of watts per shift expected by ERG mode for it's calculation. The user should target this number by adjusting Shift Step until WATTS_PER_SHIFT
 // is obtained as closely as possible during each shift.
@@ -276,8 +276,8 @@ constexpr const char* ANY = "any";
 // 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 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

platformio.ini

@@ -108,6 +108,7 @@ build_flags =
        -Wunreachable-code
 	   -std=c++11
 	   -D PLATFORMIO_ENV_NATIVE
+	   -I include
 lib_ldf_mode = chain+
 lib_compat_mode = soft
 check_tool = cppcheck

src/BLE_Client.cpp

@@ -239,6 +239,7 @@ void bleClientTask(void* pvParameters) {
           ss2k->goHome(true);
         } else {  // Startup Homing
           ss2k->goHome(false);
+          rtConfig->setShifterPosition(8);  // Set to middle position after homing on startup
         }
         spinBLEServer.spinDownFlag = 0;
       }

src/ERG_Mode.cpp

@@ -121,6 +121,7 @@ void ErgMode::runERG() {
 void ErgMode::computeErg() {
   Measurement newWatts = rtConfig->watts;
   int newCadence       = rtConfig->cad.getValue();
+  int32_t result       = RETURN_ERROR;
 
   bool isUserSpinning = this->_userIsSpinning(newCadence, ss2k->getCurrentPosition());
   if (!isUserSpinning) {
@@ -141,29 +142,29 @@ void ErgMode::computeErg() {
   }
 
 #ifdef ERG_MODE_USE_POWER_TABLE
-// SetPoint changed
-#ifdef ERG_MODE_USE_PID
   if (abs(this->setPoint - newWatts.getTarget()) > ERG_MODE_PID_WINDOW && rtConfig->getHomed()) {
-#endif
-    _setPointChangeState(newCadence, newWatts);
-    return;
-#ifdef ERG_MODE_USE_PID
+    result = _setPointChangeState(newCadence, newWatts);
   }
 #endif
-#endif
-
 #ifdef ERG_MODE_USE_PID
   // Setpoint unchanged
-  _inSetpointState(newCadence, newWatts);
+  if (result == INT32_MIN) {
+    result = _inSetpointState(newCadence, newWatts);
+  }
 #endif
+  _updateValues(newCadence, newWatts, result);
 }
 
-void ErgMode::_setPointChangeState(int newCadence, Measurement& newWatts) {
-  // It's better to undershoot increasing watts and overshoot decreasing watts, so lets set the lookup target to the nearest side of ERG_MODE_PID_WINDOW
-  int adjustedTarget = newWatts.getTarget() >= newWatts.getValue() ? newWatts.getTarget() - ERG_MODE_PID_WINDOW : newWatts.getTarget() + ERG_MODE_PID_WINDOW;
-
+int32_t ErgMode::_setPointChangeState(int newCadence, Measurement& newWatts) {
+  // It's better to undershoot increasing watts and overshoot decreasing watts, so lets set the lookup target to the nearest side of POWERTABLE_WATT_INCREMENT
+  int adjustedTarget  = newWatts.getTarget() >= newWatts.getValue() ? newWatts.getTarget() - POWERTABLE_WATT_INCREMENT : newWatts.getTarget() + POWERTABLE_WATT_INCREMENT;
   int32_t tableResult = powerTable->lookup(adjustedTarget, newCadence);
 
+  // A lot of times this likes to undershoot going from High to low. Lets fix it.
+  if (adjustedTarget < newWatts.getValue() && adjustedTarget < 200) {
+    adjustedTarget = (adjustedTarget + ss2k->getCurrentPosition()) / 2;
+  }
+
   // Test current watts against the table result. If We're already lower or higher than target, flag the result as a return error.
   if (tableResult != RETURN_ERROR) {
     if (rtConfig->watts.getValue() > adjustedTarget && tableResult > ss2k->getCurrentPosition()) {
@@ -176,26 +177,32 @@ void ErgMode::_setPointChangeState(int newCadence, Measurement& newWatts) {
     }
   }
 
+  // Sanity check - with homing enabled, we should never have a negative result. If we do, something went wrong.
+  if (rtConfig->getHomed() && tableResult < 0) {
+    SS2K_LOG(ERG_MODE_LOG_TAG, "PowerTable returned negative result with homing enabled. Using PID");
+    tableResult = RETURN_ERROR;
+  }
+
   // Handle return errors
   if (tableResult == RETURN_ERROR) {
     SS2K_LOG(ERG_MODE_LOG_TAG, "Lookup Error. Using PID");
-    _inSetpointState(newCadence, newWatts);
-    return;
-  }
-
-  SS2K_LOG(ERG_MODE_LOG_TAG, "SetPoint changed:%dw PowerTable Result: %d", adjustedTarget, tableResult);
-  _updateValues(newCadence, newWatts, tableResult);
-
-  if (rtConfig->getTargetIncline() != ss2k->getCurrentPosition()) {  // add some time to wait while the knob moves to target position.
-    isDelayed     = true;
-    int timeToAdd = abs(ss2k->getCurrentPosition() - rtConfig->getTargetIncline());
-    if (timeToAdd > 3000) {  // 3 seconds
-      SS2K_LOG(ERG_MODE_LOG_TAG, "Capping ERG seek time to 3 seconds");
-      timeToAdd = 3000;
+    tableResult = _inSetpointState(newCadence, newWatts);
+  } else {
+    SS2K_LOG(ERG_MODE_LOG_TAG, "SetPoint changed:%dw PowerTable Result: %d", adjustedTarget, tableResult);
+    if (tableResult != ss2k->getCurrentPosition()) {  // add some time to wait while the knob moves to target position.
+      isDelayed             = true;
+      long int stepDistance = abs(ss2k->getCurrentPosition() - tableResult);
+      // Calculate time to add based on step distance and stepper speed
+      long int timeToAdd = round(((double)stepDistance * 1000.0) / (double)userConfig->getStepperSpeed());
+      if (timeToAdd > 3000) {  // 3 seconds
+        SS2K_LOG(ERG_MODE_LOG_TAG, "Capping ERG seek time to 3 seconds");
+        timeToAdd = 3000;
+      }
+      ergTimer += timeToAdd;
     }
-    ergTimer += timeToAdd;
+    ergTimer += (ERG_MODE_DELAY);  // Wait for power meter to register new watts
   }
-  ergTimer += (ERG_MODE_DELAY);  // Wait for power meter to register new watts
+  return tableResult;
 }
 
 // INTRODUCING PID CONTROL LOOP
@@ -206,63 +213,53 @@ void ErgMode::_setPointChangeState(int newCadence, Measurement& newWatts) {
 // Derivative term: rate of change of error
 
 // PrevError
-void ErgMode::_inSetpointState(int newCadence, Measurement& newWatts) {
+int32_t ErgMode::_inSetpointState(int newCadence, Measurement& newWatts) {
   // Setting Gains For PID Loop
-  float Kp = userConfig->getERGSensitivity();
-  float Ki = 0.5;
-  float Kd = 0.1;
-
-  static float integral  = 0.0;
-  static float prevError = 0.0;
+  double Kp = userConfig->getERGSensitivity();  // Proportional gain based on user sensitivity
 
   // 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;
+  int error = target - watts;
+
+  // modifying gains based on error
+  if (abs(error) < 10) {
+    Kp = Kp * 0.25;  // decrease further for tiny errors
+  } else if (abs(error) < 50) {
+    Kp = Kp * 0.75;  // Moderate for medium errors
+  } else if (abs(error) > 100) {
+    Kp = Kp * 1.25;  // Aggressive for large errors
+  }
 
   // Defining proportional term
-  float proportional = Kp * error;
+  double proportional = Kp * error;
   if (newWatts.getValue() < userConfig->getMinWatts()) {
     proportional = proportional * userConfig->getERGSensitivity();  // increase proportional term when at very low watts. Prevents Zwift from timeout on initial interval.
   }
 
-  // Defining integral term
-  integral += error;
-  float integralFinal = Ki * integral;
-
-  // Clamping down integral term
-  float integralMax = 60 * userConfig->getERGSensitivity();
-  float integralMin = -60 * userConfig->getERGSensitivity();
-
-  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;
-
   // final PID output
-  float PID_output = proportional + integralFinal + derivativeTerm;
+  double PID_output = proportional;
 
-  // log proportional, integral, derivative every five seconds
+  // log proportional 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);
+    SS2K_LOG(ERG_MODE_LOG_TAG, "Proportional: %f", proportional);
+  }
+
+  // Cap the change to no more than we can move until the next reading
+  int maxChange = round((long)((userConfig->getStepperSpeed() * ERG_MODE_DELAY)) / 1000.0f);  // max change based on stepper speed and delay
+  if (PID_output > maxChange) {
+    PID_output = maxChange;
+  } else if (PID_output < -maxChange) {
+    PID_output = -maxChange;
   }
 
   // Calculate new incline
   float newIncline = ss2k->getCurrentPosition() + PID_output;
-
-  prevError = error;
-
-  // Apply the new values
-  _updateValues(newCadence, newWatts, newIncline);
+  return newIncline;
 }
 
 void ErgMode::_updateValues(int newCadence, Measurement& newWatts, float newIncline) {

src/PowerTable_Helpers.cpp

@@ -141,14 +141,15 @@ int32_t PTHelpers::lookup(int watts, int cad, PTData& ptData) {
       }
     }
   }
+
   if (resistance != RETURN_ERROR) {
     SS2K_LOG(PTDATA_LOG_TAG, "Extrapolated resistance: %d for watts=%d, cad=%d", resistance, watts, cad);
     // Return early if we found a valid extrapolated value
   } else {
     SS2K_LOG(PTDATA_LOG_TAG, "Extrapolation failed for watts=%d, cad=%d", watts, cad);
   }
 
-  return resistance;  // All lookup methods failed
+  return resistance;
 }
 
 /**
@@ -374,7 +375,7 @@ int PTHelpers::extrapolateWattsFromCadence(int cad, int32_t targetPosition, PTDa
  * prerequisite for the PAVA functions to work correctly and prevent line crossings.
  * @param ptData The main power table data structure.
  */
-void fillGaps(PTData& ptData) {
+void PTHelpers::fillGaps(PTData& ptData) {
   for (int i = 0; i < POWERTABLE_CAD_SIZE; ++i) {     // For each row
     for (int j = 0; j < POWERTABLE_WATT_SIZE; ++j) {  // For each cell
       if (ptData.tableRow[i].tableEntry[j].targetPosition == INT16_MIN) {
@@ -614,7 +615,7 @@ void PTHelpers::enterData(PTData& ptData, ptIndex index, int pos) {
   }
 
   // // After updating a point, re-process the entire table to enforce global monotonicity.
-  fillGaps(ptData);
+  this->fillGaps(ptData);
   // for (int i = 0; i < 10; i++) {  // Run the PAVA functions multiple times to ensure convergence
   bool caddone  = false;
   bool wattdone = false;
@@ -635,9 +636,11 @@ void PTHelpers::enterData(PTData& ptData, ptIndex index, int pos) {
 
 void PTHelpers::clean(PTData& ptData) {
   int removed = 0;
+  
+  // First pass: remove entries with readings < 1 or negative positions
   for (int i = 0; i < POWERTABLE_CAD_SIZE; i++) {
     for (int j = 0; j < POWERTABLE_WATT_SIZE; j++) {
-      if (ptData.tableRow[i].tableEntry[j].readings < 1) {
+      if (ptData.tableRow[i].tableEntry[j].readings < 1 || ptData.tableRow[i].tableEntry[j].targetPosition < 0) {
         if (ptData.tableRow[i].tableEntry[j].targetPosition != INT16_MIN) {
           removed++;
         }
@@ -646,6 +649,42 @@ void PTHelpers::clean(PTData& ptData) {
       }
     }
   }
+  
+  // Second pass: remove duplicate values in columns (same resistance for multiple cadences)
+  // For each column, track all unique values and remove duplicates
+  for (int j = 0; j < POWERTABLE_WATT_SIZE; j++) {
+    for (int i = 0; i < POWERTABLE_CAD_SIZE; i++) {
+      if (ptData.tableRow[i].tableEntry[j].targetPosition == INT16_MIN) continue;
+      
+      int16_t currentValue = ptData.tableRow[i].tableEntry[j].targetPosition;
+      
+      // Check all other entries in this column for the same value
+      for (int k = i + 1; k < POWERTABLE_CAD_SIZE; k++) {
+        if (ptData.tableRow[k].tableEntry[j].targetPosition == currentValue) {
+          // Found duplicate - remove the one with fewer readings
+          // If readings are equal, keep the one at higher cadence (higher index)
+          if (ptData.tableRow[k].tableEntry[j].readings < ptData.tableRow[i].tableEntry[j].readings) {
+            ptData.tableRow[k].tableEntry[j].targetPosition = INT16_MIN;
+            ptData.tableRow[k].tableEntry[j].readings = 0;
+            removed++;
+          } else if (ptData.tableRow[k].tableEntry[j].readings > ptData.tableRow[i].tableEntry[j].readings) {
+            // Current entry has fewer readings, mark it for removal and update current to the better one
+            ptData.tableRow[i].tableEntry[j].targetPosition = INT16_MIN;
+            ptData.tableRow[i].tableEntry[j].readings = 0;
+            removed++;
+            break; // Move to next i since current is removed
+          } else {
+            // Readings are equal - keep the one at higher cadence (k), remove the one at i
+            ptData.tableRow[i].tableEntry[j].targetPosition = INT16_MIN;
+            ptData.tableRow[i].tableEntry[j].readings = 0;
+            removed++;
+            break; // Move to next i since current is removed
+          }
+        }
+      }
+    }
+  }
+  
   if (removed > 0) {
     SS2K_LOG(PTDATA_LOG_TAG, "Cleaned %d readings", removed);
   }