Fix setFade, Make MASSAGE-mode work again (pre-rework)

MASSAGE mode is now useable again for now.
(no more random driving and unexpected shaking)
A full rework is still planned.

- Fix setFade method (did not change anything when not writing to nvs)

- joystick/massage:
    - reverse motor direction every 2nd cycle to preving driving
    - simplify modes (now only FW/REV and LEFT/RIGHT)
    - use joystick angle to vary the duty cycle
    - optimize delay and duty calculation (min max config)
    - drop hysteresis (not much use, just much code)

board_single/main/control.cpp

@@ -543,7 +543,7 @@ void controlledArmchair::changeMode(controlMode_t modeNew)
 
         case controlMode_t::MASSAGE:
             ESP_LOGW(TAG, "switching to MASSAGE mode -> reducing fading");
-            uint32_t shake_msFadeAccel = 200; // TODO: move this to config
+            uint32_t shake_msFadeAccel = 350; // TODO: move this to config
             uint32_t shake_msFadeDecel = 0; // TODO: move this to config
 
             // save currently set normal acceleration config (for restore when leavinge MASSAGE again)

common/joystick.cpp

@@ -438,13 +438,21 @@ uint32_t shake_timestamp_turnedOn = 0;
 uint32_t shake_timestamp_turnedOff = 0;
 bool shake_state = false;
 joystickPos_t lastStickPos = joystickPos_t::CENTER;
-//stick position quadrant only with "X_AXIS and Y_AXIS" as hysteresis
-joystickPos_t stickQuadrant = joystickPos_t::CENTER;
 
 //--- configure shake mode --- TODO: move this to config
-uint32_t shake_msOffMax = 80;
+uint32_t shake_msOffMax = 60;
 uint32_t shake_msOnMax = 120;
-float dutyShake = 60;
+uint32_t shake_minDelay = 20; //min time in ms motor stays on/off
+float dutyShakeMax = 30;
+float dutyShakeMin = 5;
+
+inline void invertMotorDirection(motorstate_t *state)
+{
+    if (*state == motorstate_t::FWD)
+        *state = motorstate_t::REV;
+    else
+        *state = motorstate_t::FWD;
+}
 
 //function that generates commands for both motors from the joystick data
 motorCommands_t joystick_generateCommandsShaking(joystickData_t data){
@@ -452,25 +460,29 @@ motorCommands_t joystick_generateCommandsShaking(joystickData_t data){
     //--- handle pulsing shake variable ---
     //TODO remove this, make individual per mode?
     //TODO only run this when not CENTER anyways?
-    motorCommands_t commands;
+    static motorCommands_t commands;
     float ratio = fabs(data.angle) / 90; //90degree = x=0 || 0degree = y=0
+    static uint32_t cycleCount = 0;
 
     //calculate on/off duration
-    uint32_t msOn = shake_msOnMax * data.radius;
+    float msOn = (shake_msOnMax - shake_minDelay) * data.radius + shake_minDelay;
-    uint32_t msOff = shake_msOffMax * data.radius;
+    float msOff = (shake_msOffMax - shake_minDelay) * data.radius + shake_minDelay;
+    float dutyShake = (dutyShakeMax - dutyShakeMin) * ratio + dutyShakeMin;
 
-    //evaluate state (on/off)
+    //evaluate state (motors on/off)
     if (data.radius > 0 ){
-        //currently off
+        //currently off:
         if (shake_state == false){
             //off long enough
             if (esp_log_timestamp() - shake_timestamp_turnedOff > msOff) {
                 //turn on
+                cycleCount++;
                 shake_state = true;
                 shake_timestamp_turnedOn = esp_log_timestamp();
+                ESP_LOGD(TAG_CMD, "shake: cycleCount=%d, msOn=%f, msOff=%f, radius=%f, shakeDuty=%f", cycleCount, msOn, msOff, data.radius, dutyShake);
             }
         } 
-        //currently on
+        //currently on:
         else {
             //on long enough
             if (esp_log_timestamp() - shake_timestamp_turnedOn > msOn) {
@@ -495,79 +507,49 @@ motorCommands_t joystick_generateCommandsShaking(joystickData_t data){
     //    float angle;
     //} joystickData_t;
 
-    //--- evaluate stick position --- 
+    // force off when stick pos changes - TODO: is this necessary?
-    //4 quadrants and center only - with X and Y axis as hysteresis
+    static joystickPos_t stickPosPrev = joystickPos_t::CENTER;
-    switch (data.position){
+    if (data.position != stickPosPrev) {
-
+        ESP_LOGW(TAG, "massage: stick quadrant changed, stopping for one cycle");
-        case joystickPos_t::CENTER:
+        shake_state = false;
-            //immediately set to center at center
+        shake_timestamp_turnedOff = esp_log_timestamp();
-            stickQuadrant = joystickPos_t::CENTER;
-            break;
-
-        case joystickPos_t::Y_AXIS:
-            //when moving from center to axis initially start in a certain quadrant
-            if (stickQuadrant == joystickPos_t::CENTER) {
-                if (data.y > 0){
-                    stickQuadrant = joystickPos_t::TOP_RIGHT;
-                } else {
-                    stickQuadrant = joystickPos_t::BOTTOM_RIGHT;
     }
-            }
+    stickPosPrev = data.position; // update last position
-            break;
-
-        case joystickPos_t::X_AXIS:
-            //when moving from center to axis initially start in a certain quadrant
-            if (stickQuadrant == joystickPos_t::CENTER) {
-                if (data.x > 0){
-                    stickQuadrant = joystickPos_t::TOP_RIGHT;
-                } else {
-                    stickQuadrant = joystickPos_t::TOP_LEFT;
-                }
-            }
-            break;
-
-        case joystickPos_t::TOP_RIGHT:
-        case joystickPos_t::TOP_LEFT:
-        case joystickPos_t::BOTTOM_LEFT:
-        case joystickPos_t::BOTTOM_RIGHT:
-            //update/change evaluated pos when in one of the 4 quadrants
-            stickQuadrant = data.position;
-            //TODO: maybe beep when switching mode? (difficult because beep object has to be passed to function)
-            break;
-    }
-
 
     //--- handle different modes (joystick in any of 4 quadrants) ---
-    switch (stickQuadrant){
+    switch (data.position){
+        // idle
         case joystickPos_t::CENTER:
-        case joystickPos_t::X_AXIS: //never true
-        case joystickPos_t::Y_AXIS: //never true
             commands.left.state = motorstate_t::IDLE;
             commands.right.state = motorstate_t::IDLE;
             commands.left.duty = 0;
             commands.right.duty = 0;
-            ESP_LOGI(TAG_CMD, "generate shake commands: CENTER -> idle");
+            ESP_LOGD(TAG_CMD, "generate shake commands: CENTER -> idle");
             return commands;
             break;
-            //4 different modes
+        // shake forward/reverse
+        case joystickPos_t::X_AXIS:
+        case joystickPos_t::Y_AXIS:
         case joystickPos_t::TOP_RIGHT:
+        case joystickPos_t::TOP_LEFT:
             commands.left.state = motorstate_t::FWD;
             commands.right.state = motorstate_t::FWD;
             break;
-        case joystickPos_t::TOP_LEFT:
+        // shake left right
-            commands.left.state = motorstate_t::REV;
-            commands.right.state = motorstate_t::REV;
-            break;
         case joystickPos_t::BOTTOM_LEFT:
-            commands.left.state = motorstate_t::REV;
-            commands.right.state = motorstate_t::FWD;
-            break;
         case joystickPos_t::BOTTOM_RIGHT:
             commands.left.state = motorstate_t::FWD;
             commands.right.state = motorstate_t::REV;
             break;
     }
 
+    // change direction every second on cycle in any mode
+    //(to not start driving on average)
+    if (cycleCount % 2 == 0)
+    {
+        invertMotorDirection(&commands.left.state);
+        invertMotorDirection(&commands.right.state);
+    }
 
     //--- turn motors on/off depending on pulsing shake variable ---
     if (shake_state == true){
@@ -582,11 +564,7 @@ motorCommands_t joystick_generateCommandsShaking(joystickData_t data){
         commands.right.duty = 0;
     }
 
-
+    ESP_LOGD(TAG_CMD, "motor left: state=%s, duty=%.3f, cycleCount=%d, msOn=%f, msOff=%f", motorstateStr[(int)commands.left.state], commands.left.duty, cycleCount, msOn, msOff);
-    ESP_LOGI(TAG_CMD, "generated commands from data: state=%s, angle=%.3f, ratio=%.3f/%.3f, radius=%.2f, x=%.2f, y=%.2f",
-            joystickPosStr[(int)data.position], data.angle, ratio, (1-ratio), data.radius, data.x, data.y);
-    ESP_LOGI(TAG_CMD, "motor left: state=%s, duty=%.3f", motorstateStr[(int)commands.left.state], commands.left.duty);
-    ESP_LOGI(TAG_CMD, "motor right: state=%s, duty=%.3f", motorstateStr[(int)commands.right.state], commands.right.duty);
 
     return commands;
 }

common/motorctl.cpp

@@ -627,12 +627,16 @@ void controlledMotor::setFade(fadeType_t fadeType, uint32_t msFadeNew, bool writ
             ESP_LOGW(TAG, "[%s] changed fade-up time from %d to %d", config.name, config.msFadeAccel, msFadeNew);
             if (writeToNvs)
                 writeAccelDuration(msFadeNew);
+            else
+                config.msFadeAccel = msFadeNew;
             break;
         case fadeType_t::DECEL:
             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
             if (writeToNvs)
                 writeDecelDuration(msFadeNew);
+            else
+                config.msFadeDecel = msFadeNew;
             break;
     }
 }