From b6e7e39da0b6f7ac97256427dab8c22c022f08da Mon Sep 17 00:00:00 2001
From: Anthony Doud <anthonydoud97@gmail.com>
Date: Tue, 1 Oct 2024 12:48:38 -0500
Subject: [PATCH] Added a final test to check if ERG mode has commanded a move
 in the proper direction.

---
 CHANGELOG.md |  1 +
 src/Main.cpp | 23 ++++++++++++++++-------
 2 files changed, 17 insertions(+), 7 deletions(-)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 05e877f9..53d5505d 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -46,6 +46,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 - Peloton resistance limit enhancements.
 - Continue updating power metrics to other clients if one client disconnects.
 - Freed 19k of ram by consolidating tasks and using timers instead of delays. 
+- Added a final test to check if ERG mode has commanded a move in the proper direction. 
 
 ### Hardware
 - added Yesoul S3.
diff --git a/src/Main.cpp b/src/Main.cpp
index 28aaf54b..0ac66e5b 100644
--- a/src/Main.cpp
+++ b/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,18 +432,18 @@ 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) {
       stepper->setAutoEnable(false);  // Keep the stepper from rolling back due to head tube slack. Motor Driver still lowers power between moves
       stepper->enableOutputs();