Marc roy

CHANGELOG.md

@@ -9,11 +9,24 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 
 ### Added
 
+### Changed
+
+### 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. 
 
 ### 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);

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,7 +213,7 @@ 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;
@@ -222,6 +229,17 @@ void ErgMode::_inSetpointState(int newCadence, Measurement& newWatts) {
   // subtracting target from current watts
   float error = target - watts;
 
+  // modifying gains based on error
+  if (abs(error) < 20) {
+    Kp = Kp * .75;  // Stabilize for small errors
+    Ki = Ki * 0.0;  
+    Kd = Kd * 0.0;  
+  } else if (abs(error) < 10) {
+    Kp = Kp * 0.25;  // decrease further for tiny errors
+    Ki = Ki * 0.0;  
+    Kd = Kd * 0.0; 
+  }
+
   // Defining proportional term
   float proportional = Kp * error;
   if (newWatts.getValue() < userConfig->getMinWatts()) {
@@ -258,11 +276,8 @@ void ErgMode::_inSetpointState(int newCadence, Measurement& newWatts) {
 
   // Calculate new incline
   float newIncline = ss2k->getCurrentPosition() + PID_output;
-
-  prevError = error;
-
-  // Apply the new values
-  _updateValues(newCadence, newWatts, newIncline);
+  prevError        = error;
+  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;
 }
 
 /**

src/SensorCollector.cpp

@@ -58,7 +58,7 @@ void collectAndSet(NimBLEUUID charUUID, NimBLEUUID serviceUUID, std::string& uni
     if ((charUUID == PELOTON_DATA_UUID) && !(strcmp(userConfig->getConnectedPowerMeter(), NONE) == 0 || strcmp(userConfig->getConnectedPowerMeter(), ANY) == 0)) {
       // Peloton connected but using BLE Power Meter. So skip cad for Peloton UUID.
     } else {
-      float cadence = sensorData->getCadence();
+      int cadence = round(sensorData->getCadence());
       if (cadence > 0.0 && cadence < 250.0) {
         rtConfig->cad.setValue(cadence);
         spinBLEClient.connectedCD = true;
@@ -75,7 +75,7 @@ void collectAndSet(NimBLEUUID charUUID, NimBLEUUID serviceUUID, std::string& uni
     if ((charUUID == PELOTON_DATA_UUID) && !((strcmp(userConfig->getConnectedPowerMeter(), NONE) == 0) || (strcmp(userConfig->getConnectedPowerMeter(), ANY) == 0))) {
       // Peloton connected but using BLE Power Meter. So skip power for Peloton UUID.
     } else {
-      int power = sensorData->getPower() * userConfig->getPowerCorrectionFactor();
+      int power = round(sensorData->getPower() * userConfig->getPowerCorrectionFactor());
       if (power > 0 && power < 3000) {
         rtConfig->watts.setValue(power);
         spinBLEClient.connectedPM = true;
@@ -88,20 +88,18 @@ void collectAndSet(NimBLEUUID charUUID, NimBLEUUID serviceUUID, std::string& uni
   }
 
   if (sensorData->hasSpeed()) {
-    float speed = sensorData->getSpeed();
-    rtConfig->setSimulatedSpeed(speed);
+    rtConfig->setSimulatedSpeed(sensorData->getSpeed());
     spinBLEClient.connectedSpeed = true;
-    logBufLength += snprintf(logBuf + logBufLength, kLogBufMaxLength - logBufLength, " SD(%.2f)", fmodf(speed, 1000.0));
+    logBufLength += snprintf(logBuf + logBufLength, kLogBufMaxLength - logBufLength, " SD(%.2f)", fmodf(sensorData->getSpeed(), 1000.0));
   }
 
   if (sensorData->hasResistance()) {
     rtConfig->resistance.setSimulate(false);  // Mark as real data
     if ((ss2k->pelotonIsConnected) && (charUUID != PELOTON_DATA_UUID)) {
       // Peloton connected but using BLE Power Meter. So skip resistance for UUID's that aren't Peloton.
     } else {
-      int resistance = sensorData->getResistance();
-      rtConfig->resistance.setValue(resistance);
-      logBufLength += snprintf(logBuf + logBufLength, kLogBufMaxLength - logBufLength, " RS(%d)", resistance % 1000);
+      rtConfig->resistance.setValue(sensorData->getResistance());
+      logBufLength += snprintf(logBuf + logBufLength, kLogBufMaxLength - logBufLength, " RS(%d)", sensorData->getResistance() % 1000);
     }
   }