Merge pull request #577 from doudar/Last_resort_ERG_move_test

Added a final test to check if ERG mode has commanded a move in the proper direction.

CHANGELOG.md

@@ -52,6 +52,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 - Continue updating power metrics to other clients if one client disconnects.
 - Freed 19k of ram by consolidating tasks and using timers instead of delays.
 - Updated baud rate to 115200 to ensure compatibility with other ESP32 variants.
+- Added a final test to check if ERG mode has commanded a move in the proper direction. 
 
 ### Hardware
 

src/Main.cpp

@@ -386,8 +386,17 @@ void SS2K::moveStepper() {
     ss2k->stepperIsRunning = stepper->isRunning();
     ss2k->currentPosition  = stepper->getCurrentPosition();
     if (!ss2k->externalControl) {
-      if ((rtConfig->getFTMSMode() == FitnessMachineControlPointProcedure::SetTargetPower) ||
-          (rtConfig->getFTMSMode() == FitnessMachineControlPointProcedure::SetTargetResistanceLevel)) {
+      if ((rtConfig->getFTMSMode() == FitnessMachineControlPointProcedure::SetTargetPower)) {
+        // 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);
+        }
+        // don't drive higher 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);
+        }
+        ss2k->targetPosition = rtConfig->getTargetIncline();
+      } else if (rtConfig->getFTMSMode() == FitnessMachineControlPointProcedure::SetTargetResistanceLevel) {
         ss2k->targetPosition = rtConfig->getTargetIncline();
       } else {
         // Simulation Mode
@@ -423,16 +432,16 @@ void SS2K::moveStepper() {
           stepper->moveTo(ss2k->targetPosition);
         }
       }
-
-    } else {
+    } else {  // Normal move code for non-Peloton
       if ((ss2k->targetPosition >= rtConfig->getMinStep()) && (ss2k->targetPosition <= rtConfig->getMaxStep())) {
         stepper->moveTo(ss2k->targetPosition);
       } else if (ss2k->targetPosition <= rtConfig->getMinStep()) {  // Limit Stepper to Min Position
-        stepper->moveTo(rtConfig->getMinStep());
+        stepper->moveTo(rtConfig->getMinStep() + 1);
       } else {  // Limit Stepper to Max Position
-        stepper->moveTo(rtConfig->getMaxStep());
+        stepper->moveTo(rtConfig->getMaxStep() - 1);
       }
     }
+    //Sync external object
     rtConfig->setCurrentIncline((float)stepper->getCurrentPosition());
 
     if (connectedClientCount() > 0) {