From 436c528eaf7b467d4b97011d5749458682f621bf Mon Sep 17 00:00:00 2001
From: jonny_l480 <jonny@wwad.de>
Date: Sat, 16 Jul 2022 12:46:56 +0200
Subject: [PATCH 1/6] Add fade-down functionality to motorctl (bug)
With this changes the motors are faded down (same as already existing fading up)
when the target duty suddenly decreases.
It can be configured or disabled with setting msFadeDown to 0 in config struct of
motorctl object.
- Add new config parameter msFadeDown
- Change msFade to msFadeDown
- Add fade down functionality to motorctl instead of just setting the
duty to lower value
!!!TODO: fix major BUG when motor direction changes suddenly (before fade got to
0) the motor immediately starts in other direction with previous duty
(no/less fading up happening)
---
main/config.cpp | 3 ++-
main/motorctl.cpp | 33 +++++++++++++++++++++++++--------
main/motorctl.hpp | 6 ++++--
3 files changed, 31 insertions(+), 11 deletions(-)
diff --git a/main/config.cpp b/main/config.cpp
index 99ccc45..2a83bd1 100644
--- a/main/config.cpp
+++ b/main/config.cpp
@@ -29,7 +29,8 @@ single100a_config_t configDriverRight = {
//--- configure motor contol ---
motorctl_config_t configMotorControl = {
- .msFade = 900,
+ .msFadeUp = 1500,
+ .msFadeDown = 3000,
.currentMax = 10
};
diff --git a/main/motorctl.cpp b/main/motorctl.cpp
index fd8cfc8..9746a9e 100644
--- a/main/motorctl.cpp
+++ b/main/motorctl.cpp
@@ -46,9 +46,16 @@ void controlledMotor::handle(){
}
//--- calculate increment ---
- //calculate increment with passed time since last run and configured fade time
+ //calculate increment for fading UP with passed time since last run and configured fade time
int64_t usPassed = esp_timer_get_time() - timestampLastRunUs;
- dutyIncrement = ( usPassed / ((float)config.msFade * 1000) ) * 100; //TODO define maximum increment - first run after startup (or long) pause can cause a very large increment
+ dutyIncrementUp = ( usPassed / ((float)config.msFadeUp * 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;
+ } else {
+ dutyIncrementDown = 100;
+ }
+
//--- calculate difference ---
dutyDelta = dutyTarget - dutyNow;
@@ -56,14 +63,24 @@ void controlledMotor::handle(){
//negative: need to decrease
//--- fade up ---
- if(dutyDelta > dutyIncrement){ //target duty his higher than current duty -> fade up
- ESP_LOGV(TAG, "*fading up*: target=%.2f%% - previous=%.2f%% - increment=%.6f%% - usSinceLastRun=%d", dutyTarget, dutyNow, dutyIncrement, (int)usPassed);
- dutyNow += dutyIncrement; //increase duty by increment
+ if(dutyDelta > dutyIncrementUp){ //target duty his higher than current duty -> fade up
+ ESP_LOGV(TAG, "*fading up*: target=%.2f%% - previous=%.2f%% - increment=%.6f%% - usSinceLastRun=%d", dutyTarget, dutyNow, dutyIncrementUp, (int)usPassed);
+ dutyNow += dutyIncrementUp; //increase duty by increment
//--- set lower ---
- } else { //target duty is lower than current duty -> immediately set to target
- ESP_LOGV(TAG, "*setting to target*: target=%.2f%% - previous=%.2f%% ", dutyTarget, dutyNow);
- dutyNow = dutyTarget; //set target duty
+ //} else { //target duty is lower than current duty -> immediately set to target
+ // ESP_LOGV(TAG, "*setting to target*: target=%.2f%% - previous=%.2f%% ", dutyTarget, dutyNow);
+ // dutyNow = dutyTarget; //set target duty
+ //}
+
+ //--- fade down ---
+ } else { //target duty is lower than current duty -> fade down
+ ESP_LOGV(TAG, "*fading up*: target=%.2f%% - previous=%.2f%% - increment=%.6f%% - usSinceLastRun=%d", dutyTarget, dutyNow, dutyIncrementDown, (int)usPassed);
+ if (dutyTarget < dutyIncrementDown){ //immediately set to target when closer than increment (avoid negative duty)
+ dutyNow = dutyTarget;
+ } else {
+ dutyNow -= dutyIncrementDown; //decrease duty by increment
+ }
}
//--- apply to motor ---
diff --git a/main/motorctl.hpp b/main/motorctl.hpp
index 86708fe..593ff11 100644
--- a/main/motorctl.hpp
+++ b/main/motorctl.hpp
@@ -30,7 +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 msFade; //acceleration of the motor (ms it should take from 0 to 100%)
+ 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%)
float currentMax;
} motorctl_config_t;
@@ -67,7 +68,8 @@ class controlledMotor {
float dutyTarget;
float dutyNow;
- float dutyIncrement;
+ float dutyIncrementUp;
+ float dutyIncrementDown;
float dutyDelta;
uint32_t ramp;
From cd804452e42ba872ca8a7a6a772f4b105c0de706 Mon Sep 17 00:00:00 2001
From: jonny_ji7 <jonny@wwad.de>
Date: Sun, 17 Jul 2022 15:23:42 +0200
Subject: [PATCH 2/6] Change motorctl.handle from STATE to duty -100-100
For fading-down and delay-between-state-change to work correctly when switching between motorstates
(e.g. FWD -> REV) the handle function was changed.
using a duty value -100 for reverse 100% and +100 for forward 100%
allows are more mathematical approach
changes to motorctl.cpp:
- convert state and duty to duty -100 to 100 when receiving target
command at start
- convert duty -100-100 back to state and duty at the end for applying
motor commands
---
main/motorctl.cpp | 38 ++++++++++++++++++++++++++++++++++----
1 file changed, 34 insertions(+), 4 deletions(-)
diff --git a/main/motorctl.cpp b/main/motorctl.cpp
index 9746a9e..b0f8a2a 100644
--- a/main/motorctl.cpp
+++ b/main/motorctl.cpp
@@ -43,12 +43,28 @@ void controlledMotor::handle(){
ESP_LOGD(TAG, "Read command from queue: state=%s, duty=%.2f", motorstateStr[(int)commandReceive.state], commandReceive.duty);
state = commandReceive.state;
dutyTarget = commandReceive.duty;
+
+ //--- convert duty ---
+ //define target duty (-100 to 100) from provided duty and motorstate
+ switch(commandReceive.state){
+ case motorstate_t::BRAKE:
+ case motorstate_t::IDLE:
+ dutyTarget = 0;
+ break;
+ case motorstate_t::FWD:
+ dutyTarget = fabs(commandReceive.duty);
+ break;
+ case motorstate_t::REV:
+ dutyTarget = - fabs(commandReceive.duty);
+ break;
+ }
}
//--- 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
+
//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;
@@ -72,7 +88,7 @@ void controlledMotor::handle(){
// ESP_LOGV(TAG, "*setting to target*: target=%.2f%% - previous=%.2f%% ", dutyTarget, dutyNow);
// dutyNow = dutyTarget; //set target duty
//}
-
+
//--- fade down ---
} else { //target duty is lower than current duty -> fade down
ESP_LOGV(TAG, "*fading up*: target=%.2f%% - previous=%.2f%% - increment=%.6f%% - usSinceLastRun=%d", dutyTarget, dutyNow, dutyIncrementDown, (int)usPassed);
@@ -82,11 +98,25 @@ void controlledMotor::handle(){
dutyNow -= dutyIncrementDown; //decrease duty by increment
}
}
-
+
//--- apply to motor ---
+ //convert duty -100 to 100 back to motorstate and duty 0-100
//apply target duty and state to motor driver
- motor.set(state, dutyNow);
-
+ //forward
+ if(dutyNow > 0){
+ motor.set(motorstate_t::FWD, dutyNow);
+ }
+ //reverse
+ else if (dutyNow < 0){
+ motor.set(motorstate_t::REV, fabs(dutyNow));
+ }
+ //idle
+ //TODO: add BRAKE case!!!
+ else {
+ motor.set(motorstate_t::IDLE, 0);
+ }
+
+
//--- update timestamp ---
timestampLastRunUs = esp_timer_get_time(); //update timestamp last run with current timestamp in microseconds
From 1360832f5ec6240d6429d4a591ee51834f86bbcb Mon Sep 17 00:00:00 2001
From: jonny_l480 <jonny@wwad.de>
Date: Sun, 17 Jul 2022 17:55:57 +0200
Subject: [PATCH 3/6] Fix bug no fwd downfade; Add BRAKE; cleanup; [bug]
- Fix bug where downfade did not work when driving forward
- Add BRAKE command functionality to motorctl (untested, since not used anywhere atm)
- Cleanup: Optimize code, add comments
!!!TODO: there is a fault in the current concept/logic:
fading up/down generally works, however when accelerating
REVERSE fade-down is utilized instead of fade-up.
=> Fix that bug, fade down should only be used when decelerating.
---
main/config.cpp | 2 +-
main/motorctl.cpp | 58 +++++++++++++++++++++++++++++++----------------
2 files changed, 39 insertions(+), 21 deletions(-)
diff --git a/main/config.cpp b/main/config.cpp
index 2a83bd1..89d40bf 100644
--- a/main/config.cpp
+++ b/main/config.cpp
@@ -30,7 +30,7 @@ single100a_config_t configDriverRight = {
//--- configure motor contol ---
motorctl_config_t configMotorControl = {
.msFadeUp = 1500,
- .msFadeDown = 3000,
+ .msFadeDown = 1000,
.currentMax = 10
};
diff --git a/main/motorctl.cpp b/main/motorctl.cpp
index b0f8a2a..55b60f8 100644
--- a/main/motorctl.cpp
+++ b/main/motorctl.cpp
@@ -46,8 +46,13 @@ void controlledMotor::handle(){
//--- convert duty ---
//define target duty (-100 to 100) from provided duty and motorstate
+ //this value is more suitable for the fading algorithm
switch(commandReceive.state){
case motorstate_t::BRAKE:
+ //update state
+ state = motorstate_t::BRAKE;
+ dutyTarget = 0;
+ break;
case motorstate_t::IDLE:
dutyTarget = 0;
break;
@@ -72,6 +77,16 @@ void controlledMotor::handle(){
dutyIncrementDown = 100;
}
+
+ //--- BRAKE ---
+ //brake immediately, update state, duty and exit this cycle of handle function
+ if (state == motorstate_t::BRAKE){
+ motor.set(motorstate_t::BRAKE, 0);
+ dutyNow = 0;
+ return; //no need to run the fade algorithm
+ }
+
+
//--- calculate difference ---
dutyDelta = dutyTarget - dutyNow;
@@ -79,42 +94,45 @@ void controlledMotor::handle(){
//negative: need to decrease
//--- fade up ---
- if(dutyDelta > dutyIncrementUp){ //target duty his higher than current duty -> 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
-
- //--- set lower ---
- //} else { //target duty is lower than current duty -> immediately set to target
- // ESP_LOGV(TAG, "*setting to target*: target=%.2f%% - previous=%.2f%% ", dutyTarget, dutyNow);
- // dutyNow = dutyTarget; //set target duty
- //}
+ }
//--- fade down ---
- } else { //target duty is lower than current duty -> fade down
- ESP_LOGV(TAG, "*fading up*: target=%.2f%% - previous=%.2f%% - increment=%.6f%% - usSinceLastRun=%d", dutyTarget, dutyNow, dutyIncrementDown, (int)usPassed);
- if (dutyTarget < dutyIncrementDown){ //immediately set to target when closer than increment (avoid negative duty)
- dutyNow = dutyTarget;
- } else {
- dutyNow -= dutyIncrementDown; //decrease duty by increment
- }
+ //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;
}
- //--- apply to motor ---
- //convert duty -100 to 100 back to motorstate and duty 0-100
+
+ //define motorstate from converted duty -100 to 100
//apply target duty and state to motor driver
//forward
if(dutyNow > 0){
- motor.set(motorstate_t::FWD, dutyNow);
+ state = motorstate_t::FWD;
}
//reverse
else if (dutyNow < 0){
- motor.set(motorstate_t::REV, fabs(dutyNow));
+ state = motorstate_t::REV;
}
//idle
- //TODO: add BRAKE case!!!
else {
- motor.set(motorstate_t::IDLE, 0);
+ state = motorstate_t::IDLE;
}
+
+ //--- apply to motor ---
+ motor.set(state, fabs(dutyNow));
+ //note: BRAKE state is handled earlier
//--- update timestamp ---
From 0a2695f45f6b6af8b315e5ec6680ce65c1a179a7 Mon Sep 17 00:00:00 2001
From: jonny_ji7 <jonny@wwad.de>
Date: Mon, 18 Jul 2022 16:58:40 +0200
Subject: [PATCH 4/6] Fix fading: correctly use different accel, decel ramps
- Change fading algorithm to handle Acceleration and Deceleration for
forward and reverse separately (4 different cases)
- rename variables to make fading direction more obvious
e.g. msFadeUp -> msFadeAccel or dutyIncrementDon -> dutyIncrementDecel
TODO: fading needs optimization
---
main/config.cpp | 4 +--
main/motorctl.cpp | 90 ++++++++++++++++++++++++++++++++++++-----------
main/motorctl.hpp | 8 ++---
3 files changed, 75 insertions(+), 27 deletions(-)
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;
From bcbd2578f692a030dc3442e656ac828adec1a621 Mon Sep 17 00:00:00 2001
From: jonny_l480 <jonny@wwad.de>
Date: Fri, 22 Jul 2022 20:11:12 +0200
Subject: [PATCH 5/6] Fix bug: fade stuck at zero; Adjust fade durations
- Fix bug where case duty=0 was not handled in fading if structure thus no
incrementing action happened anymore (stuck at duty = 0)
- Set fade duration (acceleration, deceleration) to realistic value in
config
---
main/config.cpp | 4 ++--
main/motorctl.cpp | 5 ++---
2 files changed, 4 insertions(+), 5 deletions(-)
diff --git a/main/config.cpp b/main/config.cpp
index 40d9703..1b515e1 100644
--- a/main/config.cpp
+++ b/main/config.cpp
@@ -29,8 +29,8 @@ single100a_config_t configDriverRight = {
//--- configure motor contol ---
motorctl_config_t configMotorControl = {
- .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%)
+ .msFadeAccel = 2000, //acceleration of the motor (ms it takes from 0% to 100%)
+ .msFadeDecel = 1000, //deceleration of the motor (ms it takes from 100% to 0%)
.currentMax = 10
};
diff --git a/main/motorctl.cpp b/main/motorctl.cpp
index 55d7435..c887c97 100644
--- a/main/motorctl.cpp
+++ b/main/motorctl.cpp
@@ -122,14 +122,13 @@ void controlledMotor::handle(){
if (dutyNow < 0) { //reverse, decelerating
fade(&dutyNow, dutyTarget, dutyIncrementDecel);
}
- else if (dutyNow > 0) { //forward, accelerating
+ 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
+ if (dutyNow <= 0) { //reverse, accelerating
fade(&dutyNow, dutyTarget, - dutyIncrementAccel);
-
}
else if (&dutyNow > 0) { //forward, decelerating
fade(&dutyNow, dutyTarget, - dutyIncrementDecel);
From 0b113eafb5dd84a10bd5056f319678d2d5bf1381 Mon Sep 17 00:00:00 2001
From: jonny_l480 <jonny@wwad.de>
Date: Sat, 23 Jul 2022 14:48:38 +0200
Subject: [PATCH 6/6] Add functions to modify/toggle fading (button x8)
- add 3 functions to controlledMotor class in motorctl.cpp/hpp
- setFade: set specific msFade duration for acceleration or
deceleration
- setFade: disable or set to default ramp for acceleration or
deceleration
- toggleFade: toggle between fading enabled or disables for
acceleration or deceleration
- button.cpp
- add controlledMotor objects to constructor
- add new command 8x press which now toggles fade-deceleration of both
motors
- config.cpp
- slightly decrease fading durations
---
main/button.cpp | 17 ++++++++-
main/button.hpp | 7 +++-
main/config.cpp | 4 +--
main/main.cpp | 2 +-
main/motorctl.cpp | 88 ++++++++++++++++++++++++++++++++++++++++++++---
main/motorctl.hpp | 17 ++++++---
6 files changed, 122 insertions(+), 13 deletions(-)
diff --git a/main/button.cpp b/main/button.cpp
index 0781953..927dac3 100644
--- a/main/button.cpp
+++ b/main/button.cpp
@@ -19,11 +19,13 @@ static const char * TAG = "button";
//-----------------------------
//-------- constructor --------
//-----------------------------
-buttonCommands::buttonCommands(gpio_evaluatedSwitch * button_f, controlledArmchair * control_f, buzzer_t * buzzer_f ){
+buttonCommands::buttonCommands(gpio_evaluatedSwitch * button_f, controlledArmchair * control_f, buzzer_t * buzzer_f, controlledMotor * motorLeft_f, controlledMotor * motorRight_f){
//copy object pointers
button = button_f;
control = control_f;
buzzer = buzzer_f;
+ motorLeft = motorLeft_f;
+ motorRight = motorRight_f;
//TODO declare / configure evaluatedSwitch here instead of config (unnecessary that button object is globally available - only used here)?
}
@@ -63,6 +65,19 @@ void buttonCommands::action (uint8_t count){
ESP_LOGW(TAG, "cmd %d: toggle between MASSAGE and JOYSTICK", count);
control->toggleModes(controlMode_t::MASSAGE, controlMode_t::JOYSTICK); //toggle between MASSAGE and JOYSTICK mode
break;
+
+ case 8:
+ //toggle deceleration fading between on and off
+ bool decelEnabled = motorLeft->toggleFade(fadeType_t::DECEL);
+ motorRight->toggleFade(fadeType_t::DECEL);
+ ESP_LOGW(TAG, "cmd %d: toggle deceleration fading to: %d", count, (int)decelEnabled);
+ if (decelEnabled){
+ buzzer->beep(3, 60, 50);
+ } else {
+ buzzer->beep(1, 1000, 1);
+ }
+
+
}
}
diff --git a/main/button.hpp b/main/button.hpp
index acaf520..b5d5900 100644
--- a/main/button.hpp
+++ b/main/button.hpp
@@ -3,6 +3,7 @@
#include "gpio_evaluateSwitch.hpp"
#include "buzzer.hpp"
#include "control.hpp"
+#include "motorctl.hpp"
@@ -16,7 +17,9 @@ class buttonCommands {
buttonCommands (
gpio_evaluatedSwitch * button_f,
controlledArmchair * control_f,
- buzzer_t * buzzer_f
+ buzzer_t * buzzer_f,
+ controlledMotor * motorLeft_f,
+ controlledMotor * motorRight_f
);
//--- functions ---
@@ -32,6 +35,8 @@ class buttonCommands {
gpio_evaluatedSwitch* button;
controlledArmchair * control;
buzzer_t* buzzer;
+ controlledMotor * motorLeft;
+ controlledMotor * motorRight;
//--- variables ---
uint8_t count = 0;
diff --git a/main/config.cpp b/main/config.cpp
index 1b515e1..784047e 100644
--- a/main/config.cpp
+++ b/main/config.cpp
@@ -29,8 +29,8 @@ single100a_config_t configDriverRight = {
//--- configure motor contol ---
motorctl_config_t configMotorControl = {
- .msFadeAccel = 2000, //acceleration of the motor (ms it takes from 0% to 100%)
- .msFadeDecel = 1000, //deceleration of the motor (ms it takes from 100% to 0%)
+ .msFadeAccel = 1300, //acceleration of the motor (ms it takes from 0% to 100%)
+ .msFadeDecel = 800, //deceleration of the motor (ms it takes from 100% to 0%)
.currentMax = 10
};
diff --git a/main/main.cpp b/main/main.cpp
index 8b701a6..f5bff17 100644
--- a/main/main.cpp
+++ b/main/main.cpp
@@ -75,7 +75,7 @@ void task_control( void * pvParameters ){
void task_button( void * pvParameters ){
ESP_LOGI(TAG, "Initializing command-button and starting handle loop");
//create button instance
- buttonCommands commandButton(&buttonJoystick, &control, &buzzer);
+ buttonCommands commandButton(&buttonJoystick, &control, &buzzer, &motorLeft, &motorRight);
//start handle loop
commandButton.startHandleLoop();
}
diff --git a/main/motorctl.cpp b/main/motorctl.cpp
index c887c97..62ff8c6 100644
--- a/main/motorctl.cpp
+++ b/main/motorctl.cpp
@@ -11,6 +11,9 @@ static const char * TAG = "motor-control";
controlledMotor::controlledMotor(single100a_config_t config_driver, motorctl_config_t config_control): motor(config_driver) {
//copy parameters for controlling the motor
config = config_control;
+ //copy configured default fading durations to actually used variables
+ msFadeAccel = config.msFadeAccel;
+ msFadeDecel = config.msFadeDecel;
init();
//TODO: add currentsensor object here
@@ -89,11 +92,15 @@ 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;
- dutyIncrementAccel = ( usPassed / ((float)config.msFadeAccel * 1000) ) * 100; //TODO define maximum increment - first run after startup (or long) pause can cause a very large increment
+ 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 {
+ dutyIncrementAccel = 100;
+ }
//calculate increment for fading DOWN with passed time since last run and configured fade time
- if (config.msFadeDecel > 0){
- dutyIncrementDecel = ( usPassed / ((float)config.msFadeDecel * 1000) ) * 100;
+ if (msFadeDecel > 0){
+ dutyIncrementDecel = ( usPassed / ((float)msFadeDecel * 1000) ) * 100;
} else {
dutyIncrementDecel = 100;
}
@@ -130,7 +137,7 @@ void controlledMotor::handle(){
if (dutyNow <= 0) { //reverse, accelerating
fade(&dutyNow, dutyTarget, - dutyIncrementAccel);
}
- else if (&dutyNow > 0) { //forward, decelerating
+ else if (dutyNow > 0) { //forward, decelerating
fade(&dutyNow, dutyTarget, - dutyIncrementDecel);
}
}
@@ -222,3 +229,76 @@ motorCommand_t controlledMotor::getStatus(){
return status;
};
+
+
+//===============================
+//=========== setFade ===========
+//===============================
+//function for editing or enabling the fading/ramp of the motor control
+
+//set/update fading duration/amount
+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:
+ msFadeAccel = msFadeNew;
+ break;
+ case fadeType_t::DECEL:
+ msFadeDecel = msFadeNew;
+ break;
+ }
+}
+
+
+//enable (set to default value) or disable fading
+void controlledMotor::setFade(fadeType_t fadeType, bool enabled){
+ uint32_t msFadeNew = 0; //define new fade time as default disabled
+ if(enabled){ //enable
+ //set to default/configured value
+ switch(fadeType){
+ case fadeType_t::ACCEL:
+ msFadeNew = config.msFadeAccel;
+ break;
+ case fadeType_t::DECEL:
+ msFadeNew = config.msFadeDecel;
+ break;
+ }
+ }
+ //apply new Fade value
+ setFade(fadeType, msFadeNew);
+}
+
+
+
+//==================================
+//=========== toggleFade ===========
+//==================================
+//toggle fading between OFF and default value
+bool controlledMotor::toggleFade(fadeType_t fadeType){
+ uint32_t msFadeNew = 0;
+ bool enabled = false;
+ switch(fadeType){
+ case fadeType_t::ACCEL:
+ if (msFadeAccel == 0){
+ msFadeNew = config.msFadeAccel;
+ enabled = true;
+ } else {
+ msFadeNew = 0;
+ }
+ break;
+ case fadeType_t::DECEL:
+ if (msFadeDecel == 0){
+ msFadeNew = config.msFadeAccel;
+ enabled = true;
+ } else {
+ msFadeNew = 0;
+ }
+ break;
+ }
+ //apply new Fade value
+ setFade(fadeType, msFadeNew);
+
+ //return new state (fading enabled/disabled)
+ return enabled;
+}
+
diff --git a/main/motorctl.hpp b/main/motorctl.hpp
index 4528147..322b0f5 100644
--- a/main/motorctl.hpp
+++ b/main/motorctl.hpp
@@ -13,7 +13,7 @@ extern "C"
//-------------------------------------
-//-------- struct declarations -------
+//-------- struct/type declarations -------
//-------------------------------------
//struct for sending command for one motor in the queue
@@ -35,6 +35,10 @@ typedef struct motorctl_config_t {
float currentMax;
} motorctl_config_t;
+//enum fade type (acceleration, deceleration)
+//e.g. used for specifying which fading should be modified with setFade, togleFade functions
+enum class fadeType_t {ACCEL, DECEL};
+
class controlledMotor {
@@ -45,6 +49,10 @@ class controlledMotor {
void setTarget(motorstate_t state_f, float duty_f = 0); //adds target command to queue for handle function
motorCommand_t getStatus(); //get current status of the motor (returns struct with state and duty)
+ void setFade(fadeType_t fadeType, bool enabled); //enable/disable acceleration or deceleration fading
+ void setFade(fadeType_t fadeType, uint32_t msFadeNew); //set acceleration or deceleration fade time
+ bool toggleFade(fadeType_t fadeType); //toggle acceleration or deceleration on/off
+
private:
//--- functions ---
@@ -60,7 +68,7 @@ class controlledMotor {
//--- variables ---
//struct for storing control specific parameters
motorctl_config_t config;
-
+
motorstate_t state = motorstate_t::IDLE;
float currentMax;
@@ -72,12 +80,13 @@ class controlledMotor {
float dutyIncrementDecel;
float dutyDelta;
+ uint32_t msFadeAccel;
+ uint32_t msFadeDecel;
+
uint32_t ramp;
int64_t timestampLastRunUs;
struct motorCommand_t commandReceive = {};
struct motorCommand_t commandSend = {};
-
-
};