From fa114e41389c74bfff4cc187b24ff438c5b6e6df Mon Sep 17 00:00:00 2001 From: jonny Date: Sun, 12 May 2024 16:52:44 +0200 Subject: [PATCH 1/4] Add 'decel-boost' when reversing joystick (motorctl) when moving the joystick in opposite direction the deceleration gets boosted depending on how large the opposite target duty is up to a maximum Needs testing, switching between ROTATE mode might cause issues --- common/motorctl.cpp | 24 ++++++++++++++++++++---- 1 file changed, 20 insertions(+), 4 deletions(-) diff --git a/common/motorctl.cpp b/common/motorctl.cpp index c5b1ff7..d3598cf 100644 --- a/common/motorctl.cpp +++ b/common/motorctl.cpp @@ -329,12 +329,28 @@ if ( dutyNow != 0 && esp_log_timestamp() - timestamp_commandReceived > TIMEOUT_I else //no accel limit (immediately set to 100) dutyIncrementAccel = 100; +#define DECEL_BOOST_START_THRESHOLD 10 // boost deceleration when stick/target duty is more than that value in opposite direction +#define DECEL_BOOST_MIN_DECEL_TIME 200 // milliseconds from 100% to 0% //calculate increment for fading DOWN with passed time since last run and configured fade time - if (msFadeDecel > 0) - dutyIncrementDecel = ( usPassed / ((float)msFadeDecel * 1000) ) * 100; - else //no decel limit (immediately reduce to 0) + if (msFadeDecel == 0) //no decel limit (immediately reduce to 0) dutyIncrementDecel = 100; - + //--- dynamic fading --- + //detect when quicker brake response is desired (e.g. full speed forward, joystick suddenly is full reverse -> break fast) + else if (commandReceive.state != state && fabs(dutyTarget) > DECEL_BOOST_START_THRESHOLD) + { + float normalIncrementDecel = (usPassed / ((float)msFadeDecel * 1000)) * 100; //TODO limit all increments to 100? + //calculate absolute maximum allowed deceleration + float maximumIncrementDecel = (usPassed / (DECEL_BOOST_MIN_DECEL_TIME * 1000)) * 100; + //calculate how much boost is applied (percent) depending on how much the joystick is in opposite direction + float decelBoostFactor = (fabs(dutyTarget) - DECEL_BOOST_START_THRESHOLD) / (100 - DECEL_BOOST_START_THRESHOLD); + //calculate total deceleration increment (normal + boost) + dutyIncrementDecel = normalIncrementDecel + decelBoostFactor * fabs(maximumIncrementDecel - normalIncrementDecel); + if(log) ESP_LOGW(TAG, "boosting deceleration by %.2f%% of remainder to max decel", decelBoostFactor * 100); + } + else + // normal deceleration according to configured time + dutyIncrementDecel = (usPassed / ((float)msFadeDecel * 1000)) * 100; + //fade duty to target (up and down) //TODO: this needs optimization (can be more clear and/or simpler) if (dutyDelta > 0) { //difference positive -> increasing duty (-100 -> 100) From 967f2cd8b50ab4b4917870882a96aa79ff6183e0 Mon Sep 17 00:00:00 2001 From: jonny_l480 Date: Wed, 15 May 2024 18:42:49 +0200 Subject: [PATCH 2/4] Fix dynamic 'decel-boost' - proof of concept works When moving the joystick in other direction than driving the deceleration gets speed up. The boost scales with how far the stick is in other direction up to a maximum Some issues with this approach see notes in code --- board_single/main/config.cpp | 2 +- common/motorctl.cpp | 6 ++++-- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/board_single/main/config.cpp b/board_single/main/config.cpp index 1f46f23..a5efdf7 100644 --- a/board_single/main/config.cpp +++ b/board_single/main/config.cpp @@ -32,7 +32,7 @@ void setLoglevels(void) // esp_log_level_set("motordriver", ESP_LOG_DEBUG); esp_log_level_set("motor-control", ESP_LOG_WARN); // esp_log_level_set("evaluatedJoystick", ESP_LOG_DEBUG); - // esp_log_level_set("joystickCommands", ESP_LOG_DEBUG); + esp_log_level_set("joystickCommands", ESP_LOG_DEBUG); esp_log_level_set("button", ESP_LOG_INFO); esp_log_level_set("control", ESP_LOG_INFO); // esp_log_level_set("fan-control", ESP_LOG_INFO); diff --git a/common/motorctl.cpp b/common/motorctl.cpp index d3598cf..b0c06f9 100644 --- a/common/motorctl.cpp +++ b/common/motorctl.cpp @@ -330,8 +330,9 @@ if ( dutyNow != 0 && esp_log_timestamp() - timestamp_commandReceived > TIMEOUT_I dutyIncrementAccel = 100; #define DECEL_BOOST_START_THRESHOLD 10 // boost deceleration when stick/target duty is more than that value in opposite direction -#define DECEL_BOOST_MIN_DECEL_TIME 200 // milliseconds from 100% to 0% +#define DECEL_BOOST_MIN_DECEL_TIME 350 // milliseconds from 100% to 0% //calculate increment for fading DOWN with passed time since last run and configured fade time + // FIXME: dutyTarget does not represent joystick pos (max maxDuty) -> currently breaks when maxDuty is not set to 100 if (msFadeDecel == 0) //no decel limit (immediately reduce to 0) dutyIncrementDecel = 100; //--- dynamic fading --- @@ -340,12 +341,13 @@ if ( dutyNow != 0 && esp_log_timestamp() - timestamp_commandReceived > TIMEOUT_I { float normalIncrementDecel = (usPassed / ((float)msFadeDecel * 1000)) * 100; //TODO limit all increments to 100? //calculate absolute maximum allowed deceleration - float maximumIncrementDecel = (usPassed / (DECEL_BOOST_MIN_DECEL_TIME * 1000)) * 100; + float maximumIncrementDecel = ((float)usPassed / (DECEL_BOOST_MIN_DECEL_TIME * 1000)) * 100; //calculate how much boost is applied (percent) depending on how much the joystick is in opposite direction float decelBoostFactor = (fabs(dutyTarget) - DECEL_BOOST_START_THRESHOLD) / (100 - DECEL_BOOST_START_THRESHOLD); //calculate total deceleration increment (normal + boost) dutyIncrementDecel = normalIncrementDecel + decelBoostFactor * fabs(maximumIncrementDecel - normalIncrementDecel); if(log) ESP_LOGW(TAG, "boosting deceleration by %.2f%% of remainder to max decel", decelBoostFactor * 100); + if(log) ESP_LOGW(TAG, "normalInc=%.5f, maxInc=%.5f, totalInc=%.5f", normalIncrementDecel, maximumIncrementDecel, dutyIncrementDecel); } else // normal deceleration according to configured time From 3ebcd6ad3cb327ae8b0ac255973bb92adce9f33e Mon Sep 17 00:00:00 2001 From: jonny_l480 Date: Sat, 18 May 2024 23:32:52 +0200 Subject: [PATCH 3/4] Rework brake (decel-boost): simplify, config option Briefly tested this - brake works as intended - Add config options brakeDecel and brakePauseBefureResume - control: update brakeStartThresholdDuty at startup and maxDuty change for each motor - motorctl: drop/simplify decel boost to brake (faster deceleration at certain threshold) --- board_single/main/config.cpp | 10 ++++-- board_single/main/control.cpp | 4 +++ board_single/main/control.hpp | 8 ++++- common/motorctl.cpp | 64 +++++++++++++++++++++++------------ common/motorctl.hpp | 6 ++++ common/types.hpp | 2 ++ 6 files changed, 69 insertions(+), 25 deletions(-) diff --git a/board_single/main/config.cpp b/board_single/main/config.cpp index a5efdf7..e665ce9 100644 --- a/board_single/main/config.cpp +++ b/board_single/main/config.cpp @@ -32,7 +32,7 @@ void setLoglevels(void) // esp_log_level_set("motordriver", ESP_LOG_DEBUG); esp_log_level_set("motor-control", ESP_LOG_WARN); // esp_log_level_set("evaluatedJoystick", ESP_LOG_DEBUG); - esp_log_level_set("joystickCommands", ESP_LOG_DEBUG); + esp_log_level_set("joystickCommands", ESP_LOG_WARN); esp_log_level_set("button", ESP_LOG_INFO); esp_log_level_set("control", ESP_LOG_INFO); // esp_log_level_set("fan-control", ESP_LOG_INFO); @@ -108,7 +108,9 @@ motorctl_config_t configMotorControlLeft = { .currentMax = 30, .currentInverted = true, .currentSnapToZeroThreshold = 0.15, - .deadTimeMs = 0 // minimum time motor is off between direction change + .deadTimeMs = 0, // minimum time motor is off between direction change + .brakePauseBeforeResume = 1500, + .brakeDecel = 400, }; //--- configure right motor (contol) --- @@ -124,7 +126,9 @@ motorctl_config_t configMotorControlRight = { .currentMax = 30, .currentInverted = false, .currentSnapToZeroThreshold = 0.25, - .deadTimeMs = 0 // minimum time motor is off between direction change + .deadTimeMs = 0, // minimum time motor is off between direction change + .brakePauseBeforeResume = 1500, + .brakeDecel = 400, }; //------------------------------ diff --git a/board_single/main/control.cpp b/board_single/main/control.cpp index baa6384..684f562 100644 --- a/board_single/main/control.cpp +++ b/board_single/main/control.cpp @@ -59,6 +59,10 @@ controlledArmchair::controlledArmchair( // override default config value if maxDuty is found in nvs loadMaxDuty(); + // update brake start threshold with actual max duty for motorctl + ESP_LOGW(TAG, "setting brake start threshold for both motors to %.0f", joystickGenerateCommands_config.maxDutyStraight * BRAKE_START_STICK_PERCENTAGE / 100); + motorLeft->setBrakeStartThresholdDuty(joystickGenerateCommands_config.maxDutyStraight * BRAKE_START_STICK_PERCENTAGE / 100); + motorRight->setBrakeStartThresholdDuty(joystickGenerateCommands_config.maxDutyStraight * BRAKE_START_STICK_PERCENTAGE / 100); // create semaphore for preventing race condition: mode-change operations while currently still executing certain mode handleIteration_mutex = xSemaphoreCreateMutex(); diff --git a/board_single/main/control.hpp b/board_single/main/control.hpp index ebca9b3..419e977 100644 --- a/board_single/main/control.hpp +++ b/board_single/main/control.hpp @@ -13,6 +13,8 @@ extern "C" #include "speedsensor.hpp" #include "chairAdjust.hpp" +//percentage stick has to be moved in the opposite driving direction of current motor direction for braking to start +#define BRAKE_START_STICK_PERCENTAGE 95 //-------------------------------------------- //---- struct, enum, variable declarations --- @@ -93,7 +95,11 @@ class controlledArmchair { bool toggleAltStickMapping(); // configure max dutycycle (in joystick or http mode) - void setMaxDuty(float maxDutyNew) { writeMaxDuty(maxDutyNew); }; + void setMaxDuty(float maxDutyNew) { + writeMaxDuty(maxDutyNew); + motorLeft->setBrakeStartThresholdDuty(joystickGenerateCommands_config.maxDutyStraight * BRAKE_START_STICK_PERCENTAGE/100); + motorRight->setBrakeStartThresholdDuty(joystickGenerateCommands_config.maxDutyStraight * BRAKE_START_STICK_PERCENTAGE/100); + }; float getMaxDuty() const {return joystickGenerateCommands_config.maxDutyStraight; }; // configure max boost (in joystick or http mode) void setMaxRelativeBoostPer(float newValue) { joystickGenerateCommands_config.maxRelativeBoostPercentOfMaxDuty = newValue; }; diff --git a/common/motorctl.cpp b/common/motorctl.cpp index b0c06f9..de39ef9 100644 --- a/common/motorctl.cpp +++ b/common/motorctl.cpp @@ -320,40 +320,62 @@ if ( dutyNow != 0 && esp_log_timestamp() - timestamp_commandReceived > TIMEOUT_I //calculate passed time since last run int64_t usPassed = esp_timer_get_time() - timestampLastRunUs; - //--- calculate increment --- + //--- calculate increment (acceleration) --- //calculate increment for fading UP with passed time since last run and configured fade time + //- traction control - if (tcs_isExceeded) // disable acceleration when slippage is currently detected dutyIncrementAccel = 0; + //- recent braking - + //FIXME reset timeout when duty less + else if (isBraking && (esp_log_timestamp() - timestampBrakeStart) < config.brakePauseBeforeResume) // prevent immediate direction change when currently braking with timeout (eventually currently sliding) + { + if (log) ESP_LOGI(TAG, "pause after brake... -> accel = 0"); + dutyIncrementAccel = 0; + } + //- normal accel - else if (msFadeAccel > 0) dutyIncrementAccel = (usPassed / ((float)msFadeAccel * 1000)) * 100; // TODO define maximum increment - first run after startup (or long) pause can cause a very large increment + //- sport mode - else //no accel limit (immediately set to 100) dutyIncrementAccel = 100; -#define DECEL_BOOST_START_THRESHOLD 10 // boost deceleration when stick/target duty is more than that value in opposite direction -#define DECEL_BOOST_MIN_DECEL_TIME 350 // milliseconds from 100% to 0% - //calculate increment for fading DOWN with passed time since last run and configured fade time - // FIXME: dutyTarget does not represent joystick pos (max maxDuty) -> currently breaks when maxDuty is not set to 100 - if (msFadeDecel == 0) //no decel limit (immediately reduce to 0) + //--- calculate increment (deceleration) --- + //- sport mode - + if (msFadeDecel == 0){ //no decel limit (immediately reduce to 0) dutyIncrementDecel = 100; - //--- dynamic fading --- - //detect when quicker brake response is desired (e.g. full speed forward, joystick suddenly is full reverse -> break fast) - else if (commandReceive.state != state && fabs(dutyTarget) > DECEL_BOOST_START_THRESHOLD) - { - float normalIncrementDecel = (usPassed / ((float)msFadeDecel * 1000)) * 100; //TODO limit all increments to 100? - //calculate absolute maximum allowed deceleration - float maximumIncrementDecel = ((float)usPassed / (DECEL_BOOST_MIN_DECEL_TIME * 1000)) * 100; - //calculate how much boost is applied (percent) depending on how much the joystick is in opposite direction - float decelBoostFactor = (fabs(dutyTarget) - DECEL_BOOST_START_THRESHOLD) / (100 - DECEL_BOOST_START_THRESHOLD); - //calculate total deceleration increment (normal + boost) - dutyIncrementDecel = normalIncrementDecel + decelBoostFactor * fabs(maximumIncrementDecel - normalIncrementDecel); - if(log) ESP_LOGW(TAG, "boosting deceleration by %.2f%% of remainder to max decel", decelBoostFactor * 100); - if(log) ESP_LOGW(TAG, "normalInc=%.5f, maxInc=%.5f, totalInc=%.5f", normalIncrementDecel, maximumIncrementDecel, dutyIncrementDecel); } - else + //- brake - + //detect when quicker brake response is desired (e.g. full speed forward, joystick suddenly is full reverse -> break fast) + #define NO_BRAKE_THRESHOLD_TOO_SLOW_DUTY 10 //TODO test/adjust this - dont brake when slow already (avoids starting full dead time) + else if (commandReceive.state != state && // direction differs + fabs(dutyNow) > NO_BRAKE_THRESHOLD_TOO_SLOW_DUTY && // not very slow already + fabs(dutyTarget) > brakeStartThreshold) // joystick above threshold + { + // set braking state and track start time (both for disabling acceleration for some time) + if (!isBraking) { + if (log) ESP_LOGW(TAG, "started braking..."); + timestampBrakeStart = esp_log_timestamp(); + isBraking = true; + } + // use brake deceleration instead of normal deceleration + dutyIncrementDecel = (usPassed / ((float)config.brakeDecel * 1000)) * 100; + if(log) ESP_LOGI(TAG, "braking (target duty >%.0f%% in other direction) -> using deceleration %dms", brakeStartThreshold, config.brakeDecel); + } + //- normal deceleration - + else { // normal deceleration according to configured time dutyIncrementDecel = (usPassed / ((float)msFadeDecel * 1000)) * 100; + } - //fade duty to target (up and down) + // reset braking state when start condition is no longer met (stick below threshold again) + if (isBraking && + (fabs(dutyTarget) < brakeStartThreshold || commandReceive.state == state)) + { + ESP_LOGW(TAG, "brake condition no longer met"); + isBraking = false; + } + + //--- fade duty to target (up and down) --- //TODO: this needs optimization (can be more clear and/or simpler) if (dutyDelta > 0) { //difference positive -> increasing duty (-100 -> 100) if (dutyNow < 0) { //reverse, decelerating diff --git a/common/motorctl.hpp b/common/motorctl.hpp index 1ec9a41..7c0d50f 100644 --- a/common/motorctl.hpp +++ b/common/motorctl.hpp @@ -46,6 +46,7 @@ class controlledMotor { void disableTractionControlSystem() {config.tractionControlSystemEnabled = false; tcs_isExceeded = false;}; bool getTractionControlSystemStatus() {return config.tractionControlSystemEnabled;}; void setControlMode(motorControlMode_t newMode) {mode = newMode;}; + void setBrakeStartThresholdDuty(float duty) {brakeStartThreshold = duty;}; uint32_t getFade(fadeType_t fadeType); //get currently set acceleration or deceleration fading time uint32_t getFadeDefault(fadeType_t fadeType); //get acceleration or deceleration fading time from config @@ -129,6 +130,11 @@ class controlledMotor { uint32_t tcs_usExceeded = 0; //sum up time bool tcs_isExceeded = false; //is currently too fast int64_t tcs_timestampLastRun = 0; + + //brake (decel boost) + uint32_t timestampBrakeStart = 0; + bool isBraking = false; + float brakeStartThreshold = 60; }; //==================================== diff --git a/common/types.hpp b/common/types.hpp index 5421a3c..23f5d04 100644 --- a/common/types.hpp +++ b/common/types.hpp @@ -53,6 +53,8 @@ typedef struct motorctl_config_t { bool currentInverted; float currentSnapToZeroThreshold; uint32_t deadTimeMs; //time motor stays in IDLE before direction change + uint32_t brakePauseBeforeResume; + uint32_t brakeDecel; } motorctl_config_t; //enum fade type (acceleration, deceleration) From a839d61f65f475afe9e8b26fe937a7adf418d182 Mon Sep 17 00:00:00 2001 From: jonny_l480 Date: Mon, 20 May 2024 09:08:10 +0200 Subject: [PATCH 4/4] Add menu item to set brake-deceleration Also simplify motorctl by removing two variables and using config struct instead --- board_single/main/menu.cpp | 36 +++++++++++++++++++-- common/motorctl.cpp | 65 ++++++++++++++++++-------------------- common/motorctl.hpp | 7 ++-- 3 files changed, 69 insertions(+), 39 deletions(-) diff --git a/board_single/main/menu.cpp b/board_single/main/menu.cpp index f2870e5..eb5e94d 100644 --- a/board_single/main/menu.cpp +++ b/board_single/main/menu.cpp @@ -370,6 +370,38 @@ menuItem_t item_decelLimit = { }; +// ###################### +// ##### brakeDecel ##### +// ###################### +void item_brakeDecel_action(display_task_parameters_t * objects, SSD1306_t * display, int value) +{ + objects->motorLeft->setBrakeDecel((uint32_t)value); + objects->motorRight->setBrakeDecel((uint32_t)value); +} +int item_brakeDecel_value(display_task_parameters_t * objects) +{ + return objects->motorLeft->getBrakeDecel(); +} +int item_brakeDecel_default(display_task_parameters_t * objects) +{ + return objects->motorLeft->getBrakeDecelDefault(); +} +menuItem_t item_brakeDecel = { + item_brakeDecel_action, // function action + item_brakeDecel_value, // function get initial value or NULL(show in line 2) + item_brakeDecel_default, // function get default value or NULL(dont set value, show msg) + 0, // valueMin + 10000, // valueMax + 100, // valueIncrement + "Brake decel. ", // title + " Fade down time ", // line1 (above value) + "", // line2 <= showing "default = %d" + "", // line4 * (below value) + "", // line5 * + "milliseconds ", // line6 + "from 100 to 0% ", // line7 +}; + //############################### //### select motorControlMode ### @@ -578,8 +610,8 @@ menuItem_t item_last = { //#################################################### //### store all configured menu items in one array ### //#################################################### -const menuItem_t menuItems[] = {item_centerJoystick, item_calibrateJoystick, item_debugJoystick, item_statusScreen, item_maxDuty, item_maxRelativeBoost, item_accelLimit, item_decelLimit, item_motorControlMode, item_tractionControlSystem, item_reset, item_example, item_last}; -const int itemCount = 11; +const menuItem_t menuItems[] = {item_centerJoystick, item_calibrateJoystick, item_debugJoystick, item_statusScreen, item_maxDuty, item_maxRelativeBoost, item_accelLimit, item_decelLimit, item_brakeDecel, item_motorControlMode, item_tractionControlSystem, item_reset, item_example, item_last}; +const int itemCount = 12; diff --git a/common/motorctl.cpp b/common/motorctl.cpp index de39ef9..e362e4d 100644 --- a/common/motorctl.cpp +++ b/common/motorctl.cpp @@ -30,7 +30,8 @@ void task_motorctl( void * ptrControlledMotor ){ //constructor, simultaniously initialize instance of motor driver 'motor' and current sensor 'cSensor' with provided config (see below lines after ':') controlledMotor::controlledMotor(motorSetCommandFunc_t setCommandFunc, motorctl_config_t config_control, nvs_handle_t * nvsHandle_f, speedSensor * speedSensor_f, controlledMotor ** otherMotor_f): //create current sensor - cSensor(config_control.currentSensor_adc, config_control.currentSensor_ratedCurrent, config_control.currentSnapToZeroThreshold, config_control.currentInverted) { + cSensor(config_control.currentSensor_adc, config_control.currentSensor_ratedCurrent, config_control.currentSnapToZeroThreshold, config_control.currentInverted), + configDefault(config_control){ //copy parameters for controlling the motor config = config_control; log = config.loggingEnabled; @@ -333,15 +334,15 @@ if ( dutyNow != 0 && esp_log_timestamp() - timestamp_commandReceived > TIMEOUT_I dutyIncrementAccel = 0; } //- normal accel - - else if (msFadeAccel > 0) - dutyIncrementAccel = (usPassed / ((float)msFadeAccel * 1000)) * 100; // TODO define maximum increment - first run after startup (or long) pause can cause a very large increment + else if (config.msFadeAccel > 0) + dutyIncrementAccel = (usPassed / ((float)config.msFadeAccel * 1000)) * 100; // TODO define maximum increment - first run after startup (or long) pause can cause a very large increment //- sport mode - else //no accel limit (immediately set to 100) dutyIncrementAccel = 100; //--- calculate increment (deceleration) --- //- sport mode - - if (msFadeDecel == 0){ //no decel limit (immediately reduce to 0) + if (config.msFadeDecel == 0){ //no decel limit (immediately reduce to 0) dutyIncrementDecel = 100; } //- brake - @@ -364,7 +365,7 @@ if ( dutyNow != 0 && esp_log_timestamp() - timestamp_commandReceived > TIMEOUT_I //- normal deceleration - else { // normal deceleration according to configured time - dutyIncrementDecel = (usPassed / ((float)msFadeDecel * 1000)) * 100; + dutyIncrementDecel = (usPassed / ((float)config.msFadeDecel * 1000)) * 100; } // reset braking state when start condition is no longer met (stick below threshold again) @@ -402,7 +403,7 @@ if ( dutyNow != 0 && esp_log_timestamp() - timestamp_commandReceived > TIMEOUT_I float dutyOld = dutyNow; //adaptive decrement: //Note current exceeded twice -> twice as much decrement: TODO: decrement calc needs finetuning, currently random values - dutyIncrementDecel = (currentNow/config.currentMax) * ( usPassed / ((float)msFadeDecel * 1500) ) * 100; + dutyIncrementDecel = (currentNow/config.currentMax) * ( usPassed / ((float)config.msFadeDecel * 1500) ) * 100; float currentLimitDecrement = ( (float)usPassed / ((float)1000 * 1000) ) * 100; //1000ms from 100 to 0 if (dutyNow < -currentLimitDecrement) { dutyNow += currentLimitDecrement; @@ -471,7 +472,7 @@ if ( dutyNow != 0 && esp_log_timestamp() - timestamp_commandReceived > TIMEOUT_I tcs_usExceeded = esp_timer_get_time() - tcs_timestampBeginExceeded; //time too fast already if(log) ESP_LOGI("TESTING", "[%s] TCS: faster than expected since %dms, current ratioDiff=%.2f -> slowing down", config.name, tcs_usExceeded/1000, ratioDiff); // calculate amount duty gets decreased - float dutyDecrement = (tcs_usPassed / ((float)msFadeDecel * 1000)) * 100; //TODO optimize dynamic increment: P:scale with ratio-difference, I: scale with duration exceeded + float dutyDecrement = (tcs_usPassed / ((float)config.msFadeDecel * 1000)) * 100; //TODO optimize dynamic increment: P:scale with ratio-difference, I: scale with duration exceeded // decrease duty if(log) ESP_LOGI("TESTING", "[%s] TCS: msPassed=%.3f, reducing duty by %.3f%%", config.name, (float)tcs_usPassed/1000, dutyDecrement); fade(&dutyNow, 0, -dutyDecrement); //reduce duty but not less than 0 @@ -586,10 +587,10 @@ motorCommand_t controlledMotor::getStatus(){ uint32_t controlledMotor::getFade(fadeType_t fadeType){ switch(fadeType){ case fadeType_t::ACCEL: - return msFadeAccel; + return config.msFadeAccel; break; case fadeType_t::DECEL: - return msFadeDecel; + return config.msFadeDecel; break; } return 0; @@ -602,10 +603,10 @@ uint32_t controlledMotor::getFade(fadeType_t fadeType){ uint32_t controlledMotor::getFadeDefault(fadeType_t fadeType){ switch(fadeType){ case fadeType_t::ACCEL: - return config.msFadeAccel; + return configDefault.msFadeAccel; break; case fadeType_t::DECEL: - return config.msFadeDecel; + return configDefault.msFadeDecel; break; } return 0; @@ -623,11 +624,11 @@ void controlledMotor::setFade(fadeType_t fadeType, uint32_t msFadeNew){ //TODO: mutex for msFade variable also used in handle function switch(fadeType){ case fadeType_t::ACCEL: - ESP_LOGW(TAG, "[%s] changed fade-up time from %d to %d", config.name, msFadeAccel, msFadeNew); + ESP_LOGW(TAG, "[%s] changed fade-up time from %d to %d", config.name, config.msFadeAccel, msFadeNew); writeAccelDuration(msFadeNew); break; case fadeType_t::DECEL: - ESP_LOGW(TAG, "[%s] changed fade-down time from %d to %d",config.name, msFadeDecel, msFadeNew); + ESP_LOGW(TAG, "[%s] changed fade-down time from %d to %d",config.name, config.msFadeDecel, msFadeNew); // write new value to nvs and update the variable writeDecelDuration(msFadeNew); break; @@ -663,16 +664,16 @@ bool controlledMotor::toggleFade(fadeType_t fadeType){ bool enabled = false; switch(fadeType){ case fadeType_t::ACCEL: - if (msFadeAccel == 0){ - msFadeNew = config.msFadeAccel; + if (config.msFadeAccel == 0){ + msFadeNew = configDefault.msFadeAccel; enabled = true; } else { msFadeNew = 0; } break; case fadeType_t::DECEL: - if (msFadeDecel == 0){ - msFadeNew = config.msFadeAccel; + if (config.msFadeDecel == 0){ + msFadeNew = configDefault.msFadeAccel; enabled = true; } else { msFadeNew = 0; @@ -695,8 +696,6 @@ bool controlledMotor::toggleFade(fadeType_t fadeType){ // load stored value from nvs if not successfull uses config default value void controlledMotor::loadAccelDuration(void) { - // load default value - msFadeAccel = config.msFadeAccel; // read from nvs uint32_t valueNew; char key[15]; @@ -705,11 +704,11 @@ void controlledMotor::loadAccelDuration(void) switch (err) { case ESP_OK: - ESP_LOGW(TAG, "Successfully read value '%s' from nvs. Overriding default value %d with %d", key, config.msFadeAccel, valueNew); - msFadeAccel = valueNew; + ESP_LOGW(TAG, "Successfully read value '%s' from nvs. Overriding default value %d with %d", key, configDefault.msFadeAccel, valueNew); + config.msFadeAccel = valueNew; break; case ESP_ERR_NVS_NOT_FOUND: - ESP_LOGW(TAG, "nvs: the value '%s' is not initialized yet, keeping default value %d", key, msFadeAccel); + ESP_LOGW(TAG, "nvs: the value '%s' is not initialized yet, keeping default value %d", key, config.msFadeAccel); break; default: ESP_LOGE(TAG, "Error (%s) reading nvs!", esp_err_to_name(err)); @@ -721,8 +720,6 @@ void controlledMotor::loadAccelDuration(void) //----------------------------- void controlledMotor::loadDecelDuration(void) { - // load default value - msFadeDecel = config.msFadeDecel; // read from nvs uint32_t valueNew; char key[15]; @@ -732,10 +729,10 @@ void controlledMotor::loadDecelDuration(void) { case ESP_OK: ESP_LOGW(TAG, "Successfully read value '%s' from nvs. Overriding default value %d with %d", key, config.msFadeDecel, valueNew); - msFadeDecel = valueNew; + config.msFadeDecel = valueNew; break; case ESP_ERR_NVS_NOT_FOUND: - ESP_LOGW(TAG, "nvs: the value '%s' is not initialized yet, keeping default value %d", key, msFadeDecel); + ESP_LOGW(TAG, "nvs: the value '%s' is not initialized yet, keeping default value %d", key, config.msFadeDecel); break; default: ESP_LOGE(TAG, "Error (%s) reading nvs!", esp_err_to_name(err)); @@ -748,11 +745,11 @@ void controlledMotor::loadDecelDuration(void) //------------------------------ //----- writeAccelDuration ----- //------------------------------ -// write provided value to nvs to be persistent and update the local variable msFadeAccel +// write provided value to nvs to be persistent and update the local config void controlledMotor::writeAccelDuration(uint32_t newValue) { // check if unchanged - if(msFadeAccel == newValue){ + if(config.msFadeAccel == newValue){ ESP_LOGW(TAG, "value unchanged at %d, not writing to nvs", newValue); return; } @@ -760,7 +757,7 @@ void controlledMotor::writeAccelDuration(uint32_t newValue) char key[15]; snprintf(key, 15, "m-%s-accel", config.name); // update nvs value - ESP_LOGW(TAG, "[%s] updating nvs value '%s' from %d to %d", config.name, key, msFadeAccel, newValue); + ESP_LOGW(TAG, "[%s] updating nvs value '%s' from %d to %d", config.name, key, config.msFadeAccel, newValue); esp_err_t err = nvs_set_u32(*nvsHandle, key, newValue); if (err != ESP_OK) ESP_LOGE(TAG, "nvs: failed writing"); @@ -770,18 +767,18 @@ void controlledMotor::writeAccelDuration(uint32_t newValue) else ESP_LOGI(TAG, "nvs: successfully committed updates"); // update variable - msFadeAccel = newValue; + config.msFadeAccel = newValue; } //------------------------------ //----- writeDecelDuration ----- //------------------------------ -// write provided value to nvs to be persistent and update the local variable msFadeDecel +// write provided value to nvs to be persistent and update the local config // TODO: reduce duplicate code void controlledMotor::writeDecelDuration(uint32_t newValue) { // check if unchanged - if(msFadeDecel == newValue){ + if(config.msFadeDecel == newValue){ ESP_LOGW(TAG, "value unchanged at %d, not writing to nvs", newValue); return; } @@ -789,7 +786,7 @@ void controlledMotor::writeDecelDuration(uint32_t newValue) char key[15]; snprintf(key, 15, "m-%s-decel", config.name); // update nvs value - ESP_LOGW(TAG, "[%s] updating nvs value '%s' from %d to %d", config.name, key, msFadeDecel, newValue); + ESP_LOGW(TAG, "[%s] updating nvs value '%s' from %d to %d", config.name, key, config.msFadeDecel, newValue); esp_err_t err = nvs_set_u32(*nvsHandle, key, newValue); if (err != ESP_OK) ESP_LOGE(TAG, "nvs: failed writing"); @@ -799,5 +796,5 @@ void controlledMotor::writeDecelDuration(uint32_t newValue) else ESP_LOGI(TAG, "nvs: successfully committed updates"); // update variable - msFadeDecel = newValue; + config.msFadeDecel = newValue; } \ No newline at end of file diff --git a/common/motorctl.hpp b/common/motorctl.hpp index 7c0d50f..433419d 100644 --- a/common/motorctl.hpp +++ b/common/motorctl.hpp @@ -47,6 +47,9 @@ class controlledMotor { bool getTractionControlSystemStatus() {return config.tractionControlSystemEnabled;}; void setControlMode(motorControlMode_t newMode) {mode = newMode;}; void setBrakeStartThresholdDuty(float duty) {brakeStartThreshold = duty;}; + void setBrakeDecel(uint32_t msFadeBrake) {config.brakeDecel = msFadeBrake;}; + uint32_t getBrakeDecel() {return config.brakeDecel;}; //todo store and load from nvs + uint32_t getBrakeDecelDefault() {return configDefault.brakeDecel;}; uint32_t getFade(fadeType_t fadeType); //get currently set acceleration or deceleration fading time uint32_t getFadeDefault(fadeType_t fadeType); //get acceleration or deceleration fading time from config @@ -85,6 +88,7 @@ class controlledMotor { //TODO add name for logging? //struct for storing control specific parameters motorctl_config_t config; + const motorctl_config_t configDefault; //backup default configuration (unchanged) bool log = false; motorstate_t state = motorstate_t::IDLE; motorControlMode_t mode = motorControlMode_t::DUTY; //default control mode @@ -108,9 +112,6 @@ class controlledMotor { float dutyDelta; uint32_t timeoutWaitForCommand = 0; - uint32_t msFadeAccel; - uint32_t msFadeDecel; - uint32_t ramp; int64_t timestampLastRunUs = 0;