From 3e9fc978417d2964d04472c0f47207d3b35fd875 Mon Sep 17 00:00:00 2001 From: ddtomic Date: Tue, 14 Jan 2025 19:20:51 -0600 Subject: [PATCH 1/6] added comments for readability on insetpoint --- src/ERG_Mode.cpp | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/src/ERG_Mode.cpp b/src/ERG_Mode.cpp index 51ea2ce9..3e201904 100644 --- a/src/ERG_Mode.cpp +++ b/src/ERG_Mode.cpp @@ -1182,14 +1182,22 @@ void ErgMode::_setPointChangeState(int newCadence, Measurement& newWatts) { } void ErgMode::_inSetpointState(int newCadence, Measurement& newWatts) { + // retrieves the current Watt output int watts = newWatts.getValue(); - int wattChange = newWatts.getTarget() - watts; // setpoint_form_trainer - current_torque => Amount to increase or decrease incline + // setpoint_form_trainer - current_torque => Amount to increase or decrease incline + int wattChange = newWatts.getTarget() - watts; + + // Calculates how far current power is from the target power, measred as a percentage of target float deviation = ((float)wattChange * 100.0) / ((float)newWatts.getTarget()); - float factor = abs(deviation) > 10 ? userConfig->getERGSensitivity() : userConfig->getERGSensitivity() / 2; + // retrieves the sensitivity from adjustments from userConfig + float factor = abs(deviation) > 10 ? userConfig->getERGSensitivity() : userConfig->getERGSensitivity() / 2; + + // Adjusts the incline float newIncline = ss2k->getCurrentPosition() + (wattChange * factor); + // passes to apply new cadence, power measurement and incline settings _updateValues(newCadence, newWatts, newIncline); } From 44686b22d94a32fcf2a36e93cddb176c03b010e3 Mon Sep 17 00:00:00 2001 From: ddtomic Date: Wed, 15 Jan 2025 15:17:42 -0600 Subject: [PATCH 2/6] implementing PID, WIP --- src/ERG_Mode.cpp | 69 +++++++++++++++++++++++++++++++++++++++++------- 1 file changed, 60 insertions(+), 9 deletions(-) diff --git a/src/ERG_Mode.cpp b/src/ERG_Mode.cpp index 3e201904..c71389c7 100644 --- a/src/ERG_Mode.cpp +++ b/src/ERG_Mode.cpp @@ -1181,26 +1181,77 @@ void ErgMode::_setPointChangeState(int newCadence, Measurement& newWatts) { ergTimer += (ERG_MODE_DELAY * 2); // Wait for power meter to register new watts } +// INTRODUCING PID CONTROL LOOP +// Error: Difference between TW and Current W + +// Proportional term: Directly Proportional to error +// Integral term: accumulated sum of errors over time +// Derivative term: rate of change of error + +// PrevError void ErgMode::_inSetpointState(int newCadence, Measurement& newWatts) { + // Setting Gains For PID Loop + float Kp = 0.1; + float Ki = 0.1; + float Kd = 0.1; + + // Initialize PID, Neeed to move these to prevent them resetting every loop + float integral = 0.0; + float prevError = 0.0; + // retrieves the current Watt output int watts = newWatts.getValue(); + // retrieves target Watt output + int target = newWatts.getTarget(); + // subtracting target from current watts + float error = target - watts; + + // Defining proportional term + float proportional = Kp * error; + + // Defining integral term + integral += error; + float integralFinal = Ki * integral; + + // NEED TO GO BACK AND CLAMP DOWN INTEGRAL TO PREVENT IT FROM BEING TOO LARGE - // setpoint_form_trainer - current_torque => Amount to increase or decrease incline - int wattChange = newWatts.getTarget() - watts; + // Defining derivative term + float derivative = error - prevError; // Difference between current and previous errors + float derivativeTerm = Kd * derivative; - // Calculates how far current power is from the target power, measred as a percentage of target - float deviation = ((float)wattChange * 100.0) / ((float)newWatts.getTarget()); + // final PID output + float PID_output = proportional + integralFinal + derivativeTerm; - // retrieves the sensitivity from adjustments from userConfig - float factor = abs(deviation) > 10 ? userConfig->getERGSensitivity() : userConfig->getERGSensitivity() / 2; + // Calculate the new incline + float newIncline = ss2k->getCurrentPosition() + PID_output; - // Adjusts the incline - float newIncline = ss2k->getCurrentPosition() + (wattChange * factor); + prevError = error; - // passes to apply new cadence, power measurement and incline settings + // Apply the new values _updateValues(newCadence, newWatts, newIncline); } +// OLD PI FUNCTION +// void ErgMode::_inSetpointState(int newCadence, Measurement& newWatts) { +// // retrieves the current Watt output +// int watts = newWatts.getValue(); + +// // setpoint_form_trainer - current_torque => Amount to increase or decrease incline +// int wattChange = newWatts.getTarget() - watts; + +// // Calculates how far current power is from the target power, measred as a percentage of target +// float deviation = ((float)wattChange * 100.0) / ((float)newWatts.getTarget()); + +// // retrieves the sensitivity from adjustments from userConfig +// float factor = abs(deviation) > 10 ? userConfig->getERGSensitivity() : userConfig->getERGSensitivity() / 2; + +// // Adjusts the incline +// float newIncline = ss2k->getCurrentPosition() + (wattChange * factor); + +// // passes to apply new cadence, power measurement and incline settings +// _updateValues(newCadence, newWatts, newIncline); +// } + void ErgMode::_updateValues(int newCadence, Measurement& newWatts, float newIncline) { rtConfig->setTargetIncline(newIncline); _writeLog(ss2k->getCurrentPosition(), newIncline, this->setPoint, newWatts.getTarget(), this->watts.getValue(), newWatts.getValue(), this->cadence, newCadence); From 2d506ede37fca859cf5f9c7482bf936f38a2496d Mon Sep 17 00:00:00 2001 From: ddtomic Date: Wed, 15 Jan 2025 16:57:43 -0600 Subject: [PATCH 3/6] changing variable scope PID --- 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 c71389c7..ea47e36e 100644 --- a/src/ERG_Mode.cpp +++ b/src/ERG_Mode.cpp @@ -1195,9 +1195,9 @@ void ErgMode::_inSetpointState(int newCadence, Measurement& newWatts) { float Ki = 0.1; float Kd = 0.1; - // Initialize PID, Neeed to move these to prevent them resetting every loop - float integral = 0.0; - float prevError = 0.0; + // initialize, may need to double check it's not resetting every time + static float integral = 0.0; + static float prevError = 0.0; // retrieves the current Watt output int watts = newWatts.getValue(); @@ -1222,7 +1222,7 @@ void ErgMode::_inSetpointState(int newCadence, Measurement& newWatts) { // final PID output float PID_output = proportional + integralFinal + derivativeTerm; - // Calculate the new incline + // Calculate new incline float newIncline = ss2k->getCurrentPosition() + PID_output; prevError = error; From a9aa97f788ce5af1bf6b2262ee1f15e7398f1525 Mon Sep 17 00:00:00 2001 From: Anthony Doud Date: Wed, 15 Jan 2025 18:51:59 -0600 Subject: [PATCH 4/6] Added toggles for turning on/off ERG control methods --- CHANGELOG.md | 1 + include/settings.h | 9 +++++++++ src/ERG_Mode.cpp | 19 +++++++++++++++++-- src/Main.cpp | 2 ++ 4 files changed, 29 insertions(+), 2 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 8eda3ead..7c0abe22 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -10,6 +10,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 ### Added ### Changed +-PID loop for ERG mode instead of P loop. ### Hardware diff --git a/include/settings.h b/include/settings.h index 51314d62..a0c55166 100644 --- a/include/settings.h +++ b/include/settings.h @@ -253,6 +253,15 @@ const char* const DEFAULT_PASSWORD = "password"; #define RUNTIMECONFIG_JSON_SIZE 1000 + DEBUG_LOG_BUFFER_SIZE +// Uncomment to use guardrails for ERG mode in the stepper loop. +// #define ERG_GUARDRAILS + +//Uncomment to enable the use of the power table for ERG mode. +//#define ERG_MODE_USE_POWER_TABLE + +// Uncomment to use the PID controller for ERG mode. +#define ERG_MODE_USE_PID + // PowerTable Version #define TABLE_VERSION 5 diff --git a/src/ERG_Mode.cpp b/src/ERG_Mode.cpp index ea47e36e..55ecf7e3 100644 --- a/src/ERG_Mode.cpp +++ b/src/ERG_Mode.cpp @@ -1134,14 +1134,22 @@ void ErgMode::computeErg() { return; } - // SetPoint changed +#ifdef ERG_MODE_USE_POWER_TABLE +// SetPoint changed +#ifdef ERG_MODE_USE_PID if (abs(this->setPoint - newWatts.getTarget()) > 20) { +#endif _setPointChangeState(newCadence, newWatts); return; +#ifdef ERG_MODE_USE_PID } +#endif +#endif +#ifdef ERG_MODE_USE_PID // Setpoint unchanged _inSetpointState(newCadence, newWatts); +#endif } void ErgMode::_setPointChangeState(int newCadence, Measurement& newWatts) { @@ -1191,7 +1199,7 @@ void ErgMode::_setPointChangeState(int newCadence, Measurement& newWatts) { // PrevError void ErgMode::_inSetpointState(int newCadence, Measurement& newWatts) { // Setting Gains For PID Loop - float Kp = 0.1; + float Kp = userConfig->getERGSensitivity(); float Ki = 0.1; float Kd = 0.1; @@ -1222,6 +1230,13 @@ void ErgMode::_inSetpointState(int newCadence, Measurement& newWatts) { // final PID output float PID_output = proportional + integralFinal + derivativeTerm; + // log proportional, integral, derivative every five seconds + static unsigned long lastTime = 0; + if (millis() - lastTime > 5000) { + lastTime = millis(); + SS2K_LOG(ERG_MODE_LOG_TAG, "Proportional: %f, Integral: %f, Derivative: %f", proportional, integralFinal, derivativeTerm); + } + // Calculate new incline float newIncline = ss2k->getCurrentPosition() + PID_output; diff --git a/src/Main.cpp b/src/Main.cpp index b495a40b..5b9cc449 100644 --- a/src/Main.cpp +++ b/src/Main.cpp @@ -405,6 +405,7 @@ void SS2K::moveStepper() { ss2k->currentPosition = stepper->getCurrentPosition(); if (!ss2k->externalControl) { if ((rtConfig->getFTMSMode() == FitnessMachineControlPointProcedure::SetTargetPower)) { + #ifdef ERG_GUARDRAILS // don't drive lower out of bounds. This is a final test that should never happen. if ((stepper->getCurrentPosition() > rtConfig->getTargetIncline()) && (rtConfig->watts.getValue() < rtConfig->watts.getTarget())) { rtConfig->setTargetIncline(stepper->getCurrentPosition() + 1); @@ -413,6 +414,7 @@ void SS2K::moveStepper() { if ((stepper->getCurrentPosition() < rtConfig->getTargetIncline()) && (rtConfig->watts.getValue() > rtConfig->watts.getTarget())) { rtConfig->setTargetIncline(stepper->getCurrentPosition() - 1); } + #endif ss2k->targetPosition = rtConfig->getTargetIncline(); } else if (rtConfig->getFTMSMode() == FitnessMachineControlPointProcedure::SetTargetResistanceLevel) { rtConfig->setTargetIncline(ss2k->currentPosition + ((rtConfig->resistance.getTarget() - rtConfig->resistance.getValue()) * 20)); From 0bdb0926d9b8ed197bebf934ccf36e7909d26f0c Mon Sep 17 00:00:00 2001 From: Anthony Doud Date: Thu, 16 Jan 2025 13:44:19 -0600 Subject: [PATCH 5/6] brought the ERGSensitivity setting (Kp) so it fits better with the current defaults. --- 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 55ecf7e3..cecaf661 100644 --- a/src/ERG_Mode.cpp +++ b/src/ERG_Mode.cpp @@ -1199,7 +1199,7 @@ void ErgMode::_setPointChangeState(int newCadence, Measurement& newWatts) { // PrevError void ErgMode::_inSetpointState(int newCadence, Measurement& newWatts) { // Setting Gains For PID Loop - float Kp = userConfig->getERGSensitivity(); + float Kp = userConfig->getERGSensitivity()*2; float Ki = 0.1; float Kd = 0.1; From 2c8508f6ce172700d75a6eec2cf8d30e16224183 Mon Sep 17 00:00:00 2001 From: ddtomic Date: Tue, 21 Jan 2025 19:18:53 -0600 Subject: [PATCH 6/6] clamping down integral term --- src/ERG_Mode.cpp | 13 ++++++++++--- 1 file changed, 10 insertions(+), 3 deletions(-) diff --git a/src/ERG_Mode.cpp b/src/ERG_Mode.cpp index ea47e36e..95ce3c4d 100644 --- a/src/ERG_Mode.cpp +++ b/src/ERG_Mode.cpp @@ -1191,11 +1191,10 @@ void ErgMode::_setPointChangeState(int newCadence, Measurement& newWatts) { // PrevError void ErgMode::_inSetpointState(int newCadence, Measurement& newWatts) { // Setting Gains For PID Loop - float Kp = 0.1; + float Kp = userConfig->getERGSensitivity(); float Ki = 0.1; float Kd = 0.1; - // initialize, may need to double check it's not resetting every time static float integral = 0.0; static float prevError = 0.0; @@ -1213,7 +1212,15 @@ void ErgMode::_inSetpointState(int newCadence, Measurement& newWatts) { integral += error; float integralFinal = Ki * integral; - // NEED TO GO BACK AND CLAMP DOWN INTEGRAL TO PREVENT IT FROM BEING TOO LARGE + // Clamping down integral term + float integralMax = 60; + float integralMin = -60; + + if (integral > integralMax) { + integral = integralMax; + } else if (integral < integralMin) { + integral = integralMin; + } // Defining derivative term float derivative = error - prevError; // Difference between current and previous errors