Update MASSAGE: 4 modes (each quadrant) with hysteresis

Modified function that generates commands in massage mode (joystick.cpp)
- define variable stickQuadrant with axis as hysteresis from actual joystick data/pos
- prepare switch case for implementation of 4 modes (each quadrant)
- add basic content to modes for testing (different motor directions)

TODO: remove old code with pulsing shake variable and implement actual modes

main/joystick.cpp

@@ -376,6 +376,9 @@ motorCommands_t joystick_generateCommandsDriving(joystickData_t data){
 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 = 90;
@@ -385,25 +388,17 @@ float dutyShake = 60;
 //function that generates commands for both motors from the joystick data
 motorCommands_t joystick_generateCommandsShaking(joystickData_t data){
 
-
+    //--- handle pulsing shake variable ---
-    //struct with current data of the joystick
+    //TODO remove this, make individual per mode?
-    //typedef struct joystickData_t {
+    //TODO only run this when not CENTER anyways?
-    //    joystickPos_t position;
-    //    float x;
-    //    float y;
-    //    float radius;
-    //    float angle;
-    //} joystickData_t;
-
-
     motorCommands_t commands;
     float ratio = fabs(data.angle) / 90; //90degree = x=0 || 0degree = y=0
 
-    //--- calculate on/off duration ---
+    //calculate on/off duration
     uint32_t msOn = shake_msOnMax * data.radius;
     uint32_t msOff = shake_msOffMax * data.radius;
 
-    //--- evaluate state (on/off) ---
+    //evaluate state (on/off)
     if (data.radius > 0 ){
         //currently off
         if (shake_state == false){
@@ -433,44 +428,97 @@ motorCommands_t joystick_generateCommandsShaking(joystickData_t data){
 
 
 
+    //struct with current data of the joystick
+    //typedef struct joystickData_t {
+    //    joystickPos_t position;
+    //    float x;
+    //    float y;
+    //    float radius;
+    //    float angle;
+    //} joystickData_t;
 
-    if (shake_state){
+
+    //--- evaluate stick position --- 
+    //4 quadrants and center only - with X and Y axis as hysteresis
     switch (data.position){
 
-            default:
+        case joystickPos_t::CENTER:
+            //immediately set to center at center
+            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;
+                }
+            }
+
+        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 (joystich in any of 4 quadrants) ---
+    switch (stickQuadrant){
+        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");
+            return commands;
             break;
-
+            //4 different modes
-            case joystickPos_t::Y_AXIS:
+        case joystickPos_t::TOP_RIGHT:
-                if (data.y > 0){
             commands.left.state = motorstate_t::FWD;
             commands.right.state = motorstate_t::FWD;
-                } else {
+            break;
+        case joystickPos_t::TOP_LEFT:
+            commands.left.state = motorstate_t::FWD;
+            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::REV;
             commands.right.state = motorstate_t::REV;
+            break;
     }
+
+
+    //--- turn motors on/off depending on pulsing shake variable ---
+    if (shake_state == true){
         //set duty to shake
         commands.left.duty = dutyShake;
         commands.right.duty = dutyShake;
-                break;
+        //directions are defined above depending on mode
-
-            case joystickPos_t::X_AXIS:
-                if (data.x > 0) {
-                    commands.left.state = motorstate_t::FWD;
-                    commands.right.state = motorstate_t::REV;
     } else {
-                    commands.left.state = motorstate_t::REV;
-                    commands.right.state = motorstate_t::FWD;
-                }
-                //set duty to shake
-                commands.left.duty = dutyShake;
-                commands.right.duty = dutyShake;
-                break;
-        }
-    } else { //shake state off
         commands.left.state = motorstate_t::IDLE;
         commands.right.state = motorstate_t::IDLE;
         commands.left.duty = 0;
@@ -482,5 +530,6 @@ motorCommands_t joystick_generateCommandsShaking(joystickData_t data){
             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;
 }