diff --git a/CHANGELOG.md b/CHANGELOG.md index de79e8e1..05e877f9 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -45,7 +45,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 - Removed driver temp checking. It's not accurate on the ESP32. - Peloton resistance limit enhancements. - Continue updating power metrics to other clients if one client disconnects. - +- Freed 19k of ram by consolidating tasks and using timers instead of delays. ### Hardware - added Yesoul S3. diff --git a/include/BLE_Common.h b/include/BLE_Common.h index 4d7338e2..a9b450bd 100644 --- a/include/BLE_Common.h +++ b/include/BLE_Common.h @@ -33,11 +33,9 @@ // Setup void setupBLE(); -extern TaskHandle_t BLECommunicationTask; extern TaskHandle_t BLEClientTask; // ***********************Common********************************** -void BLECommunications(void *pvParameters); - +void BLECommunications(); // *****************************Server**************************** class MyServerCallbacks : public NimBLEServerCallbacks { public: diff --git a/include/ERG_Mode.h b/include/ERG_Mode.h index ee1d2d43..afdba436 100644 --- a/include/ERG_Mode.h +++ b/include/ERG_Mode.h @@ -16,10 +16,6 @@ #define ERG_MODE_DELAY 700 #define RETURN_ERROR INT32_MIN -extern TaskHandle_t ErgTask; -void setupERG(); -void ergTaskLoop(void* pvParameters); - class PowerEntry { public: int watts; @@ -95,6 +91,9 @@ class PowerTable { public: TableRow tableRow[POWERTABLE_CAD_SIZE]; + // What used to be in the ERGTaskLoop(). This is the main control function for ERG Mode and the powertable operations. + void runERG(); + // Pick up new power value and put them into the power table void processPowerValue(PowerBuffer& powerBuffer, int cadence, Measurement power); diff --git a/include/HTTP_Server_Basic.h b/include/HTTP_Server_Basic.h index ee58f32b..d2c3b1df 100644 --- a/include/HTTP_Server_Basic.h +++ b/include/HTTP_Server_Basic.h @@ -25,7 +25,7 @@ class HTTP_Server { static void handleHrSlider(); static void FirmwareUpdate(); - static void webClientUpdate(void *pvParameters); + static void webClientUpdate(); HTTP_Server() { internetConnection = false; } }; diff --git a/include/Main.h b/include/Main.h index b09a963c..2703f60b 100644 --- a/include/Main.h +++ b/include/Main.h @@ -43,10 +43,10 @@ class SS2K { bool isUpdating = false; bool IRAM_ATTR deBounce(); - static void IRAM_ATTR moveStepper(void *pvParameters); static void IRAM_ATTR maintenanceLoop(void *pvParameters); static void IRAM_ATTR shiftUp(); static void IRAM_ATTR shiftDown(); + static void moveStepper(); void resetIfShiftersHeld(); void startTasks(); void stopTasks(); diff --git a/include/settings.h b/include/settings.h index 3d73cc87..102a9ddc 100644 --- a/include/settings.h +++ b/include/settings.h @@ -11,10 +11,10 @@ #define AUTO_FIRMWARE_UPDATE true // Default Bluetooth WiFi and MDNS Name -const char * const DEVICE_NAME = "SmartSpin2k"; +const char* const DEVICE_NAME = "SmartSpin2k"; // Default WiFi Password -const char * const DEFAULT_PASSWORD = "password"; +const char* const DEFAULT_PASSWORD = "password"; // default URL To get Updates From. // If changed you'll also need to get a root certificate from the new server @@ -38,7 +38,7 @@ const char * const DEFAULT_PASSWORD = "password"; // name of local file to save Physical Working Capacity in LittleFS #define userPWCFILENAME "/userPWC.txt" -// name of the local file to save the torque table. +// name of the local file to save the torque table. #define POWER_TABLE_FILENAME "/PowerTable.txt" // Default Incline Multiplier. @@ -80,13 +80,13 @@ const char * const DEFAULT_PASSWORD = "password"; // Amount to change watt target per shift in ERG mode. #define ERG_PER_SHIFT 10 -//Pass all of the FTMS commands sent to SS2k down to a connected FTMS device. +// Pass all of the FTMS commands sent to SS2k down to a connected FTMS device. #define FTMS_PASSTHROUGH false // Use internal ERG control on external FTMS Trainer. -//#define INTERNAL_ERG_4EXT_FTMS +// #define INTERNAL_ERG_4EXT_FTMS -//Minimum cadence where ERG mode stops. +// Minimum cadence where ERG mode stops. #define MIN_ERG_CADENCE 30 // Default Min Watts to stop stepper. @@ -268,7 +268,7 @@ const char * const DEFAULT_PASSWORD = "password"; // Size of increments (in watts) for the ERG Lookup Table. This needs to be a decimal for proper calculation. #define POWERTABLE_WATT_INCREMENT 30 -// Size of increments (in CAD) for the ERG Lookup Table. This needs to be a decimal for proper calculation. +// Size of increments (in CAD) for the ERG Lookup Table. This needs to be a decimal for proper calculation. #define POWERTABLE_CAD_INCREMENT 5 // Number of similar power samples to take before writing to the Power Table @@ -283,8 +283,8 @@ const char * const DEFAULT_PASSWORD = "password"; // Where does the CAD portion of the table start? #define MINIMUM_TABLE_CAD 60 -//Minimum positions recorded in the active table before attempting to load the saved table. -//Increase this value if the offset for the loaded table is inaccurate. +// Minimum positions recorded in the active table before attempting to load the saved table. +// Increase this value if the offset for the loaded table is inaccurate. #define MINIMUM_RELIABLE_POSITIONS 3 // Temperature of the ESP32 at which to start reducing the power output of the stepper motor driver. @@ -303,7 +303,7 @@ const char * const DEFAULT_PASSWORD = "password"; #define BLE_RECONNECT_INTERVAL 1 // Interval for polling ble battery updates -#define BATTERY_UPDATE_INTERVAL_MILLIS 300000 +#define BATTERY_UPDATE_INTERVAL_MILLIS 300000 // Initial and web scan duration. #define DEFAULT_SCAN_DURATION 5 @@ -311,13 +311,9 @@ const char * const DEFAULT_PASSWORD = "password"; // BLE automatic reconnect duration. Set this low to avoid interruption. #define BLE_RECONNECT_SCAN_DURATION 5 -//Task Stack Sizes -#define MAIN_STACK 4500 -#define ERG_STACK 6000 -#define HTTP_STACK 6000 -#define BLE_COMM_STACK 6000 +// Task Stack Sizes +#define MAIN_STACK 6000 #define BLE_CLIENT_STACK 5500 -#define STEPPER_STACK 2000 // Uncomment to enable stack size debugging info // #define DEBUG_STACK diff --git a/src/BLE_Client.cpp b/src/BLE_Client.cpp index 5948580f..9ab26d0d 100644 --- a/src/BLE_Client.cpp +++ b/src/BLE_Client.cpp @@ -58,7 +58,8 @@ static void onNotify(BLERemoteCharacteristic *pBLERemoteCharacteristic, uint8_t } } -// BLE Client loop task +// BLE Client loop task. +// Manages device connections and scanning. void bleClientTask(void *pvParameters) { long int scanDelay = millis(); for (;;) { diff --git a/src/BLE_Common.cpp b/src/BLE_Common.cpp index 4252cf6b..ff419d74 100644 --- a/src/BLE_Common.cpp +++ b/src/BLE_Common.cpp @@ -17,13 +17,10 @@ bool hr2p = false; -TaskHandle_t BLECommunicationTask; - -void BLECommunications(void *pvParameters) { - for (;;) { - // if (!spinBLEClient.dontBlockScan) { - // NimBLEDevice::getScan()->stop(); // stop routine scans - // } +void BLECommunications() { + static unsigned long int bleCommTimer = millis(); + if (((millis() - bleCommTimer) > BLE_NOTIFY_DELAY) && !ss2k->isUpdating) { + bleCommTimer = millis(); // **********************************Client*************************************** for (auto &_BLEd : spinBLEClient.myBLEDevices) { // loop through discovered devices if (_BLEd.connectedClientID != BLE_HS_CONN_HANDLE_NONE) { @@ -63,10 +60,7 @@ void BLECommunications(void *pvParameters) { spinBLEClient.handleBattInfo(pClient, false); } else if (!pClient->isConnected()) { // This shouldn't ever be - // called... - // if (pClient->disconnect() == 0) { // 0 is a successful disconnect - // BLEDevice::deleteClient(pClient); - // vTaskDelay(100 / portTICK_PERIOD_MS); + // called... SS2K_LOG(BLE_COMMON_LOG_TAG, "Workaround connect"); _BLEd.doConnect = true; //} @@ -129,9 +123,5 @@ void BLECommunications(void *pvParameters) { } 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 } } \ No newline at end of file diff --git a/src/BLE_Setup.cpp b/src/BLE_Setup.cpp index 940c8101..7d427c44 100644 --- a/src/BLE_Setup.cpp +++ b/src/BLE_Setup.cpp @@ -14,24 +14,9 @@ void setupBLE() { // Common BLE setup for both client and server SS2K_LOG(BLE_SETUP_LOG_TAG, "Starting Arduino BLE Client application..."); BLEDevice::init(userConfig->getDeviceName()); - BLEDevice::setMTU(515); //-- enabling this is very important for BLE firmware updates. + BLEDevice::setMTU(515); //-- enabling this is very important for BLE firmware updates. spinBLEClient.start(); startBLEServer(); - - xTaskCreatePinnedToCore(BLECommunications, /* Task function. */ - "BLECommunicationTask", /* name of task. */ - BLE_COMM_STACK, /* Stack size of task*/ - NULL, /* parameter of the task */ - 3, /* priority of the task*/ - &BLECommunicationTask, /* Task handle to keep track of created task */ - 1); /* pin task to core */ - - SS2K_LOG(BLE_SETUP_LOG_TAG, "BLE Notify Task Started"); - /*vTaskDelay(100 / portTICK_PERIOD_MS); - if (strcmp(userConfig->getConnectedPowerMeter(), "none") != 0 || strcmp(userConfig->getConnectedHeartMonitor(), "none") != 0) { - spinBLEClient.serverScan(true); - SS2K_LOG(BLE_SETUP_LOG_TAG, "Scanning"); - }*/ SS2K_LOG(BLE_SETUP_LOG_TAG, "%s %s %s", userConfig->getConnectedPowerMeter(), userConfig->getConnectedHeartMonitor(), userConfig->getConnectedRemote()); SS2K_LOG(BLE_SETUP_LOG_TAG, "End BLE Setup"); } diff --git a/src/ERG_Mode.cpp b/src/ERG_Mode.cpp index f6aca482..a7b1b1a4 100644 --- a/src/ERG_Mode.cpp +++ b/src/ERG_Mode.cpp @@ -16,42 +16,28 @@ #include #include -TaskHandle_t ErgTask; PowerTable* powerTable = new PowerTable; // Create a torque 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. -void setupERG() { - SS2K_LOG(ERG_MODE_LOG_TAG, "Starting ERG Mode task..."); - xTaskCreatePinnedToCore(ergTaskLoop, /* Task function. */ - "FTMSModeTask", /* name of task. */ - ERG_STACK, /* Stack size of task*/ - NULL, /* parameter of the task */ - 1, /* priority of the task*/ - &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 ergTaskLoop(void* pvParameters) { - ErgMode ergMode; - PowerBuffer powerBuffer; +static unsigned long int ergTimer = millis(); - ergMode._writeLogHeader(); - bool hasConnectedPowerMeter = false; - bool simulationRunning = false; - int loopCounter = 0; +void PowerTable::runERG() { + static ErgMode ergMode; + static PowerBuffer powerBuffer; + static bool hasConnectedPowerMeter = false; + static bool simulationRunning = false; + static int loopCounter = 0; - while (true) { + if ((millis() - ergTimer) > ERG_MODE_DELAY) { + // reset the timer. + ergTimer = millis(); // be quiet while updating via BLE - while (ss2k->isUpdating) { - vTaskDelay(ERG_MODE_DELAY / portTICK_PERIOD_MS); + if (ss2k->isUpdating) { + return; } - vTaskDelay(ERG_MODE_DELAY / portTICK_PERIOD_MS); - if (rtConfig->cad.getValue() > 0 && rtConfig->watts.getValue() > 0) { hasConnectedPowerMeter = spinBLEClient.connectedPM; simulationRunning = rtConfig->watts.getTarget(); @@ -82,12 +68,7 @@ void ergTaskLoop(void* pvParameters) { if (ss2k->resetPowerTableFlag) { powerTable->reset(); } - loopCounter++; - -#ifdef DEBUG_STACK - Serial.printf("ERG Task: %d \n", uxTaskGetStackHighWaterMark(ErgTask)); -#endif // DEBUG_STACK } } @@ -932,8 +913,8 @@ bool PowerTable::_manageSaveState() { } this->tableRow[i].tableEntry[j].targetPosition = savedTargetPosition; this->tableRow[i].tableEntry[j].readings = savedReadings; - //SS2K_LOG(POWERTABLE_LOG_TAG, "Position %d, %d, Target %d, Readings %d, loaded", i, j, this->tableRow[i].tableEntry[j].targetPosition, - // this->tableRow[i].tableEntry[j].readings); + // SS2K_LOG(POWERTABLE_LOG_TAG, "Position %d, %d, Target %d, Readings %d, loaded", i, j, this->tableRow[i].tableEntry[j].targetPosition, + // this->tableRow[i].tableEntry[j].readings); } } @@ -1146,17 +1127,15 @@ void ErgMode::_setPointChangeState(int newCadence, Measurement& newWatts) { SS2K_LOG(ERG_MODE_LOG_TAG, "SetPoint changed:%dw PowerTable Result: %d", newWatts.getTarget(), tableResult); _updateValues(newCadence, newWatts, tableResult); - int i = 0; - while (rtConfig->getTargetIncline() != rtConfig->getCurrentIncline()) { // wait while the knob moves to target position. - vTaskDelay(100 / portTICK_PERIOD_MS); - if (i > 50) { // failsafe for infinite loop - SS2K_LOG(ERG_MODE_LOG_TAG, "Stepper didn't reach target position"); - break; + if (rtConfig->getTargetIncline() != rtConfig->getCurrentIncline()) { // add some time to wait while the knob moves to target position. + int timeToAdd = abs(rtConfig->getCurrentIncline() - rtConfig->getTargetIncline()); + if (timeToAdd > 5000) { // 5 seconds + SS2K_LOG(ERG_MODE_LOG_TAG, "Capping ERG seek time to 5 seconds"); + timeToAdd = 5000; } - i++; + ergTimer += timeToAdd; } - - vTaskDelay((ERG_MODE_DELAY * 2) / portTICK_PERIOD_MS); // Wait for power meter to register new watts + ergTimer += (ERG_MODE_DELAY * 2); // Wait for power meter to register new watts } void ErgMode::_inSetpointState(int newCadence, Measurement& newWatts) { diff --git a/src/HTTP_Server_Basic.cpp b/src/HTTP_Server_Basic.cpp index 9fe2a4c3..1e4a76c3 100644 --- a/src/HTTP_Server_Basic.cpp +++ b/src/HTTP_Server_Basic.cpp @@ -26,8 +26,6 @@ File fsUploadFile; -TaskHandle_t webClientTask; - IPAddress myIP; // DNS server @@ -55,7 +53,7 @@ void _staSetup() { void _APSetup() { // WiFi.eraseAP(); //Needed if we switch back to espressif32 @6.5.0 WiFi.mode(WIFI_AP); - WiFi.setHostname("reset"); // Fixes a bug when switching Arduino Core Versions + WiFi.setHostname("reset"); // Fixes a bug when switching Arduino Core Versions WiFi.softAPsetHostname("reset"); WiFi.setHostname(userConfig->getDeviceName()); WiFi.softAPsetHostname(userConfig->getDeviceName()); @@ -191,8 +189,7 @@ void HTTP_Server::start() { SS2K_LOG(HTTP_SERVER_LOG_TAG, "Rebooting from Web Request"); String response = "Rebooting...."; server.send(200, "text/html", response); - vTaskDelay(100 / portTICK_PERIOD_MS); - ESP.restart(); + ss2k->rebootFlag = true; }); server.on("/hrslider", []() { @@ -367,8 +364,7 @@ void HTTP_Server::start() { server.send(200, "text/plain", "Littlefs Uploaded Successfully. Rebooting..."); userConfig->saveToLittleFS(); userPWC->saveToLittleFS(); - vTaskDelay(100); - ESP.restart(); + ss2k->rebootFlag == true; } else { Update.printError(Serial); } @@ -399,14 +395,6 @@ void HTTP_Server::start() { /********************************************End Server * Handlers*******************************/ - xTaskCreatePinnedToCore(HTTP_Server::webClientUpdate, /* Task function. */ - "webClientUpdate", /* name of task. */ - HTTP_STACK + (DEBUG_LOG_BUFFER_SIZE * 2), /* Stack size of task Used to be 3000*/ - NULL, /* parameter of the task */ - 10, /* priority of the task */ - &webClientTask, /* Task handle to keep track of created task */ - 0); /* pin task to core */ - #ifdef USE_TELEGRAM xTaskCreatePinnedToCore(telegramUpdate, /* Task function. */ "telegramUpdate", /* name of task. */ @@ -420,11 +408,12 @@ void HTTP_Server::start() { SS2K_LOG(HTTP_SERVER_LOG_TAG, "HTTP server started"); } -void HTTP_Server::webClientUpdate(void *pvParameters) { - static unsigned long mDnsTimer = millis(); // NOLINT: There is no overload in String for uint64_t - for (;;) { +void HTTP_Server::webClientUpdate() { + static unsigned long int _webClientTimer = millis(); + if (millis() - _webClientTimer > WEBSERVER_DELAY) { + _webClientTimer = millis(); + static unsigned long mDnsTimer = millis(); // NOLINT: There is no overload in String for uint64_t server.handleClient(); - vTaskDelay(WEBSERVER_DELAY / portTICK_RATE_MS); if (WiFi.getMode() != WIFI_MODE_STA) { dnsServer.processNextRequest(); } @@ -432,14 +421,11 @@ void HTTP_Server::webClientUpdate(void *pvParameters) { if ((millis() - mDnsTimer) > 30000) { MDNS.addServiceTxt("http", "_tcp", "lf", String(mDnsTimer)); mDnsTimer = millis(); -#ifdef DEBUG_STACK - Serial.printf("HttpServer: %d \n", uxTaskGetStackHighWaterMark(webClientTask)); -#endif // DEBUG_STACK } } } -void HTTP_Server::handleBTScanner(){ +void HTTP_Server::handleBTScanner() { spinBLEClient.doScan = true; handleLittleFSFile(); } @@ -656,8 +642,7 @@ void HTTP_Server::settingsProcessor() { "setTimeout(\"location.href = 'http://" + myIP.toString() + "/bluetoothscanner.html';\",5000);"; server.send(200, "text/html", response); - vTaskDelay(100 / portTICK_PERIOD_MS); - ESP.restart(); + ss2k->rebootFlag = true; } server.send(200, "text/html", response); } diff --git a/src/Main.cpp b/src/Main.cpp index 2e81cece..28aaf54b 100644 --- a/src/Main.cpp +++ b/src/Main.cpp @@ -30,7 +30,6 @@ AuxSerialBuffer auxSerialBuffer; FastAccelStepperEngine engine = FastAccelStepperEngine(); FastAccelStepper *stepper = NULL; -TaskHandle_t moveStepperTask; TaskHandle_t maintenanceLoopTask; Boards boards; @@ -52,12 +51,7 @@ WebSocketAppender webSocketAppender; void SS2K::startTasks() { SS2K_LOG(MAIN_LOG_TAG, "Start BLE + ERG Tasks"); spinBLEClient.intentionalDisconnect = 0; - if (BLECommunicationTask == NULL) { - setupBLE(); - } - if (ErgTask == NULL) { - setupERG(); - } + setupBLE(); } void SS2K::stopTasks() { @@ -69,14 +63,6 @@ void SS2K::stopTasks() { ss2k->stopTasks(); } SS2K_LOG(MAIN_LOG_TAG, "Stop BLE + ERG Tasks"); - if (BLECommunicationTask != NULL) { - vTaskDelete(BLECommunicationTask); - BLECommunicationTask = NULL; - } - if (ErgTask != NULL) { - vTaskDelete(ErgTask); - ErgTask = NULL; - } if (BLEClientTask != NULL) { vTaskDelete(BLEClientTask); BLEClientTask = NULL; @@ -149,14 +135,6 @@ void setup() { disableCore0WDT(); // Disable the watchdog timer on core 0 (so long stepper // moves don't cause problems) - xTaskCreatePinnedToCore(SS2K::moveStepper, /* Task function. */ - "moveStepperFunction", /* name of task. */ - STEPPER_STACK, /* Stack size of task */ - NULL, /* parameter of the task */ - 18, /* priority of the task */ - &moveStepperTask, /* Task handle to keep track of created task */ - 0); /* pin task to core */ - digitalWrite(LED_PIN, HIGH); // Configure and Initialize Logger @@ -188,18 +166,24 @@ void loop() { // Delete this task so we can make one that's more memory efficie } void SS2K::maintenanceLoop(void *pvParameters) { - static int loopCounter = 0; static unsigned long intervalTimer = millis(); static unsigned long intervalTimer2 = millis(); static unsigned long rebootTimer = millis(); static bool isScanning = false; while (true) { - vTaskDelay(73 / portTICK_RATE_MS); + vTaskDelay(5 / portTICK_RATE_MS); + // Run what used to be in the BLECommunications Task. + BLECommunications(); // send BLE notification for any userConfig values that changed. BLE_ss2kCustomCharacteristic::parseNemit(); - + // Run What used to be in the Stepper Task. + ss2k->moveStepper(); + // Run what used to be in the ERG Mode Task. + powerTable->runERG(); + // Run what used to be in the WebClient Task. + httpServer.webClientUpdate(); // If we're in ERG mode, modify shift commands to inc/dec the target watts instead. ss2k->FTMSModeShiftModifier(); // If we have a resistance bike attached, slow down when we're close to the limits. @@ -243,6 +227,7 @@ void SS2K::maintenanceLoop(void *pvParameters) { // Handle flag set for rebooting if (ss2k->rebootFlag) { static bool _loopOnce = false; + vTaskDelay(1000 / portTICK_RATE_MS); // Let the main task loop complete once before rebooting if (_loopOnce) { // Important to keep this delay high in order to allow coms to finish. @@ -302,24 +287,14 @@ void SS2K::maintenanceLoop(void *pvParameters) { rebootTimer = millis(); } - intervalTimer2 = millis(); - } - - // Things to do every 20 loops - if (loopCounter > 20) { - // Removed driver temp checking. This really doesn't do anything benificial anyway, because the thermistors in the ESP32 are not accurate. - // The Driver itsself will throttle automatically if the temp is too high. - // ss2k->checkDriverTemperature(); - #ifdef DEBUG_STACK - Serial.printf("Step Task: %d \n", uxTaskGetStackHighWaterMark(moveStepperTask)); Serial.printf("Main Task: %d \n", uxTaskGetStackHighWaterMark(maintenanceLoopTask)); Serial.printf("Free Heap: %d \n", ESP.getFreeHeap()); Serial.printf("Best Blok: %d \n", heap_caps_get_largest_free_block(MALLOC_CAP_8BIT)); #endif // DEBUG_STACK - loopCounter = 0; + + intervalTimer2 = millis(); } - loopCounter++; } } @@ -405,78 +380,74 @@ void SS2K::restartWifi() { httpServer.start(); } -void SS2K::moveStepper(void *pvParameters) { +void SS2K::moveStepper() { bool _stepperDir = userConfig->getStepperDir(); - - while (1) { - if (stepper) { - ss2k->stepperIsRunning = stepper->isRunning(); - ss2k->currentPosition = stepper->getCurrentPosition(); - if (!ss2k->externalControl) { - if ((rtConfig->getFTMSMode() == FitnessMachineControlPointProcedure::SetTargetPower) || - (rtConfig->getFTMSMode() == FitnessMachineControlPointProcedure::SetTargetResistanceLevel)) { - ss2k->targetPosition = rtConfig->getTargetIncline(); - } else { - // Simulation Mode - ss2k->targetPosition = rtConfig->getShifterPosition() * userConfig->getShiftStep(); - ss2k->targetPosition += rtConfig->getTargetIncline() * userConfig->getInclineMultiplier(); - } + if (stepper) { + ss2k->stepperIsRunning = stepper->isRunning(); + ss2k->currentPosition = stepper->getCurrentPosition(); + if (!ss2k->externalControl) { + if ((rtConfig->getFTMSMode() == FitnessMachineControlPointProcedure::SetTargetPower) || + (rtConfig->getFTMSMode() == FitnessMachineControlPointProcedure::SetTargetResistanceLevel)) { + ss2k->targetPosition = rtConfig->getTargetIncline(); + } else { + // Simulation Mode + ss2k->targetPosition = rtConfig->getShifterPosition() * userConfig->getShiftStep(); + ss2k->targetPosition += rtConfig->getTargetIncline() * userConfig->getInclineMultiplier(); } + } - if (ss2k->syncMode) { - stepper->stopMove(); - vTaskDelay(100 / portTICK_PERIOD_MS); - stepper->setCurrentPosition(ss2k->targetPosition); - vTaskDelay(100 / portTICK_PERIOD_MS); - } + if (ss2k->syncMode) { + stepper->stopMove(); + vTaskDelay(100 / portTICK_PERIOD_MS); + stepper->setCurrentPosition(ss2k->targetPosition); + vTaskDelay(100 / portTICK_PERIOD_MS); + } - if (ss2k->pelotonIsConnected) { - if ((rtConfig->resistance.getValue() > rtConfig->getMinResistance()) && (rtConfig->resistance.getValue() < rtConfig->getMaxResistance())) { + if (ss2k->pelotonIsConnected) { + if ((rtConfig->resistance.getValue() > rtConfig->getMinResistance()) && (rtConfig->resistance.getValue() < rtConfig->getMaxResistance())) { + stepper->moveTo(ss2k->targetPosition); + } else if (rtConfig->resistance.getValue() <= rtConfig->getMinResistance()) { // Limit Stepper to Min Resistance + if (rtConfig->resistance.getValue() != rtConfig->getMinResistance()) { + stepper->moveTo(stepper->getCurrentPosition() + 20); + } + // Let the user Shift Out of this Position + if (ss2k->targetPosition > stepper->getCurrentPosition()) { stepper->moveTo(ss2k->targetPosition); - } else if (rtConfig->resistance.getValue() <= rtConfig->getMinResistance()) { // Limit Stepper to Min Resistance - if (rtConfig->resistance.getValue() != rtConfig->getMinResistance()) { - stepper->moveTo(stepper->getCurrentPosition() + 20); - } - // Let the user Shift Out of this Position - if (ss2k->targetPosition > stepper->getCurrentPosition()) { - stepper->moveTo(ss2k->targetPosition); - } - } else { // Limit Stepper to Max Resistance - if (rtConfig->resistance.getValue() != rtConfig->getMaxResistance()) { - stepper->moveTo(stepper->getCurrentPosition() - 20); - } - // Let the user Shift Out of this Position - if (ss2k->targetPosition < stepper->getCurrentPosition()) { - stepper->moveTo(ss2k->targetPosition); - } } - - } else { - if ((ss2k->targetPosition >= rtConfig->getMinStep()) && (ss2k->targetPosition <= rtConfig->getMaxStep())) { + } else { // Limit Stepper to Max Resistance + if (rtConfig->resistance.getValue() != rtConfig->getMaxResistance()) { + stepper->moveTo(stepper->getCurrentPosition() - 20); + } + // Let the user Shift Out of this Position + if (ss2k->targetPosition < stepper->getCurrentPosition()) { stepper->moveTo(ss2k->targetPosition); - } else if (ss2k->targetPosition <= rtConfig->getMinStep()) { // Limit Stepper to Min Position - stepper->moveTo(rtConfig->getMinStep()); - } else { // Limit Stepper to Max Position - stepper->moveTo(rtConfig->getMaxStep()); } } - rtConfig->setCurrentIncline((float)stepper->getCurrentPosition()); - vTaskDelay(50 / portTICK_PERIOD_MS); - 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. + } else { + if ((ss2k->targetPosition >= rtConfig->getMinStep()) && (ss2k->targetPosition <= rtConfig->getMaxStep())) { + stepper->moveTo(ss2k->targetPosition); + } else if (ss2k->targetPosition <= rtConfig->getMinStep()) { // Limit Stepper to Min Position + stepper->moveTo(rtConfig->getMinStep()); + } else { // Limit Stepper to Max Position + stepper->moveTo(rtConfig->getMaxStep()); } + } + rtConfig->setCurrentIncline((float)stepper->getCurrentPosition()); + + if (connectedClientCount() > 0) { + stepper->setAutoEnable(false); // Keep the stepper from rolling back due to head tube slack. Motor Driver still lowers power between moves + stepper->enableOutputs(); + } else { + stepper->setAutoEnable(true); // disable output FETs between moves so stepper can cool. Can still shift. + } - if (_stepperDir != userConfig->getStepperDir()) { // User changed the config direction of the stepper wires - _stepperDir = userConfig->getStepperDir(); - while (stepper->isRunning()) { // Wait until the motor stops running - vTaskDelay(100 / portTICK_PERIOD_MS); - } - stepper->setDirectionPin(currentBoard.dirPin, _stepperDir); + if (_stepperDir != userConfig->getStepperDir()) { // User changed the config direction of the stepper wires + _stepperDir = userConfig->getStepperDir(); + while (stepper->isRunning()) { // Wait until the motor stops running + vTaskDelay(100 / portTICK_PERIOD_MS); } + stepper->setDirectionPin(currentBoard.dirPin, _stepperDir); } } }