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