From 5c8640327b89e2ac135c577f765c5eee9b0e7728 Mon Sep 17 00:00:00 2001
From: Markus Schneider <m.schneider@online.de>
Date: Sat, 4 Dec 2021 11:00:31 +0100
Subject: [PATCH 01/42] Moved ERG to seperate task

---
 include/BLE_Common.h           |  2 +-
 include/ERG_Mode.h             | 16 +++++++
 include/SmartSpin_parameters.h |  3 ++
 src/BLE_Common.cpp             |  2 +-
 src/BLE_Server.cpp             | 70 +++-------------------------
 src/ERG_Mode.cpp               | 84 ++++++++++++++++++++++++++++++++++
 src/Main.cpp                   |  3 ++
 7 files changed, 115 insertions(+), 65 deletions(-)
 create mode 100644 include/ERG_Mode.h
 create mode 100644 src/ERG_Mode.cpp

diff --git a/include/BLE_Common.h b/include/BLE_Common.h
index c3f34290..c6d93149 100644
--- a/include/BLE_Common.h
+++ b/include/BLE_Common.h
@@ -77,7 +77,7 @@ extern SpinBLEServer spinBLEServer;
 
 void startBLEServer();
 bool spinDown();
-void computeERG(int = 0);
+// void computeERG(int = 0);
 void computeCSC();
 void logCharacteristic(char *buffer, const size_t bufferCapacity, const byte *data, const size_t dataLength, const NimBLEUUID serviceUUID, const NimBLEUUID charUUID,
                        const char *format, ...);
diff --git a/include/ERG_Mode.h b/include/ERG_Mode.h
new file mode 100644
index 00000000..b304a0de
--- /dev/null
+++ b/include/ERG_Mode.h
@@ -0,0 +1,16 @@
+#pragma once
+
+#include <Arduino.h>
+
+#define ERG_MODE_LOG_TAG "ERG_Mode"
+#define ERG_MODE_DELAY 1000
+
+class ErgMode {
+ public:
+  static void setupERG();
+
+ private:
+  static TaskHandle_t _ergTask;
+  static void loop(void *pvParameters);
+  static void computErg(int newSetpoint);
+};
\ No newline at end of file
diff --git a/include/SmartSpin_parameters.h b/include/SmartSpin_parameters.h
index a770bf35..433e5909 100644
--- a/include/SmartSpin_parameters.h
+++ b/include/SmartSpin_parameters.h
@@ -15,6 +15,7 @@ class userParameters {
  private:
   String firmwareUpdateURL;
   float incline;
+  int targetWatts;
   int simulatedWatts;
   int simulatedHr;
   int simulatedCad;
@@ -41,6 +42,7 @@ class userParameters {
  public:
   const char* getFirmwareUpdateURL() { return firmwareUpdateURL.c_str(); }
   float getIncline() { return incline; }
+  int getTargetWatts() { return targetWatts; }
   int getSimulatedWatts() { return simulatedWatts; }
   int getSimulatedHr() { return simulatedHr; }
   int getSimulatedCad() { return simulatedCad; }
@@ -67,6 +69,7 @@ class userParameters {
   void setDefaults();
   void setFirmwareUpdateURL(String fURL) { firmwareUpdateURL = fURL; }
   void setIncline(float inc) { incline = inc; }
+  void setTargetWatts(int w) { targetWatts = w; }
   void setSimulatedWatts(int w) { simulatedWatts = w; }
   void setSimulatedHr(int hr) { simulatedHr = hr; }
   void setSimulatedCad(float cad) { simulatedCad = cad; }
diff --git a/src/BLE_Common.cpp b/src/BLE_Common.cpp
index 164f8020..e7bac585 100644
--- a/src/BLE_Common.cpp
+++ b/src/BLE_Common.cpp
@@ -142,7 +142,7 @@ void BLECommunications(void *pvParameters) {
       }
 
       processFTMSWrite();
-      computeERG();
+      // computeERG();
 
       if (updateConnParametersFlag) {
         vTaskDelay(100 / portTICK_PERIOD_MS);
diff --git a/src/BLE_Server.cpp b/src/BLE_Server.cpp
index 3f454f6f..6b79bc94 100644
--- a/src/BLE_Server.cpp
+++ b/src/BLE_Server.cpp
@@ -206,62 +206,6 @@ bool spinDown() {
   return true;
 }
 
-// as a note, Trainer Road sends 50w target whenever the app is connected.
-void computeERG(int newSetPoint) {
-  // SS2K_LOG(BLE_SERVER_LOG_TAG, "ComputeERG. Setpoint: %d", newSetPoint);
-  if (userConfig.getERGMode() && spinBLEClient.connectedPM) {
-    // continue
-  } else {
-    // SS2K_LOG(BLE_SERVER_LOG_TAG, "ERG request but ERG off or no connected PM");
-    return;
-  }
-  static bool userIsPedaling = true;
-  static int setPoint        = 0;
-  float incline              = userConfig.getIncline();
-  float newIncline           = incline;
-  int amountToChangeIncline  = 0;
-  int wattChange             = userConfig.getSimulatedWatts() - setPoint;
-
-  if (newSetPoint > 0) {  // only update the value if new value is sent
-    setPoint = newSetPoint;
-  }
-  if (setPoint < 50) {  // minumum setPoint
-    setPoint = 50;
-  }
-
-  if (userConfig.getSimulatedCad() <= 20) {
-    if (!userIsPedaling) {  // Test so motor stop command only happens once.
-      motorStop();     // release tension
-      return;
-    }
-    userIsPedaling = false;
-    userConfig.setIncline(incline-userConfig.getShiftStep()*2);
-    // Cadence too low, nothing to do here
-    return;
-  }
-  userIsPedaling = true;
-
-  amountToChangeIncline = wattChange * userConfig.getERGSensitivity();
-  if (abs(wattChange) < WATTS_PER_SHIFT) {
-    // As the desired value gets closer, make smaller changes for a smoother experience
-    amountToChangeIncline *= SUB_SHIFT_SCALE;
-  }
-  // limit to 10 shifts at a time
-  if (abs(amountToChangeIncline) > userConfig.getShiftStep() * 2) {
-    if (amountToChangeIncline > 5) {
-      amountToChangeIncline = userConfig.getShiftStep() * 2;
-    }
-    if (amountToChangeIncline < -5) {
-      amountToChangeIncline = -(userConfig.getShiftStep() * 2);
-    }
-  }
-
-  // Reduce the amount per loop (don't try to oneshot it) and scale the movement the higher the watt target is as higher wattages require less knob movement.
-  // amountToChangeIncline = amountToChangeIncline / ((userConfig.getSimulatedWatts() / 100) + .1);  // +.1 to eliminate possible divide by zero.
-  newIncline            = incline - amountToChangeIncline;
-  userConfig.setIncline(newIncline);
-  // SS2K_LOG(BLE_SERVER_LOG_TAG, "newincline: %f", newIncline);
-}
 
 void updateIndoorBikeDataChar() {
   float cadRaw = userConfig.getSimulatedCad();
@@ -321,7 +265,7 @@ void updateCyclingPowerMeasurementChar() {
     remainder                  = spinBLEClient.cscLastCrankEvtTime % 256;
     cyclingPowerMeasurement[7] = remainder;
     cyclingPowerMeasurement[8] = quotient;
-  } 
+  }
 
   cyclingPowerMeasurementCharacteristic->notify();
 
@@ -366,9 +310,7 @@ void MyServerCallbacks::onDisconnect(BLEServer *pServer) {
   BLEDevice::startAdvertising();
 }
 
-void MyCallbacks::onWrite(BLECharacteristic *pCharacteristic) {  
-  FTMSWrite = pCharacteristic->getValue();
-  }
+void MyCallbacks::onWrite(BLECharacteristic *pCharacteristic) { FTMSWrite = pCharacteristic->getValue(); }
 
 void processFTMSWrite() {
   if (FTMSWrite == "") {
@@ -394,7 +336,7 @@ void processFTMSWrite() {
       case 0x00:  // request control
         logBufLength += snprintf(logBuf + logBufLength, kLogBufCapacity - logBufLength, "-> Control Request");
         returnValue[2] = 0x01;
-        //userConfig.setERGMode(false);
+        // userConfig.setERGMode(false);
         pCharacteristic->setValue(returnValue, 3);
         ftmsTrainingStatus[1] = 0x01;
         fitnessMachineTrainingStatus->setValue(ftmsTrainingStatus, 2);
@@ -435,7 +377,9 @@ void processFTMSWrite() {
         if (spinBLEClient.connectedPM || userConfig.getSimulateWatts()) {
           int targetWatts = bytes_to_u16(rxValue[2], rxValue[1]);
           userConfig.setERGMode(true);
-          computeERG(targetWatts);
+          userConfig.setTargetWatts(targetWatts);
+
+          // computeERG(targetWatts);
           logBufLength += snprintf(logBuf + logBufLength, kLogBufCapacity - logBufLength, "-> ERG Mode Target: %d Current: %d Incline: %2f", targetWatts,
                                    userConfig.getSimulatedWatts(), userConfig.getIncline() / 100);
           returnValue[2]       = 0x01;
@@ -501,7 +445,7 @@ void processFTMSWrite() {
     SS2K_LOG(BLE_SERVER_LOG_TAG, "App wrote nothing ");
     SS2K_LOG(BLE_SERVER_LOG_TAG, "assuming it's a Control request");
     uint8_t controlPoint[3] = {0x80, 0x00, 0x01};
-    //userConfig.setERGMode(true);
+    // userConfig.setERGMode(true);
     pCharacteristic->setValue(controlPoint, 3);
     ftmsTrainingStatus[1] = 0x01;
     fitnessMachineTrainingStatus->setValue(ftmsTrainingStatus, 2);
diff --git a/src/ERG_Mode.cpp b/src/ERG_Mode.cpp
new file mode 100644
index 00000000..eca9a4b3
--- /dev/null
+++ b/src/ERG_Mode.cpp
@@ -0,0 +1,84 @@
+#include "ERG_Mode.h"
+#include "SS2KLog.h"
+#include "Main.h"
+
+TaskHandle_t ErgMode::_ergTask;
+
+void ErgMode::setupERG() {
+  SS2K_LOG(ERG_MODE_LOG_TAG, "Starting ERG Mode task...");
+  xTaskCreatePinnedToCore(loop,          /* Task function. */
+                          "ERGModeTask", /* name of task. */
+                          1800,          /* Stack size of task*/
+                          NULL,          /* parameter of the task */
+                          1,             /* priority of the task*/
+                          &_ergTask,     /* Task handle to keep track of created task */
+                          1);            /* pin task to core 0 */
+
+  SS2K_LOG(ERG_MODE_LOG_TAG, "ERG Mode task started");
+}
+
+void ErgMode::loop(void *pvParameters) {
+  vTaskDelay(ERG_MODE_DELAY / portTICK_PERIOD_MS);
+
+  //   int newSetPoint             = userConfig.getTargetWatts();
+  //   bool isInErgMode            = userConfig.getERGMode();
+  //   bool hasConnectedPowerMeter = spinBLEClient.connectedPM;
+
+  //   if (isInErgMode && hasConnectedPowerMeter) {
+  //     SS2K_LOG(ERG_MODE_LOG_TAG, "ComputeERG. Setpoint: %d", newSetPoint);
+  //     ErgMode::computErg(newSetPoint);
+  //   } else {
+  // SS2K_LOG(ERG_MODE_LOG_TAG, "ERG request but ERG off or no connected PM");
+  //   }
+}
+
+// as a note, Trainer Road sends 50w target whenever the app is connected.
+void ErgMode::computErg(int newSetPoint) {
+  static bool userIsPedaling = true;
+  static int setPoint        = 0;
+  float incline              = userConfig.getIncline();
+  float newIncline           = incline;
+  int amountToChangeIncline  = 0;
+  int wattChange             = userConfig.getSimulatedWatts() - setPoint;
+  int cadance                = userConfig.getSimulatedCad();
+
+  if (newSetPoint > 0) {  // only update the value if new value is sent
+    setPoint = newSetPoint;
+  }
+  if (setPoint < 50) {  // minumum setPoint
+    setPoint = 50;
+  }
+
+  if (cadance <= 20) {
+    if (!userIsPedaling) {  // Test so motor stop command only happens once.
+      motorStop();          // release tension
+      return;
+    }
+    userIsPedaling = false;
+    userConfig.setIncline(incline - userConfig.getShiftStep() * 2);
+    // Cadence too low, nothing to do here
+    return;
+  }
+  userIsPedaling = true;
+
+  amountToChangeIncline = wattChange * userConfig.getERGSensitivity() * (75.0 / (double)cadance);
+  if (abs(wattChange) < WATTS_PER_SHIFT) {
+    // As the desired value gets closer, make smaller changes for a smoother experience
+    amountToChangeIncline *= SUB_SHIFT_SCALE;
+  }
+  // limit to 10 shifts at a time
+  if (abs(amountToChangeIncline) > userConfig.getShiftStep() * 2) {
+    if (amountToChangeIncline > 5) {
+      amountToChangeIncline = userConfig.getShiftStep() * 2;
+    }
+    if (amountToChangeIncline < -5) {
+      amountToChangeIncline = -(userConfig.getShiftStep() * 2);
+    }
+  }
+
+  // Reduce the amount per loop (don't try to oneshot it) and scale the movement the higher the watt target is as higher wattages require less knob movement.
+  // amountToChangeIncline = amountToChangeIncline / ((userConfig.getSimulatedWatts() / 100) + .1);  // +.1 to eliminate possible divide by zero.
+  newIncline = incline - amountToChangeIncline;
+  userConfig.setIncline(newIncline);
+  SS2K_LOG(ERG_MODE_LOG_TAG, "newincline: %f", newIncline);
+}
diff --git a/src/Main.cpp b/src/Main.cpp
index 6ee7ac90..302e4861 100644
--- a/src/Main.cpp
+++ b/src/Main.cpp
@@ -12,6 +12,7 @@
 #include <SPIFFS.h>
 #include <HardwareSerial.h>
 #include "FastAccelStepper.h"
+#include "ERG_Mode.h"
 
 bool lastDir = true;  // Stepper Last Direction
 
@@ -109,6 +110,8 @@ void setup() {
 
   setupBLE();
   startHttpServer();
+  ErgMode::setupERG();
+
   resetIfShiftersHeld();
   SS2K_LOG(MAIN_LOG_TAG, "Creating Shifter Interrupts");
   // Setup Interrups so shifters work anytime

From 6b35e295c9ae88e32ec311860c4170341407882f Mon Sep 17 00:00:00 2001
From: Markus Schneider <m.schneider@online.de>
Date: Sat, 4 Dec 2021 14:07:18 +0100
Subject: [PATCH 02/42] Added TargetWatts and ERG Mode simulation

---
 data/btsimulator.html          | 123 +++++++++++++++++++++++++++------
 include/ERG_Mode.h             |  13 ++--
 include/SmartSpin_parameters.h |   3 +
 include/settings.h             |   2 +-
 src/ERG_Mode.cpp               |  47 ++++++++-----
 src/HTTP_Server_Basic.cpp      |  32 ++++++++-
 src/Main.cpp                   |   2 +-
 src/SmartSpin_parameters.cpp   |  10 ++-
 8 files changed, 179 insertions(+), 53 deletions(-)

diff --git a/data/btsimulator.html b/data/btsimulator.html
index 3d5a2933..34327707 100644
--- a/data/btsimulator.html
+++ b/data/btsimulator.html
@@ -23,44 +23,65 @@ <h1>BLE Device Simulator</h1>
 
     <h2>Sim Heart Rate</h2>
     <p><span id="hrValue"></span></p>
-    <p><input type="range" onload="requestConfigValues()" onchange="updateHRSlider(this)" id="HRSlider" min="40"
+    <p><input type="range" onload="requestConfigValues()" onchange="updatehrSlider(this)" id="hrSlider" min="40"
         max="250" value="0" step="1" class="slider2"></p>
     <p><label class="switch"><input type="checkbox" onload="toggleHRCheckbox(this, true)"
           onchange="toggleHRCheckbox(this,true)" id="hrOutput"><span class="slider"></span></label></p>
 
     <h2>Sim Power Output</h2>
     <p><span id="wattsValue"></span></p>
-    <p><input type="range" onload="requestConfigValues()" onchange="updateWattsSlider(this)" id="WattsSlider" min="0"
+    <p><input type="range" onload="requestConfigValues()" onchange="updateWattsSlider(this)" id="wattsSlider" min="0"
         max="2000" value="0" step="1" class="slider2"></p>
     <p><label class="switch"><input type="checkbox" onload="toggleWattsCheckbox(this,true)"
           onchange="toggleWattsCheckbox(this,true)" id="wattsOutput"><span class="slider"></span></label></p>
 
     <h2>Sim CAD Output</h2>
     <p><span id="cadValue"></span></p>
-    <p><input type="range" onload="requestConfigValues()" onchange="updateCadSlider(this)" id="CadSlider" min="0"
+    <p><input type="range" onload="requestConfigValues()" onchange="updateCadSlider(this)" id="cadSlider" min="0"
         max="180" value="0" step="1" class="slider2"></p>
     <p><label class="switch"><input type="checkbox" onload="toggleCadCheckbox(this,true)"
           onchange="toggleCadCheckbox(this,true)" id="cadOutput"><span class="slider"></span></label></p>
+
+    <h1>Trainer Simulator</h1>
+    <h2>Enable ERG</h2>
+    <p>
+      <label class="switch">
+        <input type="checkbox" onload="toggleEnableErgCheckbox(this)" onchange="toggleEnableErgCheckbox(this)"
+          id="enableErgCheckbox">
+        <span class="slider"></span>
+      </label>
+    </p>
+    <h2>ERG Target Watts</h2>
+    <p><span id="targetWattsValue"></span></p>
+    <p><input type="range" onload="requestConfigValues()" onchange="updateTargetWattsSlider(this)"
+        id="targetWattsSlider" min="0" max="600" value="0" step="1" class="slider2"></p>
+    <p>
+      <label class="switch">
+        <input type="checkbox" onload="toggleTargetWattsCheckbox(this,true)"
+          onchange="toggleTargetWattsCheckbox(this,true)" id="targetWattsOutput">
+        <span class="slider"></span>
+      </label>
+    </p>
 </body>
 <script>
 
   function toggleHRCheckbox(element, updateServer) {
     if (element.checked) {
-      document.getElementById("HRSlider").hidden = false;
+      document.getElementById("hrSlider").hidden = false;
       document.getElementById("hrValue").hidden = false;
     }
     else {
-      document.getElementById("HRSlider").hidden = true;
+      document.getElementById("hrSlider").hidden = true;
       document.getElementById("hrValue").hidden = true;
     }
     if (updateServer) {
       var xhr = new XMLHttpRequest();
       if (element.checked) {
-        xhr.open("GET", "/hrslider?value=enable", true);
+        xhr.open("GET", "/hrSlider?value=enable", true);
         xhr.send();
       }
       else {
-        xhr.open("GET", "/hrslider?value=disable", true);
+        xhr.open("GET", "/hrSlider?value=disable", true);
         xhr.send();
       }
     }
@@ -68,11 +89,11 @@ <h2>Sim CAD Output</h2>
 
   function toggleWattsCheckbox(element, updateServer) {
     if (element.checked) {
-      document.getElementById("WattsSlider").hidden = false;
+      document.getElementById("wattsSlider").hidden = false;
       document.getElementById("wattsValue").hidden = false;
     }
     else {
-      document.getElementById("WattsSlider").hidden = true;
+      document.getElementById("wattsSlider").hidden = true;
       document.getElementById("wattsValue").hidden = true;
     }
 
@@ -91,11 +112,11 @@ <h2>Sim CAD Output</h2>
 
   function toggleCadCheckbox(element, updateServer) {
     if (element.checked) {
-      document.getElementById("CadSlider").hidden = false;
+      document.getElementById("cadSlider").hidden = false;
       document.getElementById("cadValue").hidden = false;
     }
     else {
-      document.getElementById("CadSlider").hidden = true;
+      document.getElementById("cadSlider").hidden = true;
       document.getElementById("cadValue").hidden = true;
     }
 
@@ -112,16 +133,16 @@ <h2>Sim CAD Output</h2>
     }
   }
 
-  function updateHRSlider(element) {
-    var sliderValue = document.getElementById("HRSlider").value;
+  function updatehrSlider(element) {
+    var sliderValue = document.getElementById("hrSlider").value;
     document.getElementById("hrValue").innerHTML = sliderValue + " BPM";
     var xhr = new XMLHttpRequest();
-    xhr.open("GET", "/hrslider?value=" + sliderValue, true);
+    xhr.open("GET", "/hrSlider?value=" + sliderValue, true);
     xhr.send();
   }
 
   function updateWattsSlider(element) {
-    var sliderValue = document.getElementById("WattsSlider").value;
+    var sliderValue = document.getElementById("wattsSlider").value;
     document.getElementById("wattsValue").innerHTML = sliderValue + " Watts";
     var xhr = new XMLHttpRequest();
     xhr.open("GET", "/wattsslider?value=" + sliderValue, true);
@@ -129,13 +150,56 @@ <h2>Sim CAD Output</h2>
   }
 
   function updateCadSlider(element) {
-    var sliderValue = document.getElementById("CadSlider").value;
+    var sliderValue = document.getElementById("cadSlider").value;
     document.getElementById("cadValue").innerHTML = sliderValue + " RPM";
     var xhr = new XMLHttpRequest();
     xhr.open("GET", "/cadslider?value=" + sliderValue, true);
     xhr.send();
   }
 
+  function updateTargetWattsSlider(element) {
+    var sliderValue = document.getElementById("targetWattsSlider").value;
+    document.getElementById("targetWattsValue").innerHTML = sliderValue + " Watts";
+    var xhr = new XMLHttpRequest();
+    xhr.open("GET", "/targetwattsslider?value=" + sliderValue, true);
+    xhr.send();
+  }
+
+  function toggleTargetWattsCheckbox(element, updateServer) {
+    if (element.checked) {
+      document.getElementById("targetWattsSlider").hidden = false;
+      document.getElementById("targetWattsValue").hidden = false;
+    }
+    else {
+      document.getElementById("targetWattsSlider").hidden = true;
+      document.getElementById("targetWattsValue").hidden = true;
+    }
+
+    if (updateServer) {
+      var xhr = new XMLHttpRequest();
+      if (element.checked) {
+        xhr.open("GET", "/targetwattsslider?value=enable", true);
+        xhr.send();
+      }
+      else {
+        xhr.open("GET", "/targetwattsslider?value=disable", true);
+        xhr.send();
+      }
+    }
+  }
+
+  function toggleEnableErgCheckbox(element) {
+    var xhr = new XMLHttpRequest();
+    if (element.checked) {
+      xhr.open("GET", "/ergmode?value=enable", true);
+      xhr.send();
+    }
+    else {
+      xhr.open("GET", "/ergmode?value=disable", true);
+      xhr.send();
+    }
+  }
+
   //Update values on specified interval
   setInterval(function () {
 
@@ -149,24 +213,37 @@ <h2>Sim CAD Output</h2>
       if (this.readyState == 4 && this.status == 200) {
         var obj = JSON.parse(this.responseText);
         document.getElementById("wattsValue").innerHTML = obj.simulatedWatts + " Watts";
-        document.getElementById("WattsSlider").value = obj.simulatedWatts;
+        document.getElementById("wattsSlider").value = obj.simulatedWatts;
         document.getElementById("wattsOutput").checked = obj.simulateWatts;
-        document.getElementById("WattsSlider").hidden = !obj.simulateWatts;
+        document.getElementById("wattsSlider").hidden = !obj.simulateWatts;
         document.getElementById("wattsValue").hidden = !obj.simulateWatts;
 
         document.getElementById("hrValue").innerHTML = obj.simulatedHr + " BPM";
-        document.getElementById("HRSlider").value = obj.simulatedHr;
+        document.getElementById("hrSlider").value = obj.simulatedHr;
         document.getElementById("hrOutput").checked = obj.simulateHr;
-        document.getElementById("HRSlider").hidden = !obj.simulateHr;
+        document.getElementById("hrSlider").hidden = !obj.simulateHr;
         document.getElementById("hrValue").hidden = !obj.simulateHr;
 
         document.getElementById("cadValue").innerHTML = obj.simulatedCad + " RPM";
-        document.getElementById("CadSlider").value = obj.simulatedCad;
+        document.getElementById("cadSlider").value = obj.simulatedCad;
         document.getElementById("cadOutput").checked = obj.simulateCad;
-        document.getElementById("CadSlider").hidden = !obj.simulateCad;
+        document.getElementById("cadSlider").hidden = !obj.simulateCad;
         document.getElementById("cadValue").hidden = !obj.simulateCad;
 
-        setTimeout(function () { document.getElementById("loadingWatermark").remove(); }, 1000);
+        document.getElementById("enableErgCheckbox").checked = obj.ERGMode;
+
+        document.getElementById("targetWattsValue").innerHTML = obj.targetWatts + " Watts";
+        document.getElementById("targetWattsSlider").value = obj.targetWatts == null ? 0 : obj.targetWatts;
+        document.getElementById("targetWattsOutput").checked = obj.simulateTargetWatts;
+        document.getElementById("targetWattsSlider").hidden = !obj.simulateTargetWatts;
+        document.getElementById("targetWattsValue").hidden = !obj.simulateTargetWatts;
+
+        setTimeout(function () {
+          const watermark = document.getElementById("loadingWatermark");
+          if (!!watermark) {
+            document.getElementById("loadingWatermark")?.remove();
+          }
+        }, 1000);
       }
     };
     xhttp.open("GET", "/configJSON", true);
diff --git a/include/ERG_Mode.h b/include/ERG_Mode.h
index b304a0de..2ba75af1 100644
--- a/include/ERG_Mode.h
+++ b/include/ERG_Mode.h
@@ -3,14 +3,13 @@
 #include <Arduino.h>
 
 #define ERG_MODE_LOG_TAG "ERG_Mode"
-#define ERG_MODE_DELAY 1000
+#define ERG_MODE_DELAY   1000
+
+extern TaskHandle_t ErgTask;
+void setupERG();
+void ergTaskLoop(void *pvParameters);
 
 class ErgMode {
  public:
-  static void setupERG();
-
- private:
-  static TaskHandle_t _ergTask;
-  static void loop(void *pvParameters);
-  static void computErg(int newSetpoint);
+  void computErg(int newSetpoint);
 };
\ No newline at end of file
diff --git a/include/SmartSpin_parameters.h b/include/SmartSpin_parameters.h
index 433e5909..bd61f415 100644
--- a/include/SmartSpin_parameters.h
+++ b/include/SmartSpin_parameters.h
@@ -28,6 +28,7 @@ class userParameters {
   float powerCorrectionFactor;
   bool simulateHr;
   bool simulateWatts;
+  bool simulateTargetWatts;
   bool simulateCad;
   bool ERGMode;
   float ERGSensitivity;
@@ -55,6 +56,7 @@ class userParameters {
   float getPowerCorrectionFactor() { return powerCorrectionFactor; }
   bool getSimulateHr() { return simulateHr; }
   bool getSimulateWatts() { return simulateWatts; }
+  bool getSimulateTargetWatts() { return simulateTargetWatts; }
   bool getSimulateCad() { return simulateCad; }
   bool getERGMode() { return ERGMode; }
   float getERGSensitivity() { return ERGSensitivity; }
@@ -82,6 +84,7 @@ class userParameters {
   void setPowerCorrectionFactor(float pm) { powerCorrectionFactor = pm; }
   void setSimulateHr(bool shr) { simulateHr = shr; }
   void setSimulateWatts(bool swt) { simulateWatts = swt; }
+  void setSimulateTargetWatts(bool swt) { simulateTargetWatts = swt; }
   void setSimulateCad(bool scd) { simulateCad = scd; }
   void setERGMode(bool erg) { ERGMode = erg; }
   void setERGSensitivity(float ergS) { ERGSensitivity = ergS; }
diff --git a/include/settings.h b/include/settings.h
index e12c4443..b05c2f7e 100644
--- a/include/settings.h
+++ b/include/settings.h
@@ -159,7 +159,7 @@
 // #define USE_TELEGRAM
 
 // Uncomment to enable stack size debugging info
-//#define DEBUG_STACK
+// #define DEBUG_STACK
 
 // Uncomment to enable HR->PWR debugging info. Always displays HR->PWR
 // Calculation. Never sets userConfig.setSimulatedPower(); #define
diff --git a/src/ERG_Mode.cpp b/src/ERG_Mode.cpp
index eca9a4b3..c32824a8 100644
--- a/src/ERG_Mode.cpp
+++ b/src/ERG_Mode.cpp
@@ -2,34 +2,45 @@
 #include "SS2KLog.h"
 #include "Main.h"
 
-TaskHandle_t ErgMode::_ergTask;
+TaskHandle_t ErgTask;
 
-void ErgMode::setupERG() {
+void setupERG() {
+  TaskHandle_t task_handle;
   SS2K_LOG(ERG_MODE_LOG_TAG, "Starting ERG Mode task...");
-  xTaskCreatePinnedToCore(loop,          /* Task function. */
+  xTaskCreatePinnedToCore(ergTaskLoop,   /* Task function. */
                           "ERGModeTask", /* name of task. */
-                          1800,          /* Stack size of task*/
+                          2500,          /* Stack size of task*/
                           NULL,          /* parameter of the task */
                           1,             /* priority of the task*/
-                          &_ergTask,     /* Task handle to keep track of created task */
-                          1);            /* pin task to core 0 */
+                          &ErgTask,      /* Task handle to keep track of created task */
+                          0);            /* pin task to core 0 */
 
   SS2K_LOG(ERG_MODE_LOG_TAG, "ERG Mode task started");
 }
 
-void ErgMode::loop(void *pvParameters) {
-  vTaskDelay(ERG_MODE_DELAY / portTICK_PERIOD_MS);
+void ergTaskLoop(void* pvParameters) {
+  ErgMode ergMode;
+  while (true) {
+    vTaskDelay(1000 / portTICK_PERIOD_MS);
 
-  //   int newSetPoint             = userConfig.getTargetWatts();
-  //   bool isInErgMode            = userConfig.getERGMode();
-  //   bool hasConnectedPowerMeter = spinBLEClient.connectedPM;
+    int newSetPoint             = userConfig.getTargetWatts();
+    bool isInErgMode            = userConfig.getERGMode();
+    bool hasConnectedPowerMeter = spinBLEClient.connectedPM;
+    bool simulationRunning      = userConfig.getSimulateTargetWatts();
 
-  //   if (isInErgMode && hasConnectedPowerMeter) {
-  //     SS2K_LOG(ERG_MODE_LOG_TAG, "ComputeERG. Setpoint: %d", newSetPoint);
-  //     ErgMode::computErg(newSetPoint);
-  //   } else {
-  // SS2K_LOG(ERG_MODE_LOG_TAG, "ERG request but ERG off or no connected PM");
-  //   }
+    if (isInErgMode && (hasConnectedPowerMeter || simulationRunning)) {
+      SS2K_LOG(ERG_MODE_LOG_TAG, "ComputeERG. Setpoint: %d", newSetPoint);
+      ergMode.computErg(newSetPoint);
+    } else {
+      if (newSetPoint > 0) {
+        SS2K_LOG(ERG_MODE_LOG_TAG, "ERG request but ERG off or no connected PM");
+      }
+    }
+
+    // #ifdef DEBUG_STACK
+    Serial.printf("ERG Task: %d \n", uxTaskGetStackHighWaterMark(ErgTask));
+    // #endif  // DEBUG_STACK
+  }
 }
 
 // as a note, Trainer Road sends 50w target whenever the app is connected.
@@ -42,6 +53,8 @@ void ErgMode::computErg(int newSetPoint) {
   int wattChange             = userConfig.getSimulatedWatts() - setPoint;
   int cadance                = userConfig.getSimulatedCad();
 
+  SS2K_LOG(ERG_MODE_LOG_TAG, "Incline = %f, SetPoint = %d, NewSetPoint = %d, WattChange = %d", incline, setPoint, newSetPoint, wattChange);
+
   if (newSetPoint > 0) {  // only update the value if new value is sent
     setPoint = newSetPoint;
   }
diff --git a/src/HTTP_Server_Basic.cpp b/src/HTTP_Server_Basic.cpp
index 277a77e7..fd4f6bd4 100644
--- a/src/HTTP_Server_Basic.cpp
+++ b/src/HTTP_Server_Basic.cpp
@@ -214,7 +214,37 @@ void startHttpServer() {
       SS2K_LOG(HTTP_SERVER_LOG_TAG, "CAD Simulator turned off");
     } else {
       userConfig.setSimulatedCad(value.toInt());
-      SS2K_LOG(HTTP_SERVER_LOG_TAG, "CAD is now: %f", userConfig.getSimulatedCad());
+      SS2K_LOG(HTTP_SERVER_LOG_TAG, "CAD is now: %d", userConfig.getSimulatedCad());
+      server.send(200, "text/plain", "OK");
+    }
+  });
+
+  server.on("/ergmode", []() {
+    String value = server.arg("value");
+    if (value == "enable") {
+      userConfig.setERGMode(true);
+      server.send(200, "text/plain", "OK");
+      SS2K_LOG(HTTP_SERVER_LOG_TAG, "ERG Mode turned on");
+    } else {
+      userConfig.setERGMode(false);
+      server.send(200, "text/plain", "OK");
+      SS2K_LOG(HTTP_SERVER_LOG_TAG, "ERG Mode turned off");
+    }
+  });
+
+  server.on("/targetwattsslider", []() {
+    String value = server.arg("value");
+    if (value == "enable") {
+      userConfig.setSimulateTargetWatts(true);
+      server.send(200, "text/plain", "OK");
+      SS2K_LOG(HTTP_SERVER_LOG_TAG, "Target Watts Simulator turned on");
+    } else if (value == "disable") {
+      userConfig.setSimulateTargetWatts(false);
+      server.send(200, "text/plain", "OK");
+      SS2K_LOG(HTTP_SERVER_LOG_TAG, "Target Watts Simulator turned off");
+    } else {
+      userConfig.setTargetWatts(value.toInt());
+      SS2K_LOG(HTTP_SERVER_LOG_TAG, "Target Watts are now: %d", userConfig.getTargetWatts());
       server.send(200, "text/plain", "OK");
     }
   });
diff --git a/src/Main.cpp b/src/Main.cpp
index 302e4861..ec855dd5 100644
--- a/src/Main.cpp
+++ b/src/Main.cpp
@@ -109,8 +109,8 @@ void setup() {
   FirmwareUpdate();
 
   setupBLE();
+  setupERG();
   startHttpServer();
-  ErgMode::setupERG();
 
   resetIfShiftersHeld();
   SS2K_LOG(MAIN_LOG_TAG, "Creating Shifter Interrupts");
diff --git a/src/SmartSpin_parameters.cpp b/src/SmartSpin_parameters.cpp
index 0e7ac5de..e60434dc 100644
--- a/src/SmartSpin_parameters.cpp
+++ b/src/SmartSpin_parameters.cpp
@@ -17,9 +17,10 @@ void userParameters::setDefaults() {  // Move these to set the values as #define
                                       // in main.h
   firmwareUpdateURL     = FW_UPDATEURL;
   incline               = 0.0;
-  simulatedWatts        = 00;
-  simulatedHr           = 00;
-  simulatedCad          = 00;
+  targetWatts           = 0;
+  simulatedWatts        = 0;
+  simulatedHr           = 0;
+  simulatedCad          = 0;
   deviceName            = DEVICE_NAME;
   shiftStep             = 600;
   stepperPower          = STEPPER_POWER;
@@ -28,6 +29,7 @@ void userParameters::setDefaults() {  // Move these to set the values as #define
   powerCorrectionFactor = 1.0;
   simulateHr            = false;
   simulateWatts         = false;
+  simulateTargetWatts   = false;
   simulateCad           = false;
   ERGMode               = false;
   ERGSensitivity        = ERG_SENSITIVITY;
@@ -52,6 +54,7 @@ String userParameters::returnJSON(bool includeDebugLog) {
   doc["firmwareUpdateURL"]     = firmwareUpdateURL;
   doc["firmwareVersion"]       = FIRMWARE_VERSION;
   doc["incline"]               = incline;
+  doc["targetWatts"]           = targetWatts;
   doc["simulatedWatts"]        = simulatedWatts;
   doc["simulatedHr"]           = simulatedHr;
   doc["simulatedCad"]          = simulatedCad;
@@ -63,6 +66,7 @@ String userParameters::returnJSON(bool includeDebugLog) {
   doc["powerCorrectionFactor"] = powerCorrectionFactor;
   doc["simulateHr"]            = simulateHr;
   doc["simulateWatts"]         = simulateWatts;
+  doc["simulateTargetWatts"]   = simulateTargetWatts;
   doc["simulateCad"]           = simulateCad;
   doc["ERGMode"]               = ERGMode;
   doc["ERGSensitivity"]        = ERGSensitivity;

From 1bfcd5604ac21b8122e3cd91205b62be30dbc71c Mon Sep 17 00:00:00 2001
From: Markus Schneider <m.schneider@online.de>
Date: Sat, 4 Dec 2021 15:55:53 +0100
Subject: [PATCH 03/42] skip erg comp if stepper is running

---
 include/Main.h                 |  2 ++
 include/SmartSpin_parameters.h | 22 ++++++++++++++--------
 src/ERG_Mode.cpp               |  8 ++++++--
 src/HTTP_Server_Basic.cpp      |  1 +
 src/Main.cpp                   | 25 +++++++++++++++++++++++--
 src/SmartSpin_parameters.cpp   |  2 ++
 6 files changed, 48 insertions(+), 12 deletions(-)

diff --git a/include/Main.h b/include/Main.h
index a69ee27e..e72c7c14 100644
--- a/include/Main.h
+++ b/include/Main.h
@@ -26,6 +26,8 @@ void updateStepperPower();
 void updateStealthchop();
 void checkDriverTemperature();
 void motorStop(bool releaseTension = false);
+void startTasks();
+void stopTasks();
 
 // Main program variable that stores most everything
 extern userParameters userConfig;
diff --git a/include/SmartSpin_parameters.h b/include/SmartSpin_parameters.h
index bd61f415..4d39c4a9 100644
--- a/include/SmartSpin_parameters.h
+++ b/include/SmartSpin_parameters.h
@@ -13,32 +13,36 @@
 
 class userParameters {
  private:
-  String firmwareUpdateURL;
+  // Runtime Settings
   float incline;
   int targetWatts;
   int simulatedWatts;
   int simulatedHr;
   int simulatedCad;
   float simulatedSpeed;
-  String deviceName;
-  int shiftStep;
   int stepperPower;
-  bool stealthchop;
-  float inclineMultiplier;
-  float powerCorrectionFactor;
   bool simulateHr;
   bool simulateWatts;
   bool simulateTargetWatts;
   bool simulateCad;
   bool ERGMode;
+  int shifterPosition;
+  String foundDevices = " ";
+  bool stepperRunning;
+
+  // System Settings
+  String firmwareUpdateURL;
+  String deviceName;
+  int shiftStep;
+  bool stealthchop;
+  float inclineMultiplier;
+  float powerCorrectionFactor;
   float ERGSensitivity;
   bool autoUpdate;
   String ssid;
   String password;
-  String foundDevices          = " ";
   String connectedPowerMeter   = "any";
   String connectedHeartMonitor = "any";
-  int shifterPosition;
 
  public:
   const char* getFirmwareUpdateURL() { return firmwareUpdateURL.c_str(); }
@@ -67,6 +71,7 @@ class userParameters {
   const char* getconnectedPowerMeter() { return connectedPowerMeter.c_str(); }
   const char* getconnectedHeartMonitor() { return connectedHeartMonitor.c_str(); }
   int getShifterPosition() { return shifterPosition; }
+  bool getStepperRunning() { return stepperRunning; }
 
   void setDefaults();
   void setFirmwareUpdateURL(String fURL) { firmwareUpdateURL = fURL; }
@@ -95,6 +100,7 @@ class userParameters {
   void setConnectedPowerMeter(String cpm) { connectedPowerMeter = cpm; }
   void setConnectedHeartMonitor(String cHr) { connectedHeartMonitor = cHr; }
   void setShifterPosition(int sp) { shifterPosition = sp; }
+  void setStepperRunning(bool running) { stepperRunning = running; }
 
   String returnJSON(bool includeDebugLog = false);
   void saveToSPIFFS();
diff --git a/src/ERG_Mode.cpp b/src/ERG_Mode.cpp
index c32824a8..ffac6528 100644
--- a/src/ERG_Mode.cpp
+++ b/src/ERG_Mode.cpp
@@ -37,9 +37,9 @@ void ergTaskLoop(void* pvParameters) {
       }
     }
 
-    // #ifdef DEBUG_STACK
+#ifdef DEBUG_STACK
     Serial.printf("ERG Task: %d \n", uxTaskGetStackHighWaterMark(ErgTask));
-    // #endif  // DEBUG_STACK
+#endif  // DEBUG_STACK
   }
 }
 
@@ -52,8 +52,12 @@ void ErgMode::computErg(int newSetPoint) {
   int amountToChangeIncline  = 0;
   int wattChange             = userConfig.getSimulatedWatts() - setPoint;
   int cadance                = userConfig.getSimulatedCad();
+  bool isStepperRunning      = userConfig.getStepperRunning();
 
   SS2K_LOG(ERG_MODE_LOG_TAG, "Incline = %f, SetPoint = %d, NewSetPoint = %d, WattChange = %d", incline, setPoint, newSetPoint, wattChange);
+  if (isStepperRunning) {
+    SS2K_LOG(ERG_MODE_LOG_TAG, "Stepper is running. Skip ERG computation");
+  }
 
   if (newSetPoint > 0) {  // only update the value if new value is sent
     setPoint = newSetPoint;
diff --git a/src/HTTP_Server_Basic.cpp b/src/HTTP_Server_Basic.cpp
index fd4f6bd4..b451f344 100644
--- a/src/HTTP_Server_Basic.cpp
+++ b/src/HTTP_Server_Basic.cpp
@@ -284,6 +284,7 @@ void startHttpServer() {
         if (upload.filename == String("firmware.bin").c_str()) {
           if (upload.status == UPLOAD_FILE_START) {
             SS2K_LOG(HTTP_SERVER_LOG_TAG, "Update: %s", upload.filename.c_str());
+            stopTasks();
             if (!Update.begin(UPDATE_SIZE_UNKNOWN)) {  // start with max
                                                        // available size
               Update.printError(Serial);
diff --git a/src/Main.cpp b/src/Main.cpp
index ec855dd5..0456368f 100644
--- a/src/Main.cpp
+++ b/src/Main.cpp
@@ -47,6 +47,27 @@ physicalWorkingCapacity userPWC;
 
 ///////////// BEGIN SETUP /////////////
 #ifndef UNIT_TEST
+
+void startTasks() {
+  SS2K_LOG(MAIN_LOG_TAG, "Start BLE & ERG Tasks");
+  if (BLECommunicationTask == NULL) {
+    setupBLE();
+  }
+  if (ErgTask == NULL) {
+    setupERG();
+  }
+}
+
+void stopTasks() {
+  SS2K_LOG(MAIN_LOG_TAG, "Stop BLE & ERG Tasks");
+  if (BLECommunicationTask != NULL) {
+    vTaskDelete(BLECommunicationTask);
+  }
+  if (ErgTask != NULL) {
+    vTaskDelete(ErgTask);
+  }
+}
+
 void setup() {
   // Serial port for debugging purposes
   Serial.begin(512000);
@@ -108,8 +129,7 @@ void setup() {
   // fails to update which really sucks when it corrupts your settings.
   FirmwareUpdate();
 
-  setupBLE();
-  setupERG();
+  startTasks();
   startHttpServer();
 
   resetIfShiftersHeld();
@@ -175,6 +195,7 @@ void moveStepper(void *pvParameters) {
       stepper->moveTo(targetPosition);
     }
     vTaskDelay(100 / portTICK_PERIOD_MS);
+    userConfig.setStepperRunning(stepper->isRunning());
     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();
diff --git a/src/SmartSpin_parameters.cpp b/src/SmartSpin_parameters.cpp
index e60434dc..721171b5 100644
--- a/src/SmartSpin_parameters.cpp
+++ b/src/SmartSpin_parameters.cpp
@@ -32,6 +32,7 @@ void userParameters::setDefaults() {  // Move these to set the values as #define
   simulateTargetWatts   = false;
   simulateCad           = false;
   ERGMode               = false;
+  stepperRunning        = false;
   ERGSensitivity        = ERG_SENSITIVITY;
   autoUpdate            = AUTO_FIRMWARE_UPDATE;
   ssid                  = DEVICE_NAME;
@@ -69,6 +70,7 @@ String userParameters::returnJSON(bool includeDebugLog) {
   doc["simulateTargetWatts"]   = simulateTargetWatts;
   doc["simulateCad"]           = simulateCad;
   doc["ERGMode"]               = ERGMode;
+  doc["stepperRunning"]        = stepperRunning;
   doc["ERGSensitivity"]        = ERGSensitivity;
   doc["autoUpdate"]            = autoUpdate;
   doc["ssid"]                  = ssid;

From 707cdd22f6948f57a9d1d765445611c061156f84 Mon Sep 17 00:00:00 2001
From: Markus Schneider <m.schneider@online.de>
Date: Sat, 4 Dec 2021 20:11:23 +0100
Subject: [PATCH 04/42] splitted settings to userSettings and RuntimeSettings

---
 include/ERG_Mode.h             |   2 +-
 include/Main.h                 |   1 +
 include/SmartSpin_parameters.h | 111 +++++++++++++++++++--------------
 include/settings.h             |   2 +-
 src/BLE_Client.cpp             |   2 +-
 src/BLE_Common.cpp             |  46 ++++++++------
 src/BLE_Server.cpp             | 102 +++++++++++++++---------------
 src/ERG_Mode.cpp               |  20 +++---
 src/HTTP_Server_Basic.cpp      |  36 +++++------
 src/Main.cpp                   |  21 ++++---
 src/SmartSpin_parameters.cpp   |  54 ++--------------
 11 files changed, 189 insertions(+), 208 deletions(-)

diff --git a/include/ERG_Mode.h b/include/ERG_Mode.h
index 2ba75af1..0b0c33d1 100644
--- a/include/ERG_Mode.h
+++ b/include/ERG_Mode.h
@@ -3,7 +3,7 @@
 #include <Arduino.h>
 
 #define ERG_MODE_LOG_TAG "ERG_Mode"
-#define ERG_MODE_DELAY   1000
+#define ERG_MODE_DELAY   100
 
 extern TaskHandle_t ErgTask;
 void setupERG();
diff --git a/include/Main.h b/include/Main.h
index e72c7c14..7d0c09c8 100644
--- a/include/Main.h
+++ b/include/Main.h
@@ -31,6 +31,7 @@ void stopTasks();
 
 // Main program variable that stores most everything
 extern userParameters userConfig;
+extern RuntimeParameters rtConfig;
 extern int32_t targetPosition;
 extern bool externalControl;
 extern bool syncMode;
diff --git a/include/SmartSpin_parameters.h b/include/SmartSpin_parameters.h
index 4d39c4a9..4784e58d 100644
--- a/include/SmartSpin_parameters.h
+++ b/include/SmartSpin_parameters.h
@@ -11,26 +11,68 @@
 
 #define CONFIG_LOG_TAG "Config"
 
-class userParameters {
+class Measurement {
+ public:
+  int value;
+  long timestamp;
+
+  Measurement(int value) {
+    this->value     = value;
+    this->timestamp = millis();
+  }
+};
+
+class RuntimeParameters {
  private:
-  // Runtime Settings
-  float incline;
-  int targetWatts;
-  int simulatedWatts;
-  int simulatedHr;
-  int simulatedCad;
-  float simulatedSpeed;
-  int stepperPower;
-  bool simulateHr;
-  bool simulateWatts;
-  bool simulateTargetWatts;
-  bool simulateCad;
-  bool ERGMode;
-  int shifterPosition;
-  String foundDevices = " ";
-  bool stepperRunning;
+  float incline              = 0.0;
+  int targetWatts            = 0;
+  Measurement simulatedWatts = Measurement(0);
+  int simulatedHr            = 0;
+  int simulatedCad           = 0;
+  float simulatedSpeed       = 0.0;
+  bool simulateHr            = false;
+  bool simulateWatts         = false;
+  bool simulateTargetWatts   = false;
+  bool simulateCad           = false;
+  bool ERGMode               = false;
+  int shifterPosition        = 0;
+  String foundDevices        = " ";
+  bool stepperRunning        = false;
 
-  // System Settings
+ public:
+  float getIncline() { return incline; }
+  int getTargetWatts() { return targetWatts; }
+  Measurement getSimulatedWatts() { return simulatedWatts; }
+  int getSimulatedHr() { return simulatedHr; }
+  int getSimulatedCad() { return simulatedCad; }
+  float getSimulatedSpeed() { return simulatedSpeed; }
+  bool getSimulateHr() { return simulateHr; }
+  bool getSimulateWatts() { return simulateWatts; }
+  bool getSimulateTargetWatts() { return simulateTargetWatts; }
+  bool getSimulateCad() { return simulateCad; }
+  bool getERGMode() { return ERGMode; }
+  int getShifterPosition() { return shifterPosition; }
+  bool getStepperRunning() { return stepperRunning; }
+  const char* getFoundDevices() { return foundDevices.c_str(); }
+
+  void setIncline(float inc) { incline = inc; }
+  void setTargetWatts(int w) { targetWatts = w; }
+  void setSimulatedWatts(Measurement w) { simulatedWatts = w; }
+  void setSimulatedHr(int hr) { simulatedHr = hr; }
+  void setSimulatedCad(float cad) { simulatedCad = cad; }
+  void setSimulatedSpeed(float spd) { simulatedSpeed = spd; }
+  void setSimulateHr(bool shr) { simulateHr = shr; }
+  void setSimulateWatts(bool swt) { simulateWatts = swt; }
+  void setSimulateTargetWatts(bool swt) { simulateTargetWatts = swt; }
+  void setSimulateCad(bool scd) { simulateCad = scd; }
+  void setERGMode(bool erg) { ERGMode = erg; }
+  void setShifterPosition(int sp) { shifterPosition = sp; }
+  void setStepperRunning(bool running) { stepperRunning = running; }
+  void setFoundDevices(String fdev) { foundDevices = fdev; }
+};
+
+class userParameters {
+ private:
   String firmwareUpdateURL;
   String deviceName;
   int shiftStep;
@@ -39,6 +81,7 @@ class userParameters {
   float powerCorrectionFactor;
   float ERGSensitivity;
   bool autoUpdate;
+  int stepperPower;
   String ssid;
   String password;
   String connectedPowerMeter   = "any";
@@ -46,61 +89,33 @@ class userParameters {
 
  public:
   const char* getFirmwareUpdateURL() { return firmwareUpdateURL.c_str(); }
-  float getIncline() { return incline; }
-  int getTargetWatts() { return targetWatts; }
-  int getSimulatedWatts() { return simulatedWatts; }
-  int getSimulatedHr() { return simulatedHr; }
-  int getSimulatedCad() { return simulatedCad; }
-  float getSimulatedSpeed() { return simulatedSpeed; }
   const char* getDeviceName() { return deviceName.c_str(); }
   int getShiftStep() { return shiftStep; }
-  int getStepperPower() { return stepperPower; }
   bool getStealthchop() { return stealthchop; }
   float getInclineMultiplier() { return inclineMultiplier; }
   float getPowerCorrectionFactor() { return powerCorrectionFactor; }
-  bool getSimulateHr() { return simulateHr; }
-  bool getSimulateWatts() { return simulateWatts; }
-  bool getSimulateTargetWatts() { return simulateTargetWatts; }
-  bool getSimulateCad() { return simulateCad; }
-  bool getERGMode() { return ERGMode; }
   float getERGSensitivity() { return ERGSensitivity; }
   bool getautoUpdate() { return autoUpdate; }
   const char* getSsid() { return ssid.c_str(); }
   const char* getPassword() { return password.c_str(); }
-  const char* getFoundDevices() { return foundDevices.c_str(); }
   const char* getconnectedPowerMeter() { return connectedPowerMeter.c_str(); }
   const char* getconnectedHeartMonitor() { return connectedHeartMonitor.c_str(); }
-  int getShifterPosition() { return shifterPosition; }
-  bool getStepperRunning() { return stepperRunning; }
+  int getStepperPower() { return stepperPower; }
 
   void setDefaults();
   void setFirmwareUpdateURL(String fURL) { firmwareUpdateURL = fURL; }
-  void setIncline(float inc) { incline = inc; }
-  void setTargetWatts(int w) { targetWatts = w; }
-  void setSimulatedWatts(int w) { simulatedWatts = w; }
-  void setSimulatedHr(int hr) { simulatedHr = hr; }
-  void setSimulatedCad(float cad) { simulatedCad = cad; }
-  void setSimulatedSpeed(float spd) { simulatedSpeed = spd; }
   void setDeviceName(String dvcn) { deviceName = dvcn; }
   void setShiftStep(int ss) { shiftStep = ss; }
-  void setStepperPower(int sp) { stepperPower = sp; }
   void setStealthChop(bool sc) { stealthchop = sc; }
   void setInclineMultiplier(float im) { inclineMultiplier = im; }
   void setPowerCorrectionFactor(float pm) { powerCorrectionFactor = pm; }
-  void setSimulateHr(bool shr) { simulateHr = shr; }
-  void setSimulateWatts(bool swt) { simulateWatts = swt; }
-  void setSimulateTargetWatts(bool swt) { simulateTargetWatts = swt; }
-  void setSimulateCad(bool scd) { simulateCad = scd; }
-  void setERGMode(bool erg) { ERGMode = erg; }
   void setERGSensitivity(float ergS) { ERGSensitivity = ergS; }
   void setAutoUpdate(bool atupd) { autoUpdate = atupd; }
   void setSsid(String sid) { ssid = sid; }
   void setPassword(String pwd) { password = pwd; }
-  void setFoundDevices(String fdev) { foundDevices = fdev; }
   void setConnectedPowerMeter(String cpm) { connectedPowerMeter = cpm; }
   void setConnectedHeartMonitor(String cHr) { connectedHeartMonitor = cHr; }
-  void setShifterPosition(int sp) { shifterPosition = sp; }
-  void setStepperRunning(bool running) { stepperRunning = running; }
+  void setStepperPower(int sp) { stepperPower = sp; }
 
   String returnJSON(bool includeDebugLog = false);
   void saveToSPIFFS();
diff --git a/include/settings.h b/include/settings.h
index b05c2f7e..978b9c10 100644
--- a/include/settings.h
+++ b/include/settings.h
@@ -122,7 +122,7 @@
 #define MAX_SCAN_RETRIES 1
 
 // loop speed for the SmartSpin2k BLE communications
-#define BLE_NOTIFY_DELAY 701
+#define BLE_NOTIFY_DELAY 200
 
 // loop speed for the SmartSpin2k BLE Client reconnect
 #define BLE_CLIENT_DELAY 1000
diff --git a/src/BLE_Client.cpp b/src/BLE_Client.cpp
index 4dc118b2..751a4155 100644
--- a/src/BLE_Client.cpp
+++ b/src/BLE_Client.cpp
@@ -420,7 +420,7 @@ void SpinBLEClient::scanProcess() {
 #ifdef USE_TELEGRAM
   SEND_TO_TELEGRAM("Bluetooth Client Found Devices: " + output);
 #endif
-  userConfig.setFoundDevices(output);
+  rtConfig.setFoundDevices(output);
   pBLEScan = nullptr;  // free up memory
 }
 
diff --git a/src/BLE_Common.cpp b/src/BLE_Common.cpp
index e7bac585..96cd1aec 100644
--- a/src/BLE_Common.cpp
+++ b/src/BLE_Common.cpp
@@ -68,27 +68,27 @@ void BLECommunications(void *pvParameters) {
                       pRemoteBLECharacteristic->getUUID(), (uint64_t)pRemoteBLECharacteristic->getRemoteService()->getClient()->getPeerAddress(), pData, length);
 
                   logBufLength += snprintf(logBuf + logBufLength, kLogBufMaxLength - logBufLength, " | %s[", sensorData->getId().c_str());
-                  if (sensorData->hasHeartRate() && !userConfig.getSimulateHr()) {
+                  if (sensorData->hasHeartRate() && !rtConfig.getSimulateHr()) {
                     int heartRate = sensorData->getHeartRate();
-                    userConfig.setSimulatedHr(heartRate);
+                    rtConfig.setSimulatedHr(heartRate);
                     spinBLEClient.connectedHR |= true;
                     logBufLength += snprintf(logBuf + logBufLength, kLogBufMaxLength - logBufLength, " HR(%d)", heartRate % 1000);
                   }
-                  if (sensorData->hasCadence() && !userConfig.getSimulateCad()) {
+                  if (sensorData->hasCadence() && !rtConfig.getSimulateCad()) {
                     float cadence = sensorData->getCadence();
-                    userConfig.setSimulatedCad(cadence);
+                    rtConfig.setSimulatedCad(cadence);
                     spinBLEClient.connectedCD |= true;
                     logBufLength += snprintf(logBuf + logBufLength, kLogBufMaxLength - logBufLength, " CD(%.2f)", fmodf(cadence, 1000.0));
                   }
-                  if (sensorData->hasPower() && !userConfig.getSimulateWatts()) {
+                  if (sensorData->hasPower() && !rtConfig.getSimulateWatts()) {
                     int power = sensorData->getPower() * userConfig.getPowerCorrectionFactor();
-                    userConfig.setSimulatedWatts(power);
+                    rtConfig.setSimulatedWatts(power);
                     spinBLEClient.connectedPM |= true;
                     logBufLength += snprintf(logBuf + logBufLength, kLogBufMaxLength - logBufLength, " PW(%d)", power % 10000);
                   }
                   if (sensorData->hasSpeed()) {
                     float speed = sensorData->getSpeed();
-                    userConfig.setSimulatedSpeed(speed);
+                    rtConfig.setSimulatedSpeed(speed);
                     logBufLength += snprintf(logBuf + logBufLength, kLogBufMaxLength - logBufLength, " SD(%.2f)", fmodf(speed, 1000.0));
                   }
                   strncat(logBuf + logBufLength, " ]", kLogBufMaxLength - logBufLength);
@@ -111,7 +111,7 @@ void BLECommunications(void *pvParameters) {
     }
 
     // ***********************************SERVER**************************************
-    if ((spinBLEClient.connectedHR || userConfig.getSimulateHr()) && !spinBLEClient.connectedPM && !userConfig.getSimulateWatts() && (userConfig.getSimulatedHr() > 0) &&
+    if ((spinBLEClient.connectedHR || rtConfig.getSimulateHr()) && !spinBLEClient.connectedPM && !rtConfig.getSimulateWatts() && (rtConfig.getSimulatedHr() > 0) &&
         userPWC.hr2Pwr) {
       calculateInstPwrFromHR();
       hr2p = true;
@@ -122,12 +122,12 @@ void BLECommunications(void *pvParameters) {
     calculateInstPwrFromHR();
 #endif  // DEBUG_HR_TO_PWR
 
-    if (!spinBLEClient.connectedPM && !hr2p && !userConfig.getSimulateWatts() && !userConfig.getSimulateCad()) {
-      userConfig.setSimulatedCad(0);
-      userConfig.setSimulatedWatts(0);
+    if (!spinBLEClient.connectedPM && !hr2p && !rtConfig.getSimulateWatts() && !rtConfig.getSimulateCad()) {
+      rtConfig.setSimulatedCad(0);
+      rtConfig.setSimulatedWatts(0);
     }
-    if (!spinBLEClient.connectedHR && !userConfig.getSimulateHr()) {
-      userConfig.setSimulatedHr(0);
+    if (!spinBLEClient.connectedHR && !rtConfig.getSimulateHr()) {
+      rtConfig.setSimulatedHr(0);
     }
 
     if (connectedClientCount() > 0) {
@@ -149,11 +149,8 @@ void BLECommunications(void *pvParameters) {
         BLEDevice::getServer()->updateConnParams(bleConnDesc, 20, 100, 0, 2000);
         updateConnParametersFlag = false;
       }
-    } else {
-    }
-    if (connectedClientCount() == 0) {
-      digitalWrite(LED_PIN, LOW);  // blink if no client connected
     }
+
     if (BLEDevice::getAdvertising()) {
       if (!(BLEDevice::getAdvertising()->isAdvertising()) && (BLEDevice::getServer()->getConnectedCount() < CONFIG_BT_NIMBLE_MAX_CONNECTIONS - NUM_BLE_DEVICES)) {
         SS2K_LOG(BLE_COMMON_LOG_TAG, "Starting Advertising From Communication Loop");
@@ -161,9 +158,18 @@ void BLECommunications(void *pvParameters) {
       }
     }
 
-    vTaskDelay((BLE_NOTIFY_DELAY / 2) / portTICK_PERIOD_MS);
-    digitalWrite(LED_PIN, HIGH);
-    vTaskDelay((BLE_NOTIFY_DELAY / 2) / portTICK_PERIOD_MS);
+    // blink if no client connected
+    if (connectedClientCount() == 0) {
+      if ((millis() / 500) % 2 == 0) {
+        digitalWrite(LED_PIN, LOW);
+      } else {
+        digitalWrite(LED_PIN, HIGH);
+      }
+    } else {
+      digitalWrite(LED_PIN, HIGH);
+    }
+
+    vTaskDelay((BLE_NOTIFY_DELAY) / portTICK_PERIOD_MS);
 #ifdef DEBUG_STACK
     Serial.printf("BLEComm: %d \n", uxTaskGetStackHighWaterMark(BLECommunicationTask));
 #endif  // DEBUG_STACK
diff --git a/src/BLE_Server.cpp b/src/BLE_Server.cpp
index 6b79bc94..a478063b 100644
--- a/src/BLE_Server.cpp
+++ b/src/BLE_Server.cpp
@@ -208,14 +208,14 @@ bool spinDown() {
 
 
 void updateIndoorBikeDataChar() {
-  float cadRaw = userConfig.getSimulatedCad();
+  float cadRaw = rtConfig.getSimulatedCad();
   int cad      = static_cast<int>(cadRaw * 2);
 
-  int watts = userConfig.getSimulatedWatts();
-  int hr    = userConfig.getSimulatedHr();
+  int watts = rtConfig.getSimulatedWatts().value;
+  int hr    = rtConfig.getSimulatedHr();
 
   int speed      = 0;
-  float speedRaw = userConfig.getSimulatedSpeed();
+  float speedRaw = rtConfig.getSimulatedSpeed();
   if (speedRaw <= 0) {
     float gearRatio = 1;
     speed           = ((cad * 2.75 * 2.08 * 60 * gearRatio) / 10);
@@ -243,7 +243,7 @@ void updateIndoorBikeDataChar() {
 }  // ^^Using the New Way of setting Bytes.
 
 void updateCyclingPowerMeasurementChar() {
-  int power = userConfig.getSimulatedWatts();
+  int power = rtConfig.getSimulatedWatts().value;
   int remainder, quotient;
   quotient                   = power / 256;
   remainder                  = power % 256;
@@ -251,7 +251,7 @@ void updateCyclingPowerMeasurementChar() {
   cyclingPowerMeasurement[3] = quotient;
   cyclingPowerMeasurementCharacteristic->setValue(cyclingPowerMeasurement, 9);
 
-  float cadence = userConfig.getSimulatedCad();
+  float cadence = rtConfig.getSimulatedCad();
   if (cadence > 0) {
     float crankRevPeriod = (60 * 1024) / cadence;
     spinBLEClient.cscCumulativeCrankRev++;
@@ -278,7 +278,7 @@ void updateCyclingPowerMeasurementChar() {
 }
 
 void updateHeartRateMeasurementChar() {
-  int hr                  = userConfig.getSimulatedHr();
+  int hr                  = rtConfig.getSimulatedHr();
   heartRateMeasurement[1] = hr;
   heartRateMeasurementCharacteristic->setValue(heartRateMeasurement, 2);
   heartRateMeasurementCharacteristic->notify();
@@ -347,9 +347,9 @@ void processFTMSWrite() {
       case 0x03: {  // inclination level setting - differs from sim mode as no negative numbers
         port = (rxValue[2] << 8) + rxValue[1];
         port *= 10;
-        userConfig.setIncline(port);
-        userConfig.setERGMode(false);
-        logBufLength += snprintf(logBuf + logBufLength, kLogBufCapacity - logBufLength, "-> Incline Mode: %2f", userConfig.getIncline() / 100);
+        rtConfig.setIncline(port);
+        rtConfig.setERGMode(false);
+        logBufLength += snprintf(logBuf + logBufLength, kLogBufCapacity - logBufLength, "-> Incline Mode: %2f", rtConfig.getIncline() / 100);
         returnValue[2]           = 0x01;
         uint8_t inclineStatus[3] = {0x06, (uint8_t)rxValue[1], (uint8_t)rxValue[2]};
         fitnessMachineStatusCharacteristic->setValue(inclineStatus, 3);
@@ -361,9 +361,9 @@ void processFTMSWrite() {
 
       case 0x04: {  // Resistance level setting
         int targetResistance = rxValue[1];
-        userConfig.setShifterPosition(targetResistance);
-        userConfig.setERGMode(false);
-        logBufLength += snprintf(logBuf + logBufLength, kLogBufCapacity - logBufLength, "-> Resistance Mode: %d", userConfig.getShifterPosition());
+        rtConfig.setShifterPosition(targetResistance);
+        rtConfig.setERGMode(false);
+        logBufLength += snprintf(logBuf + logBufLength, kLogBufCapacity - logBufLength, "-> Resistance Mode: %d", rtConfig.getShifterPosition());
         returnValue[2]              = 0x01;
         uint8_t resistanceStatus[2] = {0x07, rxValue[1]};
         fitnessMachineStatusCharacteristic->setValue(resistanceStatus, 3);
@@ -374,14 +374,14 @@ void processFTMSWrite() {
       } break;
 
       case 0x05: {  // Power Level Mode
-        if (spinBLEClient.connectedPM || userConfig.getSimulateWatts()) {
+        if (spinBLEClient.connectedPM || rtConfig.getSimulateWatts()) {
           int targetWatts = bytes_to_u16(rxValue[2], rxValue[1]);
-          userConfig.setERGMode(true);
-          userConfig.setTargetWatts(targetWatts);
+          rtConfig.setERGMode(true);
+          rtConfig.setTargetWatts(targetWatts);
 
           // computeERG(targetWatts);
           logBufLength += snprintf(logBuf + logBufLength, kLogBufCapacity - logBufLength, "-> ERG Mode Target: %d Current: %d Incline: %2f", targetWatts,
-                                   userConfig.getSimulatedWatts(), userConfig.getIncline() / 100);
+                                   rtConfig.getSimulatedWatts(), rtConfig.getIncline() / 100);
           returnValue[2]       = 0x01;
           uint8_t ERGStatus[3] = {0x08, (uint8_t)rxValue[1], 0x01};
           fitnessMachineStatusCharacteristic->setValue(ERGStatus, 3);
@@ -412,9 +412,9 @@ void processFTMSWrite() {
         // int8_t rollingResistance = rxValue[5];
         // int8_t windResistance    = rxValue[6];
         port = bytes_to_u16(buf[1], buf[0]);
-        userConfig.setIncline(port);
-        logBufLength += snprintf(logBuf + logBufLength, kLogBufCapacity - logBufLength, "-> Sim Mode Incline %2f", userConfig.getIncline() / 100);
-        userConfig.setERGMode(false);
+        rtConfig.setIncline(port);
+        logBufLength += snprintf(logBuf + logBufLength, kLogBufCapacity - logBufLength, "-> Sim Mode Incline %2f", rtConfig.getIncline() / 100);
+        rtConfig.setERGMode(false);
         returnValue[2]       = 0x01;
         uint8_t simStatus[7] = {0x12, (uint8_t)rxValue[1], (uint8_t)rxValue[2], (uint8_t)rxValue[3], (uint8_t)rxValue[4], (uint8_t)rxValue[5], (uint8_t)rxValue[6]};
         fitnessMachineStatusCharacteristic->setValue(simStatus, 7);
@@ -466,11 +466,11 @@ int connectedClientCount() {
 }
 
 void calculateInstPwrFromHR() {
-  static int oldHR    = userConfig.getSimulatedHr();
-  static int newHR    = userConfig.getSimulatedHr();
+  static int oldHR    = rtConfig.getSimulatedHr();
+  static int newHR    = rtConfig.getSimulatedHr();
   static double delta = 0;
   oldHR               = newHR;  // Copying HR from Last loop
-  newHR               = userConfig.getSimulatedHr();
+  newHR               = rtConfig.getSimulatedHr();
 
   delta = (newHR - oldHR) / (BLE_CLIENT_DELAY / 1000);
 
@@ -491,8 +491,8 @@ void calculateInstPwrFromHR() {
   }
 
 #ifndef DEBUG_HR_TO_PWR
-  userConfig.setSimulatedWatts(avgP);
-  userConfig.setSimulatedCad(90);
+  rtConfig.setSimulatedWatts(avgP);
+  rtConfig.setSimulatedCad(90);
 #endif  // DEBUG_HR_TO_PWR
 
   SS2K_LOG(BLE_SERVER_LOG_TAG, "Power From HR: %d", avgP);
@@ -552,62 +552,62 @@ void ss2kCustomCharacteristicCallbacks::onWrite(BLECharacteristic *pCharacterist
     case BLE_incline: {  // 0x02
       returnValue[0] = success;
       if (rxValue[0] == read) {
-        int inc        = userConfig.getIncline() * 100;
+        int inc        = rtConfig.getIncline() * 100;
         returnValue[2] = (uint8_t)(inc & 0xff);
         returnValue[3] = (uint8_t)(inc >> 8);
         returnLength += 2;
       }
       if (rxValue[0] == write) {
-        userConfig.setIncline(bytes_to_u16(rxValue[3], rxValue[2]) / 100);
+        rtConfig.setIncline(bytes_to_u16(rxValue[3], rxValue[2]) / 100);
       }
     } break;
 
     case BLE_simulatedWatts:  // 0x03
       returnValue[0] = success;
       if (rxValue[0] == read) {
-        returnValue[2] = (uint8_t)(userConfig.getSimulatedWatts() & 0xff);
-        returnValue[3] = (uint8_t)(userConfig.getSimulatedWatts() >> 8);
+        returnValue[2] = (uint8_t)(rtConfig.getSimulatedWatts().value & 0xff);
+        returnValue[3] = (uint8_t)(rtConfig.getSimulatedWatts().value >> 8);
         returnLength += 2;
       }
       if (rxValue[0] == write) {
-        userConfig.setSimulatedWatts(bytes_to_u16(rxValue[3], rxValue[2]));
+        rtConfig.setSimulatedWatts(bytes_to_u16(rxValue[3], rxValue[2]));
       }
       break;
 
     case BLE_simulatedHr:  // 0x04
       returnValue[0] = success;
       if (rxValue[0] == read) {
-        returnValue[2] = (uint8_t)(userConfig.getSimulatedHr() & 0xff);
-        returnValue[3] = (uint8_t)(userConfig.getSimulatedHr() >> 8);
+        returnValue[2] = (uint8_t)(rtConfig.getSimulatedHr() & 0xff);
+        returnValue[3] = (uint8_t)(rtConfig.getSimulatedHr() >> 8);
         returnLength += 2;
       }
       if (rxValue[0] == write) {
-        userConfig.setSimulatedHr(bytes_to_u16(rxValue[3], rxValue[2]));
+        rtConfig.setSimulatedHr(bytes_to_u16(rxValue[3], rxValue[2]));
       }
       break;
 
     case BLE_simulatedCad:  // 0x05
       returnValue[0] = success;
       if (rxValue[0] == read) {
-        returnValue[2] = (uint8_t)(userConfig.getSimulatedCad() & 0xff);
-        returnValue[3] = (uint8_t)(userConfig.getSimulatedCad() >> 8);
+        returnValue[2] = (uint8_t)(rtConfig.getSimulatedCad() & 0xff);
+        returnValue[3] = (uint8_t)(rtConfig.getSimulatedCad() >> 8);
         returnLength += 2;
       }
       if (rxValue[0] == write) {
-        userConfig.setSimulatedCad(bytes_to_u16(rxValue[3], rxValue[2]));
+        rtConfig.setSimulatedCad(bytes_to_u16(rxValue[3], rxValue[2]));
       }
       break;
 
     case BLE_simulatedSpeed: {  // 0x06
       returnValue[0] = success;
-      int spd        = userConfig.getSimulatedSpeed() * 10;
+      int spd        = rtConfig.getSimulatedSpeed() * 10;
       if (rxValue[0] == read) {
         returnValue[2] = (uint8_t)(spd & 0xff);
         returnValue[3] = (uint8_t)(spd >> 8);
         returnLength += 2;
       }
       if (rxValue[0] == write) {
-        userConfig.setSimulatedSpeed(bytes_to_u16(rxValue[3], rxValue[2]) / 10);
+        rtConfig.setSimulatedSpeed(bytes_to_u16(rxValue[3], rxValue[2]) / 10);
       }
     } break;
 
@@ -681,44 +681,44 @@ void ss2kCustomCharacteristicCallbacks::onWrite(BLECharacteristic *pCharacterist
     case BLE_simulateHr:  // 0x0D
       returnValue[0] = success;
       if (rxValue[0] == read) {
-        returnValue[2] = (uint8_t)(userConfig.getSimulateHr());
+        returnValue[2] = (uint8_t)(rtConfig.getSimulateHr());
         returnLength += 1;
       }
       if (rxValue[0] == write) {
-        userConfig.setSimulateHr(rxValue[2]);
+        rtConfig.setSimulateHr(rxValue[2]);
       }
       break;
 
     case BLE_simulateWatts:  // 0x0E
       returnValue[0] = success;
       if (rxValue[0] == read) {
-        returnValue[2] = (uint8_t)(userConfig.getSimulateWatts());
+        returnValue[2] = (uint8_t)(rtConfig.getSimulateWatts());
         returnLength += 1;
       }
       if (rxValue[0] == write) {
-        userConfig.setSimulateWatts(rxValue[2]);
+        rtConfig.setSimulateWatts(rxValue[2]);
       }
       break;
 
     case BLE_simulateCad:  // 0x0F
       returnValue[0] = success;
       if (rxValue[0] == read) {
-        returnValue[2] = (uint8_t)(userConfig.getSimulateCad());
+        returnValue[2] = (uint8_t)(rtConfig.getSimulateCad());
         returnLength += 1;
       }
       if (rxValue[0] == write) {
-        userConfig.setSimulateCad(rxValue[2]);
+        rtConfig.setSimulateCad(rxValue[2]);
       }
       break;
 
     case BLE_ERGMode:  // 0x10
       returnValue[0] = success;
       if (rxValue[0] == read) {
-        returnValue[2] = (uint8_t)(userConfig.getERGMode());
+        returnValue[2] = (uint8_t)(rtConfig.getERGMode());
         returnLength += 1;
       }
       if (rxValue[0] == write) {
-        userConfig.setERGMode(rxValue[2]);
+        rtConfig.setERGMode(rxValue[2]);
       }
       break;
 
@@ -756,12 +756,12 @@ void ss2kCustomCharacteristicCallbacks::onWrite(BLECharacteristic *pCharacterist
     case BLE_shifterPosition:  // 0x17
       returnValue[0] = success;
       if (rxValue[0] == read) {
-        returnValue[2] = (uint8_t)(userConfig.getShifterPosition() & 0xff);
-        returnValue[3] = (uint8_t)(userConfig.getShifterPosition() >> 8);
+        returnValue[2] = (uint8_t)(rtConfig.getShifterPosition() & 0xff);
+        returnValue[3] = (uint8_t)(rtConfig.getShifterPosition() >> 8);
         returnLength += 2;
       }
       if (rxValue[0] == write) {
-        userConfig.setShifterPosition(bytes_to_u16(rxValue[3], rxValue[2]));
+        rtConfig.setShifterPosition(bytes_to_u16(rxValue[3], rxValue[2]));
       }
       break;
 
@@ -815,8 +815,8 @@ void SpinBLEServer::notifyShift(bool upDown) {
   uint8_t returnValue[4];
   returnValue[0] = 0x80;
   returnValue[1] = BLE_shifterPosition;
-  returnValue[2] = (uint8_t)(userConfig.getShifterPosition() & 0xff);
-  returnValue[3] = (uint8_t)(userConfig.getShifterPosition() >> 8);
+  returnValue[2] = (uint8_t)(rtConfig.getShifterPosition() & 0xff);
+  returnValue[3] = (uint8_t)(rtConfig.getShifterPosition() >> 8);
   smartSpin2kCharacteristic->setValue(returnValue, 4);
   smartSpin2kCharacteristic->notify(true);
 }
diff --git a/src/ERG_Mode.cpp b/src/ERG_Mode.cpp
index ffac6528..275605e7 100644
--- a/src/ERG_Mode.cpp
+++ b/src/ERG_Mode.cpp
@@ -21,12 +21,12 @@ void setupERG() {
 void ergTaskLoop(void* pvParameters) {
   ErgMode ergMode;
   while (true) {
-    vTaskDelay(1000 / portTICK_PERIOD_MS);
+    vTaskDelay(ERG_MODE_DELAY / portTICK_PERIOD_MS);
 
-    int newSetPoint             = userConfig.getTargetWatts();
-    bool isInErgMode            = userConfig.getERGMode();
+    int newSetPoint             = rtConfig.getTargetWatts();
+    bool isInErgMode            = rtConfig.getERGMode();
     bool hasConnectedPowerMeter = spinBLEClient.connectedPM;
-    bool simulationRunning      = userConfig.getSimulateTargetWatts();
+    bool simulationRunning      = rtConfig.getSimulateTargetWatts();
 
     if (isInErgMode && (hasConnectedPowerMeter || simulationRunning)) {
       SS2K_LOG(ERG_MODE_LOG_TAG, "ComputeERG. Setpoint: %d", newSetPoint);
@@ -47,12 +47,12 @@ void ergTaskLoop(void* pvParameters) {
 void ErgMode::computErg(int newSetPoint) {
   static bool userIsPedaling = true;
   static int setPoint        = 0;
-  float incline              = userConfig.getIncline();
+  float incline              = rtConfig.getIncline();
   float newIncline           = incline;
   int amountToChangeIncline  = 0;
-  int wattChange             = userConfig.getSimulatedWatts() - setPoint;
-  int cadance                = userConfig.getSimulatedCad();
-  bool isStepperRunning      = userConfig.getStepperRunning();
+  int wattChange             = rtConfig.getSimulatedWatts().value - setPoint;
+  int cadance                = rtConfig.getSimulatedCad();
+  bool isStepperRunning      = rtConfig.getStepperRunning();
 
   SS2K_LOG(ERG_MODE_LOG_TAG, "Incline = %f, SetPoint = %d, NewSetPoint = %d, WattChange = %d", incline, setPoint, newSetPoint, wattChange);
   if (isStepperRunning) {
@@ -72,7 +72,7 @@ void ErgMode::computErg(int newSetPoint) {
       return;
     }
     userIsPedaling = false;
-    userConfig.setIncline(incline - userConfig.getShiftStep() * 2);
+    rtConfig.setIncline(incline - userConfig.getShiftStep() * 2);
     // Cadence too low, nothing to do here
     return;
   }
@@ -96,6 +96,6 @@ void ErgMode::computErg(int newSetPoint) {
   // Reduce the amount per loop (don't try to oneshot it) and scale the movement the higher the watt target is as higher wattages require less knob movement.
   // amountToChangeIncline = amountToChangeIncline / ((userConfig.getSimulatedWatts() / 100) + .1);  // +.1 to eliminate possible divide by zero.
   newIncline = incline - amountToChangeIncline;
-  userConfig.setIncline(newIncline);
+  rtConfig.setIncline(newIncline);
   SS2K_LOG(ERG_MODE_LOG_TAG, "newincline: %f", newIncline);
 }
diff --git a/src/HTTP_Server_Basic.cpp b/src/HTTP_Server_Basic.cpp
index b451f344..46bfccf3 100644
--- a/src/HTTP_Server_Basic.cpp
+++ b/src/HTTP_Server_Basic.cpp
@@ -171,16 +171,16 @@ void startHttpServer() {
   server.on("/hrslider", []() {
     String value = server.arg("value");
     if (value == "enable") {
-      userConfig.setSimulateHr(true);
+      rtConfig.setSimulateHr(true);
       server.send(200, "text/plain", "OK");
       SS2K_LOG(HTTP_SERVER_LOG_TAG, "HR Simulator turned on");
     } else if (value == "disable") {
-      userConfig.setSimulateHr(false);
+      rtConfig.setSimulateHr(false);
       server.send(200, "text/plain", "OK");
       SS2K_LOG(HTTP_SERVER_LOG_TAG, "HR Simulator turned off");
     } else {
-      userConfig.setSimulatedHr(value.toInt());
-      SS2K_LOG(HTTP_SERVER_LOG_TAG, "HR is now: %d", userConfig.getSimulatedHr());
+      rtConfig.setSimulatedHr(value.toInt());
+      SS2K_LOG(HTTP_SERVER_LOG_TAG, "HR is now: %d", rtConfig.getSimulatedHr());
       server.send(200, "text/plain", "OK");
     }
   });
@@ -188,16 +188,16 @@ void startHttpServer() {
   server.on("/wattsslider", []() {
     String value = server.arg("value");
     if (value == "enable") {
-      userConfig.setSimulateWatts(true);
+      rtConfig.setSimulateWatts(true);
       server.send(200, "text/plain", "OK");
       SS2K_LOG(HTTP_SERVER_LOG_TAG, "Watt Simulator turned on");
     } else if (value == "disable") {
-      userConfig.setSimulateWatts(false);
+      rtConfig.setSimulateWatts(false);
       server.send(200, "text/plain", "OK");
       SS2K_LOG(HTTP_SERVER_LOG_TAG, "Watt Simulator turned off");
     } else {
-      userConfig.setSimulatedWatts(value.toInt());
-      SS2K_LOG(HTTP_SERVER_LOG_TAG, "Watts are now: %d", userConfig.getSimulatedWatts());
+      rtConfig.setSimulatedWatts(value.toInt());
+      SS2K_LOG(HTTP_SERVER_LOG_TAG, "Watts are now: %d", rtConfig.getSimulatedWatts());
       server.send(200, "text/plain", "OK");
     }
   });
@@ -205,16 +205,16 @@ void startHttpServer() {
   server.on("/cadslider", []() {
     String value = server.arg("value");
     if (value == "enable") {
-      userConfig.setSimulateCad(true);
+      rtConfig.setSimulateCad(true);
       server.send(200, "text/plain", "OK");
       SS2K_LOG(HTTP_SERVER_LOG_TAG, "CAD Simulator turned on");
     } else if (value == "disable") {
-      userConfig.setSimulateCad(false);
+      rtConfig.setSimulateCad(false);
       server.send(200, "text/plain", "OK");
       SS2K_LOG(HTTP_SERVER_LOG_TAG, "CAD Simulator turned off");
     } else {
-      userConfig.setSimulatedCad(value.toInt());
-      SS2K_LOG(HTTP_SERVER_LOG_TAG, "CAD is now: %d", userConfig.getSimulatedCad());
+      rtConfig.setSimulatedCad(value.toInt());
+      SS2K_LOG(HTTP_SERVER_LOG_TAG, "CAD is now: %d", rtConfig.getSimulatedCad());
       server.send(200, "text/plain", "OK");
     }
   });
@@ -222,11 +222,11 @@ void startHttpServer() {
   server.on("/ergmode", []() {
     String value = server.arg("value");
     if (value == "enable") {
-      userConfig.setERGMode(true);
+      rtConfig.setERGMode(true);
       server.send(200, "text/plain", "OK");
       SS2K_LOG(HTTP_SERVER_LOG_TAG, "ERG Mode turned on");
     } else {
-      userConfig.setERGMode(false);
+      rtConfig.setERGMode(false);
       server.send(200, "text/plain", "OK");
       SS2K_LOG(HTTP_SERVER_LOG_TAG, "ERG Mode turned off");
     }
@@ -235,16 +235,16 @@ void startHttpServer() {
   server.on("/targetwattsslider", []() {
     String value = server.arg("value");
     if (value == "enable") {
-      userConfig.setSimulateTargetWatts(true);
+      rtConfig.setSimulateTargetWatts(true);
       server.send(200, "text/plain", "OK");
       SS2K_LOG(HTTP_SERVER_LOG_TAG, "Target Watts Simulator turned on");
     } else if (value == "disable") {
-      userConfig.setSimulateTargetWatts(false);
+      rtConfig.setSimulateTargetWatts(false);
       server.send(200, "text/plain", "OK");
       SS2K_LOG(HTTP_SERVER_LOG_TAG, "Target Watts Simulator turned off");
     } else {
-      userConfig.setTargetWatts(value.toInt());
-      SS2K_LOG(HTTP_SERVER_LOG_TAG, "Target Watts are now: %d", userConfig.getTargetWatts());
+      rtConfig.setTargetWatts(value.toInt());
+      SS2K_LOG(HTTP_SERVER_LOG_TAG, "Target Watts are now: %d", rtConfig.getTargetWatts());
       server.send(200, "text/plain", "OK");
     }
   });
diff --git a/src/Main.cpp b/src/Main.cpp
index 0456368f..469f1d16 100644
--- a/src/Main.cpp
+++ b/src/Main.cpp
@@ -43,6 +43,7 @@ TaskHandle_t moveStepperTask;
 
 ///////////// Initialize the Config /////////////
 userParameters userConfig;
+RuntimeParameters rtConfig;
 physicalWorkingCapacity userPWC;
 
 ///////////// BEGIN SETUP /////////////
@@ -127,7 +128,7 @@ void setup() {
   // Check for firmware update. It's important that this stays before BLE &
   // HTTP setup because otherwise they use too much traffic and the device
   // fails to update which really sucks when it corrupts your settings.
-  FirmwareUpdate();
+  // FirmwareUpdate();
 
   startTasks();
   startHttpServer();
@@ -145,16 +146,16 @@ void loop() {
 
   vTaskDelay(200 / portTICK_RATE_MS);
 
-  if (userConfig.getShifterPosition() > lastShifterPosition) {
-    SS2K_LOG(MAIN_LOG_TAG, "Shift UP: %l", userConfig.getShifterPosition());
+  if (rtConfig.getShifterPosition() > lastShifterPosition) {
+    SS2K_LOG(MAIN_LOG_TAG, "Shift UP: %l", rtConfig.getShifterPosition());
     Serial.println(targetPosition);
     spinBLEServer.notifyShift(1);
-  } else if (userConfig.getShifterPosition() < lastShifterPosition) {
-    SS2K_LOG(MAIN_LOG_TAG, "Shift DOWN: %l", userConfig.getShifterPosition());
+  } else if (rtConfig.getShifterPosition() < lastShifterPosition) {
+    SS2K_LOG(MAIN_LOG_TAG, "Shift DOWN: %l", rtConfig.getShifterPosition());
     Serial.println(targetPosition);
     spinBLEServer.notifyShift(0);
   }
-  lastShifterPosition = userConfig.getShifterPosition();
+  lastShifterPosition = rtConfig.getShifterPosition();
 
   if (loopCounter > 4) {
     scanIfShiftersHeld();
@@ -183,7 +184,7 @@ void moveStepper(void *pvParameters) {
 
   while (1) {
     if (!externalControl) {
-      targetPosition = (userConfig.getShifterPosition() * userConfig.getShiftStep()) + (userConfig.getIncline() * userConfig.getInclineMultiplier());
+      targetPosition = (rtConfig.getShifterPosition() * userConfig.getShiftStep()) + (rtConfig.getIncline() * userConfig.getInclineMultiplier());
     }
     if (syncMode) {
       stepper->stopMove();
@@ -195,7 +196,7 @@ void moveStepper(void *pvParameters) {
       stepper->moveTo(targetPosition);
     }
     vTaskDelay(100 / portTICK_PERIOD_MS);
-    userConfig.setStepperRunning(stepper->isRunning());
+    rtConfig.setStepperRunning(stepper->isRunning());
     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();
@@ -219,7 +220,7 @@ bool IRAM_ATTR deBounce() {
 void IRAM_ATTR shiftUp() {  // Handle the shift up interrupt IRAM_ATTR is to keep the interrput code in ram always
   if (deBounce()) {
     if (!digitalRead(SHIFT_UP_PIN)) {  // double checking to make sure the interrupt wasn't triggered by emf
-      userConfig.setShifterPosition(userConfig.getShifterPosition() + 1);
+      rtConfig.setShifterPosition(rtConfig.getShifterPosition() + 1);
     } else {
       lastDebounceTime = 0;
     }  // Probably Triggered by EMF, reset the debounce
@@ -229,7 +230,7 @@ void IRAM_ATTR shiftUp() {  // Handle the shift up interrupt IRAM_ATTR is to kee
 void IRAM_ATTR shiftDown() {  // Handle the shift down interrupt
   if (deBounce()) {
     if (!digitalRead(SHIFT_DOWN_PIN)) {  // double checking to make sure the interrupt wasn't triggered by emf
-      userConfig.setShifterPosition(userConfig.getShifterPosition() - 1);
+      rtConfig.setShifterPosition(rtConfig.getShifterPosition() - 1);
     } else {
       lastDebounceTime = 0;
     }  // Probably Triggered by EMF, reset the debounce
diff --git a/src/SmartSpin_parameters.cpp b/src/SmartSpin_parameters.cpp
index 721171b5..91e0d982 100644
--- a/src/SmartSpin_parameters.cpp
+++ b/src/SmartSpin_parameters.cpp
@@ -16,31 +16,17 @@
 void userParameters::setDefaults() {  // Move these to set the values as #define
                                       // in main.h
   firmwareUpdateURL     = FW_UPDATEURL;
-  incline               = 0.0;
-  targetWatts           = 0;
-  simulatedWatts        = 0;
-  simulatedHr           = 0;
-  simulatedCad          = 0;
   deviceName            = DEVICE_NAME;
   shiftStep             = 600;
-  stepperPower          = STEPPER_POWER;
   stealthchop           = STEALTHCHOP;
   inclineMultiplier     = 3.0;
   powerCorrectionFactor = 1.0;
-  simulateHr            = false;
-  simulateWatts         = false;
-  simulateTargetWatts   = false;
-  simulateCad           = false;
-  ERGMode               = false;
-  stepperRunning        = false;
   ERGSensitivity        = ERG_SENSITIVITY;
   autoUpdate            = AUTO_FIRMWARE_UPDATE;
   ssid                  = DEVICE_NAME;
   password              = DEFAULT_PASSWORD;
-  foundDevices          = "";
   connectedPowerMeter   = CONNECTED_POWER_METER;
   connectedHeartMonitor = CONNECTED_HEART_MONITOR;
-  shifterPosition       = 0;
 }
 
 //---------------------------------------------------------------------------------
@@ -52,33 +38,21 @@ String userParameters::returnJSON(bool includeDebugLog) {
   StaticJsonDocument<USERCONFIG_JSON_SIZE> doc;
   // Set the values in the document
 
-  doc["firmwareUpdateURL"]     = firmwareUpdateURL;
-  doc["firmwareVersion"]       = FIRMWARE_VERSION;
-  doc["incline"]               = incline;
-  doc["targetWatts"]           = targetWatts;
-  doc["simulatedWatts"]        = simulatedWatts;
-  doc["simulatedHr"]           = simulatedHr;
-  doc["simulatedCad"]          = simulatedCad;
-  doc["deviceName"]            = deviceName;
-  doc["shiftStep"]             = shiftStep;
-  doc["stepperPower"]          = stepperPower;
+  doc["firmwareUpdateURL"] = firmwareUpdateURL;
+  doc["firmwareVersion"]   = FIRMWARE_VERSION;
+  doc["deviceName"]        = deviceName;
+  doc["shiftStep"]         = shiftStep;
+  doc["stepperPower"]      = stepperPower;
+
   doc["stealthchop"]           = stealthchop;
   doc["inclineMultiplier"]     = inclineMultiplier;
   doc["powerCorrectionFactor"] = powerCorrectionFactor;
-  doc["simulateHr"]            = simulateHr;
-  doc["simulateWatts"]         = simulateWatts;
-  doc["simulateTargetWatts"]   = simulateTargetWatts;
-  doc["simulateCad"]           = simulateCad;
-  doc["ERGMode"]               = ERGMode;
-  doc["stepperRunning"]        = stepperRunning;
   doc["ERGSensitivity"]        = ERGSensitivity;
   doc["autoUpdate"]            = autoUpdate;
   doc["ssid"]                  = ssid;
   doc["password"]              = password;
-  doc["foundDevices"]          = foundDevices;
   doc["connectedPowerMeter"]   = connectedPowerMeter;
   doc["connectedHeartMonitor"] = connectedHeartMonitor;
-  doc["shifterPosition"]       = shifterPosition;
   if (includeDebugLog) {
     doc["debug"] = DebugInfo::get_and_clear_logs();
   }
@@ -109,29 +83,19 @@ void userParameters::saveToSPIFFS() {
   // commented items are not needed in save file
 
   doc["firmwareUpdateURL"] = firmwareUpdateURL;
-  // doc["incline"]           = incline;
-  // doc["simulatedWatts"]       = simulatedWatts;
-  // doc["simulatedHr"]          = simulatedHr;
-  // doc["simulatedCad"]         = simulatedCad;
   doc["deviceName"]            = deviceName;
   doc["shiftStep"]             = shiftStep;
   doc["stepperPower"]          = stepperPower;
   doc["stealthchop"]           = stealthchop;
   doc["inclineMultiplier"]     = inclineMultiplier;
   doc["powerCorrectionFactor"] = powerCorrectionFactor;
-  // doc["simulateHr"]            = simulateHr;
-  // doc["simulateWatts"]         = simulateWatts;
-  // doc["simulateCad"]           = simulateCad;
-  // doc["ERGMode"]               = ERGMode;
   doc["ERGSensitivity"] = ERGSensitivity;
   doc["autoUpdate"]     = autoUpdate;
   doc["ssid"]           = ssid;
   doc["password"]       = password;
-  // doc["foundDevices"]         = foundDevices; //I don't see a need
   // currently in keeping this boot to boot
   doc["connectedPowerMeter"]   = connectedPowerMeter;
   doc["connectedHeartMonitor"] = connectedHeartMonitor;
-  // doc["shifterPosition"]       = shifterPosition;
 
   // Serialize JSON to file
   if (serializeJson(doc, file) == 0) {
@@ -183,20 +147,14 @@ void userParameters::loadFromSPIFFS() {
       setPowerCorrectionFactor(1);
     }
   }
-  setSimulateHr(false);  // Set these false because previous config versions may return true and these values are no longer saved.
-  setSimulateWatts(false);
-  setSimulateCad(false);
-  // setERGMode(doc["ERGMode"]);
   if (doc["ERGSensitivity"]) {
     setERGSensitivity(doc["ERGSensitivity"]);
   }
   setAutoUpdate(doc["autoUpdate"]);
   setSsid(doc["ssid"]);
   setPassword(doc["password"]);
-  // setfoundDevices       (doc["foundDevices"]);
   setConnectedPowerMeter(doc["connectedPowerMeter"]);
   setConnectedHeartMonitor(doc["connectedHeartMonitor"]);
-  // setShifterPosition[doc["shifterPosition"]);
 
   SS2K_LOG(CONFIG_LOG_TAG, "Config File Loaded: %s", configFILENAME);
   file.close();

From b1a85376e580e5c73b721b9e3bef813edbbad56c Mon Sep 17 00:00:00 2001
From: Markus Schneider <m.schneider@online.de>
Date: Sat, 4 Dec 2021 20:30:47 +0100
Subject: [PATCH 05/42] ERG Mode class variables and fix of Measurment access

---
 include/ERG_Mode.h             |  6 ++++++
 include/SmartSpin_parameters.h |  2 +-
 src/BLE_Server.cpp             |  2 +-
 src/ERG_Mode.cpp               | 21 ++++++++++++---------
 src/HTTP_Server_Basic.cpp      |  2 +-
 src/SmartSpin_parameters.cpp   |  4 ----
 6 files changed, 21 insertions(+), 16 deletions(-)

diff --git a/include/ERG_Mode.h b/include/ERG_Mode.h
index 0b0c33d1..f78b54f5 100644
--- a/include/ERG_Mode.h
+++ b/include/ERG_Mode.h
@@ -1,6 +1,7 @@
 #pragma once
 
 #include <Arduino.h>
+#include "SmartSpin_parameters.h"
 
 #define ERG_MODE_LOG_TAG "ERG_Mode"
 #define ERG_MODE_DELAY   100
@@ -12,4 +13,9 @@ void ergTaskLoop(void *pvParameters);
 class ErgMode {
  public:
   void computErg(int newSetpoint);
+
+ private:
+  bool userIsPedaling          = true;
+  int setPoint                 = 0;
+  Measurement watts = Measurement(0);
 };
\ No newline at end of file
diff --git a/include/SmartSpin_parameters.h b/include/SmartSpin_parameters.h
index 4784e58d..e751ec83 100644
--- a/include/SmartSpin_parameters.h
+++ b/include/SmartSpin_parameters.h
@@ -57,7 +57,7 @@ class RuntimeParameters {
 
   void setIncline(float inc) { incline = inc; }
   void setTargetWatts(int w) { targetWatts = w; }
-  void setSimulatedWatts(Measurement w) { simulatedWatts = w; }
+  void setSimulatedWatts(int w) { simulatedWatts = Measurement(w); }
   void setSimulatedHr(int hr) { simulatedHr = hr; }
   void setSimulatedCad(float cad) { simulatedCad = cad; }
   void setSimulatedSpeed(float spd) { simulatedSpeed = spd; }
diff --git a/src/BLE_Server.cpp b/src/BLE_Server.cpp
index a478063b..19f6adbc 100644
--- a/src/BLE_Server.cpp
+++ b/src/BLE_Server.cpp
@@ -381,7 +381,7 @@ void processFTMSWrite() {
 
           // computeERG(targetWatts);
           logBufLength += snprintf(logBuf + logBufLength, kLogBufCapacity - logBufLength, "-> ERG Mode Target: %d Current: %d Incline: %2f", targetWatts,
-                                   rtConfig.getSimulatedWatts(), rtConfig.getIncline() / 100);
+                                   rtConfig.getSimulatedWatts().value, rtConfig.getIncline() / 100);
           returnValue[2]       = 0x01;
           uint8_t ERGStatus[3] = {0x08, (uint8_t)rxValue[1], 0x01};
           fitnessMachineStatusCharacteristic->setValue(ERGStatus, 3);
diff --git a/src/ERG_Mode.cpp b/src/ERG_Mode.cpp
index 275605e7..e7311bd9 100644
--- a/src/ERG_Mode.cpp
+++ b/src/ERG_Mode.cpp
@@ -45,14 +45,17 @@ void ergTaskLoop(void* pvParameters) {
 
 // as a note, Trainer Road sends 50w target whenever the app is connected.
 void ErgMode::computErg(int newSetPoint) {
-  static bool userIsPedaling = true;
-  static int setPoint        = 0;
-  float incline              = rtConfig.getIncline();
-  float newIncline           = incline;
-  int amountToChangeIncline  = 0;
-  int wattChange             = rtConfig.getSimulatedWatts().value - setPoint;
-  int cadance                = rtConfig.getSimulatedCad();
-  bool isStepperRunning      = rtConfig.getStepperRunning();
+  Measurement newWatts      = rtConfig.getSimulatedWatts();
+  float incline             = rtConfig.getIncline();
+  int amountToChangeIncline = 0;
+  int wattChange            = newWatts.value - setPoint;
+  int cadance               = rtConfig.getSimulatedCad();
+  bool isStepperRunning     = rtConfig.getStepperRunning();
+
+  if (watts.timestamp == newWatts.timestamp) {
+    return;  // no new power measurement.
+  }
+  watts = newWatts;
 
   SS2K_LOG(ERG_MODE_LOG_TAG, "Incline = %f, SetPoint = %d, NewSetPoint = %d, WattChange = %d", incline, setPoint, newSetPoint, wattChange);
   if (isStepperRunning) {
@@ -95,7 +98,7 @@ void ErgMode::computErg(int newSetPoint) {
 
   // Reduce the amount per loop (don't try to oneshot it) and scale the movement the higher the watt target is as higher wattages require less knob movement.
   // amountToChangeIncline = amountToChangeIncline / ((userConfig.getSimulatedWatts() / 100) + .1);  // +.1 to eliminate possible divide by zero.
-  newIncline = incline - amountToChangeIncline;
+  float newIncline = incline - amountToChangeIncline;
   rtConfig.setIncline(newIncline);
   SS2K_LOG(ERG_MODE_LOG_TAG, "newincline: %f", newIncline);
 }
diff --git a/src/HTTP_Server_Basic.cpp b/src/HTTP_Server_Basic.cpp
index 46bfccf3..4d4343b3 100644
--- a/src/HTTP_Server_Basic.cpp
+++ b/src/HTTP_Server_Basic.cpp
@@ -197,7 +197,7 @@ void startHttpServer() {
       SS2K_LOG(HTTP_SERVER_LOG_TAG, "Watt Simulator turned off");
     } else {
       rtConfig.setSimulatedWatts(value.toInt());
-      SS2K_LOG(HTTP_SERVER_LOG_TAG, "Watts are now: %d", rtConfig.getSimulatedWatts());
+      SS2K_LOG(HTTP_SERVER_LOG_TAG, "Watts are now: %d", rtConfig.getSimulatedWatts().value);
       server.send(200, "text/plain", "OK");
     }
   });
diff --git a/src/SmartSpin_parameters.cpp b/src/SmartSpin_parameters.cpp
index 91e0d982..12664584 100644
--- a/src/SmartSpin_parameters.cpp
+++ b/src/SmartSpin_parameters.cpp
@@ -132,10 +132,6 @@ void userParameters::loadFromSPIFFS() {
 
   // Copy values from the JsonDocument to the Config
   setFirmwareUpdateURL(doc["firmwareUpdateURL"]);
-  // setIncline(doc["incline"]);
-  // setSimulatedWatts     (doc["simulatedWatts"]);
-  // setSimulatedHr        (doc["simulatedHr"]);
-  // setSimulatedCad       (doc["simulatedCad"]);
   setDeviceName(doc["deviceName"]);
   setShiftStep(doc["shiftStep"]);
   setStepperPower(doc["stepperPower"]);

From 50ffd60e7a1e040b5a0861711c29692ffe62515d Mon Sep 17 00:00:00 2001
From: Markus Schneider <m.schneider@online.de>
Date: Sat, 4 Dec 2021 20:32:35 +0100
Subject: [PATCH 06/42] Enabled FW update

---
 src/Main.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/Main.cpp b/src/Main.cpp
index 469f1d16..2e056254 100644
--- a/src/Main.cpp
+++ b/src/Main.cpp
@@ -128,7 +128,7 @@ void setup() {
   // Check for firmware update. It's important that this stays before BLE &
   // HTTP setup because otherwise they use too much traffic and the device
   // fails to update which really sucks when it corrupts your settings.
-  // FirmwareUpdate();
+  FirmwareUpdate();
 
   startTasks();
   startHttpServer();

From 65312abc6e075556d9e5b6c7a92ef4d8ab010bc7 Mon Sep 17 00:00:00 2001
From: Markus Schneider <m.schneider@online.de>
Date: Sun, 5 Dec 2021 17:40:19 +0100
Subject: [PATCH 07/42] update rtSetting: added currentIncline. Fixed
 btsimulator: request rtSettings Modified ERG Modu to use currentIncline

---
 data/btsimulator.html          | 14 +++++-------
 include/ERG_Mode.h             |  6 ++---
 include/SmartSpin_parameters.h | 15 +++++++-----
 include/settings.h             |  2 ++
 src/BLE_Server.cpp             | 14 ++++++------
 src/ERG_Mode.cpp               | 31 ++++++++++++-------------
 src/HTTP_Server_Basic.cpp      |  6 +++++
 src/Main.cpp                   | 42 +++++++++++++++++++---------------
 src/SmartSpin_parameters.cpp   | 37 ++++++++++++++++++++++++++----
 9 files changed, 104 insertions(+), 63 deletions(-)

diff --git a/data/btsimulator.html b/data/btsimulator.html
index 34327707..1eee764f 100644
--- a/data/btsimulator.html
+++ b/data/btsimulator.html
@@ -64,6 +64,11 @@ <h2>ERG Target Watts</h2>
     </p>
 </body>
 <script>
+  //Update values on specified interval
+  setInterval(function () {
+    requestConfigValues();
+
+  }, 2000);
 
   function toggleHRCheckbox(element, updateServer) {
     if (element.checked) {
@@ -200,13 +205,6 @@ <h2>ERG Target Watts</h2>
     }
   }
 
-  //Update values on specified interval
-  setInterval(function () {
-
-    requestConfigValues();
-
-  }, 2000);
-
   function requestConfigValues() {
     var xhttp = new XMLHttpRequest();
     xhttp.onreadystatechange = function () {
@@ -246,7 +244,7 @@ <h2>ERG Target Watts</h2>
         }, 1000);
       }
     };
-    xhttp.open("GET", "/configJSON", true);
+    xhttp.open("GET", "/runtimeConfigJSON", true);
     xhttp.send();
   }
 
diff --git a/include/ERG_Mode.h b/include/ERG_Mode.h
index f78b54f5..04a6055a 100644
--- a/include/ERG_Mode.h
+++ b/include/ERG_Mode.h
@@ -15,7 +15,7 @@ class ErgMode {
   void computErg(int newSetpoint);
 
  private:
-  bool userIsPedaling          = true;
-  int setPoint                 = 0;
-  Measurement watts = Measurement(0);
+  bool userIsPedaling = true;
+  // int setPoint        = 0;
+  Measurement watts   = Measurement(0);
 };
\ No newline at end of file
diff --git a/include/SmartSpin_parameters.h b/include/SmartSpin_parameters.h
index e751ec83..3a436ad5 100644
--- a/include/SmartSpin_parameters.h
+++ b/include/SmartSpin_parameters.h
@@ -24,7 +24,8 @@ class Measurement {
 
 class RuntimeParameters {
  private:
-  float incline              = 0.0;
+  float targetIncline        = 0.0;
+  float currentIncline       = 0.0;
   int targetWatts            = 0;
   Measurement simulatedWatts = Measurement(0);
   int simulatedHr            = 0;
@@ -37,10 +38,11 @@ class RuntimeParameters {
   bool ERGMode               = false;
   int shifterPosition        = 0;
   String foundDevices        = " ";
-  bool stepperRunning        = false;
 
  public:
-  float getIncline() { return incline; }
+  float getTargetIncline() { return targetIncline; }
+  float getCurrentIncline() { return currentIncline; }
+
   int getTargetWatts() { return targetWatts; }
   Measurement getSimulatedWatts() { return simulatedWatts; }
   int getSimulatedHr() { return simulatedHr; }
@@ -52,10 +54,10 @@ class RuntimeParameters {
   bool getSimulateCad() { return simulateCad; }
   bool getERGMode() { return ERGMode; }
   int getShifterPosition() { return shifterPosition; }
-  bool getStepperRunning() { return stepperRunning; }
   const char* getFoundDevices() { return foundDevices.c_str(); }
 
-  void setIncline(float inc) { incline = inc; }
+  void setTargetIncline(float inc) { targetIncline = inc; }
+  void setCurrentIncline(float inc) { currentIncline = inc; }
   void setTargetWatts(int w) { targetWatts = w; }
   void setSimulatedWatts(int w) { simulatedWatts = Measurement(w); }
   void setSimulatedHr(int hr) { simulatedHr = hr; }
@@ -67,8 +69,9 @@ class RuntimeParameters {
   void setSimulateCad(bool scd) { simulateCad = scd; }
   void setERGMode(bool erg) { ERGMode = erg; }
   void setShifterPosition(int sp) { shifterPosition = sp; }
-  void setStepperRunning(bool running) { stepperRunning = running; }
   void setFoundDevices(String fdev) { foundDevices = fdev; }
+
+  String returnJSON();
 };
 
 class userParameters {
diff --git a/include/settings.h b/include/settings.h
index 978b9c10..eec5e1a6 100644
--- a/include/settings.h
+++ b/include/settings.h
@@ -154,6 +154,8 @@
 // Max size of userconfig
 #define USERCONFIG_JSON_SIZE 768 + DEBUG_LOG_BUFFER_SIZE
 
+#define RUNTIMECONFIG_JSON_SIZE 512
+
 // Uncomment to enable sending Telegram debug messages back to the chat
 // specified in telegram_token.h
 // #define USE_TELEGRAM
diff --git a/src/BLE_Server.cpp b/src/BLE_Server.cpp
index 19f6adbc..974505d0 100644
--- a/src/BLE_Server.cpp
+++ b/src/BLE_Server.cpp
@@ -347,9 +347,9 @@ void processFTMSWrite() {
       case 0x03: {  // inclination level setting - differs from sim mode as no negative numbers
         port = (rxValue[2] << 8) + rxValue[1];
         port *= 10;
-        rtConfig.setIncline(port);
+        rtConfig.setTargetIncline(port);
         rtConfig.setERGMode(false);
-        logBufLength += snprintf(logBuf + logBufLength, kLogBufCapacity - logBufLength, "-> Incline Mode: %2f", rtConfig.getIncline() / 100);
+        logBufLength += snprintf(logBuf + logBufLength, kLogBufCapacity - logBufLength, "-> Incline Mode: %2f", rtConfig.getTargetIncline() / 100);
         returnValue[2]           = 0x01;
         uint8_t inclineStatus[3] = {0x06, (uint8_t)rxValue[1], (uint8_t)rxValue[2]};
         fitnessMachineStatusCharacteristic->setValue(inclineStatus, 3);
@@ -381,7 +381,7 @@ void processFTMSWrite() {
 
           // computeERG(targetWatts);
           logBufLength += snprintf(logBuf + logBufLength, kLogBufCapacity - logBufLength, "-> ERG Mode Target: %d Current: %d Incline: %2f", targetWatts,
-                                   rtConfig.getSimulatedWatts().value, rtConfig.getIncline() / 100);
+                                   rtConfig.getSimulatedWatts().value, rtConfig.getTargetIncline() / 100);
           returnValue[2]       = 0x01;
           uint8_t ERGStatus[3] = {0x08, (uint8_t)rxValue[1], 0x01};
           fitnessMachineStatusCharacteristic->setValue(ERGStatus, 3);
@@ -412,8 +412,8 @@ void processFTMSWrite() {
         // int8_t rollingResistance = rxValue[5];
         // int8_t windResistance    = rxValue[6];
         port = bytes_to_u16(buf[1], buf[0]);
-        rtConfig.setIncline(port);
-        logBufLength += snprintf(logBuf + logBufLength, kLogBufCapacity - logBufLength, "-> Sim Mode Incline %2f", rtConfig.getIncline() / 100);
+        rtConfig.setTargetIncline(port);
+        logBufLength += snprintf(logBuf + logBufLength, kLogBufCapacity - logBufLength, "-> Sim Mode Incline %2f", rtConfig.getTargetIncline() / 100);
         rtConfig.setERGMode(false);
         returnValue[2]       = 0x01;
         uint8_t simStatus[7] = {0x12, (uint8_t)rxValue[1], (uint8_t)rxValue[2], (uint8_t)rxValue[3], (uint8_t)rxValue[4], (uint8_t)rxValue[5], (uint8_t)rxValue[6]};
@@ -552,13 +552,13 @@ void ss2kCustomCharacteristicCallbacks::onWrite(BLECharacteristic *pCharacterist
     case BLE_incline: {  // 0x02
       returnValue[0] = success;
       if (rxValue[0] == read) {
-        int inc        = rtConfig.getIncline() * 100;
+        int inc        = rtConfig.getTargetIncline() * 100;
         returnValue[2] = (uint8_t)(inc & 0xff);
         returnValue[3] = (uint8_t)(inc >> 8);
         returnLength += 2;
       }
       if (rxValue[0] == write) {
-        rtConfig.setIncline(bytes_to_u16(rxValue[3], rxValue[2]) / 100);
+        rtConfig.setTargetIncline(bytes_to_u16(rxValue[3], rxValue[2]) / 100);
       }
     } break;
 
diff --git a/src/ERG_Mode.cpp b/src/ERG_Mode.cpp
index e7311bd9..e1ac5e4a 100644
--- a/src/ERG_Mode.cpp
+++ b/src/ERG_Mode.cpp
@@ -46,28 +46,24 @@ void ergTaskLoop(void* pvParameters) {
 // as a note, Trainer Road sends 50w target whenever the app is connected.
 void ErgMode::computErg(int newSetPoint) {
   Measurement newWatts      = rtConfig.getSimulatedWatts();
-  float incline             = rtConfig.getIncline();
+  float incline             = rtConfig.getCurrentIncline();
   int amountToChangeIncline = 0;
-  int wattChange            = newWatts.value - setPoint;
+  int wattChange            = newWatts.value;  // - setPoint;
   int cadance               = rtConfig.getSimulatedCad();
-  bool isStepperRunning     = rtConfig.getStepperRunning();
 
   if (watts.timestamp == newWatts.timestamp) {
     return;  // no new power measurement.
   }
   watts = newWatts;
 
-  SS2K_LOG(ERG_MODE_LOG_TAG, "Incline = %f, SetPoint = %d, NewSetPoint = %d, WattChange = %d", incline, setPoint, newSetPoint, wattChange);
-  if (isStepperRunning) {
-    SS2K_LOG(ERG_MODE_LOG_TAG, "Stepper is running. Skip ERG computation");
-  }
+  SS2K_LOG(ERG_MODE_LOG_TAG, "Incline = %f, NewSetPoint = %d, CurrentWatts = %d", incline, newSetPoint, wattChange);
 
-  if (newSetPoint > 0) {  // only update the value if new value is sent
-    setPoint = newSetPoint;
-  }
-  if (setPoint < 50) {  // minumum setPoint
-    setPoint = 50;
-  }
+  // if (newSetPoint > 0) {  // only update the value if new value is sent
+  //   setPoint = newSetPoint;
+  // }
+  // if (setPoint < 50) {  // minumum setPoint
+  //   setPoint = 50;
+  // }
 
   if (cadance <= 20) {
     if (!userIsPedaling) {  // Test so motor stop command only happens once.
@@ -75,16 +71,18 @@ void ErgMode::computErg(int newSetPoint) {
       return;
     }
     userIsPedaling = false;
-    rtConfig.setIncline(incline - userConfig.getShiftStep() * 2);
+    rtConfig.setTargetIncline(incline - userConfig.getShiftStep() * 2);
     // Cadence too low, nothing to do here
     return;
   }
   userIsPedaling = true;
 
-  amountToChangeIncline = wattChange * userConfig.getERGSensitivity() * (75.0 / (double)cadance);
+  amountToChangeIncline = wattChange * userConfig.getERGSensitivity();  // * (75.0 / (double)cadance);
+  SS2K_LOG(ERG_MODE_LOG_TAG, "Set amountToChangeIncline to: %f", amountToChangeIncline);
   if (abs(wattChange) < WATTS_PER_SHIFT) {
     // As the desired value gets closer, make smaller changes for a smoother experience
     amountToChangeIncline *= SUB_SHIFT_SCALE;
+    SS2K_LOG(ERG_MODE_LOG_TAG, "Watt change < %d watts. Correct amountToChangeIncline to: %f", WATTS_PER_SHIFT, amountToChangeIncline);
   }
   // limit to 10 shifts at a time
   if (abs(amountToChangeIncline) > userConfig.getShiftStep() * 2) {
@@ -94,11 +92,12 @@ void ErgMode::computErg(int newSetPoint) {
     if (amountToChangeIncline < -5) {
       amountToChangeIncline = -(userConfig.getShiftStep() * 2);
     }
+    SS2K_LOG(ERG_MODE_LOG_TAG, "Watt change > %d watts. Correct amountToChangeIncline to: %f", userConfig.getShiftStep() * 2, amountToChangeIncline);
   }
 
   // Reduce the amount per loop (don't try to oneshot it) and scale the movement the higher the watt target is as higher wattages require less knob movement.
   // amountToChangeIncline = amountToChangeIncline / ((userConfig.getSimulatedWatts() / 100) + .1);  // +.1 to eliminate possible divide by zero.
   float newIncline = incline - amountToChangeIncline;
-  rtConfig.setIncline(newIncline);
+  rtConfig.setTargetIncline(newIncline);
   SS2K_LOG(ERG_MODE_LOG_TAG, "newincline: %f", newIncline);
 }
diff --git a/src/HTTP_Server_Basic.cpp b/src/HTTP_Server_Basic.cpp
index 4d4343b3..153d7db6 100644
--- a/src/HTTP_Server_Basic.cpp
+++ b/src/HTTP_Server_Basic.cpp
@@ -255,6 +255,12 @@ void startHttpServer() {
     server.send(200, "text/plain", tString);
   });
 
+  server.on("/runtimeConfigJSON", []() {
+    String tString;
+    tString = rtConfig.returnJSON();
+    server.send(200, "text/plain", tString);
+  });
+
   server.on("/PWCJSON", []() {
     String tString;
     tString = userPWC.returnJSON();
diff --git a/src/Main.cpp b/src/Main.cpp
index 2e056254..a82a7740 100644
--- a/src/Main.cpp
+++ b/src/Main.cpp
@@ -128,7 +128,7 @@ void setup() {
   // Check for firmware update. It's important that this stays before BLE &
   // HTTP setup because otherwise they use too much traffic and the device
   // fails to update which really sucks when it corrupts your settings.
-  FirmwareUpdate();
+  // FirmwareUpdate();
 
   startTasks();
   startHttpServer();
@@ -183,25 +183,31 @@ void moveStepper(void *pvParameters) {
   stepper->setDelayToDisable(1000);
 
   while (1) {
-    if (!externalControl) {
-      targetPosition = (rtConfig.getShifterPosition() * userConfig.getShiftStep()) + (rtConfig.getIncline() * userConfig.getInclineMultiplier());
-    }
-    if (syncMode) {
-      stepper->stopMove();
-      vTaskDelay(100 / portTICK_PERIOD_MS);
-      stepper->setCurrentPosition(targetPosition);
-      vTaskDelay(100 / portTICK_PERIOD_MS);
-    }
     if (stepper) {
+      int shifterIncline = rtConfig.getShifterPosition() * userConfig.getShiftStep();
+      int ergIncline     = 0;
+      if (!externalControl) {
+        ergIncline = rtConfig.getTargetIncline() * userConfig.getInclineMultiplier();
+      }
+      targetPosition = shifterIncline + ergIncline;
+
+      if (syncMode) {
+        stepper->stopMove();
+        vTaskDelay(100 / portTICK_PERIOD_MS);
+        stepper->setCurrentPosition(targetPosition);
+        vTaskDelay(100 / portTICK_PERIOD_MS);
+      }
       stepper->moveTo(targetPosition);
-    }
-    vTaskDelay(100 / portTICK_PERIOD_MS);
-    rtConfig.setStepperRunning(stepper->isRunning());
-    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();
-    } else {
-      stepper->setAutoEnable(true);  // disable output FETs between moves so stepper can cool. Can still shift.
+      vTaskDelay(100 / portTICK_PERIOD_MS);
+
+      rtConfig.setCurrentIncline(((float)stepper->getCurrentPosition() / (float)userConfig.getInclineMultiplier()) + shifterIncline);
+
+      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();
+      } else {
+        stepper->setAutoEnable(true);  // disable output FETs between moves so stepper can cool. Can still shift.
+      }
     }
   }
 }
diff --git a/src/SmartSpin_parameters.cpp b/src/SmartSpin_parameters.cpp
index 12664584..99d2615b 100644
--- a/src/SmartSpin_parameters.cpp
+++ b/src/SmartSpin_parameters.cpp
@@ -12,6 +12,33 @@
 #include <ArduinoJson.h>
 #include <SPIFFS.h>
 
+String RuntimeParameters::returnJSON() {
+  // Allocate a temporary JsonDocument
+  // Don't forget to change the capacity to match your requirements.
+  // Use arduinojson.org/assistant to compute the capacity.
+  StaticJsonDocument<RUNTIMECONFIG_JSON_SIZE> doc;
+  // Set the values in the document
+
+  doc["targetIncline"]       = targetIncline;
+  doc["currentIncline"]      = currentIncline;
+  doc["targetWatts"]         = targetWatts;
+  doc["simulatedWatts"]      = simulatedWatts.value;
+  doc["simulatedHr"]         = simulatedHr;
+  doc["simulatedCad"]        = simulatedCad;
+  doc["simulatedSpeed"]      = simulatedSpeed;
+  doc["simulateHr"]          = simulateHr;
+  doc["simulateWatts"]       = simulateWatts;
+  doc["simulateTargetWatts"] = simulateTargetWatts;
+  doc["simulateCad"]         = simulateCad;
+  doc["ERGMode"]             = ERGMode;
+  doc["shifterPosition"]     = shifterPosition;
+  doc["foundDevices"]        = foundDevices;
+
+  String output;
+  serializeJson(doc, output);
+  return output;
+}
+
 // Default Values
 void userParameters::setDefaults() {  // Move these to set the values as #define
                                       // in main.h
@@ -82,17 +109,17 @@ void userParameters::saveToSPIFFS() {
   // Set the values in the document
   // commented items are not needed in save file
 
-  doc["firmwareUpdateURL"] = firmwareUpdateURL;
+  doc["firmwareUpdateURL"]     = firmwareUpdateURL;
   doc["deviceName"]            = deviceName;
   doc["shiftStep"]             = shiftStep;
   doc["stepperPower"]          = stepperPower;
   doc["stealthchop"]           = stealthchop;
   doc["inclineMultiplier"]     = inclineMultiplier;
   doc["powerCorrectionFactor"] = powerCorrectionFactor;
-  doc["ERGSensitivity"] = ERGSensitivity;
-  doc["autoUpdate"]     = autoUpdate;
-  doc["ssid"]           = ssid;
-  doc["password"]       = password;
+  doc["ERGSensitivity"]        = ERGSensitivity;
+  doc["autoUpdate"]            = autoUpdate;
+  doc["ssid"]                  = ssid;
+  doc["password"]              = password;
   // currently in keeping this boot to boot
   doc["connectedPowerMeter"]   = connectedPowerMeter;
   doc["connectedHeartMonitor"] = connectedHeartMonitor;

From 7f5d4a11603e71ed36a937c501ddb95d7f3dd9e4 Mon Sep 17 00:00:00 2001
From: Markus Schneider <m.schneider@online.de>
Date: Sun, 5 Dec 2021 20:01:05 +0100
Subject: [PATCH 08/42] Added ERG Mode to stepper driver.

---
 include/ERG_Mode.h |  6 ++---
 src/ERG_Mode.cpp   | 56 ++++++++++++++++++++++------------------------
 src/Main.cpp       | 22 +++++++++++++-----
 3 files changed, 46 insertions(+), 38 deletions(-)

diff --git a/include/ERG_Mode.h b/include/ERG_Mode.h
index 04a6055a..2841f67a 100644
--- a/include/ERG_Mode.h
+++ b/include/ERG_Mode.h
@@ -15,7 +15,7 @@ class ErgMode {
   void computErg(int newSetpoint);
 
  private:
-  bool userIsPedaling = true;
-  // int setPoint        = 0;
-  Measurement watts   = Measurement(0);
+  bool engineStopped = false;
+  int setPoint       = 0;
+  Measurement watts  = Measurement(0);
 };
\ No newline at end of file
diff --git a/src/ERG_Mode.cpp b/src/ERG_Mode.cpp
index e1ac5e4a..e1777f92 100644
--- a/src/ERG_Mode.cpp
+++ b/src/ERG_Mode.cpp
@@ -29,7 +29,6 @@ void ergTaskLoop(void* pvParameters) {
     bool simulationRunning      = rtConfig.getSimulateTargetWatts();
 
     if (isInErgMode && (hasConnectedPowerMeter || simulationRunning)) {
-      SS2K_LOG(ERG_MODE_LOG_TAG, "ComputeERG. Setpoint: %d", newSetPoint);
       ergMode.computErg(newSetPoint);
     } else {
       if (newSetPoint > 0) {
@@ -45,37 +44,37 @@ void ergTaskLoop(void* pvParameters) {
 
 // as a note, Trainer Road sends 50w target whenever the app is connected.
 void ErgMode::computErg(int newSetPoint) {
-  Measurement newWatts      = rtConfig.getSimulatedWatts();
-  float incline             = rtConfig.getCurrentIncline();
-  int amountToChangeIncline = 0;
-  int wattChange            = newWatts.value;  // - setPoint;
-  int cadance               = rtConfig.getSimulatedCad();
+  Measurement newWatts        = rtConfig.getSimulatedWatts();
+  float incline               = rtConfig.getCurrentIncline();
+  float amountToChangeIncline = 0;
+  float wattChange            = newSetPoint - newWatts.value;  // setpoint_form_trainer - current_power => Amount to increase or decrease incline
+  int cadance                 = rtConfig.getSimulatedCad();
 
-  if (watts.timestamp == newWatts.timestamp) {
+  if (watts.timestamp == newWatts.timestamp && this->setPoint == newSetPoint) {
     return;  // no new power measurement.
   }
-  watts = newWatts;
 
-  SS2K_LOG(ERG_MODE_LOG_TAG, "Incline = %f, NewSetPoint = %d, CurrentWatts = %d", incline, newSetPoint, wattChange);
+  // set minimum SetPoint to 50 watt if trainer sends setpoints lower than 50 watt.
+  if (newSetPoint < 50) {
+    newSetPoint = 50;
+  }
+
+  SS2K_LOG(ERG_MODE_LOG_TAG, "Incline = %f, SetPoint = %d, NewSetPoint = %d, CurrentWatts = %d, Watts Change (NewSetPoint - CurrentWatts)= %f", incline, this->setPoint,
+           newSetPoint, newWatts.value, wattChange);
 
-  // if (newSetPoint > 0) {  // only update the value if new value is sent
-  //   setPoint = newSetPoint;
-  // }
-  // if (setPoint < 50) {  // minumum setPoint
-  //   setPoint = 50;
-  // }
+  // store for next cycle for timestamp and value compare
+  this->watts    = newWatts;
+  this->setPoint = newSetPoint;
 
   if (cadance <= 20) {
-    if (!userIsPedaling) {  // Test so motor stop command only happens once.
-      motorStop();          // release tension
-      return;
+    if (!this->engineStopped) {                              // Test so motor stop command only happens once.
+      motorStop();                                           // release tension
+      rtConfig.setTargetIncline(incline - WATTS_PER_SHIFT);  // release incline
+      this->engineStopped = true;
     }
-    userIsPedaling = false;
-    rtConfig.setTargetIncline(incline - userConfig.getShiftStep() * 2);
-    // Cadence too low, nothing to do here
-    return;
+    return;  // Cadence too low, nothing to do here
   }
-  userIsPedaling = true;
+  this->engineStopped = false;
 
   amountToChangeIncline = wattChange * userConfig.getERGSensitivity();  // * (75.0 / (double)cadance);
   SS2K_LOG(ERG_MODE_LOG_TAG, "Set amountToChangeIncline to: %f", amountToChangeIncline);
@@ -85,19 +84,18 @@ void ErgMode::computErg(int newSetPoint) {
     SS2K_LOG(ERG_MODE_LOG_TAG, "Watt change < %d watts. Correct amountToChangeIncline to: %f", WATTS_PER_SHIFT, amountToChangeIncline);
   }
   // limit to 10 shifts at a time
-  if (abs(amountToChangeIncline) > userConfig.getShiftStep() * 2) {
+  if (abs(amountToChangeIncline) > WATTS_PER_SHIFT * 2) {
     if (amountToChangeIncline > 5) {
-      amountToChangeIncline = userConfig.getShiftStep() * 2;
+      amountToChangeIncline = WATTS_PER_SHIFT * 2;
     }
     if (amountToChangeIncline < -5) {
-      amountToChangeIncline = -(userConfig.getShiftStep() * 2);
+      amountToChangeIncline = -(WATTS_PER_SHIFT * 2);
     }
-    SS2K_LOG(ERG_MODE_LOG_TAG, "Watt change > %d watts. Correct amountToChangeIncline to: %f", userConfig.getShiftStep() * 2, amountToChangeIncline);
+    SS2K_LOG(ERG_MODE_LOG_TAG, "Watt change > %d watts. Correct amountToChangeIncline to: %f", WATTS_PER_SHIFT * 2, amountToChangeIncline);
   }
 
   // Reduce the amount per loop (don't try to oneshot it) and scale the movement the higher the watt target is as higher wattages require less knob movement.
-  // amountToChangeIncline = amountToChangeIncline / ((userConfig.getSimulatedWatts() / 100) + .1);  // +.1 to eliminate possible divide by zero.
-  float newIncline = incline - amountToChangeIncline;
+  float newIncline = incline + amountToChangeIncline;
   rtConfig.setTargetIncline(newIncline);
   SS2K_LOG(ERG_MODE_LOG_TAG, "newincline: %f", newIncline);
 }
diff --git a/src/Main.cpp b/src/Main.cpp
index a82a7740..31ddcf65 100644
--- a/src/Main.cpp
+++ b/src/Main.cpp
@@ -50,7 +50,7 @@ physicalWorkingCapacity userPWC;
 #ifndef UNIT_TEST
 
 void startTasks() {
-  SS2K_LOG(MAIN_LOG_TAG, "Start BLE & ERG Tasks");
+  SS2K_LOG(MAIN_LOG_TAG, "Start BLE + ERG Tasks");
   if (BLECommunicationTask == NULL) {
     setupBLE();
   }
@@ -60,7 +60,7 @@ void startTasks() {
 }
 
 void stopTasks() {
-  SS2K_LOG(MAIN_LOG_TAG, "Stop BLE & ERG Tasks");
+  SS2K_LOG(MAIN_LOG_TAG, "Stop BLE + ERG Tasks");
   if (BLECommunicationTask != NULL) {
     vTaskDelete(BLECommunicationTask);
   }
@@ -185,11 +185,21 @@ void moveStepper(void *pvParameters) {
   while (1) {
     if (stepper) {
       int shifterIncline = rtConfig.getShifterPosition() * userConfig.getShiftStep();
-      int ergIncline     = 0;
+      int incline        = 0;
+      float stepsPerWatt = (float)userConfig.getShiftStep() / 30.0;
       if (!externalControl) {
-        ergIncline = rtConfig.getTargetIncline() * userConfig.getInclineMultiplier();
+        if (rtConfig.getERGMode()) {
+          // ERG Mode
+          // Shifter step should taget 30 watt.
+          // TargetIncline is the change in watt -> must devide ShifterStep / 30 to get the steps for 1 watt.
+          incline = rtConfig.getTargetIncline() * stepsPerWatt;
+        } else {
+          // Simulation Mode
+          incline = rtConfig.getTargetIncline() * userConfig.getInclineMultiplier();
+        }
       }
-      targetPosition = shifterIncline + ergIncline;
+
+      targetPosition = shifterIncline + incline;
 
       if (syncMode) {
         stepper->stopMove();
@@ -200,7 +210,7 @@ void moveStepper(void *pvParameters) {
       stepper->moveTo(targetPosition);
       vTaskDelay(100 / portTICK_PERIOD_MS);
 
-      rtConfig.setCurrentIncline(((float)stepper->getCurrentPosition() / (float)userConfig.getInclineMultiplier()) + shifterIncline);
+      rtConfig.setCurrentIncline(((float)(stepper->getCurrentPosition() - shifterIncline)) / stepsPerWatt);
 
       if (connectedClientCount() > 0) {
         stepper->setAutoEnable(false);  // Keep the stepper from rolling back due to head tube slack. Motor Driver still lowers power between moves

From 992d26cec828678836cc9e552186ae3efd56b4b4 Mon Sep 17 00:00:00 2001
From: Markus Schneider <m.schneider@online.de>
Date: Mon, 6 Dec 2021 15:23:22 +0100
Subject: [PATCH 09/42] reordered parameters

---
 include/SmartSpin_parameters.h | 41 +++++++++++++++++++++-------------
 src/ERG_Mode.cpp               |  4 ++--
 2 files changed, 28 insertions(+), 17 deletions(-)

diff --git a/include/SmartSpin_parameters.h b/include/SmartSpin_parameters.h
index 3a436ad5..171526ca 100644
--- a/include/SmartSpin_parameters.h
+++ b/include/SmartSpin_parameters.h
@@ -40,36 +40,47 @@ class RuntimeParameters {
   String foundDevices        = " ";
 
  public:
+  void setTargetIncline(float inc) { targetIncline = inc; }
   float getTargetIncline() { return targetIncline; }
-  float getCurrentIncline() { return currentIncline; }
-
-  int getTargetWatts() { return targetWatts; }
-  Measurement getSimulatedWatts() { return simulatedWatts; }
-  int getSimulatedHr() { return simulatedHr; }
-  int getSimulatedCad() { return simulatedCad; }
-  float getSimulatedSpeed() { return simulatedSpeed; }
-  bool getSimulateHr() { return simulateHr; }
-  bool getSimulateWatts() { return simulateWatts; }
-  bool getSimulateTargetWatts() { return simulateTargetWatts; }
-  bool getSimulateCad() { return simulateCad; }
-  bool getERGMode() { return ERGMode; }
-  int getShifterPosition() { return shifterPosition; }
-  const char* getFoundDevices() { return foundDevices.c_str(); }
 
-  void setTargetIncline(float inc) { targetIncline = inc; }
   void setCurrentIncline(float inc) { currentIncline = inc; }
+  float getCurrentIncline() { return currentIncline; }
+
   void setTargetWatts(int w) { targetWatts = w; }
+  int getTargetWatts() { return targetWatts; }
+  
   void setSimulatedWatts(int w) { simulatedWatts = Measurement(w); }
+  Measurement getSimulatedWatts() { return simulatedWatts; }
+  
   void setSimulatedHr(int hr) { simulatedHr = hr; }
+  int getSimulatedHr() { return simulatedHr; }
+  
   void setSimulatedCad(float cad) { simulatedCad = cad; }
+  int getSimulatedCad() { return simulatedCad; }
+  
   void setSimulatedSpeed(float spd) { simulatedSpeed = spd; }
+  float getSimulatedSpeed() { return simulatedSpeed; }
+  
   void setSimulateHr(bool shr) { simulateHr = shr; }
+  bool getSimulateHr() { return simulateHr; }
+  
   void setSimulateWatts(bool swt) { simulateWatts = swt; }
+  bool getSimulateWatts() { return simulateWatts; }
+  
   void setSimulateTargetWatts(bool swt) { simulateTargetWatts = swt; }
+  bool getSimulateTargetWatts() { return simulateTargetWatts; }
+  
   void setSimulateCad(bool scd) { simulateCad = scd; }
+  bool getSimulateCad() { return simulateCad; }
+  
   void setERGMode(bool erg) { ERGMode = erg; }
+  bool getERGMode() { return ERGMode; }
+  
   void setShifterPosition(int sp) { shifterPosition = sp; }
+  int getShifterPosition() { return shifterPosition; }
+  
   void setFoundDevices(String fdev) { foundDevices = fdev; }
+  const char* getFoundDevices() { return foundDevices.c_str(); }
 
   String returnJSON();
 };
diff --git a/src/ERG_Mode.cpp b/src/ERG_Mode.cpp
index e1777f92..dc17d03b 100644
--- a/src/ERG_Mode.cpp
+++ b/src/ERG_Mode.cpp
@@ -46,9 +46,9 @@ void ergTaskLoop(void* pvParameters) {
 void ErgMode::computErg(int newSetPoint) {
   Measurement newWatts        = rtConfig.getSimulatedWatts();
   float incline               = rtConfig.getCurrentIncline();
-  float amountToChangeIncline = 0;
-  float wattChange            = newSetPoint - newWatts.value;  // setpoint_form_trainer - current_power => Amount to increase or decrease incline
   int cadance                 = rtConfig.getSimulatedCad();
+  float wattChange            = newSetPoint - newWatts.value;  // setpoint_form_trainer - current_power => Amount to increase or decrease incline
+  float amountToChangeIncline = 0;
 
   if (watts.timestamp == newWatts.timestamp && this->setPoint == newSetPoint) {
     return;  // no new power measurement.

From ea713334070d4be4a481d216753f5a7e1edd78e7 Mon Sep 17 00:00:00 2001
From: Markus Schneider <m.schneider@online.de>
Date: Mon, 6 Dec 2021 16:21:21 +0100
Subject: [PATCH 10/42] changed BLE Client delay, refactored ERG

---
 include/ERG_Mode.h |  2 ++
 include/settings.h |  4 ++--
 src/ERG_Mode.cpp   | 31 +++++++++++++++++++++----------
 3 files changed, 25 insertions(+), 12 deletions(-)

diff --git a/include/ERG_Mode.h b/include/ERG_Mode.h
index 2841f67a..76747a1c 100644
--- a/include/ERG_Mode.h
+++ b/include/ERG_Mode.h
@@ -18,4 +18,6 @@ class ErgMode {
   bool engineStopped = false;
   int setPoint       = 0;
   Measurement watts  = Measurement(0);
+
+  bool _userIsSpinning(int cadence, float incline);
 };
\ No newline at end of file
diff --git a/include/settings.h b/include/settings.h
index eec5e1a6..eaa4e374 100644
--- a/include/settings.h
+++ b/include/settings.h
@@ -122,10 +122,10 @@
 #define MAX_SCAN_RETRIES 1
 
 // loop speed for the SmartSpin2k BLE communications
-#define BLE_NOTIFY_DELAY 200
+#define BLE_NOTIFY_DELAY 701
 
 // loop speed for the SmartSpin2k BLE Client reconnect
-#define BLE_CLIENT_DELAY 1000
+#define BLE_CLIENT_DELAY 200
 
 // Number of devices that can be connected to the Client (myBLEDevices size)
 #define NUM_BLE_DEVICES 4
diff --git a/src/ERG_Mode.cpp b/src/ERG_Mode.cpp
index dc17d03b..c521497e 100644
--- a/src/ERG_Mode.cpp
+++ b/src/ERG_Mode.cpp
@@ -59,30 +59,27 @@ void ErgMode::computErg(int newSetPoint) {
     newSetPoint = 50;
   }
 
-  SS2K_LOG(ERG_MODE_LOG_TAG, "Incline = %f, SetPoint = %d, NewSetPoint = %d, CurrentWatts = %d, Watts Change (NewSetPoint - CurrentWatts)= %f", incline, this->setPoint,
-           newSetPoint, newWatts.value, wattChange);
+  SS2K_LOG(ERG_MODE_LOG_TAG, "Incline = %f, SetPoint = %d, NewSetPoint = %d, Watts = (%lu, %d), CurrentWatts = (%lu, %d), Watts Change (NewSetPoint - CurrentWatts)= %f", incline,
+           this->setPoint, newSetPoint, this->watts.timestamp, this->watts.value, newWatts.timestamp, newWatts.value, wattChange);
 
   // store for next cycle for timestamp and value compare
   this->watts    = newWatts;
   this->setPoint = newSetPoint;
 
-  if (cadance <= 20) {
-    if (!this->engineStopped) {                              // Test so motor stop command only happens once.
-      motorStop();                                           // release tension
-      rtConfig.setTargetIncline(incline - WATTS_PER_SHIFT);  // release incline
-      this->engineStopped = true;
-    }
-    return;  // Cadence too low, nothing to do here
+  if (!this->_userIsSpinning(cadance, incline)) {
+    return;
   }
-  this->engineStopped = false;
 
   amountToChangeIncline = wattChange * userConfig.getERGSensitivity();  // * (75.0 / (double)cadance);
   SS2K_LOG(ERG_MODE_LOG_TAG, "Set amountToChangeIncline to: %f", amountToChangeIncline);
+
+  // Minor Steps
   if (abs(wattChange) < WATTS_PER_SHIFT) {
     // As the desired value gets closer, make smaller changes for a smoother experience
     amountToChangeIncline *= SUB_SHIFT_SCALE;
     SS2K_LOG(ERG_MODE_LOG_TAG, "Watt change < %d watts. Correct amountToChangeIncline to: %f", WATTS_PER_SHIFT, amountToChangeIncline);
   }
+
   // limit to 10 shifts at a time
   if (abs(amountToChangeIncline) > WATTS_PER_SHIFT * 2) {
     if (amountToChangeIncline > 5) {
@@ -99,3 +96,17 @@ void ErgMode::computErg(int newSetPoint) {
   rtConfig.setTargetIncline(newIncline);
   SS2K_LOG(ERG_MODE_LOG_TAG, "newincline: %f", newIncline);
 }
+
+bool ErgMode::_userIsSpinning(int cadance, float incline) {
+  if (cadance <= 20) {
+    if (!this->engineStopped) {                              // Test so motor stop command only happens once.
+      motorStop();                                           // release tension
+      rtConfig.setTargetIncline(incline - WATTS_PER_SHIFT);  // release incline
+      this->engineStopped = true;
+    }
+    return false;  // Cadence too low, nothing to do here
+  }
+
+  this->engineStopped = false;
+  return true;
+}

From e4020baccaa2aaeec8c5178ce8e0b0f1a8bb98e2 Mon Sep 17 00:00:00 2001
From: Markus Schneider <m.schneider@online.de>
Date: Fri, 10 Dec 2021 17:00:55 +0100
Subject: [PATCH 11/42] ERG_Mode: added two steps

---
 data/btsimulator.html     | 26 ++++++++-----
 data/settings.html        |  4 +-
 include/ERG_Mode.h        | 13 ++++---
 include/settings.h        |  4 +-
 src/ERG_Mode.cpp          | 78 +++++++++++++++++++++++----------------
 src/HTTP_Server_Basic.cpp |  2 +-
 src/Main.cpp              | 15 +++-----
 7 files changed, 82 insertions(+), 60 deletions(-)

diff --git a/data/btsimulator.html b/data/btsimulator.html
index 1eee764f..674d0382 100644
--- a/data/btsimulator.html
+++ b/data/btsimulator.html
@@ -23,21 +23,21 @@ <h1>BLE Device Simulator</h1>
 
     <h2>Sim Heart Rate</h2>
     <p><span id="hrValue"></span></p>
-    <p><input type="range" onload="requestConfigValues()" onchange="updatehrSlider(this)" id="hrSlider" min="40"
+    <p><input type="range" onload="requestConfigValues()" onchange="updateHrSlider()" id="hrSlider" min="40"
         max="250" value="0" step="1" class="slider2"></p>
     <p><label class="switch"><input type="checkbox" onload="toggleHRCheckbox(this, true)"
           onchange="toggleHRCheckbox(this,true)" id="hrOutput"><span class="slider"></span></label></p>
 
     <h2>Sim Power Output</h2>
     <p><span id="wattsValue"></span></p>
-    <p><input type="range" onload="requestConfigValues()" onchange="updateWattsSlider(this)" id="wattsSlider" min="0"
-        max="2000" value="0" step="1" class="slider2"></p>
+    <p><input type="range" onload="requestConfigValues()" onchange="updateWattsSlider()" id="wattsSlider" min="0"
+        max="600" value="0" step="1" class="slider2"></p>
     <p><label class="switch"><input type="checkbox" onload="toggleWattsCheckbox(this,true)"
           onchange="toggleWattsCheckbox(this,true)" id="wattsOutput"><span class="slider"></span></label></p>
 
     <h2>Sim CAD Output</h2>
     <p><span id="cadValue"></span></p>
-    <p><input type="range" onload="requestConfigValues()" onchange="updateCadSlider(this)" id="cadSlider" min="0"
+    <p><input type="range" onload="requestConfigValues()" onchange="updateCadSlider()" id="cadSlider" min="0"
         max="180" value="0" step="1" class="slider2"></p>
     <p><label class="switch"><input type="checkbox" onload="toggleCadCheckbox(this,true)"
           onchange="toggleCadCheckbox(this,true)" id="cadOutput"><span class="slider"></span></label></p>
@@ -53,7 +53,7 @@ <h2>Enable ERG</h2>
     </p>
     <h2>ERG Target Watts</h2>
     <p><span id="targetWattsValue"></span></p>
-    <p><input type="range" onload="requestConfigValues()" onchange="updateTargetWattsSlider(this)"
+    <p><input type="range" onload="requestConfigValues()" onchange="updateTargetWattsSlider()"
         id="targetWattsSlider" min="0" max="600" value="0" step="1" class="slider2"></p>
     <p>
       <label class="switch">
@@ -70,6 +70,8 @@ <h2>ERG Target Watts</h2>
 
   }, 2000);
 
+  let pwrTimer;
+
   function toggleHRCheckbox(element, updateServer) {
     if (element.checked) {
       document.getElementById("hrSlider").hidden = false;
@@ -96,10 +98,16 @@ <h2>ERG Target Watts</h2>
     if (element.checked) {
       document.getElementById("wattsSlider").hidden = false;
       document.getElementById("wattsValue").hidden = false;
+      pwrTimer = setInterval(() => {
+        updateWattsSlider();
+      }, 1000)
     }
     else {
       document.getElementById("wattsSlider").hidden = true;
       document.getElementById("wattsValue").hidden = true;
+      if (!!pwrTimer) {
+        clearInterval(pwrTimer);
+      }
     }
 
     if (updateServer) {
@@ -138,7 +146,7 @@ <h2>ERG Target Watts</h2>
     }
   }
 
-  function updatehrSlider(element) {
+  function updateHrSlider() {
     var sliderValue = document.getElementById("hrSlider").value;
     document.getElementById("hrValue").innerHTML = sliderValue + " BPM";
     var xhr = new XMLHttpRequest();
@@ -146,7 +154,7 @@ <h2>ERG Target Watts</h2>
     xhr.send();
   }
 
-  function updateWattsSlider(element) {
+  function updateWattsSlider() {
     var sliderValue = document.getElementById("wattsSlider").value;
     document.getElementById("wattsValue").innerHTML = sliderValue + " Watts";
     var xhr = new XMLHttpRequest();
@@ -154,7 +162,7 @@ <h2>ERG Target Watts</h2>
     xhr.send();
   }
 
-  function updateCadSlider(element) {
+  function updateCadSlider() {
     var sliderValue = document.getElementById("cadSlider").value;
     document.getElementById("cadValue").innerHTML = sliderValue + " RPM";
     var xhr = new XMLHttpRequest();
@@ -162,7 +170,7 @@ <h2>ERG Target Watts</h2>
     xhr.send();
   }
 
-  function updateTargetWattsSlider(element) {
+  function updateTargetWattsSlider() {
     var sliderValue = document.getElementById("targetWattsSlider").value;
     document.getElementById("targetWattsValue").innerHTML = sliderValue + " Watts";
     var xhr = new XMLHttpRequest();
diff --git a/data/settings.html b/data/settings.html
index 8ab67037..22e3c657 100644
--- a/data/settings.html
+++ b/data/settings.html
@@ -97,8 +97,8 @@ <h2>
                 <input type='button' onclick="clickStep(document.getElementById('ERGSensitivity'), this.value)"
                   value="-">
                 <input style="width:50%; position: relative; top: 5px;" type="range" id="ERGSensitivity"
-                  name="ERGSensitivity" value="1.0" min=".5" max="5" step=".1" class="slider1"
-                  onchange="updateSlider(this, document.getElementById('ERGSensitivityValue'))" />
+                  name="ERGSensitivity" value="1.0" min=".5" max="20" step=".1" class="slider1"
+                  onchange="updateSlider(this.value, document.getElementById('ERGSensitivityValue'))" />
                 <input type='button' onclick="clickStep(document.getElementById('ERGSensitivity'), this.value)"
                   value="+">
               </td>
diff --git a/include/ERG_Mode.h b/include/ERG_Mode.h
index 76747a1c..072c760f 100644
--- a/include/ERG_Mode.h
+++ b/include/ERG_Mode.h
@@ -4,7 +4,7 @@
 #include "SmartSpin_parameters.h"
 
 #define ERG_MODE_LOG_TAG "ERG_Mode"
-#define ERG_MODE_DELAY   100
+#define ERG_MODE_DELAY   250
 
 extern TaskHandle_t ErgTask;
 void setupERG();
@@ -15,9 +15,12 @@ class ErgMode {
   void computErg(int newSetpoint);
 
  private:
-  bool engineStopped = false;
-  int setPoint       = 0;
-  Measurement watts  = Measurement(0);
+  bool engineStopped  = false;
+  bool initialized    = false;
+  int setPoint        = 0;
+  int cycles          = 0;
+  int offsetMuliplier = 0;
+  Measurement watts   = Measurement(0);
 
   bool _userIsSpinning(int cadence, float incline);
-};
\ No newline at end of file
+};
diff --git a/include/settings.h b/include/settings.h
index eaa4e374..e43b4db9 100644
--- a/include/settings.h
+++ b/include/settings.h
@@ -122,10 +122,10 @@
 #define MAX_SCAN_RETRIES 1
 
 // loop speed for the SmartSpin2k BLE communications
-#define BLE_NOTIFY_DELAY 701
+#define BLE_NOTIFY_DELAY 250
 
 // loop speed for the SmartSpin2k BLE Client reconnect
-#define BLE_CLIENT_DELAY 200
+#define BLE_CLIENT_DELAY 1000
 
 // Number of devices that can be connected to the Client (myBLEDevices size)
 #define NUM_BLE_DEVICES 4
diff --git a/src/ERG_Mode.cpp b/src/ERG_Mode.cpp
index c521497e..a051e23c 100644
--- a/src/ERG_Mode.cpp
+++ b/src/ERG_Mode.cpp
@@ -39,16 +39,24 @@ void ergTaskLoop(void* pvParameters) {
 #ifdef DEBUG_STACK
     Serial.printf("ERG Task: %d \n", uxTaskGetStackHighWaterMark(ErgTask));
 #endif  // DEBUG_STACK
+
+    // collect bike charateristics
+    // if (hasConnectedPowerMeter) {
+    //   float incline = rtConfig.getCurrentIncline();
+    //   int cadence   = rtConfig.getSimulatedCad();
+    //   int power     = rtConfig.getSimulatedWatts().value;
+    //   SS2K_LOG(ERG_MODE_LOG_TAG, "Bike characteristics: INC: %f; CAD: %d; PWR: %d", incline, cadence, power);
+    // }
   }
 }
 
 // as a note, Trainer Road sends 50w target whenever the app is connected.
 void ErgMode::computErg(int newSetPoint) {
-  Measurement newWatts        = rtConfig.getSimulatedWatts();
-  float incline               = rtConfig.getCurrentIncline();
-  int cadance                 = rtConfig.getSimulatedCad();
-  float wattChange            = newSetPoint - newWatts.value;  // setpoint_form_trainer - current_power => Amount to increase or decrease incline
-  float amountToChangeIncline = 0;
+  Measurement newWatts = rtConfig.getSimulatedWatts();
+  float incline        = rtConfig.getCurrentIncline();
+  int cadance          = rtConfig.getSimulatedCad();
+  int wattChange       = newSetPoint - newWatts.value;  // setpoint_form_trainer - current_power => Amount to increase or decrease incline
+  float diviation      = ((float)wattChange * 100.0) / ((float)newSetPoint);
 
   if (watts.timestamp == newWatts.timestamp && this->setPoint == newSetPoint) {
     return;  // no new power measurement.
@@ -59,42 +67,50 @@ void ErgMode::computErg(int newSetPoint) {
     newSetPoint = 50;
   }
 
-  SS2K_LOG(ERG_MODE_LOG_TAG, "Incline = %f, SetPoint = %d, NewSetPoint = %d, Watts = (%lu, %d), CurrentWatts = (%lu, %d), Watts Change (NewSetPoint - CurrentWatts)= %f", incline,
-           this->setPoint, newSetPoint, this->watts.timestamp, this->watts.value, newWatts.timestamp, newWatts.value, wattChange);
+  // reset cycle counter to start new erg calculation cycle
+  if (this->setPoint != newSetPoint) {
+    this->cycles = 0;
+  }
 
   // store for next cycle for timestamp and value compare
   this->watts    = newWatts;
   this->setPoint = newSetPoint;
 
-  if (!this->_userIsSpinning(cadance, incline)) {
+  bool isUserSpinning = this->_userIsSpinning(cadance, incline);
+  if (!isUserSpinning) {
     return;
   }
 
-  amountToChangeIncline = wattChange * userConfig.getERGSensitivity();  // * (75.0 / (double)cadance);
-  SS2K_LOG(ERG_MODE_LOG_TAG, "Set amountToChangeIncline to: %f", amountToChangeIncline);
+  float factor        = abs(diviation) > 10 ? userConfig.getERGSensitivity() : userConfig.getERGSensitivity() / 2;
+  float targetIncline = incline + (wattChange * factor);
 
-  // Minor Steps
-  if (abs(wattChange) < WATTS_PER_SHIFT) {
-    // As the desired value gets closer, make smaller changes for a smoother experience
-    amountToChangeIncline *= SUB_SHIFT_SCALE;
-    SS2K_LOG(ERG_MODE_LOG_TAG, "Watt change < %d watts. Correct amountToChangeIncline to: %f", WATTS_PER_SHIFT, amountToChangeIncline);
-  }
+  // SS2K_LOG(ERG_MODE_LOG_TAG, "Set amountToChangeIncline to: %f", amountToChangeIncline);
 
-  // limit to 10 shifts at a time
-  if (abs(amountToChangeIncline) > WATTS_PER_SHIFT * 2) {
-    if (amountToChangeIncline > 5) {
-      amountToChangeIncline = WATTS_PER_SHIFT * 2;
-    }
-    if (amountToChangeIncline < -5) {
-      amountToChangeIncline = -(WATTS_PER_SHIFT * 2);
-    }
-    SS2K_LOG(ERG_MODE_LOG_TAG, "Watt change > %d watts. Correct amountToChangeIncline to: %f", WATTS_PER_SHIFT * 2, amountToChangeIncline);
-  }
-
-  // Reduce the amount per loop (don't try to oneshot it) and scale the movement the higher the watt target is as higher wattages require less knob movement.
-  float newIncline = incline + amountToChangeIncline;
-  rtConfig.setTargetIncline(newIncline);
-  SS2K_LOG(ERG_MODE_LOG_TAG, "newincline: %f", newIncline);
+  // Minor Steps
+  // if (abs(wattChange) < WATTS_PER_SHIFT) {
+  //   // As the desired value gets closer, make smaller changes for a smoother experience
+  //   amountToChangeIncline *= SUB_SHIFT_SCALE;
+  //   SS2K_LOG(ERG_MODE_LOG_TAG, "Watt change < %d watts. Correct amountToChangeIncline to: %f", WATTS_PER_SHIFT, amountToChangeIncline);
+  // }
+
+  // // limit to 10 shifts at a time
+  // if (abs(amountToChangeIncline) > WATTS_PER_SHIFT * 2) {
+  //   if (amountToChangeIncline > 5) {
+  //     amountToChangeIncline = WATTS_PER_SHIFT * 2;
+  //   }
+  //   if (amountToChangeIncline < -5) {
+  //     amountToChangeIncline = -(WATTS_PER_SHIFT * 2);
+  //   }
+  //   SS2K_LOG(ERG_MODE_LOG_TAG, "Watt change > %d watts. Correct amountToChangeIncline to: %f", WATTS_PER_SHIFT * 2, amountToChangeIncline);
+  // }
+
+  // float newIncline = incline + amountToChangeIncline;
+  rtConfig.setTargetIncline(targetIncline);
+  SS2K_LOG(ERG_MODE_LOG_TAG,
+           "Cycle=%d, " + "Incline = %f, " + "TargetIncline = %f, " + "SetPoint = %d, " + "NewSetPoint = %d, " + "Watts = (%lu, %d), " + "CurrentWatts = (%lu, %d), " +
+               "Watts Change (NewSetPoint - CurrentWatts) = %d, " + "diviation = %f",
+           this->cycles, incline, targetIncline, this->setPoint, newSetPoint, this->watts.timestamp, this->watts.value, newWatts.timestamp, newWatts.value, wattChange, diviation);
+  this->cycles++;
 }
 
 bool ErgMode::_userIsSpinning(int cadance, float incline) {
diff --git a/src/HTTP_Server_Basic.cpp b/src/HTTP_Server_Basic.cpp
index 153d7db6..63a01202 100644
--- a/src/HTTP_Server_Basic.cpp
+++ b/src/HTTP_Server_Basic.cpp
@@ -446,7 +446,7 @@ void settingsProcessor() {
   }
   if (!server.arg("ERGSensitivity").isEmpty()) {
     float ERGSensitivity = server.arg("ERGSensitivity").toFloat();
-    if (ERGSensitivity >= .5 && ERGSensitivity <= 3) {
+    if (ERGSensitivity >= .5 && ERGSensitivity <= 20) {
       userConfig.setERGSensitivity(ERGSensitivity);
     }
   }
diff --git a/src/Main.cpp b/src/Main.cpp
index 31ddcf65..0a07d266 100644
--- a/src/Main.cpp
+++ b/src/Main.cpp
@@ -184,23 +184,18 @@ void moveStepper(void *pvParameters) {
 
   while (1) {
     if (stepper) {
-      int shifterIncline = rtConfig.getShifterPosition() * userConfig.getShiftStep();
-      int incline        = 0;
-      float stepsPerWatt = (float)userConfig.getShiftStep() / 30.0;
+      int targetPosition = rtConfig.getShifterPosition() * userConfig.getShiftStep();
       if (!externalControl) {
         if (rtConfig.getERGMode()) {
           // ERG Mode
-          // Shifter step should taget 30 watt.
-          // TargetIncline is the change in watt -> must devide ShifterStep / 30 to get the steps for 1 watt.
-          incline = rtConfig.getTargetIncline() * stepsPerWatt;
+          // Shifter not used.
+          targetPosition = rtConfig.getTargetIncline();
         } else {
           // Simulation Mode
-          incline = rtConfig.getTargetIncline() * userConfig.getInclineMultiplier();
+          targetPosition += rtConfig.getTargetIncline() * userConfig.getInclineMultiplier();
         }
       }
 
-      targetPosition = shifterIncline + incline;
-
       if (syncMode) {
         stepper->stopMove();
         vTaskDelay(100 / portTICK_PERIOD_MS);
@@ -210,7 +205,7 @@ void moveStepper(void *pvParameters) {
       stepper->moveTo(targetPosition);
       vTaskDelay(100 / portTICK_PERIOD_MS);
 
-      rtConfig.setCurrentIncline(((float)(stepper->getCurrentPosition() - shifterIncline)) / stepsPerWatt);
+      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

From 9345f3552e91805a36ef0645c856f88f3c0b1fb0 Mon Sep 17 00:00:00 2001
From: Markus Schneider <m.schneider@online.de>
Date: Sun, 12 Dec 2021 13:25:52 +0100
Subject: [PATCH 12/42] added csv logging to erg_mode

---
 include/ERG_Mode.h |  8 +++++--
 src/ERG_Mode.cpp   | 60 +++++++++++++++++++---------------------------
 2 files changed, 30 insertions(+), 38 deletions(-)

diff --git a/include/ERG_Mode.h b/include/ERG_Mode.h
index 072c760f..608d2fca 100644
--- a/include/ERG_Mode.h
+++ b/include/ERG_Mode.h
@@ -3,8 +3,9 @@
 #include <Arduino.h>
 #include "SmartSpin_parameters.h"
 
-#define ERG_MODE_LOG_TAG "ERG_Mode"
-#define ERG_MODE_DELAY   250
+#define ERG_MODE_LOG_TAG     "ERG_Mode"
+#define ERG_MODE_LOG_CSV_TAG "ERG_Mode_CSV"
+#define ERG_MODE_DELAY       250
 
 extern TaskHandle_t ErgTask;
 void setupERG();
@@ -13,6 +14,8 @@ void ergTaskLoop(void *pvParameters);
 class ErgMode {
  public:
   void computErg(int newSetpoint);
+  void _writeLogHeader();
+  void _writeLog(int cycles, float currentIncline, float newIncline, int currentSetPoint, int newSetPoint, int currentWatts, int newWatts, int currentCadence, int newCadence);
 
  private:
   bool engineStopped  = false;
@@ -21,6 +24,7 @@ class ErgMode {
   int cycles          = 0;
   int offsetMuliplier = 0;
   Measurement watts   = Measurement(0);
+  int cadence         = 0;
 
   bool _userIsSpinning(int cadence, float incline);
 };
diff --git a/src/ERG_Mode.cpp b/src/ERG_Mode.cpp
index a051e23c..20e09614 100644
--- a/src/ERG_Mode.cpp
+++ b/src/ERG_Mode.cpp
@@ -20,6 +20,7 @@ void setupERG() {
 
 void ergTaskLoop(void* pvParameters) {
   ErgMode ergMode;
+  ergMode._writeLogHeader();
   while (true) {
     vTaskDelay(ERG_MODE_DELAY / portTICK_PERIOD_MS);
 
@@ -53,8 +54,8 @@ void ergTaskLoop(void* pvParameters) {
 // as a note, Trainer Road sends 50w target whenever the app is connected.
 void ErgMode::computErg(int newSetPoint) {
   Measurement newWatts = rtConfig.getSimulatedWatts();
-  float incline        = rtConfig.getCurrentIncline();
-  int cadance          = rtConfig.getSimulatedCad();
+  float currentIncline = rtConfig.getCurrentIncline();
+  int newCadance       = rtConfig.getSimulatedCad();
   int wattChange       = newSetPoint - newWatts.value;  // setpoint_form_trainer - current_power => Amount to increase or decrease incline
   float diviation      = ((float)wattChange * 100.0) / ((float)newSetPoint);
 
@@ -72,44 +73,21 @@ void ErgMode::computErg(int newSetPoint) {
     this->cycles = 0;
   }
 
-  // store for next cycle for timestamp and value compare
-  this->watts    = newWatts;
-  this->setPoint = newSetPoint;
-
-  bool isUserSpinning = this->_userIsSpinning(cadance, incline);
+  bool isUserSpinning = this->_userIsSpinning(newCadance, currentIncline);
   if (!isUserSpinning) {
     return;
   }
 
-  float factor        = abs(diviation) > 10 ? userConfig.getERGSensitivity() : userConfig.getERGSensitivity() / 2;
-  float targetIncline = incline + (wattChange * factor);
-
-  // SS2K_LOG(ERG_MODE_LOG_TAG, "Set amountToChangeIncline to: %f", amountToChangeIncline);
-
-  // Minor Steps
-  // if (abs(wattChange) < WATTS_PER_SHIFT) {
-  //   // As the desired value gets closer, make smaller changes for a smoother experience
-  //   amountToChangeIncline *= SUB_SHIFT_SCALE;
-  //   SS2K_LOG(ERG_MODE_LOG_TAG, "Watt change < %d watts. Correct amountToChangeIncline to: %f", WATTS_PER_SHIFT, amountToChangeIncline);
-  // }
-
-  // // limit to 10 shifts at a time
-  // if (abs(amountToChangeIncline) > WATTS_PER_SHIFT * 2) {
-  //   if (amountToChangeIncline > 5) {
-  //     amountToChangeIncline = WATTS_PER_SHIFT * 2;
-  //   }
-  //   if (amountToChangeIncline < -5) {
-  //     amountToChangeIncline = -(WATTS_PER_SHIFT * 2);
-  //   }
-  //   SS2K_LOG(ERG_MODE_LOG_TAG, "Watt change > %d watts. Correct amountToChangeIncline to: %f", WATTS_PER_SHIFT * 2, amountToChangeIncline);
-  // }
-
-  // float newIncline = incline + amountToChangeIncline;
-  rtConfig.setTargetIncline(targetIncline);
-  SS2K_LOG(ERG_MODE_LOG_TAG,
-           "Cycle=%d, " + "Incline = %f, " + "TargetIncline = %f, " + "SetPoint = %d, " + "NewSetPoint = %d, " + "Watts = (%lu, %d), " + "CurrentWatts = (%lu, %d), " +
-               "Watts Change (NewSetPoint - CurrentWatts) = %d, " + "diviation = %f",
-           this->cycles, incline, targetIncline, this->setPoint, newSetPoint, this->watts.timestamp, this->watts.value, newWatts.timestamp, newWatts.value, wattChange, diviation);
+  float factor     = abs(diviation) > 10 ? userConfig.getERGSensitivity() : userConfig.getERGSensitivity() / 2;
+  float newIncline = currentIncline + (wattChange * factor);
+
+  rtConfig.setTargetIncline(newIncline);
+  _writeLog(this->cycles, currentIncline, newIncline, setPoint, newSetPoint, this->watts.value, newWatts.value, this->cadence, newCadance);
+
+  // store for next cycle for timestamp and value compare
+  this->watts    = newWatts;
+  this->setPoint = newSetPoint;
+  this->cadence  = newCadance;
   this->cycles++;
 }
 
@@ -126,3 +104,13 @@ bool ErgMode::_userIsSpinning(int cadance, float incline) {
   this->engineStopped = false;
   return true;
 }
+
+void ErgMode::_writeLogHeader() {
+  SS2K_LOG(ERG_MODE_LOG_CSV_TAG, "cycles;current incline;new incline;current setpoint;new setpoint;current watts;new watts;current cadence;new cadence;");
+}
+
+void ErgMode::_writeLog(int cycles, float currentIncline, float newIncline, int currentSetPoint, int newSetPoint, int currentWatts, int newWatts, int currentCadence,
+                        int newCadence) {
+  SS2K_LOG(ERG_MODE_LOG_CSV_TAG, "%d;%.2f;%.2f;%d;%d;%d;%d;%d;%d", cycles, currentIncline, newIncline, currentSetPoint, newSetPoint, currentWatts, newWatts, currentCadence,
+           newCadence);
+}

From 9617ef44e3ac04908c4a70b3c0376a93615c33d8 Mon Sep 17 00:00:00 2001
From: Markus Schneider <m.schneider@online.de>
Date: Mon, 13 Dec 2021 16:21:11 +0100
Subject: [PATCH 13/42] reduced minor step to 1/4

---
 src/ERG_Mode.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/ERG_Mode.cpp b/src/ERG_Mode.cpp
index 20e09614..4d3e3396 100644
--- a/src/ERG_Mode.cpp
+++ b/src/ERG_Mode.cpp
@@ -78,7 +78,7 @@ void ErgMode::computErg(int newSetPoint) {
     return;
   }
 
-  float factor     = abs(diviation) > 10 ? userConfig.getERGSensitivity() : userConfig.getERGSensitivity() / 2;
+  float factor     = abs(diviation) > 10 ? userConfig.getERGSensitivity() : userConfig.getERGSensitivity() / 4;
   float newIncline = currentIncline + (wattChange * factor);
 
   rtConfig.setTargetIncline(newIncline);

From d4a93b50937111c50588ecebba63e8dcd923213e Mon Sep 17 00:00:00 2001
From: Markus Schneider <m.schneider@online.de>
Date: Fri, 17 Dec 2021 17:38:22 +0100
Subject: [PATCH 14/42] changed minore steps to ergSensitivity/2

---
 src/ERG_Mode.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/ERG_Mode.cpp b/src/ERG_Mode.cpp
index 4d3e3396..20e09614 100644
--- a/src/ERG_Mode.cpp
+++ b/src/ERG_Mode.cpp
@@ -78,7 +78,7 @@ void ErgMode::computErg(int newSetPoint) {
     return;
   }
 
-  float factor     = abs(diviation) > 10 ? userConfig.getERGSensitivity() : userConfig.getERGSensitivity() / 4;
+  float factor     = abs(diviation) > 10 ? userConfig.getERGSensitivity() : userConfig.getERGSensitivity() / 2;
   float newIncline = currentIncline + (wattChange * factor);
 
   rtConfig.setTargetIncline(newIncline);

From 80adc83d98153bedef1824b772be13a07b25278c Mon Sep 17 00:00:00 2001
From: doudar <anthonydoud97@gmail.com>
Date: Sun, 19 Dec 2021 10:01:28 -0600
Subject: [PATCH 15/42] Integrated Shifting WebPage and small testing fixes.

---
 CHANGELOG.md              | 7 +++++++
 data/settings.html        | 2 +-
 data/shift.html           | 2 +-
 src/HTTP_Server_Basic.cpp | 4 ++--
 src/Main.cpp              | 2 +-
 5 files changed, 12 insertions(+), 5 deletions(-)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 7cb76c65..a172f9ad 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -8,12 +8,19 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 
 ### Added
 - Added Webpage for Shifting.
+- Added /shift server on backend.
+- Split userConfig into userConfig and rtConfig.
+- Added ERG testing to btsimulator.html
+- Broke out ERG computation into it's own task.
 - Added image for wiki.
 
 ### Changed
 - Adjusted the order of "Submit" "Reboot" and "Reset to Defaults" on the settings page. 
 - Adjusted the setting webpage so "reset to defaults" is harder to accidentally press. 
 - Increased the amount of free stack by removing the default Arduino loop();
+- Updated /shift server on to rtConfig.
+- Fixed redeclaring global targetposition in moveStepper().
+- Renamed Settings page "Submit" button to "Save Setting"
 
 ### Hardware
 - Increased hex head and nut size to 13mm.
diff --git a/data/settings.html b/data/settings.html
index e8126007..01577682 100644
--- a/data/settings.html
+++ b/data/settings.html
@@ -140,7 +140,7 @@ <h2>
             </tr>
           </tbody>
         </table>
-        <input type="submit" value="Submit" />
+        <input type="submit" value="Save Settings" />
       </form>
       <p></p>
       <form action="/reboot.html">
diff --git a/data/shift.html b/data/shift.html
index b33cf77a..a44f0307 100644
--- a/data/shift.html
+++ b/data/shift.html
@@ -68,7 +68,7 @@ <h1>Shift</h1>
                 }
             }
         };
-        xhttp.open("GET", "/configJSON", true);
+        xhttp.open("GET", "/runtimeConfigJSON", true);
         xhttp.send();
     }
 
diff --git a/src/HTTP_Server_Basic.cpp b/src/HTTP_Server_Basic.cpp
index 8e6b0b7a..fd1a6a47 100644
--- a/src/HTTP_Server_Basic.cpp
+++ b/src/HTTP_Server_Basic.cpp
@@ -253,11 +253,11 @@ void startHttpServer() {
   server.on("/shift", []() {
     int value = server.arg("value").toInt();
     if ((value > -10) && (value < 10)) {
-      userConfig.setShifterPosition(userConfig.getShifterPosition() + value);
+      rtConfig.setShifterPosition(rtConfig.getShifterPosition() + value);
       server.send(200, "text/plain", "OK");
       SS2K_LOG(HTTP_SERVER_LOG_TAG, "Shift From HTML");
     } else {
-      userConfig.setShifterPosition(value);
+      rtConfig.setShifterPosition(value);
       SS2K_LOG(HTTP_SERVER_LOG_TAG, "Invalid HTML Shift");
       server.send(200, "text/plain", "OK");
     }
diff --git a/src/Main.cpp b/src/Main.cpp
index 091cb343..d87a6770 100644
--- a/src/Main.cpp
+++ b/src/Main.cpp
@@ -200,7 +200,7 @@ void moveStepper(void *pvParameters) {
 
   while (1) {
     if (stepper) {
-      int targetPosition = rtConfig.getShifterPosition() * userConfig.getShiftStep();
+      targetPosition = rtConfig.getShifterPosition() * userConfig.getShiftStep();
       if (!externalControl) {
         if (rtConfig.getERGMode()) {
           // ERG Mode

From 77ca1c4cf43824551541c457323bf22e840a4a3f Mon Sep 17 00:00:00 2001
From: doudar <anthonydoud97@gmail.com>
Date: Sun, 19 Dec 2021 10:17:17 -0600
Subject: [PATCH 16/42] Inserted Licence for pre-commit checks

---
 include/ERG_Mode.h | 7 +++++++
 src/ERG_Mode.cpp   | 7 +++++++
 2 files changed, 14 insertions(+)

diff --git a/include/ERG_Mode.h b/include/ERG_Mode.h
index 608d2fca..f8fc2ac1 100644
--- a/include/ERG_Mode.h
+++ b/include/ERG_Mode.h
@@ -1,3 +1,10 @@
+/*
+ * Copyright (C) 2020  Anthony Doud & Joel Baranick
+ * All rights reserved
+ *
+ * SPDX-License-Identifier: GPL-2.0-only
+ */
+
 #pragma once
 
 #include <Arduino.h>
diff --git a/src/ERG_Mode.cpp b/src/ERG_Mode.cpp
index 20e09614..8366acd3 100644
--- a/src/ERG_Mode.cpp
+++ b/src/ERG_Mode.cpp
@@ -1,3 +1,10 @@
+/*
+ * Copyright (C) 2020  Anthony Doud & Joel Baranick
+ * All rights reserved
+ *
+ * SPDX-License-Identifier: GPL-2.0-only
+ */
+
 #include "ERG_Mode.h"
 #include "SS2KLog.h"
 #include "Main.h"

From 98b0c87d213b840391fe716763ffccd2b79bbce0 Mon Sep 17 00:00:00 2001
From: doudar <anthonydoud97@gmail.com>
Date: Sun, 19 Dec 2021 10:42:08 -0600
Subject: [PATCH 17/42] Created framework for a power table

---
 include/ERG_Mode.h | 15 +++++++++++++++
 src/ERG_Mode.cpp   |  5 +++++
 2 files changed, 20 insertions(+)

diff --git a/include/ERG_Mode.h b/include/ERG_Mode.h
index f8fc2ac1..06af728a 100644
--- a/include/ERG_Mode.h
+++ b/include/ERG_Mode.h
@@ -35,3 +35,18 @@ class ErgMode {
 
   bool _userIsSpinning(int cadence, float incline);
 };
+
+class PowerEntry {
+ public:
+  int watts;
+  float incline;
+  int cad;
+  long timestamp;
+
+  PowerEntry(int watts, float incline, int cad) {
+    this->power     = watts;
+    this->incline   = incline;
+    this->cad       = cad;
+    this->timestamp = millis();
+  }
+};
diff --git a/src/ERG_Mode.cpp b/src/ERG_Mode.cpp
index 8366acd3..c8a98bde 100644
--- a/src/ERG_Mode.cpp
+++ b/src/ERG_Mode.cpp
@@ -11,6 +11,11 @@
 
 TaskHandle_t ErgTask;
 
+//Create a power table representing 0w-1000w in 50w increments.
+//i.e. powerTable[1] corresponds to the incline required for 50w. powerTable[2] is the incline required for 100w and so on.
+
+PowerEntry ergPowerTable[20]; 
+
 void setupERG() {
   TaskHandle_t task_handle;
   SS2K_LOG(ERG_MODE_LOG_TAG, "Starting ERG Mode task...");

From 683cb04f2061eb8390e9fd38a35843ba2d5f836a Mon Sep 17 00:00:00 2001
From: doudar <anthonydoud97@gmail.com>
Date: Sun, 19 Dec 2021 11:14:43 -0600
Subject: [PATCH 18/42] Added comments on suggestions for ergPowerTable

---
 include/ERG_Mode.h |  2 ++
 src/ERG_Mode.cpp   | 31 ++++++++++++++++++++++++++++---
 2 files changed, 30 insertions(+), 3 deletions(-)

diff --git a/include/ERG_Mode.h b/include/ERG_Mode.h
index 06af728a..47366f23 100644
--- a/include/ERG_Mode.h
+++ b/include/ERG_Mode.h
@@ -41,12 +41,14 @@ class PowerEntry {
   int watts;
   float incline;
   int cad;
+  int readings;
   long timestamp;
 
   PowerEntry(int watts, float incline, int cad) {
     this->power     = watts;
     this->incline   = incline;
     this->cad       = cad;
+    this->readings  = readings;
     this->timestamp = millis();
   }
 };
diff --git a/src/ERG_Mode.cpp b/src/ERG_Mode.cpp
index c8a98bde..3ae8f96f 100644
--- a/src/ERG_Mode.cpp
+++ b/src/ERG_Mode.cpp
@@ -11,10 +11,10 @@
 
 TaskHandle_t ErgTask;
 
-//Create a power table representing 0w-1000w in 50w increments.
-//i.e. powerTable[1] corresponds to the incline required for 50w. powerTable[2] is the incline required for 100w and so on.
+// Create a power table representing 0w-1000w in 50w increments.
+// i.e. powerTable[1] corresponds to the incline required for 50w. powerTable[2] is the incline required for 100w and so on.
 
-PowerEntry ergPowerTable[20]; 
+PowerEntry ergPowerTable[20];
 
 void setupERG() {
   TaskHandle_t task_handle;
@@ -60,6 +60,31 @@ void ergTaskLoop(void* pvParameters) {
     //   int power     = rtConfig.getSimulatedWatts().value;
     //   SS2K_LOG(ERG_MODE_LOG_TAG, "Bike characteristics: INC: %f; CAD: %d; PWR: %d", incline, cadence, power);
     // }
+
+    /* Continuously build and update powertable here. Obviously this won't work (boot to boot at least) without some sort of homing routine to begin with. 
+      
+      Method should be if power has been held within 20w for ~3 seconds, update the corresponding ergPowerTable[x] entry by rounding the watts to the nearest 50:
+      
+      int x = rtConfig.getSimulatedWatts() + 50/2;
+       x -= x % 50; 
+       x = x/50; 
+       Then update the power table:
+      if (!ergPowerTable[x].watts){
+        ergPowerTable[x].watts = rtConfig.getSimulatedWatts();
+        ergPowerTable[x].cad = rtConfig.getSimulatedCad(); 
+        ergPowerTable[x].incline = rtConfig.getSimulatedIncline();
+        ergPowerTable[x].readings = 1;
+      } else{ //Average and update the readings.  Probably should throw out outliers here as well.  
+        ergPowerTable[x].watts = (rtConfig.getSimulatedWatts() + ergPowerTable[x].watts * ergPowerTable[x].readings)/ergPowerTable.readings[x]+1;
+        ergPowerTable[x].cad = (rtConfig.getSimulatedCad() + ERGPowerTable[x].cad * ergPowerTable[x].readings)/ergPowerTable.readings[x]+1; 
+        ergPowerTable[x].incline = rtConfig.getSimulatedIncline() + ERGPowerTable[x].incline *ERGPowerTable[x].readings)/ERGPowerTable.readings[x]+1; 
+        ergPowerTable[x].readings ++;
+        if(ergPowerTable[x].readings > 10){
+          ergPowerTable.readings = 10; //keep from diluting recent readings too far. 
+        }
+      }
+
+    */
   }
 }
 

From 32a2197897b91acf3c03e7cc4d09afd66f0fb73c Mon Sep 17 00:00:00 2001
From: doudar <anthonydoud97@gmail.com>
Date: Sun, 19 Dec 2021 11:38:45 -0600
Subject: [PATCH 19/42] Initialized PowerEntry with 0's

---
 include/ERG_Mode.h | 5 +++--
 src/ERG_Mode.cpp   | 5 ++---
 2 files changed, 5 insertions(+), 5 deletions(-)

diff --git a/include/ERG_Mode.h b/include/ERG_Mode.h
index 47366f23..40bbe036 100644
--- a/include/ERG_Mode.h
+++ b/include/ERG_Mode.h
@@ -44,11 +44,12 @@ class PowerEntry {
   int readings;
   long timestamp;
 
-  PowerEntry(int watts, float incline, int cad) {
-    this->power     = watts;
+  PowerEntry(int watts = 0, float incline = 0, int cad = 0, int readings = 0) {
+    this->watts     = watts;
     this->incline   = incline;
     this->cad       = cad;
     this->readings  = readings;
     this->timestamp = millis();
   }
 };
+
diff --git a/src/ERG_Mode.cpp b/src/ERG_Mode.cpp
index 3ae8f96f..88aceae8 100644
--- a/src/ERG_Mode.cpp
+++ b/src/ERG_Mode.cpp
@@ -10,12 +10,11 @@
 #include "Main.h"
 
 TaskHandle_t ErgTask;
+PowerEntry ergPowerTable[20];
 
 // Create a power table representing 0w-1000w in 50w increments.
 // i.e. powerTable[1] corresponds to the incline required for 50w. powerTable[2] is the incline required for 100w and so on.
 
-PowerEntry ergPowerTable[20];
-
 void setupERG() {
   TaskHandle_t task_handle;
   SS2K_LOG(ERG_MODE_LOG_TAG, "Starting ERG Mode task...");
@@ -69,7 +68,7 @@ void ergTaskLoop(void* pvParameters) {
        x -= x % 50; 
        x = x/50; 
        Then update the power table:
-      if (!ergPowerTable[x].watts){
+      if (ergPowerTable[x].readings = 0){
         ergPowerTable[x].watts = rtConfig.getSimulatedWatts();
         ergPowerTable[x].cad = rtConfig.getSimulatedCad(); 
         ergPowerTable[x].incline = rtConfig.getSimulatedIncline();

From 312ff574642f79c8e8ac135c6356f2260f6f26b7 Mon Sep 17 00:00:00 2001
From: doudar <anthonydoud97@gmail.com>
Date: Mon, 20 Dec 2021 09:47:31 -0600
Subject: [PATCH 20/42] Removed millis in powerEntry

---
 include/ERG_Mode.h | 14 ++++++--------
 1 file changed, 6 insertions(+), 8 deletions(-)

diff --git a/include/ERG_Mode.h b/include/ERG_Mode.h
index 40bbe036..1cac802d 100644
--- a/include/ERG_Mode.h
+++ b/include/ERG_Mode.h
@@ -42,14 +42,12 @@ class PowerEntry {
   float incline;
   int cad;
   int readings;
-  long timestamp;
-
-  PowerEntry(int watts = 0, float incline = 0, int cad = 0, int readings = 0) {
-    this->watts     = watts;
-    this->incline   = incline;
-    this->cad       = cad;
-    this->readings  = readings;
-    this->timestamp = millis();
+
+  PowerEntry() {
+    this->watts     = 0;
+    this->incline   = 0;
+    this->cad       = 0;
+    this->readings  = 0;
   }
 };
 

From bfc183fc932b9f8ca90eef3f683bc221ae331026 Mon Sep 17 00:00:00 2001
From: doudar <anthonydoud97@gmail.com>
Date: Mon, 20 Dec 2021 12:26:45 -0600
Subject: [PATCH 21/42] Expanded the PowerTable Class (untested)

---
 include/ERG_Mode.h | 17 ++++++++---
 src/ERG_Mode.cpp   | 75 +++++++++++++++++++++++++++++++---------------
 2 files changed, 64 insertions(+), 28 deletions(-)

diff --git a/include/ERG_Mode.h b/include/ERG_Mode.h
index 1cac802d..e35df220 100644
--- a/include/ERG_Mode.h
+++ b/include/ERG_Mode.h
@@ -44,10 +44,19 @@ class PowerEntry {
   int readings;
 
   PowerEntry() {
-    this->watts     = 0;
-    this->incline   = 0;
-    this->cad       = 0;
-    this->readings  = 0;
+    this->watts    = 0;
+    this->incline  = 0;
+    this->cad      = 0;
+    this->readings = 0;
   }
 };
 
+class PowerTable {
+ public:
+  PowerEntry powerEntry[20];
+
+
+  void newEntry(int watts, float incline, int cad);
+  // returns incline for wattTarget. Null if not found.
+  float lookup(int watts, int cad);
+};
diff --git a/src/ERG_Mode.cpp b/src/ERG_Mode.cpp
index 88aceae8..a4c6aa46 100644
--- a/src/ERG_Mode.cpp
+++ b/src/ERG_Mode.cpp
@@ -10,7 +10,7 @@
 #include "Main.h"
 
 TaskHandle_t ErgTask;
-PowerEntry ergPowerTable[20];
+PowerTable powerTable;
 
 // Create a power table representing 0w-1000w in 50w increments.
 // i.e. powerTable[1] corresponds to the incline required for 50w. powerTable[2] is the incline required for 100w and so on.
@@ -59,32 +59,59 @@ void ergTaskLoop(void* pvParameters) {
     //   int power     = rtConfig.getSimulatedWatts().value;
     //   SS2K_LOG(ERG_MODE_LOG_TAG, "Bike characteristics: INC: %f; CAD: %d; PWR: %d", incline, cadence, power);
     // }
+  }
+}
 
-    /* Continuously build and update powertable here. Obviously this won't work (boot to boot at least) without some sort of homing routine to begin with. 
-      
-      Method should be if power has been held within 20w for ~3 seconds, update the corresponding ergPowerTable[x] entry by rounding the watts to the nearest 50:
-      
-      int x = rtConfig.getSimulatedWatts() + 50/2;
-       x -= x % 50; 
-       x = x/50; 
-       Then update the power table:
-      if (ergPowerTable[x].readings = 0){
-        ergPowerTable[x].watts = rtConfig.getSimulatedWatts();
-        ergPowerTable[x].cad = rtConfig.getSimulatedCad(); 
-        ergPowerTable[x].incline = rtConfig.getSimulatedIncline();
-        ergPowerTable[x].readings = 1;
-      } else{ //Average and update the readings.  Probably should throw out outliers here as well.  
-        ergPowerTable[x].watts = (rtConfig.getSimulatedWatts() + ergPowerTable[x].watts * ergPowerTable[x].readings)/ergPowerTable.readings[x]+1;
-        ergPowerTable[x].cad = (rtConfig.getSimulatedCad() + ERGPowerTable[x].cad * ergPowerTable[x].readings)/ergPowerTable.readings[x]+1; 
-        ergPowerTable[x].incline = rtConfig.getSimulatedIncline() + ERGPowerTable[x].incline *ERGPowerTable[x].readings)/ERGPowerTable.readings[x]+1; 
-        ergPowerTable[x].readings ++;
-        if(ergPowerTable[x].readings > 10){
-          ergPowerTable.readings = 10; //keep from diluting recent readings too far. 
-        }
-      }
+  // Accepts new data into the table and averages input by number of readings in the power entry.
+void PowerTable::newEntry(int watts, float incline, int cad) {
+  int i = round(watts / 50.0);
+  if (this->powerEntry[i].readings = 0) {
+    this->powerEntry[i].watts    = watts;
+    this->powerEntry[i].cad      = cad;
+    this->powerEntry[i].incline  = incline;
+    this->powerEntry[i].readings = 1;
+  } else {  // Average and update the readings.  Probably should throw out outliers here as well.
+    this->powerEntry[i].watts   = (watts + this->powerEntry[i].watts * this->powerEntry[i].readings) / this->powerEntry[i].readings + 1;
+    this->powerEntry[i].cad     = (cad + this->powerEntry[i].cad * this->powerEntry[i].readings) / this->powerEntry[i].readings + 1;
+    this->powerEntry[i].incline = (incline + this->powerEntry[i].incline * this->powerEntry[i].readings) / this->powerEntry[i].readings + 1;
+    this->powerEntry[i].readings++;
+    if (this->powerEntry[i].readings > 10) {
+      this->powerEntry[i].readings = 10;  // keep from diluting recent readings too far.
+    }
+  }
+}
 
-    */
+// looks up an incline for the requested power and cadence and interpolates the result. 
+float PowerTable::lookup(int watts, int cad) {
+  struct entry {
+    float power;
+    float incline;
+  };
+  int i       = round(watts / 50.0); //find the closest entry
+  float scale = watts / 50.0 - i;
+  entry above;
+  entry below;
+  if (scale > 0) { //pick the element above closest entry for interpolation
+    below.power   = this->powerEntry[i].watts;
+    below.incline = this->powerEntry[i].incline;
+    above.power   = this->powerEntry[i+1].watts;
+    above.incline = this->powerEntry[i+1].incline;
+  } else if (scale < 0) { //pick the element below the closest entry for interpolation
+    below.power   = this->powerEntry[i-1].watts;
+    below.incline = this->powerEntry[i-1].incline;
+    above.power   = this->powerEntry[i].watts;
+    above.incline = this->powerEntry[i].incline;
+  } else {
+    // Compute cad offset and return without interpolation
+    //
+    // Cad offset TODO HERE
+    //
+    return this->powerEntry->incline;
   }
+
+  float rIncline = below.incline + ((watts - below.power) / (above.power - below.power)) * (above.incline - below.incline);
+
+  return rIncline;
 }
 
 // as a note, Trainer Road sends 50w target whenever the app is connected.

From 0a83fc9a64c1a0ee0060f11587505f5473589ff1 Mon Sep 17 00:00:00 2001
From: doudar <anthonydoud97@gmail.com>
Date: Mon, 20 Dec 2021 13:47:00 -0600
Subject: [PATCH 22/42] Added more iteration to powertable lookup

---
 include/ERG_Mode.h |  4 ++--
 include/settings.h |  6 ++++++
 src/ERG_Mode.cpp   | 41 ++++++++++++++++++++++++++++++-----------
 3 files changed, 38 insertions(+), 13 deletions(-)

diff --git a/include/ERG_Mode.h b/include/ERG_Mode.h
index e35df220..1c4a8ad8 100644
--- a/include/ERG_Mode.h
+++ b/include/ERG_Mode.h
@@ -9,6 +9,7 @@
 
 #include <Arduino.h>
 #include "SmartSpin_parameters.h"
+#include "settings.h"
 
 #define ERG_MODE_LOG_TAG     "ERG_Mode"
 #define ERG_MODE_LOG_CSV_TAG "ERG_Mode_CSV"
@@ -53,8 +54,7 @@ class PowerEntry {
 
 class PowerTable {
  public:
-  PowerEntry powerEntry[20];
-
+  PowerEntry powerEntry[POWERTABLE_SIZE];
 
   void newEntry(int watts, float incline, int cad);
   // returns incline for wattTarget. Null if not found.
diff --git a/include/settings.h b/include/settings.h
index e43b4db9..50c31c17 100644
--- a/include/settings.h
+++ b/include/settings.h
@@ -156,6 +156,12 @@
 
 #define RUNTIMECONFIG_JSON_SIZE 512
 
+// Number of entries in the ERG Power Lookup Table
+#define POWERTABLE_SIZE 20
+
+// Size of increments (in watts) for the ERG Lookup Table. Needs to be one decimal place for proper calculations i.e. 50.0
+#define POWERTABLE_INCREMENT 50.0
+
 // Uncomment to enable sending Telegram debug messages back to the chat
 // specified in telegram_token.h
 // #define USE_TELEGRAM
diff --git a/src/ERG_Mode.cpp b/src/ERG_Mode.cpp
index a4c6aa46..afdb0e3a 100644
--- a/src/ERG_Mode.cpp
+++ b/src/ERG_Mode.cpp
@@ -62,9 +62,9 @@ void ergTaskLoop(void* pvParameters) {
   }
 }
 
-  // Accepts new data into the table and averages input by number of readings in the power entry.
+// Accepts new data into the table and averages input by number of readings in the power entry.
 void PowerTable::newEntry(int watts, float incline, int cad) {
-  int i = round(watts / 50.0);
+  int i = round(watts / POWERTABLE_INCREMENT);
   if (this->powerEntry[i].readings = 0) {
     this->powerEntry[i].watts    = watts;
     this->powerEntry[i].cad      = cad;
@@ -81,26 +81,44 @@ void PowerTable::newEntry(int watts, float incline, int cad) {
   }
 }
 
-// looks up an incline for the requested power and cadence and interpolates the result. 
+// looks up an incline for the requested power and cadence and interpolates the result.
+// Returns NULL of no entry matched.
 float PowerTable::lookup(int watts, int cad) {
   struct entry {
     float power;
     float incline;
   };
-  int i       = round(watts / 50.0); //find the closest entry
-  float scale = watts / 50.0 - i;
+  int i       = round(watts / POWERTABLE_INCREMENT);  // find the closest entry
+  float scale = watts / POWERTABLE_INCREMENT - i;
   entry above;
   entry below;
-  if (scale > 0) { //pick the element above closest entry for interpolation
+  above.power = NULL;
+  below.power = NULL;
+  if (this->powerEntry[i].readings = 0) {
+    return NULL;
+  }
+  //TODO Needs to check elements in the opposite order if not found in the suggested order. 
+
+  if (scale > 0) {  // select the element above closest entry for interpolation
     below.power   = this->powerEntry[i].watts;
     below.incline = this->powerEntry[i].incline;
-    above.power   = this->powerEntry[i+1].watts;
-    above.incline = this->powerEntry[i+1].incline;
-  } else if (scale < 0) { //pick the element below the closest entry for interpolation
-    below.power   = this->powerEntry[i-1].watts;
-    below.incline = this->powerEntry[i-1].incline;
+    for (int nextIndex = i + 1; nextIndex <= POWERTABLE_SIZE; nextIndex++) {
+      if (this->powerEntry[nextIndex].readings > 0) {
+        above.power   = this->powerEntry[nextIndex].watts;
+        above.incline = this->powerEntry[nextIndex].incline;
+        break;
+      }
+    }
+  } else if (scale < 0) {  // select the element below the closest entry for interpolation
     above.power   = this->powerEntry[i].watts;
     above.incline = this->powerEntry[i].incline;
+    for (int previousIndex = i - 1; previousIndex >= 0; previousIndex--) {
+      if (this->powerEntry[previousIndex].readings > 0) {
+        below.power   = this->powerEntry[previousIndex].watts;
+        below.incline = this->powerEntry[previousIndex].incline;
+        break;
+      }
+    }
   } else {
     // Compute cad offset and return without interpolation
     //
@@ -109,6 +127,7 @@ float PowerTable::lookup(int watts, int cad) {
     return this->powerEntry->incline;
   }
 
+//actual interpolation
   float rIncline = below.incline + ((watts - below.power) / (above.power - below.power)) * (above.incline - below.incline);
 
   return rIncline;

From 32be88ead9641da04e7014d8941a74f64e4e1837 Mon Sep 17 00:00:00 2001
From: doudar <anthonydoud97@gmail.com>
Date: Mon, 20 Dec 2021 22:37:43 -0600
Subject: [PATCH 23/42] PowerTable::lookup complete. Not tested.

---
 src/ERG_Mode.cpp | 124 +++++++++++++++++++++++++++++++++++------------
 1 file changed, 94 insertions(+), 30 deletions(-)

diff --git a/src/ERG_Mode.cpp b/src/ERG_Mode.cpp
index afdb0e3a..949270a0 100644
--- a/src/ERG_Mode.cpp
+++ b/src/ERG_Mode.cpp
@@ -82,52 +82,116 @@ void PowerTable::newEntry(int watts, float incline, int cad) {
 }
 
 // looks up an incline for the requested power and cadence and interpolates the result.
-// Returns NULL of no entry matched.
+// Returns -99 if no entry matched.
 float PowerTable::lookup(int watts, int cad) {
+#define RETURN_ERROR -99
   struct entry {
     float power;
     float incline;
+    float cad;
   };
-  int i       = round(watts / POWERTABLE_INCREMENT);  // find the closest entry
-  float scale = watts / POWERTABLE_INCREMENT - i;
+  int i         = round(watts / POWERTABLE_INCREMENT);  // find the closest entry
+  float scale   = watts / POWERTABLE_INCREMENT - i;     // Should we look at the next higher or next lower index for comparison?
+  int indexPair = -1;                                   // The next closes index with data for interpolation
   entry above;
   entry below;
-  above.power = NULL;
-  below.power = NULL;
-  if (this->powerEntry[i].readings = 0) {
-    return NULL;
+  above.power = 0;
+  below.power = 0;
+
+  if (this->powerEntry[i].readings = 0) {  // If matching entry is empty, find the next closest index with data
+    for (int x = 1; x <= POWERTABLE_SIZE; x++) {
+      if (i + x <= POWERTABLE_SIZE) {
+        if (this->powerEntry[i + x].readings > 0) {
+          i += x;
+          break;
+        }
+      }
+      if (i - x >= 0) {
+        if (this->powerEntry[i - x].readings > 0) {
+          i -= x;
+          break;
+        }
+      }
+      if ((i - x <= 0) || (i + x >= POWERTABLE_SIZE)) {
+        SS2K_LOG(ERG_MODE_LOG_TAG, "No data found in powertable.");
+        return RETURN_ERROR;
+      }
+    }
   }
-  //TODO Needs to check elements in the opposite order if not found in the suggested order. 
-
-  if (scale > 0) {  // select the element above closest entry for interpolation
-    below.power   = this->powerEntry[i].watts;
-    below.incline = this->powerEntry[i].incline;
-    for (int nextIndex = i + 1; nextIndex <= POWERTABLE_SIZE; nextIndex++) {
-      if (this->powerEntry[nextIndex].readings > 0) {
-        above.power   = this->powerEntry[nextIndex].watts;
-        above.incline = this->powerEntry[nextIndex].incline;
+  if (scale > 0) {  // select the paired element (preferably) above the entry for interpolation
+    for (int x = 1; x <= POWERTABLE_SIZE; x++) {
+      if (i + x <= POWERTABLE_SIZE) {
+        if (this->powerEntry[i + x].readings > 0) {
+          indexPair = i + x;
+          break;
+        }
+      }
+      if (i - x >= 0) {
+        if (this->powerEntry[i - x].readings > 0) {
+          indexPair = i - x;
+          break;
+        }
+      }
+      if ((i - x <= 0) || (i + x >= POWERTABLE_SIZE)) {
+        SS2K_LOG(ERG_MODE_LOG_TAG, "No pair found in powertable.");
+        indexPair = -1;
         break;
       }
     }
-  } else if (scale < 0) {  // select the element below the closest entry for interpolation
-    above.power   = this->powerEntry[i].watts;
-    above.incline = this->powerEntry[i].incline;
-    for (int previousIndex = i - 1; previousIndex >= 0; previousIndex--) {
-      if (this->powerEntry[previousIndex].readings > 0) {
-        below.power   = this->powerEntry[previousIndex].watts;
-        below.incline = this->powerEntry[previousIndex].incline;
+  } else if (scale < 0) {  // select the paired element (preferably) below the entry for interpolation
+    for (int x = 1; x <= POWERTABLE_SIZE; x++) {
+      if (i - x <= POWERTABLE_SIZE) {
+        if (this->powerEntry[i + x].readings > 0) {
+          indexPair = i - x;
+          break;
+        }
+      }
+      if (i + x >= 0) {
+        if (this->powerEntry[i - x].readings > 0) {
+          indexPair = i + x;
+          break;
+        }
+      }
+      if ((i - x <= 0) || (i + x >= POWERTABLE_SIZE)) {
+        SS2K_LOG(ERG_MODE_LOG_TAG, "No pair found in powertable.");
+        indexPair = -1;
         break;
       }
     }
-  } else {
-    // Compute cad offset and return without interpolation
-    //
-    // Cad offset TODO HERE
-    //
-    return this->powerEntry->incline;
   }
 
-//actual interpolation
+  if (indexPair != -1) {
+    if (i > indexPair) {
+      below.power   = this->powerEntry[indexPair].watts;
+      below.incline = this->powerEntry[indexPair].incline;
+      below.cad     = this->powerEntry[indexPair].cad;
+      above.power   = this->powerEntry[i].watts;
+      above.incline = this->powerEntry[i].incline;
+      above.cad     = this->powerEntry[i].cad;
+    } else if (i < indexPair) {
+      below.power   = this->powerEntry[i].watts;
+      below.incline = this->powerEntry[i].incline;
+      below.cad     = this->powerEntry[i].cad;
+      above.power   = this->powerEntry[indexPair].watts;
+      above.incline = this->powerEntry[indexPair].incline;
+      above.cad     = this->powerEntry[indexPair].cad;
+    }
+  } else {  // no interpolation and so no CAD correction
+    return (this->powerEntry[i].incline);
+  }
+
+  // @MarkusSchneider's data shows a linear relationship between CAD and Watts for a given resistance level.
+  // It looks like for every 20 CAD increase there is ~50w increase in power. This may need to be adjusted later
+  // as higher resistance levels have a steeper slope (bigger increase in power/cad) than low resistance levels.
+  float averageCAD = (below.cad - above.cad) / 2;
+  float deltaCAD   = abs(averageCAD - cad);
+  if (cad > averageCAD) {  // cad is higher than the table so we need to target a lower wattage (and incline)
+    watts -= (deltaCAD / 20) * 50;
+  }
+  if (cad < averageCAD) {  // cad is lower than the table so we need to target a higher wattage (and incline)
+    watts += (deltaCAD / 20) * 50;
+  }
+  // actual interpolation
   float rIncline = below.incline + ((watts - below.power) / (above.power - below.power)) * (above.incline - below.incline);
 
   return rIncline;

From d165d68b139c0593e083317da292b11a9c8b598f Mon Sep 17 00:00:00 2001
From: doudar <anthonydoud97@gmail.com>
Date: Tue, 21 Dec 2021 14:39:30 -0600
Subject: [PATCH 24/42] 1st Tests.

Switched PowerTable to using Stepper Position vs incline- That way it can record in all modes.

Initial debugging and testing of table write functions - seems to be working!
---
 data/btsimulator.html |   3 -
 include/ERG_Mode.h    |  26 ++++++--
 include/Main.h        |  17 ++++-
 include/settings.h    |   5 +-
 src/BLE_Client.cpp    |   2 +-
 src/BLE_Server.cpp    |  18 +++---
 src/ERG_Mode.cpp      | 142 ++++++++++++++++++++++++++++++++++--------
 src/Main.cpp          |  28 ++++-----
 8 files changed, 180 insertions(+), 61 deletions(-)

diff --git a/data/btsimulator.html b/data/btsimulator.html
index 674d0382..5dbba903 100644
--- a/data/btsimulator.html
+++ b/data/btsimulator.html
@@ -98,9 +98,6 @@ <h2>ERG Target Watts</h2>
     if (element.checked) {
       document.getElementById("wattsSlider").hidden = false;
       document.getElementById("wattsValue").hidden = false;
-      pwrTimer = setInterval(() => {
-        updateWattsSlider();
-      }, 1000)
     }
     else {
       document.getElementById("wattsSlider").hidden = true;
diff --git a/include/ERG_Mode.h b/include/ERG_Mode.h
index 1c4a8ad8..56b90ca2 100644
--- a/include/ERG_Mode.h
+++ b/include/ERG_Mode.h
@@ -13,7 +13,8 @@
 
 #define ERG_MODE_LOG_TAG     "ERG_Mode"
 #define ERG_MODE_LOG_CSV_TAG "ERG_Mode_CSV"
-#define ERG_MODE_DELAY       250
+#define POWERTABLE_LOG_TAG   "PowTab"
+#define ERG_MODE_DELAY       2000
 
 extern TaskHandle_t ErgTask;
 void setupERG();
@@ -40,23 +41,38 @@ class ErgMode {
 class PowerEntry {
  public:
   int watts;
-  float incline;
+  int32_t targetPosition;
   int cad;
   int readings;
 
   PowerEntry() {
     this->watts    = 0;
-    this->incline  = 0;
+    this->targetPosition = 0;
     this->cad      = 0;
     this->readings = 0;
   }
 };
 
+class PowerBuffer {
+  public:
+  PowerEntry powerEntry[POWER_SAMPLES];
+  void set(int);
+  void reset();
+};
+
 class PowerTable {
  public:
   PowerEntry powerEntry[POWERTABLE_SIZE];
-
-  void newEntry(int watts, float incline, int cad);
+  // Catalogs a new entry into the power table.
+  void newEntry(PowerBuffer powerBuffer);
   // returns incline for wattTarget. Null if not found.
   float lookup(int watts, int cad);
+  // load power table from spiffs
+  bool load();
+  // save powertable from spiffs
+  bool save();
+  // Display power table in log
+  void toLog();
 };
+
+
diff --git a/include/Main.h b/include/Main.h
index 1e20e616..d1ab833b 100644
--- a/include/Main.h
+++ b/include/Main.h
@@ -33,10 +33,21 @@ void stopTasks();
 // Main program variable that stores most everything
 extern userParameters userConfig;
 extern RuntimeParameters rtConfig;
-extern int32_t targetPosition;
-extern bool externalControl;
-extern bool syncMode;
+
+class SS2K {
+ public:
+  int32_t targetPosition;
+  bool externalControl;
+  bool syncMode;
+
+  SS2K() {
+    targetPosition  = 0;
+    externalControl = false;
+    syncMode        = false;
+  }
+};
 
 // Users Physical Working Capacity Calculation Parameters (heartrate to Power
 // calculation)
 extern physicalWorkingCapacity userPWC;
+extern SS2K ss2k;
diff --git a/include/settings.h b/include/settings.h
index 50c31c17..a007d57f 100644
--- a/include/settings.h
+++ b/include/settings.h
@@ -122,7 +122,7 @@
 #define MAX_SCAN_RETRIES 1
 
 // loop speed for the SmartSpin2k BLE communications
-#define BLE_NOTIFY_DELAY 250
+#define BLE_NOTIFY_DELAY 700
 
 // loop speed for the SmartSpin2k BLE Client reconnect
 #define BLE_CLIENT_DELAY 1000
@@ -162,6 +162,9 @@
 // Size of increments (in watts) for the ERG Lookup Table. Needs to be one decimal place for proper calculations i.e. 50.0
 #define POWERTABLE_INCREMENT 50.0
 
+//Number of similar power samples to take before writing to the Power Table
+#define POWER_SAMPLES 3 
+
 // Uncomment to enable sending Telegram debug messages back to the chat
 // specified in telegram_token.h
 // #define USE_TELEGRAM
diff --git a/src/BLE_Client.cpp b/src/BLE_Client.cpp
index 751a4155..9a1e8c18 100644
--- a/src/BLE_Client.cpp
+++ b/src/BLE_Client.cpp
@@ -211,7 +211,7 @@ bool SpinBLEClient::connectToServer() {
   pClient->setConnectTimeout(5);
   pClient->connect(myDevice->getAddress());  // if you pass BLEAdvertisedDevice instead of address, it will be recognized type of peer device address (public or private)
   SS2K_LOG(BLE_CLIENT_LOG_TAG, " - Connected to server");
-  SS2K_LOG(BLE_CLIENT_LOG_TAG, " - RSSI %d", pClient->getRssi());
+  //SS2K_LOG(BLE_CLIENT_LOG_TAG, " - RSSI %d", pClient->getRssi());
   // Obtain a reference to the service we are after in the remote BLE server.
   BLERemoteService *pRemoteService = pClient->getService(serviceUUID);
   if (pRemoteService == nullptr) {
diff --git a/src/BLE_Server.cpp b/src/BLE_Server.cpp
index 974505d0..f74b2abe 100644
--- a/src/BLE_Server.cpp
+++ b/src/BLE_Server.cpp
@@ -773,36 +773,36 @@ void ss2kCustomCharacteristicCallbacks::onWrite(BLECharacteristic *pCharacterist
     case BLE_targetPosition:  // 0x19
       returnValue[0] = success;
       if (rxValue[0] == read) {
-        returnValue[2] = (uint8_t)(targetPosition & 0xff);
-        returnValue[3] = (uint8_t)(targetPosition >> 8);
-        returnValue[4] = (uint8_t)(targetPosition >> 16);
-        returnValue[5] = (uint8_t)(targetPosition >> 24);
+        returnValue[2] = (uint8_t)(ss2k.targetPosition & 0xff);
+        returnValue[3] = (uint8_t)(ss2k.targetPosition >> 8);
+        returnValue[4] = (uint8_t)(ss2k.targetPosition >> 16);
+        returnValue[5] = (uint8_t)(ss2k.targetPosition >> 24);
         returnLength += 4;
       }
       if (rxValue[0] == write) {
-        targetPosition = (int32_t((uint8_t)(rxValue[2]) << 0 | (uint8_t)(rxValue[3]) << 8 | (uint8_t)(rxValue[4]) << 16 | (uint8_t)(rxValue[5]) << 24));
+        ss2k.targetPosition = (int32_t((uint8_t)(rxValue[2]) << 0 | (uint8_t)(rxValue[3]) << 8 | (uint8_t)(rxValue[4]) << 16 | (uint8_t)(rxValue[5]) << 24));
       }
       break;
 
     case BLE_externalControl:  // 0x1A
       returnValue[0] = success;
       if (rxValue[0] == read) {
-        returnValue[2] = (uint8_t)(externalControl);
+        returnValue[2] = (uint8_t)(ss2k.externalControl);
         returnLength += 1;
       }
       if (rxValue[0] == write) {
-        externalControl = static_cast<bool>(rxValue[2]);
+        ss2k.externalControl = static_cast<bool>(rxValue[2]);
       }
       break;
 
     case BLE_syncMode:  // 0x1B
       returnValue[0] = success;
       if (rxValue[0] == read) {
-        returnValue[2] = (uint8_t)(syncMode);
+        returnValue[2] = (uint8_t)(ss2k.syncMode);
         returnLength += 1;
       }
       if (rxValue[0] == write) {
-        syncMode = static_cast<bool>(rxValue[2]);
+        ss2k.syncMode = static_cast<bool>(rxValue[2]);
       }
       break;
   }
diff --git a/src/ERG_Mode.cpp b/src/ERG_Mode.cpp
index 949270a0..8e0eca88 100644
--- a/src/ERG_Mode.cpp
+++ b/src/ERG_Mode.cpp
@@ -20,7 +20,7 @@ void setupERG() {
   SS2K_LOG(ERG_MODE_LOG_TAG, "Starting ERG Mode task...");
   xTaskCreatePinnedToCore(ergTaskLoop,   /* Task function. */
                           "ERGModeTask", /* name of task. */
-                          2500,          /* Stack size of task*/
+                          6500,          /* Stack size of task*/
                           NULL,          /* parameter of the task */
                           1,             /* priority of the task*/
                           &ErgTask,      /* Task handle to keep track of created task */
@@ -31,20 +31,50 @@ void setupERG() {
 
 void ergTaskLoop(void* pvParameters) {
   ErgMode ergMode;
+  PowerBuffer powerBuffer;
+
   ergMode._writeLogHeader();
+  int newSetPoint             = rtConfig.getTargetWatts();
+  bool isInErgMode            = rtConfig.getERGMode();
+  bool hasConnectedPowerMeter = spinBLEClient.connectedPM;
+  bool simulationRunning      = rtConfig.getSimulateTargetWatts();
   while (true) {
     vTaskDelay(ERG_MODE_DELAY / portTICK_PERIOD_MS);
+    newSetPoint            = rtConfig.getTargetWatts();
+    isInErgMode            = rtConfig.getERGMode();
+    hasConnectedPowerMeter = spinBLEClient.connectedPM;
+    simulationRunning      = rtConfig.getSimulateTargetWatts();
 
-    int newSetPoint             = rtConfig.getTargetWatts();
-    bool isInErgMode            = rtConfig.getERGMode();
-    bool hasConnectedPowerMeter = spinBLEClient.connectedPM;
-    bool simulationRunning      = rtConfig.getSimulateTargetWatts();
+    if ((rtConfig.getSimulatedCad() > 50) && (rtConfig.getSimulatedWatts().value > 10) && (rtConfig.getSimulatedWatts().value<POWERTABLE_SIZE*POWERTABLE_INCREMENT)) {
+      if (powerBuffer.powerEntry[0].readings == 0) {
+        powerBuffer.set(0);
+        SS2K_LOG(ERG_MODE_LOG_TAG, "powerBuffer.set(0);");
+      } else if (abs(powerBuffer.powerEntry[0].watts - rtConfig.getSimulatedWatts().value) < POWERTABLE_INCREMENT) {
+        for (int i = 1; i < POWER_SAMPLES; i++) {
+          if (powerBuffer.powerEntry[i].readings == 0) {
+            powerBuffer.set(i);
+            SS2K_LOG(ERG_MODE_LOG_TAG, "powerBuffer.set(%d);", i);
+            break;
+          }
+        }
+        if (powerBuffer.powerEntry[POWER_SAMPLES - 1].readings == 1) {
+          SS2K_LOG(ERG_MODE_LOG_TAG, "powerTable.newEntry(powerBuffer);");
+          powerTable.newEntry(powerBuffer);
+          powerBuffer.reset();
+        }
+      } else {
+        powerBuffer.reset();
+        SS2K_LOG(ERG_MODE_LOG_TAG, "powerBuffer.reset();");
+      }
+    }
 
+    powerTable.toLog();
     if (isInErgMode && (hasConnectedPowerMeter || simulationRunning)) {
       ergMode.computErg(newSetPoint);
     } else {
       if (newSetPoint > 0) {
         SS2K_LOG(ERG_MODE_LOG_TAG, "ERG request but ERG off or no connected PM");
+        rtConfig.setERGMode(false);
       }
     }
 
@@ -62,18 +92,46 @@ void ergTaskLoop(void* pvParameters) {
   }
 }
 
+void PowerBuffer::set(int i) {
+  this->powerEntry[i].readings = 1;
+  this->powerEntry[i].watts    = rtConfig.getSimulatedWatts().value;
+  this->powerEntry[i].cad      = rtConfig.getSimulatedCad();
+  this->powerEntry[i].targetPosition  = ss2k.targetPosition;
+}
+
+void PowerBuffer::reset() {
+  for (int i = 0; i < POWER_SAMPLES; i++) {
+    this->powerEntry[i].readings = 0;
+    this->powerEntry[i].watts    = 0;
+    this->powerEntry[i].cad      = 0;
+    this->powerEntry[i].targetPosition  = 0;
+  }
+}
+
 // Accepts new data into the table and averages input by number of readings in the power entry.
-void PowerTable::newEntry(int watts, float incline, int cad) {
+void PowerTable::newEntry(PowerBuffer powerBuffer) {
+  float tWatts, tCad = 0;
+  int32_t tTargetPosition = 0;
+
+  for (int i = 0; i < POWER_SAMPLES; i++) {
+    tWatts += powerBuffer.powerEntry[i].watts;
+    tTargetPosition += powerBuffer.powerEntry[i].targetPosition;
+    tCad += powerBuffer.powerEntry[i].cad;
+  }
+  int watts     = tWatts / POWER_SAMPLES;
+  int cad       = tCad / POWER_SAMPLES;
+  int32_t targetPosition = tTargetPosition / POWER_SAMPLES;
+
   int i = round(watts / POWERTABLE_INCREMENT);
-  if (this->powerEntry[i].readings = 0) {
+  if (this->powerEntry[i].readings == 0) {
     this->powerEntry[i].watts    = watts;
     this->powerEntry[i].cad      = cad;
-    this->powerEntry[i].incline  = incline;
+    this->powerEntry[i].targetPosition  = targetPosition;
     this->powerEntry[i].readings = 1;
-  } else {  // Average and update the readings.  Probably should throw out outliers here as well.
-    this->powerEntry[i].watts   = (watts + this->powerEntry[i].watts * this->powerEntry[i].readings) / this->powerEntry[i].readings + 1;
-    this->powerEntry[i].cad     = (cad + this->powerEntry[i].cad * this->powerEntry[i].readings) / this->powerEntry[i].readings + 1;
-    this->powerEntry[i].incline = (incline + this->powerEntry[i].incline * this->powerEntry[i].readings) / this->powerEntry[i].readings + 1;
+  } else {  // Average and update the readings. 
+    this->powerEntry[i].watts   = (watts + (this->powerEntry[i].watts * this->powerEntry[i].readings)) / (this->powerEntry[i].readings + 1);
+    this->powerEntry[i].cad     = (cad + (this->powerEntry[i].cad * this->powerEntry[i].readings)) / (this->powerEntry[i].readings + 1);
+    this->powerEntry[i].targetPosition = (targetPosition + (this->powerEntry[i].targetPosition * this->powerEntry[i].readings)) / (this->powerEntry[i].readings + 1);
     this->powerEntry[i].readings++;
     if (this->powerEntry[i].readings > 10) {
       this->powerEntry[i].readings = 10;  // keep from diluting recent readings too far.
@@ -98,9 +156,9 @@ float PowerTable::lookup(int watts, int cad) {
   above.power = 0;
   below.power = 0;
 
-  if (this->powerEntry[i].readings = 0) {  // If matching entry is empty, find the next closest index with data
-    for (int x = 1; x <= POWERTABLE_SIZE; x++) {
-      if (i + x <= POWERTABLE_SIZE) {
+  if (this->powerEntry[i].readings == 0) {  // If matching entry is empty, find the next closest index with data
+    for (int x = 1; x < POWERTABLE_SIZE; x++) {
+      if (i + x < POWERTABLE_SIZE) {
         if (this->powerEntry[i + x].readings > 0) {
           i += x;
           break;
@@ -119,8 +177,8 @@ float PowerTable::lookup(int watts, int cad) {
     }
   }
   if (scale > 0) {  // select the paired element (preferably) above the entry for interpolation
-    for (int x = 1; x <= POWERTABLE_SIZE; x++) {
-      if (i + x <= POWERTABLE_SIZE) {
+    for (int x = 1; x < POWERTABLE_SIZE; x++) {
+      if (i + x < POWERTABLE_SIZE) {
         if (this->powerEntry[i + x].readings > 0) {
           indexPair = i + x;
           break;
@@ -139,8 +197,8 @@ float PowerTable::lookup(int watts, int cad) {
       }
     }
   } else if (scale < 0) {  // select the paired element (preferably) below the entry for interpolation
-    for (int x = 1; x <= POWERTABLE_SIZE; x++) {
-      if (i - x <= POWERTABLE_SIZE) {
+    for (int x = 1; x < POWERTABLE_SIZE; x++) {
+      if (i - x < POWERTABLE_SIZE) {
         if (this->powerEntry[i + x].readings > 0) {
           indexPair = i - x;
           break;
@@ -163,21 +221,21 @@ float PowerTable::lookup(int watts, int cad) {
   if (indexPair != -1) {
     if (i > indexPair) {
       below.power   = this->powerEntry[indexPair].watts;
-      below.incline = this->powerEntry[indexPair].incline;
+      below.incline = this->powerEntry[indexPair].targetPosition;
       below.cad     = this->powerEntry[indexPair].cad;
       above.power   = this->powerEntry[i].watts;
-      above.incline = this->powerEntry[i].incline;
+      above.incline = this->powerEntry[i].targetPosition;
       above.cad     = this->powerEntry[i].cad;
     } else if (i < indexPair) {
       below.power   = this->powerEntry[i].watts;
-      below.incline = this->powerEntry[i].incline;
+      below.incline = this->powerEntry[i].targetPosition;
       below.cad     = this->powerEntry[i].cad;
       above.power   = this->powerEntry[indexPair].watts;
-      above.incline = this->powerEntry[indexPair].incline;
+      above.incline = this->powerEntry[indexPair].targetPosition;
       above.cad     = this->powerEntry[indexPair].cad;
     }
-  } else {  // no interpolation and so no CAD correction
-    return (this->powerEntry[i].incline);
+  } else {  // no pair so no interpolation and no CAD correction
+    return (this->powerEntry[i].targetPosition);
   }
 
   // @MarkusSchneider's data shows a linear relationship between CAD and Watts for a given resistance level.
@@ -197,6 +255,40 @@ float PowerTable::lookup(int watts, int cad) {
   return rIncline;
 }
 
+bool PowerTable::load() {
+  // load power table from spiffs
+  return false;  // return unsuccessful
+}
+
+bool PowerTable::save() {
+  // save powertable from spiffs
+  return false;  // return unsuccessful
+}
+// Display power table in log
+void PowerTable::toLog() {
+  char buffer[6];
+  String oString = "|";
+  int n;
+  for (int i = 0; i < POWERTABLE_SIZE; i++) {
+    n = sprintf(buffer, "%4d|", this->powerEntry[i].watts);
+    oString += buffer;
+  }
+  // Serial.println(oString);
+  SS2K_LOG(POWERTABLE_LOG_TAG, | % s, oString.c_str());
+  oString = "|";
+  for (int i = 0; i < POWERTABLE_SIZE; i++) {
+    n = sprintf(buffer, "%4d|", this->powerEntry[i].cad);
+    oString += buffer;
+  }
+  SS2K_LOG(POWERTABLE_LOG_TAG, | % s, oString.c_str());
+  oString = "|";
+  for (int i = 0; i < POWERTABLE_SIZE; i++) {
+    n = sprintf(buffer, "%4d|", this->powerEntry[i].targetPosition);
+    oString += buffer;
+  }
+  SS2K_LOG(POWERTABLE_LOG_TAG, | % s, oString.c_str());
+}
+
 // as a note, Trainer Road sends 50w target whenever the app is connected.
 void ErgMode::computErg(int newSetPoint) {
   Measurement newWatts = rtConfig.getSimulatedWatts();
diff --git a/src/Main.cpp b/src/Main.cpp
index d87a6770..41ed8595 100644
--- a/src/Main.cpp
+++ b/src/Main.cpp
@@ -22,10 +22,8 @@ uint64_t debounceDelay    = 500;  // the debounce time; increase if the output f
 
 // Stepper Speed - Lower is faster
 int maxStepperSpeed     = 500;
-int32_t targetPosition  = 0;
 int lastShifterPosition = 0;
-bool externalControl    = false;
-bool syncMode           = false;
+
 HardwareSerial stepperSerial(2);
 TMC2208Stepper driver(&SERIAL_PORT, R_SENSE);  // Hardware Serial
 FastAccelStepperEngine engine = FastAccelStepperEngine();
@@ -43,6 +41,7 @@ TaskHandle_t moveStepperTask;
 TaskHandle_t shifterCheckTask;
 
 ///////////// Initialize the Config /////////////
+SS2K ss2k;
 userParameters userConfig;
 RuntimeParameters rtConfig;
 physicalWorkingCapacity userPWC;
@@ -148,6 +147,7 @@ void setup() {
                           1,                      /* priority of the task */
                           &shifterCheckTask,      /* Task handle to keep track of created task */
                           1);                     /* pin task to core 0 */
+
 }
 
 void loop() {  // Delete this task so we can make one that's more memory efficient.
@@ -161,11 +161,11 @@ void shifterCheck(void *pvParameters) {
 
   if (rtConfig.getShifterPosition() > lastShifterPosition) {
     SS2K_LOG(MAIN_LOG_TAG, "Shift UP: %l", rtConfig.getShifterPosition());
-    Serial.println(targetPosition);
+    Serial.println(ss2k.targetPosition);
     spinBLEServer.notifyShift(1);
   } else if (rtConfig.getShifterPosition() < lastShifterPosition) {
     SS2K_LOG(MAIN_LOG_TAG, "Shift DOWN: %l", rtConfig.getShifterPosition());
-    Serial.println(targetPosition);
+    Serial.println(ss2k.targetPosition);
     spinBLEServer.notifyShift(0);
   }
   lastShifterPosition = rtConfig.getShifterPosition();
@@ -200,25 +200,25 @@ void moveStepper(void *pvParameters) {
 
   while (1) {
     if (stepper) {
-      targetPosition = rtConfig.getShifterPosition() * userConfig.getShiftStep();
-      if (!externalControl) {
+      ss2k.targetPosition = rtConfig.getShifterPosition() * userConfig.getShiftStep();
+      if (!ss2k.externalControl) {
         if (rtConfig.getERGMode()) {
           // ERG Mode
           // Shifter not used.
-          targetPosition = rtConfig.getTargetIncline();
+          ss2k.targetPosition = rtConfig.getTargetIncline();
         } else {
           // Simulation Mode
-          targetPosition += rtConfig.getTargetIncline() * userConfig.getInclineMultiplier();
+          ss2k.targetPosition += rtConfig.getTargetIncline() * userConfig.getInclineMultiplier();
         }
       }
 
-      if (syncMode) {
+      if (ss2k.syncMode) {
         stepper->stopMove();
         vTaskDelay(100 / portTICK_PERIOD_MS);
-        stepper->setCurrentPosition(targetPosition);
+        stepper->setCurrentPosition(ss2k.targetPosition);
         vTaskDelay(100 / portTICK_PERIOD_MS);
       }
-      stepper->moveTo(targetPosition);
+      stepper->moveTo(ss2k.targetPosition);
       vTaskDelay(100 / portTICK_PERIOD_MS);
 
       rtConfig.setCurrentIncline((float)stepper->getCurrentPosition());
@@ -366,8 +366,8 @@ void checkDriverTemperature() {
 
 void motorStop(bool releaseTension) {
   stepper->stopMove();
-  stepper->setCurrentPosition(targetPosition);
+  stepper->setCurrentPosition(ss2k.targetPosition);
   if (releaseTension) {
-    stepper->moveTo(targetPosition - userConfig.getShiftStep() * 4);
+    stepper->moveTo(ss2k.targetPosition - userConfig.getShiftStep() * 4);
   }
 }

From 5324e2d16e1f4db8668b9a2eb773086117fc4653 Mon Sep 17 00:00:00 2001
From: doudar <anthonydoud97@gmail.com>
Date: Tue, 21 Dec 2021 15:18:39 -0600
Subject: [PATCH 25/42] Adjusted loop timing and logs for actual testing.

---
 include/ERG_Mode.h |  4 +-
 include/settings.h |  2 +-
 src/ERG_Mode.cpp   | 91 ++++++++++++++++++++++++----------------------
 3 files changed, 51 insertions(+), 46 deletions(-)

diff --git a/include/ERG_Mode.h b/include/ERG_Mode.h
index 56b90ca2..f148bb0b 100644
--- a/include/ERG_Mode.h
+++ b/include/ERG_Mode.h
@@ -14,7 +14,7 @@
 #define ERG_MODE_LOG_TAG     "ERG_Mode"
 #define ERG_MODE_LOG_CSV_TAG "ERG_Mode_CSV"
 #define POWERTABLE_LOG_TAG   "PowTab"
-#define ERG_MODE_DELAY       2000
+#define ERG_MODE_DELAY       700
 
 extern TaskHandle_t ErgTask;
 void setupERG();
@@ -66,7 +66,7 @@ class PowerTable {
   // Catalogs a new entry into the power table.
   void newEntry(PowerBuffer powerBuffer);
   // returns incline for wattTarget. Null if not found.
-  float lookup(int watts, int cad);
+  int32_t lookup(int watts, int cad);
   // load power table from spiffs
   bool load();
   // save powertable from spiffs
diff --git a/include/settings.h b/include/settings.h
index a007d57f..87f924b6 100644
--- a/include/settings.h
+++ b/include/settings.h
@@ -163,7 +163,7 @@
 #define POWERTABLE_INCREMENT 50.0
 
 //Number of similar power samples to take before writing to the Power Table
-#define POWER_SAMPLES 3 
+#define POWER_SAMPLES 5 
 
 // Uncomment to enable sending Telegram debug messages back to the chat
 // specified in telegram_token.h
diff --git a/src/ERG_Mode.cpp b/src/ERG_Mode.cpp
index 8e0eca88..ceef6e3f 100644
--- a/src/ERG_Mode.cpp
+++ b/src/ERG_Mode.cpp
@@ -45,30 +45,29 @@ void ergTaskLoop(void* pvParameters) {
     hasConnectedPowerMeter = spinBLEClient.connectedPM;
     simulationRunning      = rtConfig.getSimulateTargetWatts();
 
-    if ((rtConfig.getSimulatedCad() > 50) && (rtConfig.getSimulatedWatts().value > 10) && (rtConfig.getSimulatedWatts().value<POWERTABLE_SIZE*POWERTABLE_INCREMENT)) {
+    if ((rtConfig.getSimulatedCad() > 50) && (rtConfig.getSimulatedWatts().value > 10) && (rtConfig.getSimulatedWatts().value < POWERTABLE_SIZE * POWERTABLE_INCREMENT)) {
       if (powerBuffer.powerEntry[0].readings == 0) {
         powerBuffer.set(0);
-        SS2K_LOG(ERG_MODE_LOG_TAG, "powerBuffer.set(0);");
+        //SS2K_LOG(ERG_MODE_LOG_TAG, "powerBuffer.set(0);");
       } else if (abs(powerBuffer.powerEntry[0].watts - rtConfig.getSimulatedWatts().value) < POWERTABLE_INCREMENT) {
         for (int i = 1; i < POWER_SAMPLES; i++) {
           if (powerBuffer.powerEntry[i].readings == 0) {
             powerBuffer.set(i);
-            SS2K_LOG(ERG_MODE_LOG_TAG, "powerBuffer.set(%d);", i);
+            //SS2K_LOG(ERG_MODE_LOG_TAG, "powerBuffer.set(%d);", i);
             break;
           }
         }
         if (powerBuffer.powerEntry[POWER_SAMPLES - 1].readings == 1) {
-          SS2K_LOG(ERG_MODE_LOG_TAG, "powerTable.newEntry(powerBuffer);");
+          //SS2K_LOG(ERG_MODE_LOG_TAG, "powerTable.newEntry(powerBuffer);");
           powerTable.newEntry(powerBuffer);
+          powerTable.toLog();
           powerBuffer.reset();
         }
       } else {
         powerBuffer.reset();
-        SS2K_LOG(ERG_MODE_LOG_TAG, "powerBuffer.reset();");
+        //SS2K_LOG(ERG_MODE_LOG_TAG, "powerBuffer.reset();");
       }
     }
-
-    powerTable.toLog();
     if (isInErgMode && (hasConnectedPowerMeter || simulationRunning)) {
       ergMode.computErg(newSetPoint);
     } else {
@@ -93,18 +92,18 @@ void ergTaskLoop(void* pvParameters) {
 }
 
 void PowerBuffer::set(int i) {
-  this->powerEntry[i].readings = 1;
-  this->powerEntry[i].watts    = rtConfig.getSimulatedWatts().value;
-  this->powerEntry[i].cad      = rtConfig.getSimulatedCad();
-  this->powerEntry[i].targetPosition  = ss2k.targetPosition;
+  this->powerEntry[i].readings       = 1;
+  this->powerEntry[i].watts          = rtConfig.getSimulatedWatts().value;
+  this->powerEntry[i].cad            = rtConfig.getSimulatedCad();
+  this->powerEntry[i].targetPosition = ss2k.targetPosition;
 }
 
 void PowerBuffer::reset() {
   for (int i = 0; i < POWER_SAMPLES; i++) {
-    this->powerEntry[i].readings = 0;
-    this->powerEntry[i].watts    = 0;
-    this->powerEntry[i].cad      = 0;
-    this->powerEntry[i].targetPosition  = 0;
+    this->powerEntry[i].readings       = 0;
+    this->powerEntry[i].watts          = 0;
+    this->powerEntry[i].cad            = 0;
+    this->powerEntry[i].targetPosition = 0;
   }
 }
 
@@ -118,19 +117,19 @@ void PowerTable::newEntry(PowerBuffer powerBuffer) {
     tTargetPosition += powerBuffer.powerEntry[i].targetPosition;
     tCad += powerBuffer.powerEntry[i].cad;
   }
-  int watts     = tWatts / POWER_SAMPLES;
-  int cad       = tCad / POWER_SAMPLES;
+  int watts              = tWatts / POWER_SAMPLES;
+  int cad                = tCad / POWER_SAMPLES;
   int32_t targetPosition = tTargetPosition / POWER_SAMPLES;
 
   int i = round(watts / POWERTABLE_INCREMENT);
   if (this->powerEntry[i].readings == 0) {
-    this->powerEntry[i].watts    = watts;
-    this->powerEntry[i].cad      = cad;
-    this->powerEntry[i].targetPosition  = targetPosition;
-    this->powerEntry[i].readings = 1;
-  } else {  // Average and update the readings. 
-    this->powerEntry[i].watts   = (watts + (this->powerEntry[i].watts * this->powerEntry[i].readings)) / (this->powerEntry[i].readings + 1);
-    this->powerEntry[i].cad     = (cad + (this->powerEntry[i].cad * this->powerEntry[i].readings)) / (this->powerEntry[i].readings + 1);
+    this->powerEntry[i].watts          = watts;
+    this->powerEntry[i].cad            = cad;
+    this->powerEntry[i].targetPosition = targetPosition;
+    this->powerEntry[i].readings       = 1;
+  } else {  // Average and update the readings.
+    this->powerEntry[i].watts          = (watts + (this->powerEntry[i].watts * this->powerEntry[i].readings)) / (this->powerEntry[i].readings + 1);
+    this->powerEntry[i].cad            = (cad + (this->powerEntry[i].cad * this->powerEntry[i].readings)) / (this->powerEntry[i].readings + 1);
     this->powerEntry[i].targetPosition = (targetPosition + (this->powerEntry[i].targetPosition * this->powerEntry[i].readings)) / (this->powerEntry[i].readings + 1);
     this->powerEntry[i].readings++;
     if (this->powerEntry[i].readings > 10) {
@@ -141,11 +140,11 @@ void PowerTable::newEntry(PowerBuffer powerBuffer) {
 
 // looks up an incline for the requested power and cadence and interpolates the result.
 // Returns -99 if no entry matched.
-float PowerTable::lookup(int watts, int cad) {
+int32_t PowerTable::lookup(int watts, int cad) {
 #define RETURN_ERROR -99
   struct entry {
     float power;
-    float incline;
+    int32_t targetPosition;
     float cad;
   };
   int i         = round(watts / POWERTABLE_INCREMENT);  // find the closest entry
@@ -220,19 +219,19 @@ float PowerTable::lookup(int watts, int cad) {
 
   if (indexPair != -1) {
     if (i > indexPair) {
-      below.power   = this->powerEntry[indexPair].watts;
-      below.incline = this->powerEntry[indexPair].targetPosition;
-      below.cad     = this->powerEntry[indexPair].cad;
-      above.power   = this->powerEntry[i].watts;
-      above.incline = this->powerEntry[i].targetPosition;
-      above.cad     = this->powerEntry[i].cad;
+      below.power          = this->powerEntry[indexPair].watts;
+      below.targetPosition = this->powerEntry[indexPair].targetPosition;
+      below.cad            = this->powerEntry[indexPair].cad;
+      above.power          = this->powerEntry[i].watts;
+      above.targetPosition = this->powerEntry[i].targetPosition;
+      above.cad            = this->powerEntry[i].cad;
     } else if (i < indexPair) {
-      below.power   = this->powerEntry[i].watts;
-      below.incline = this->powerEntry[i].targetPosition;
-      below.cad     = this->powerEntry[i].cad;
-      above.power   = this->powerEntry[indexPair].watts;
-      above.incline = this->powerEntry[indexPair].targetPosition;
-      above.cad     = this->powerEntry[indexPair].cad;
+      below.power          = this->powerEntry[i].watts;
+      below.targetPosition = this->powerEntry[i].targetPosition;
+      below.cad            = this->powerEntry[i].cad;
+      above.power          = this->powerEntry[indexPair].watts;
+      above.targetPosition = this->powerEntry[indexPair].targetPosition;
+      above.cad            = this->powerEntry[indexPair].cad;
     }
   } else {  // no pair so no interpolation and no CAD correction
     return (this->powerEntry[i].targetPosition);
@@ -243,16 +242,16 @@ float PowerTable::lookup(int watts, int cad) {
   // as higher resistance levels have a steeper slope (bigger increase in power/cad) than low resistance levels.
   float averageCAD = (below.cad - above.cad) / 2;
   float deltaCAD   = abs(averageCAD - cad);
-  if (cad > averageCAD) {  // cad is higher than the table so we need to target a lower wattage (and incline)
+  if (cad > averageCAD) {  // cad is higher than the table so we need to target a lower wattage (and targetPosition)
     watts -= (deltaCAD / 20) * 50;
   }
-  if (cad < averageCAD) {  // cad is lower than the table so we need to target a higher wattage (and incline)
+  if (cad < averageCAD) {  // cad is lower than the table so we need to target a higher wattage (and targetPosition)
     watts += (deltaCAD / 20) * 50;
   }
   // actual interpolation
-  float rIncline = below.incline + ((watts - below.power) / (above.power - below.power)) * (above.incline - below.incline);
+  int32_t rtargetPosition = below.targetPosition + ((watts - below.power) / (above.power - below.power)) * (above.targetPosition - below.targetPosition);
 
-  return rIncline;
+  return rtargetPosition;
 }
 
 bool PowerTable::load() {
@@ -308,7 +307,13 @@ void ErgMode::computErg(int newSetPoint) {
 
   // reset cycle counter to start new erg calculation cycle
   if (this->setPoint != newSetPoint) {
-    this->cycles = 0;
+    this->cycles        = 0;
+    int32_t tableResult = powerTable.lookup(newSetPoint, newCadance);
+    if (tableResult != -99) {
+      SS2K_LOG(ERG_MODE_LOG_TAG, "Using PowerTable Result");
+      rtConfig.setTargetIncline(tableResult);
+      return;
+    }
   }
 
   bool isUserSpinning = this->_userIsSpinning(newCadance, currentIncline);

From fa9b423304ac351decd97b0f2e0255af673f9eeb Mon Sep 17 00:00:00 2001
From: doudar <anthonydoud97@gmail.com>
Date: Tue, 21 Dec 2021 15:28:36 -0600
Subject: [PATCH 26/42] Added Logging...This might actually work...

---
 src/ERG_Mode.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/ERG_Mode.cpp b/src/ERG_Mode.cpp
index ceef6e3f..243cc3bf 100644
--- a/src/ERG_Mode.cpp
+++ b/src/ERG_Mode.cpp
@@ -310,7 +310,7 @@ void ErgMode::computErg(int newSetPoint) {
     this->cycles        = 0;
     int32_t tableResult = powerTable.lookup(newSetPoint, newCadance);
     if (tableResult != -99) {
-      SS2K_LOG(ERG_MODE_LOG_TAG, "Using PowerTable Result");
+      SS2K_LOG(ERG_MODE_LOG_TAG, "Using PowerTable Result %d", tableResult);
       rtConfig.setTargetIncline(tableResult);
       return;
     }

From dd0025f8485c23c69283d935280ab3547c3a7e67 Mon Sep 17 00:00:00 2001
From: doudar <anthonydoud97@gmail.com>
Date: Tue, 21 Dec 2021 21:23:42 -0600
Subject: [PATCH 27/42] moved isSpinning test earlier in erg

---
 src/ERG_Mode.cpp | 30 +++++++++++++++++++-----------
 1 file changed, 19 insertions(+), 11 deletions(-)

diff --git a/src/ERG_Mode.cpp b/src/ERG_Mode.cpp
index 243cc3bf..5ecf2cbd 100644
--- a/src/ERG_Mode.cpp
+++ b/src/ERG_Mode.cpp
@@ -48,24 +48,24 @@ void ergTaskLoop(void* pvParameters) {
     if ((rtConfig.getSimulatedCad() > 50) && (rtConfig.getSimulatedWatts().value > 10) && (rtConfig.getSimulatedWatts().value < POWERTABLE_SIZE * POWERTABLE_INCREMENT)) {
       if (powerBuffer.powerEntry[0].readings == 0) {
         powerBuffer.set(0);
-        //SS2K_LOG(ERG_MODE_LOG_TAG, "powerBuffer.set(0);");
+        // SS2K_LOG(ERG_MODE_LOG_TAG, "powerBuffer.set(0);");
       } else if (abs(powerBuffer.powerEntry[0].watts - rtConfig.getSimulatedWatts().value) < POWERTABLE_INCREMENT) {
         for (int i = 1; i < POWER_SAMPLES; i++) {
           if (powerBuffer.powerEntry[i].readings == 0) {
             powerBuffer.set(i);
-            //SS2K_LOG(ERG_MODE_LOG_TAG, "powerBuffer.set(%d);", i);
+            // SS2K_LOG(ERG_MODE_LOG_TAG, "powerBuffer.set(%d);", i);
             break;
           }
         }
         if (powerBuffer.powerEntry[POWER_SAMPLES - 1].readings == 1) {
-          //SS2K_LOG(ERG_MODE_LOG_TAG, "powerTable.newEntry(powerBuffer);");
+          // SS2K_LOG(ERG_MODE_LOG_TAG, "powerTable.newEntry(powerBuffer);");
           powerTable.newEntry(powerBuffer);
           powerTable.toLog();
           powerBuffer.reset();
         }
       } else {
         powerBuffer.reset();
-        //SS2K_LOG(ERG_MODE_LOG_TAG, "powerBuffer.reset();");
+        // SS2K_LOG(ERG_MODE_LOG_TAG, "powerBuffer.reset();");
       }
     }
     if (isInErgMode && (hasConnectedPowerMeter || simulationRunning)) {
@@ -249,9 +249,9 @@ int32_t PowerTable::lookup(int watts, int cad) {
     watts += (deltaCAD / 20) * 50;
   }
   // actual interpolation
-  int32_t rtargetPosition = below.targetPosition + ((watts - below.power) / (above.power - below.power)) * (above.targetPosition - below.targetPosition);
+  int32_t rTargetPosition = below.targetPosition + ((watts - below.power) / (above.power - below.power)) * (above.targetPosition - below.targetPosition);
 
-  return rtargetPosition;
+  return rTargetPosition;
 }
 
 bool PowerTable::load() {
@@ -304,6 +304,14 @@ void ErgMode::computErg(int newSetPoint) {
   if (newSetPoint < 50) {
     newSetPoint = 50;
   }
+    bool isUserSpinning = this->_userIsSpinning(newCadance, currentIncline);
+  if (!isUserSpinning) {
+    return;
+  }
+  bool isUserSpinning = this->_userIsSpinning(newCadance, currentIncline);
+  if (!isUserSpinning) {
+    return;
+  }
 
   // reset cycle counter to start new erg calculation cycle
   if (this->setPoint != newSetPoint) {
@@ -312,15 +320,15 @@ void ErgMode::computErg(int newSetPoint) {
     if (tableResult != -99) {
       SS2K_LOG(ERG_MODE_LOG_TAG, "Using PowerTable Result %d", tableResult);
       rtConfig.setTargetIncline(tableResult);
+      // store for next cycle for timestamp and value compare
+      this->watts    = newWatts;
+      this->setPoint = newSetPoint;
+      this->cadence  = newCadance;
+      this->cycles++;
       return;
     }
   }
 
-  bool isUserSpinning = this->_userIsSpinning(newCadance, currentIncline);
-  if (!isUserSpinning) {
-    return;
-  }
-
   float factor     = abs(diviation) > 10 ? userConfig.getERGSensitivity() : userConfig.getERGSensitivity() / 2;
   float newIncline = currentIncline + (wattChange * factor);
 

From cbfdae0d40f5190171957984882641d3cc0a6b8a Mon Sep 17 00:00:00 2001
From: doudar <anthonydoud97@gmail.com>
Date: Tue, 21 Dec 2021 22:37:22 -0600
Subject: [PATCH 28/42] added comments

---
 src/ERG_Mode.cpp | 20 +++++++++-----------
 1 file changed, 9 insertions(+), 11 deletions(-)

diff --git a/src/ERG_Mode.cpp b/src/ERG_Mode.cpp
index 5ecf2cbd..b9e436c3 100644
--- a/src/ERG_Mode.cpp
+++ b/src/ERG_Mode.cpp
@@ -47,24 +47,24 @@ void ergTaskLoop(void* pvParameters) {
 
     if ((rtConfig.getSimulatedCad() > 50) && (rtConfig.getSimulatedWatts().value > 10) && (rtConfig.getSimulatedWatts().value < POWERTABLE_SIZE * POWERTABLE_INCREMENT)) {
       if (powerBuffer.powerEntry[0].readings == 0) {
-        powerBuffer.set(0);
+        powerBuffer.set(0);  // Take Initial reading
         // SS2K_LOG(ERG_MODE_LOG_TAG, "powerBuffer.set(0);");
       } else if (abs(powerBuffer.powerEntry[0].watts - rtConfig.getSimulatedWatts().value) < POWERTABLE_INCREMENT) {
         for (int i = 1; i < POWER_SAMPLES; i++) {
-          if (powerBuffer.powerEntry[i].readings == 0) {
-            powerBuffer.set(i);
+          if (powerBuffer.powerEntry[i].readings == 0) {  //
+            powerBuffer.set(i);                           // Add additional readings to the buffer.
             // SS2K_LOG(ERG_MODE_LOG_TAG, "powerBuffer.set(%d);", i);
             break;
           }
         }
-        if (powerBuffer.powerEntry[POWER_SAMPLES - 1].readings == 1) {
+        if (powerBuffer.powerEntry[POWER_SAMPLES - 1].readings == 1) {  // If buffer is full, create a new table entry and clear the buffer.
           // SS2K_LOG(ERG_MODE_LOG_TAG, "powerTable.newEntry(powerBuffer);");
           powerTable.newEntry(powerBuffer);
           powerTable.toLog();
           powerBuffer.reset();
         }
-      } else {
-        powerBuffer.reset();
+      } else { //Reading was outside the range - clear the buffer and start over.
+        powerBuffer.reset(); 
         // SS2K_LOG(ERG_MODE_LOG_TAG, "powerBuffer.reset();");
       }
     }
@@ -122,7 +122,8 @@ void PowerTable::newEntry(PowerBuffer powerBuffer) {
   int32_t targetPosition = tTargetPosition / POWER_SAMPLES;
 
   int i = round(watts / POWERTABLE_INCREMENT);
-  if (this->powerEntry[i].readings == 0) {
+
+  if (this->powerEntry[i].readings == 0) {  // if first reading in this entry
     this->powerEntry[i].watts          = watts;
     this->powerEntry[i].cad            = cad;
     this->powerEntry[i].targetPosition = targetPosition;
@@ -304,10 +305,7 @@ void ErgMode::computErg(int newSetPoint) {
   if (newSetPoint < 50) {
     newSetPoint = 50;
   }
-    bool isUserSpinning = this->_userIsSpinning(newCadance, currentIncline);
-  if (!isUserSpinning) {
-    return;
-  }
+
   bool isUserSpinning = this->_userIsSpinning(newCadance, currentIncline);
   if (!isUserSpinning) {
     return;

From 6f85362ff16ed63cde29e24c23e2436b85b1a53b Mon Sep 17 00:00:00 2001
From: doudar <anthonydoud97@gmail.com>
Date: Tue, 21 Dec 2021 22:45:40 -0600
Subject: [PATCH 29/42] fixed typo

---
 src/ERG_Mode.cpp | 10 +++++-----
 1 file changed, 5 insertions(+), 5 deletions(-)

diff --git a/src/ERG_Mode.cpp b/src/ERG_Mode.cpp
index b9e436c3..4b46cbb2 100644
--- a/src/ERG_Mode.cpp
+++ b/src/ERG_Mode.cpp
@@ -63,8 +63,8 @@ void ergTaskLoop(void* pvParameters) {
           powerTable.toLog();
           powerBuffer.reset();
         }
-      } else { //Reading was outside the range - clear the buffer and start over.
-        powerBuffer.reset(); 
+      } else {  // Reading was outside the range - clear the buffer and start over.
+        powerBuffer.reset();
         // SS2K_LOG(ERG_MODE_LOG_TAG, "powerBuffer.reset();");
       }
     }
@@ -274,19 +274,19 @@ void PowerTable::toLog() {
     oString += buffer;
   }
   // Serial.println(oString);
-  SS2K_LOG(POWERTABLE_LOG_TAG, | % s, oString.c_str());
+  SS2K_LOG(POWERTABLE_LOG_TAG, "|%s", oString.c_str());
   oString = "|";
   for (int i = 0; i < POWERTABLE_SIZE; i++) {
     n = sprintf(buffer, "%4d|", this->powerEntry[i].cad);
     oString += buffer;
   }
-  SS2K_LOG(POWERTABLE_LOG_TAG, | % s, oString.c_str());
+  SS2K_LOG(POWERTABLE_LOG_TAG, "|%s", oString.c_str());
   oString = "|";
   for (int i = 0; i < POWERTABLE_SIZE; i++) {
     n = sprintf(buffer, "%4d|", this->powerEntry[i].targetPosition);
     oString += buffer;
   }
-  SS2K_LOG(POWERTABLE_LOG_TAG, | % s, oString.c_str());
+  SS2K_LOG(POWERTABLE_LOG_TAG, "|%s", oString.c_str());
 }
 
 // as a note, Trainer Road sends 50w target whenever the app is connected.

From e8591910c3d1d9147598dd4e214395894bf9973c Mon Sep 17 00:00:00 2001
From: doudar <anthonydoud97@gmail.com>
Date: Tue, 21 Dec 2021 22:54:11 -0600
Subject: [PATCH 30/42] fixed a bug

---
 src/ERG_Mode.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/ERG_Mode.cpp b/src/ERG_Mode.cpp
index 4b46cbb2..62e02312 100644
--- a/src/ERG_Mode.cpp
+++ b/src/ERG_Mode.cpp
@@ -241,7 +241,7 @@ int32_t PowerTable::lookup(int watts, int cad) {
   // @MarkusSchneider's data shows a linear relationship between CAD and Watts for a given resistance level.
   // It looks like for every 20 CAD increase there is ~50w increase in power. This may need to be adjusted later
   // as higher resistance levels have a steeper slope (bigger increase in power/cad) than low resistance levels.
-  float averageCAD = (below.cad - above.cad) / 2;
+  float averageCAD = (below.cad + above.cad) / 2;
   float deltaCAD   = abs(averageCAD - cad);
   if (cad > averageCAD) {  // cad is higher than the table so we need to target a lower wattage (and targetPosition)
     watts -= (deltaCAD / 20) * 50;

From 3e0e5f8ee1cf749c807a1138710235b0bfe8d3db Mon Sep 17 00:00:00 2001
From: doudar <anthonydoud97@gmail.com>
Date: Tue, 21 Dec 2021 23:10:31 -0600
Subject: [PATCH 31/42] added comment

---
 src/ERG_Mode.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/ERG_Mode.cpp b/src/ERG_Mode.cpp
index 62e02312..0a6267c3 100644
--- a/src/ERG_Mode.cpp
+++ b/src/ERG_Mode.cpp
@@ -266,7 +266,7 @@ bool PowerTable::save() {
 }
 // Display power table in log
 void PowerTable::toLog() {
-  char buffer[6];
+  char buffer[6]; // crashes when targetPosition gets over 9999
   String oString = "|";
   int n;
   for (int i = 0; i < POWERTABLE_SIZE; i++) {

From 8cb092a9cf77d1acfdae5f7d76074b65518cd864 Mon Sep 17 00:00:00 2001
From: Markus Schneider <m.schneider@online.de>
Date: Wed, 22 Dec 2021 18:25:23 +0100
Subject: [PATCH 32/42] fixed bug in newEntry()

---
 include/ERG_Mode.h               | 10 ++++++----
 src/ERG_Mode.cpp                 | 34 ++++++++++++++++++++++----------
 test/native/test.cpp             |  9 ++++++++-
 test/native/test.h               |  7 ++++++-
 test/native/test_PowerBuffer.cpp | 29 +++++++++++++++++++++++++++
 5 files changed, 73 insertions(+), 16 deletions(-)
 create mode 100644 test/native/test_PowerBuffer.cpp

diff --git a/include/ERG_Mode.h b/include/ERG_Mode.h
index f148bb0b..f78e8023 100644
--- a/include/ERG_Mode.h
+++ b/include/ERG_Mode.h
@@ -7,9 +7,8 @@
 
 #pragma once
 
-#include <Arduino.h>
-#include "SmartSpin_parameters.h"
 #include "settings.h"
+#include "SmartSpin_parameters.h"
 
 #define ERG_MODE_LOG_TAG     "ERG_Mode"
 #define ERG_MODE_LOG_CSV_TAG "ERG_Mode_CSV"
@@ -63,16 +62,19 @@ class PowerBuffer {
 class PowerTable {
  public:
   PowerEntry powerEntry[POWERTABLE_SIZE];
+  
   // Catalogs a new entry into the power table.
   void newEntry(PowerBuffer powerBuffer);
+  
   // returns incline for wattTarget. Null if not found.
   int32_t lookup(int watts, int cad);
+  
   // load power table from spiffs
   bool load();
+  
   // save powertable from spiffs
   bool save();
+  
   // Display power table in log
   void toLog();
 };
-
-
diff --git a/src/ERG_Mode.cpp b/src/ERG_Mode.cpp
index 0a6267c3..b05b326f 100644
--- a/src/ERG_Mode.cpp
+++ b/src/ERG_Mode.cpp
@@ -68,6 +68,7 @@ void ergTaskLoop(void* pvParameters) {
         // SS2K_LOG(ERG_MODE_LOG_TAG, "powerBuffer.reset();");
       }
     }
+
     if (isInErgMode && (hasConnectedPowerMeter || simulationRunning)) {
       ergMode.computErg(newSetPoint);
     } else {
@@ -109,17 +110,28 @@ void PowerBuffer::reset() {
 
 // Accepts new data into the table and averages input by number of readings in the power entry.
 void PowerTable::newEntry(PowerBuffer powerBuffer) {
-  float tWatts, tCad = 0;
-  int32_t tTargetPosition = 0;
+  int watts              = 0;
+  int cad                = 0;
+  int32_t targetPosition = 0;
 
   for (int i = 0; i < POWER_SAMPLES; i++) {
-    tWatts += powerBuffer.powerEntry[i].watts;
-    tTargetPosition += powerBuffer.powerEntry[i].targetPosition;
-    tCad += powerBuffer.powerEntry[i].cad;
+    if (powerBuffer.powerEntry[i].readings == 0) {
+      // break if powerEntry is not set
+      break;
+    }
+
+    if (i == 0) {  // first loop -> assign values
+      watts          = powerBuffer.powerEntry[i].watts;
+      targetPosition = powerBuffer.powerEntry[i].targetPosition;
+      cad            = powerBuffer.powerEntry[i].cad;
+      continue;
+    }
+
+    // calculate average
+    watts          = (watts + powerBuffer.powerEntry[i].watts) / 2;
+    targetPosition = (targetPosition + powerBuffer.powerEntry[i].targetPosition) / 2;
+    cad            = (cad + powerBuffer.powerEntry[i].cad) / 2;
   }
-  int watts              = tWatts / POWER_SAMPLES;
-  int cad                = tCad / POWER_SAMPLES;
-  int32_t targetPosition = tTargetPosition / POWER_SAMPLES;
 
   int i = round(watts / POWERTABLE_INCREMENT);
 
@@ -148,6 +160,7 @@ int32_t PowerTable::lookup(int watts, int cad) {
     int32_t targetPosition;
     float cad;
   };
+
   int i         = round(watts / POWERTABLE_INCREMENT);  // find the closest entry
   float scale   = watts / POWERTABLE_INCREMENT - i;     // Should we look at the next higher or next lower index for comparison?
   int indexPair = -1;                                   // The next closes index with data for interpolation
@@ -264,9 +277,10 @@ bool PowerTable::save() {
   // save powertable from spiffs
   return false;  // return unsuccessful
 }
+
 // Display power table in log
 void PowerTable::toLog() {
-  char buffer[6]; // crashes when targetPosition gets over 9999
+  char buffer[6];  // crashes when targetPosition gets over 9999
   String oString = "|";
   int n;
   for (int i = 0; i < POWERTABLE_SIZE; i++) {
@@ -283,7 +297,7 @@ void PowerTable::toLog() {
   SS2K_LOG(POWERTABLE_LOG_TAG, "|%s", oString.c_str());
   oString = "|";
   for (int i = 0; i < POWERTABLE_SIZE; i++) {
-    n = sprintf(buffer, "%4d|", this->powerEntry[i].targetPosition);
+    n = sprintf(buffer, "%8d|", this->powerEntry[i].targetPosition);
     oString += buffer;
   }
   SS2K_LOG(POWERTABLE_LOG_TAG, "|%s", oString.c_str());
diff --git a/test/native/test.cpp b/test/native/test.cpp
index 1e514be5..6dbbc860 100644
--- a/test/native/test.cpp
+++ b/test/native/test.cpp
@@ -18,14 +18,21 @@ void process() {
     RUN_TEST(test.test_parses_cadence);
     RUN_TEST(test.test_parses_power);
   }
+
   // Cycle Power Tests
-  test_cyclePowerData test;
   {
+    test_cyclePowerData test;
     RUN_TEST(test.test_parses_power);
     RUN_TEST(test.test_parses_cadence);
     RUN_TEST(test.test_parses_heartrate);
     RUN_TEST(test.test_parses_speed);
   }
+
+  // ERG Mode
+  {
+    TestPowerBuffer testPowerBuffer;
+    RUN_TEST(testPowerBuffer.set__should_set_values__expect_values_added_to_correct_index);
+  }
   UNITY_END();
 }
 
diff --git a/test/native/test.h b/test/native/test.h
index e6d71274..a3e2edd4 100644
--- a/test/native/test.h
+++ b/test/native/test.h
@@ -5,7 +5,7 @@
  * SPDX-License-Identifier: GPL-2.0-only
  */
 
-#pragma once 
+#pragma once
 
 class test_fitnessMachineIndoorBikeData {
  public:
@@ -21,3 +21,8 @@ class test_cyclePowerData {
   static void test_parses_heartrate(void);
   static void test_parses_speed(void);
 };
+
+class TestPowerBuffer {
+ public:
+  static void set__should_set_values__expect_values_added_to_correct_index(void);
+};
diff --git a/test/native/test_PowerBuffer.cpp b/test/native/test_PowerBuffer.cpp
new file mode 100644
index 00000000..ccf87fcf
--- /dev/null
+++ b/test/native/test_PowerBuffer.cpp
@@ -0,0 +1,29 @@
+/*
+ * Copyright (C) 2020  Anthony Doud & Joel Baranick
+ * All rights reserved
+ *
+ * SPDX-License-Identifier: GPL-2.0-only
+ */
+
+#include <unity.h>
+#include "test.h"
+#include "ERG_Mode.h"
+
+void TestPowerBuffer::set__should_set_values__expect_values_added_to_correct_index(void) {
+  const PowerBuffer powerBuffer
+  // // Pre parse state
+  // TEST_ASSERT_FALSE(sensor.hasPower());
+  // TEST_ASSERT_EQUAL_INT(INT_MIN, sensor.getPower());
+
+  // // Power is instantaneous, not much calculation to do so skipping some samples
+  // sensor.decode(t0, sizeof(t0));
+  // TEST_ASSERT_TRUE(sensor.hasPower());
+  // TEST_ASSERT_EQUAL_INT(0, sensor.getPower());
+
+  // sensor.decode(t3, sizeof(t0));
+  // TEST_ASSERT_TRUE(sensor.hasPower());
+  // TEST_ASSERT_EQUAL_INT(45, sensor.getPower());
+
+  // sensor.decode(t7, sizeof(t0));
+  // TEST_ASSERT_EQUAL_INT(57, sensor.getPower());
+}

From cbafef67479c854c57bbeba2e72ff1a7d2c406cc Mon Sep 17 00:00:00 2001
From: Markus Schneider <m.schneider@online.de>
Date: Wed, 22 Dec 2021 20:27:54 +0100
Subject: [PATCH 33/42] fixed buffer overflow in PowerTable::toLog()

---
 src/ERG_Mode.cpp | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/src/ERG_Mode.cpp b/src/ERG_Mode.cpp
index b05b326f..48faba51 100644
--- a/src/ERG_Mode.cpp
+++ b/src/ERG_Mode.cpp
@@ -280,24 +280,24 @@ bool PowerTable::save() {
 
 // Display power table in log
 void PowerTable::toLog() {
-  char buffer[6];  // crashes when targetPosition gets over 9999
+  char buffer[8];  // crashes when targetPosition gets over 9999
   String oString = "|";
   int n;
   for (int i = 0; i < POWERTABLE_SIZE; i++) {
-    n = sprintf(buffer, "%4d|", this->powerEntry[i].watts);
+    n = sprintf(buffer, "%6d|", this->powerEntry[i].watts);
     oString += buffer;
   }
   // Serial.println(oString);
   SS2K_LOG(POWERTABLE_LOG_TAG, "|%s", oString.c_str());
   oString = "|";
   for (int i = 0; i < POWERTABLE_SIZE; i++) {
-    n = sprintf(buffer, "%4d|", this->powerEntry[i].cad);
+    n = sprintf(buffer, "%6d|", this->powerEntry[i].cad);
     oString += buffer;
   }
   SS2K_LOG(POWERTABLE_LOG_TAG, "|%s", oString.c_str());
   oString = "|";
   for (int i = 0; i < POWERTABLE_SIZE; i++) {
-    n = sprintf(buffer, "%8d|", this->powerEntry[i].targetPosition);
+    n = sprintf(buffer, "%6d|", this->powerEntry[i].targetPosition);
     oString += buffer;
   }
   SS2K_LOG(POWERTABLE_LOG_TAG, "|%s", oString.c_str());

From f2a48e6f36cc23ca0b331423847e729818384b0e Mon Sep 17 00:00:00 2001
From: doudar <anthonydoud97@gmail.com>
Date: Wed, 22 Dec 2021 16:45:04 -0600
Subject: [PATCH 34/42] Cleaned up the formatting of the powerTable log.

---
 src/ERG_Mode.cpp | 20 +++++++++++---------
 1 file changed, 11 insertions(+), 9 deletions(-)

diff --git a/src/ERG_Mode.cpp b/src/ERG_Mode.cpp
index 48faba51..ee4be846 100644
--- a/src/ERG_Mode.cpp
+++ b/src/ERG_Mode.cpp
@@ -281,26 +281,28 @@ bool PowerTable::save() {
 // Display power table in log
 void PowerTable::toLog() {
   char buffer[8];  // crashes when targetPosition gets over 9999
-  String oString = "|";
+  String oString = "";
   int n;
+  char oFormat[5] = "|%6d";
+
   for (int i = 0; i < POWERTABLE_SIZE; i++) {
-    n = sprintf(buffer, "%6d|", this->powerEntry[i].watts);
+    n = sprintf(buffer, oFormat, this->powerEntry[i].watts);
     oString += buffer;
   }
   // Serial.println(oString);
-  SS2K_LOG(POWERTABLE_LOG_TAG, "|%s", oString.c_str());
-  oString = "|";
+  SS2K_LOG(POWERTABLE_LOG_TAG, "%s|", oString.c_str());
+  oString = "";
   for (int i = 0; i < POWERTABLE_SIZE; i++) {
-    n = sprintf(buffer, "%6d|", this->powerEntry[i].cad);
+    n = sprintf(buffer, oFormat, this->powerEntry[i].cad);
     oString += buffer;
   }
-  SS2K_LOG(POWERTABLE_LOG_TAG, "|%s", oString.c_str());
-  oString = "|";
+  SS2K_LOG(POWERTABLE_LOG_TAG, "%s|", oString.c_str());
+  oString = "";
   for (int i = 0; i < POWERTABLE_SIZE; i++) {
-    n = sprintf(buffer, "%6d|", this->powerEntry[i].targetPosition);
+    n = sprintf(buffer, oFormat, this->powerEntry[i].targetPosition);
     oString += buffer;
   }
-  SS2K_LOG(POWERTABLE_LOG_TAG, "|%s", oString.c_str());
+  SS2K_LOG(POWERTABLE_LOG_TAG, "%s|", oString.c_str());
 }
 
 // as a note, Trainer Road sends 50w target whenever the app is connected.

From 1e13f04eaa07661f97e9785724730979b8034fe0 Mon Sep 17 00:00:00 2001
From: doudar <anthonydoud97@gmail.com>
Date: Wed, 22 Dec 2021 16:45:37 -0600
Subject: [PATCH 35/42] removed obsolete comments

---
 src/ERG_Mode.cpp | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/src/ERG_Mode.cpp b/src/ERG_Mode.cpp
index ee4be846..c87ba8e7 100644
--- a/src/ERG_Mode.cpp
+++ b/src/ERG_Mode.cpp
@@ -280,7 +280,7 @@ bool PowerTable::save() {
 
 // Display power table in log
 void PowerTable::toLog() {
-  char buffer[8];  // crashes when targetPosition gets over 9999
+  char buffer[8]; 
   String oString = "";
   int n;
   char oFormat[5] = "|%6d";
@@ -289,7 +289,6 @@ void PowerTable::toLog() {
     n = sprintf(buffer, oFormat, this->powerEntry[i].watts);
     oString += buffer;
   }
-  // Serial.println(oString);
   SS2K_LOG(POWERTABLE_LOG_TAG, "%s|", oString.c_str());
   oString = "";
   for (int i = 0; i < POWERTABLE_SIZE; i++) {

From 6e832df8912ecb3f35d4768997eae4df6ff8f349 Mon Sep 17 00:00:00 2001
From: doudar <anthonydoud97@gmail.com>
Date: Wed, 22 Dec 2021 16:51:30 -0600
Subject: [PATCH 36/42] fixed missing semi in test container

---
 src/ERG_Mode.cpp                 | 2 +-
 test/native/test_PowerBuffer.cpp | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/ERG_Mode.cpp b/src/ERG_Mode.cpp
index c87ba8e7..f7433b78 100644
--- a/src/ERG_Mode.cpp
+++ b/src/ERG_Mode.cpp
@@ -280,7 +280,7 @@ bool PowerTable::save() {
 
 // Display power table in log
 void PowerTable::toLog() {
-  char buffer[8]; 
+  char buffer[8];  // add code here to truncate the uint32_t value for display. Or change the value system wide to be a regular int since we probaby dont need that much anymore.
   String oString = "";
   int n;
   char oFormat[5] = "|%6d";
diff --git a/test/native/test_PowerBuffer.cpp b/test/native/test_PowerBuffer.cpp
index ccf87fcf..b16cdad0 100644
--- a/test/native/test_PowerBuffer.cpp
+++ b/test/native/test_PowerBuffer.cpp
@@ -10,7 +10,7 @@
 #include "ERG_Mode.h"
 
 void TestPowerBuffer::set__should_set_values__expect_values_added_to_correct_index(void) {
-  const PowerBuffer powerBuffer
+  const PowerBuffer powerBuffer;
   // // Pre parse state
   // TEST_ASSERT_FALSE(sensor.hasPower());
   // TEST_ASSERT_EQUAL_INT(INT_MIN, sensor.getPower());

From 99212ac2dc8ce179f2add3cb75b3c1b9303a10d0 Mon Sep 17 00:00:00 2001
From: Markus Schneider <m.schneider@online.de>
Date: Thu, 23 Dec 2021 11:02:16 +0100
Subject: [PATCH 37/42] Added ArduinoFake for native unit testing

---
 include/SS2KLog.h              |  5 +++++
 include/SmartSpin_parameters.h | 26 +++++++++++++++-----------
 platformio.ini                 |  1 +
 3 files changed, 21 insertions(+), 11 deletions(-)

diff --git a/include/SS2KLog.h b/include/SS2KLog.h
index a792f7fc..5daa82e0 100644
--- a/include/SS2KLog.h
+++ b/include/SS2KLog.h
@@ -7,6 +7,8 @@
 
 #pragma once
 
+#ifndef UNIT_TEST
+
 #include "sdkconfig.h"
 #include "esp_log.h"
 #include <stdio.h>
@@ -103,3 +105,6 @@ void ss2k_log_write(esp_log_level_t level, const char *format, ...);
 
 void ss2k_log_writev(esp_log_level_t level, const char *format, va_list args);
 
+#else
+
+#endif
\ No newline at end of file
diff --git a/include/SmartSpin_parameters.h b/include/SmartSpin_parameters.h
index 171526ca..aeeb33c4 100644
--- a/include/SmartSpin_parameters.h
+++ b/include/SmartSpin_parameters.h
@@ -7,7 +7,11 @@
 
 #pragma once
 
+#ifndef UNIT_TEST
 #include <Arduino.h>
+#else
+#include <ArduinoFake.h>
+#endif
 
 #define CONFIG_LOG_TAG "Config"
 
@@ -48,37 +52,37 @@ class RuntimeParameters {
 
   void setTargetWatts(int w) { targetWatts = w; }
   int getTargetWatts() { return targetWatts; }
-  
+
   void setSimulatedWatts(int w) { simulatedWatts = Measurement(w); }
   Measurement getSimulatedWatts() { return simulatedWatts; }
-  
+
   void setSimulatedHr(int hr) { simulatedHr = hr; }
   int getSimulatedHr() { return simulatedHr; }
-  
+
   void setSimulatedCad(float cad) { simulatedCad = cad; }
   int getSimulatedCad() { return simulatedCad; }
-  
+
   void setSimulatedSpeed(float spd) { simulatedSpeed = spd; }
   float getSimulatedSpeed() { return simulatedSpeed; }
-  
+
   void setSimulateHr(bool shr) { simulateHr = shr; }
   bool getSimulateHr() { return simulateHr; }
-  
+
   void setSimulateWatts(bool swt) { simulateWatts = swt; }
   bool getSimulateWatts() { return simulateWatts; }
-  
+
   void setSimulateTargetWatts(bool swt) { simulateTargetWatts = swt; }
   bool getSimulateTargetWatts() { return simulateTargetWatts; }
-  
+
   void setSimulateCad(bool scd) { simulateCad = scd; }
   bool getSimulateCad() { return simulateCad; }
-  
+
   void setERGMode(bool erg) { ERGMode = erg; }
   bool getERGMode() { return ERGMode; }
-  
+
   void setShifterPosition(int sp) { shifterPosition = sp; }
   int getShifterPosition() { return shifterPosition; }
-  
+
   void setFoundDevices(String fdev) { foundDevices = fdev; }
   const char* getFoundDevices() { return foundDevices.c_str(); }
 
diff --git a/platformio.ini b/platformio.ini
index 54fbc48d..4c7e626a 100644
--- a/platformio.ini
+++ b/platformio.ini
@@ -54,6 +54,7 @@ check_skip_packages = true
 [env:native]
 platform = native
 lib_deps = 
+    ArduinoFake
 	lib/ArduinoCompat
 	lib/SS2K
 build_flags =

From e88e0537d0ccfa269cf5670ac55038244652b7af Mon Sep 17 00:00:00 2001
From: doudar <anthonydoud97@gmail.com>
Date: Thu, 23 Dec 2021 12:29:31 -0600
Subject: [PATCH 38/42] ERG waits for knob to stop after powertab lookup

---
 src/ERG_Mode.cpp | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/src/ERG_Mode.cpp b/src/ERG_Mode.cpp
index f7433b78..e01b0736 100644
--- a/src/ERG_Mode.cpp
+++ b/src/ERG_Mode.cpp
@@ -338,6 +338,9 @@ void ErgMode::computErg(int newSetPoint) {
       this->setPoint = newSetPoint;
       this->cadence  = newCadance;
       this->cycles++;
+      while(rtConfig.getTargetIncline()!=rtConfig.getCurrentIncline()){ //wait while the knob moves to target position.
+        vTaskDelay(100 / portTICK_PERIOD_MS);
+      }
       return;
     }
   }

From b82f2cce6581dbb01a2f511353750d202febf3bd Mon Sep 17 00:00:00 2001
From: doudar <anthonydoud97@gmail.com>
Date: Fri, 24 Dec 2021 08:41:01 -0600
Subject: [PATCH 39/42] added option to return log in rtConfig.returnJSON

---
 src/HTTP_Server_Basic.cpp    | 2 +-
 src/SmartSpin_parameters.cpp | 5 ++++-
 2 files changed, 5 insertions(+), 2 deletions(-)

diff --git a/src/HTTP_Server_Basic.cpp b/src/HTTP_Server_Basic.cpp
index fd1a6a47..bae43360 100644
--- a/src/HTTP_Server_Basic.cpp
+++ b/src/HTTP_Server_Basic.cpp
@@ -271,7 +271,7 @@ void startHttpServer() {
 
   server.on("/runtimeConfigJSON", []() {
     String tString;
-    tString = rtConfig.returnJSON();
+    tString = rtConfig.returnJSON(!server.arg("includeDebugLog").isEmpty());
     server.send(200, "text/plain", tString);
   });
 
diff --git a/src/SmartSpin_parameters.cpp b/src/SmartSpin_parameters.cpp
index 99d2615b..d2ebbad9 100644
--- a/src/SmartSpin_parameters.cpp
+++ b/src/SmartSpin_parameters.cpp
@@ -12,7 +12,7 @@
 #include <ArduinoJson.h>
 #include <SPIFFS.h>
 
-String RuntimeParameters::returnJSON() {
+String RuntimeParameters::returnJSON(bool includeDebugLog) {
   // Allocate a temporary JsonDocument
   // Don't forget to change the capacity to match your requirements.
   // Use arduinojson.org/assistant to compute the capacity.
@@ -33,6 +33,9 @@ String RuntimeParameters::returnJSON() {
   doc["ERGMode"]             = ERGMode;
   doc["shifterPosition"]     = shifterPosition;
   doc["foundDevices"]        = foundDevices;
+  if (includeDebugLog) {
+    doc["debug"] = DebugInfo::get_and_clear_logs();
+  }
 
   String output;
   serializeJson(doc, output);

From b0a754bfe4f5664d1634333f16bef55164e791c9 Mon Sep 17 00:00:00 2001
From: doudar <anthonydoud97@gmail.com>
Date: Fri, 24 Dec 2021 08:45:58 -0600
Subject: [PATCH 40/42] updates smartspin_parameters.h

---
 include/SmartSpin_parameters.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/include/SmartSpin_parameters.h b/include/SmartSpin_parameters.h
index aeeb33c4..fd812f04 100644
--- a/include/SmartSpin_parameters.h
+++ b/include/SmartSpin_parameters.h
@@ -86,7 +86,7 @@ class RuntimeParameters {
   void setFoundDevices(String fdev) { foundDevices = fdev; }
   const char* getFoundDevices() { return foundDevices.c_str(); }
 
-  String returnJSON();
+  String returnJSON(bool includeDebugLog = false);
 };
 
 class userParameters {

From 04f34039f4b2b7bcc15cfedcfd9e7b0f28ce7851 Mon Sep 17 00:00:00 2001
From: doudar <anthonydoud97@gmail.com>
Date: Fri, 24 Dec 2021 11:38:35 -0600
Subject: [PATCH 41/42] Fixed erronious logging.

---
 src/ERG_Mode.cpp | 11 ++++-------
 1 file changed, 4 insertions(+), 7 deletions(-)

diff --git a/src/ERG_Mode.cpp b/src/ERG_Mode.cpp
index e01b0736..a2366533 100644
--- a/src/ERG_Mode.cpp
+++ b/src/ERG_Mode.cpp
@@ -48,32 +48,29 @@ void ergTaskLoop(void* pvParameters) {
     if ((rtConfig.getSimulatedCad() > 50) && (rtConfig.getSimulatedWatts().value > 10) && (rtConfig.getSimulatedWatts().value < POWERTABLE_SIZE * POWERTABLE_INCREMENT)) {
       if (powerBuffer.powerEntry[0].readings == 0) {
         powerBuffer.set(0);  // Take Initial reading
-        // SS2K_LOG(ERG_MODE_LOG_TAG, "powerBuffer.set(0);");
       } else if (abs(powerBuffer.powerEntry[0].watts - rtConfig.getSimulatedWatts().value) < POWERTABLE_INCREMENT) {
         for (int i = 1; i < POWER_SAMPLES; i++) {
-          if (powerBuffer.powerEntry[i].readings == 0) {  //
+          if (powerBuffer.powerEntry[i].readings == 0) {
             powerBuffer.set(i);                           // Add additional readings to the buffer.
-            // SS2K_LOG(ERG_MODE_LOG_TAG, "powerBuffer.set(%d);", i);
             break;
           }
         }
         if (powerBuffer.powerEntry[POWER_SAMPLES - 1].readings == 1) {  // If buffer is full, create a new table entry and clear the buffer.
-          // SS2K_LOG(ERG_MODE_LOG_TAG, "powerTable.newEntry(powerBuffer);");
           powerTable.newEntry(powerBuffer);
           powerTable.toLog();
           powerBuffer.reset();
         }
       } else {  // Reading was outside the range - clear the buffer and start over.
         powerBuffer.reset();
-        // SS2K_LOG(ERG_MODE_LOG_TAG, "powerBuffer.reset();");
       }
     }
 
     if (isInErgMode && (hasConnectedPowerMeter || simulationRunning)) {
       ergMode.computErg(newSetPoint);
     } else {
-      if (newSetPoint > 0) {
-        SS2K_LOG(ERG_MODE_LOG_TAG, "ERG request but ERG off or no connected PM");
+      if (newSetPoint > 0) { //reset ERG state
+        SS2K_LOG(ERG_MODE_LOG_TAG, "Resetting Target Watts to 0");
+        rtConfig.setTargetWatts(0);
         rtConfig.setERGMode(false);
       }
     }

From 6ee719287de4a10d6e862fd204e2fb28ea096d72 Mon Sep 17 00:00:00 2001
From: doudar <anthonydoud97@gmail.com>
Date: Fri, 24 Dec 2021 13:12:53 -0600
Subject: [PATCH 42/42] Added additional cadence checks for delta correct

---
 src/ERG_Mode.cpp | 19 +++++++++++--------
 1 file changed, 11 insertions(+), 8 deletions(-)

diff --git a/src/ERG_Mode.cpp b/src/ERG_Mode.cpp
index a2366533..dd66c34f 100644
--- a/src/ERG_Mode.cpp
+++ b/src/ERG_Mode.cpp
@@ -51,7 +51,7 @@ void ergTaskLoop(void* pvParameters) {
       } else if (abs(powerBuffer.powerEntry[0].watts - rtConfig.getSimulatedWatts().value) < POWERTABLE_INCREMENT) {
         for (int i = 1; i < POWER_SAMPLES; i++) {
           if (powerBuffer.powerEntry[i].readings == 0) {
-            powerBuffer.set(i);                           // Add additional readings to the buffer.
+            powerBuffer.set(i);  // Add additional readings to the buffer.
             break;
           }
         }
@@ -68,7 +68,7 @@ void ergTaskLoop(void* pvParameters) {
     if (isInErgMode && (hasConnectedPowerMeter || simulationRunning)) {
       ergMode.computErg(newSetPoint);
     } else {
-      if (newSetPoint > 0) { //reset ERG state
+      if (newSetPoint > 0) {  // reset ERG state
         SS2K_LOG(ERG_MODE_LOG_TAG, "Resetting Target Watts to 0");
         rtConfig.setTargetWatts(0);
         rtConfig.setERGMode(false);
@@ -253,11 +253,14 @@ int32_t PowerTable::lookup(int watts, int cad) {
   // as higher resistance levels have a steeper slope (bigger increase in power/cad) than low resistance levels.
   float averageCAD = (below.cad + above.cad) / 2;
   float deltaCAD   = abs(averageCAD - cad);
-  if (cad > averageCAD) {  // cad is higher than the table so we need to target a lower wattage (and targetPosition)
-    watts -= (deltaCAD / 20) * 50;
-  }
-  if (cad < averageCAD) {  // cad is lower than the table so we need to target a higher wattage (and targetPosition)
-    watts += (deltaCAD / 20) * 50;
+
+  if (deltaCAD > 10) {
+    if (cad > averageCAD) {  // cad is higher than the table so we need to target a lower wattage (and targetPosition)
+      watts -= (deltaCAD / 20) * 50;
+    }
+    if (cad < averageCAD) {  // cad is lower than the table so we need to target a higher wattage (and targetPosition)
+      watts += (deltaCAD / 20) * 50;
+    }
   }
   // actual interpolation
   int32_t rTargetPosition = below.targetPosition + ((watts - below.power) / (above.power - below.power)) * (above.targetPosition - below.targetPosition);
@@ -335,7 +338,7 @@ void ErgMode::computErg(int newSetPoint) {
       this->setPoint = newSetPoint;
       this->cadence  = newCadance;
       this->cycles++;
-      while(rtConfig.getTargetIncline()!=rtConfig.getCurrentIncline()){ //wait while the knob moves to target position.
+      while (rtConfig.getTargetIncline() != rtConfig.getCurrentIncline()) {  // wait while the knob moves to target position.
         vTaskDelay(100 / portTICK_PERIOD_MS);
       }
       return;