diff --git a/main/config.cpp b/main/config.cpp index 89d40bf..40d9703 100644 --- a/main/config.cpp +++ b/main/config.cpp @@ -29,8 +29,8 @@ single100a_config_t configDriverRight = { //--- configure motor contol --- motorctl_config_t configMotorControl = { - .msFadeUp = 1500, - .msFadeDown = 1000, + .msFadeAccel = 5000, //acceleration of the motor (ms it takes from 0% to 100%) + .msFadeDecel = 3000, //deceleration of the motor (ms it takes from 100% to 0%) .currentMax = 10 }; diff --git a/main/motorctl.cpp b/main/motorctl.cpp index 55b60f8..55d7435 100644 --- a/main/motorctl.cpp +++ b/main/motorctl.cpp @@ -27,6 +27,26 @@ void controlledMotor::init(){ +//---------------- +//----- fade ----- +//---------------- +//local function that fades a variable +//- increments a variable (pointer) by given value +//- sets to target if already closer than increment +//TODO this needs testing +void fade(float * dutyNow, float dutyTarget, float dutyIncrement){ + float dutyDelta = dutyTarget - *dutyNow; + if ( fabs(dutyDelta) > fabs(dutyIncrement) ) { //check if already close to target + *dutyNow = *dutyNow + dutyIncrement; + } + //already closer to target than increment + else { + *dutyNow = dutyTarget; + } +} + + + //============================== //=========== handle =========== //============================== @@ -65,16 +85,17 @@ void controlledMotor::handle(){ } } + //--- calculate increment --- //calculate increment for fading UP with passed time since last run and configured fade time int64_t usPassed = esp_timer_get_time() - timestampLastRunUs; - dutyIncrementUp = ( usPassed / ((float)config.msFadeUp * 1000) ) * 100; //TODO define maximum increment - first run after startup (or long) pause can cause a very large increment + dutyIncrementAccel = ( usPassed / ((float)config.msFadeAccel * 1000) ) * 100; //TODO define maximum increment - first run after startup (or long) pause can cause a very large increment //calculate increment for fading DOWN with passed time since last run and configured fade time - if (config.msFadeDown > 0){ - dutyIncrementDown = ( usPassed / ((float)config.msFadeDown * 1000) ) * 100; + if (config.msFadeDecel > 0){ + dutyIncrementDecel = ( usPassed / ((float)config.msFadeDecel * 1000) ) * 100; } else { - dutyIncrementDown = 100; + dutyIncrementDecel = 100; } @@ -93,28 +114,55 @@ void controlledMotor::handle(){ //positive: need to increase by that value //negative: need to decrease - //--- fade up --- - //dutyDelta is higher than IncrementUp -> fade up - if(dutyDelta > dutyIncrementUp){ - ESP_LOGV(TAG, "*fading up*: target=%.2f%% - previous=%.2f%% - increment=%.6f%% - usSinceLastRun=%d", dutyTarget, dutyNow, dutyIncrementUp, (int)usPassed); - dutyNow += dutyIncrementUp; //increase duty by increment + + + //--- 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 + fade(&dutyNow, dutyTarget, dutyIncrementDecel); + } + else if (dutyNow > 0) { //forward, accelerating + fade(&dutyNow, dutyTarget, dutyIncrementAccel); + } + } + else if (dutyDelta < 0) { //difference negative -> decreasing duty (100 -> -100) + if (dutyNow < 0) { //reverse, accelerating + fade(&dutyNow, dutyTarget, - dutyIncrementAccel); + + } + else if (&dutyNow > 0) { //forward, decelerating + fade(&dutyNow, dutyTarget, - dutyIncrementDecel); + } } - //--- fade down --- - //dutyDelta is more negative than -IncrementDown -> fade down - else if (dutyDelta < -dutyIncrementDown){ - ESP_LOGV(TAG, "*fading down*: target=%.2f%% - previous=%.2f%% - increment=%.6f%% - usSinceLastRun=%d", dutyTarget, dutyNow, dutyIncrementDown, (int)usPassed); - dutyNow -= dutyIncrementDown; //decrease duty by increment - } - //--- set to target --- - //duty is already very close to target (closer than IncrementUp or IncrementDown) - else{ - //immediately set to target duty - dutyNow = dutyTarget; - } + + //previous approach: (resulted in bug where accel/decel fade is swaped in reverse) +// //--- fade up --- +// //dutyDelta is higher than IncrementUp -> fade up +// if(dutyDelta > dutyIncrementUp){ +// ESP_LOGV(TAG, "*fading up*: target=%.2f%% - previous=%.2f%% - increment=%.6f%% - usSinceLastRun=%d", dutyTarget, dutyNow, dutyIncrementUp, (int)usPassed); +// dutyNow += dutyIncrementUp; //increase duty by increment +// } +// +// //--- fade down --- +// //dutyDelta is more negative than -IncrementDown -> fade down +// else if (dutyDelta < -dutyIncrementDown){ +// ESP_LOGV(TAG, "*fading down*: target=%.2f%% - previous=%.2f%% - increment=%.6f%% - usSinceLastRun=%d", dutyTarget, dutyNow, dutyIncrementDown, (int)usPassed); +// +// dutyNow -= dutyIncrementDown; //decrease duty by increment +// } +// +// //--- set to target --- +// //duty is already very close to target (closer than IncrementUp or IncrementDown) +// else{ +// //immediately set to target duty +// dutyNow = dutyTarget; +// } + //define motorstate from converted duty -100 to 100 //apply target duty and state to motor driver //forward diff --git a/main/motorctl.hpp b/main/motorctl.hpp index 593ff11..4528147 100644 --- a/main/motorctl.hpp +++ b/main/motorctl.hpp @@ -30,8 +30,8 @@ typedef struct motorCommands_t { //struct with all config parameters for a motor regarding ramp and current limit typedef struct motorctl_config_t { - uint32_t msFadeUp; //acceleration of the motor (ms it should take from 0% to 100%) - uint32_t msFadeDown; //acceleration of the motor (ms it should take from 100% to 0%) + uint32_t msFadeAccel; //acceleration of the motor (ms it takes from 0% to 100%) + uint32_t msFadeDecel; //deceleration of the motor (ms it takes from 100% to 0%) float currentMax; } motorctl_config_t; @@ -68,8 +68,8 @@ class controlledMotor { float dutyTarget; float dutyNow; - float dutyIncrementUp; - float dutyIncrementDown; + float dutyIncrementAccel; + float dutyIncrementDecel; float dutyDelta; uint32_t ramp;