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] 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;