Add working command instructions, optimize control

auto.cpp:
  - forward instructions from commands to newly passed pointer
control.cpp:
  - handle instructions returned at generation of commands in auto mode
  - add method toggleMode
  - remove dupliate code in toggleIdle method
button.cpp:
  - optimize command for enabling leg support, add instruction

main/auto.cpp

@@ -18,12 +18,16 @@ automatedArmchair::automatedArmchair(void) {
 //==============================
 //====== generateCommands ======
 //==============================
-motorCommands_t automatedArmchair::generateCommands() {
+motorCommands_t automatedArmchair::generateCommands(auto_instruction_t * instruction) {
+    //reset instruction
+    *instruction = auto_instruction_t::NONE;
     //check if previous command is finished
     if ( esp_log_timestamp() > timestampCmdFinished ) {
         //get next command from queue
         if( xQueueReceive( commandQueue, &cmdCurrent, pdMS_TO_TICKS(500) ) ) {
             ESP_LOGI(TAG, "running next command from queue...");
+            //copy instruction to be provided to control task
+            *instruction = cmdCurrent.instruction;
             //set acceleration / fading parameters according to command
             motorLeft.setFade(fadeType_t::DECEL, cmdCurrent.fadeDecel);
             motorRight.setFade(fadeType_t::DECEL, cmdCurrent.fadeDecel);

main/auto.hpp

@@ -18,7 +18,7 @@ extern "C"
 //---- struct, enum, variable declarations ---
 //--------------------------------------------
 //enum for special instructions / commands to be run in control task
-enum class auto_instruction_t { NONE, SWITCH_PREV_MODE, RESET_ACCEL, RESET_DECEL };
+enum class auto_instruction_t { NONE, SWITCH_PREV_MODE, SWITCH_JOYSTICK_MODE, RESET_ACCEL_DECEL, RESET_ACCEL, RESET_DECEL };
 
 //struct for a simple command
 //e.g. put motors in a certain state for certain time
@@ -46,7 +46,9 @@ class automatedArmchair {
         //  - receive commands with queue anyways
         //  - => use delay function
         //  - have a queue that outputs current motor state/commands -> repeatedly check the queue in control task
-        motorCommands_t generateCommands(); //repeatedly called by control task
+        //function that handles automatic driving and returns motor commands
+        //also provides instructions to be executed in control task via pointer
+        motorCommands_t generateCommands(auto_instruction_t * instruction);
 
         //function that adds a basic command to the queue
         void addCommand(commandSimple_t command);

main/button.cpp

@@ -66,20 +66,20 @@ void buttonCommands::action (uint8_t count){
                     .left = {motorstate_t::REV, 90},
                     .right = {motorstate_t::REV, 90}
                 },
-                .msDuration = 700,
+                .msDuration = 1200,
                 .fadeDecel = 800,
-                .fadeAccel = 1200,
+                .fadeAccel = 1300,
                 .instruction = auto_instruction_t::NONE
             };
             cmds[1] =
             {
                 .motorCmds = {
-                    .left = {motorstate_t::FWD, 50},
+                    .left = {motorstate_t::FWD, 70},
-                    .right = {motorstate_t::FWD, 50}
+                    .right = {motorstate_t::FWD, 70}
                 },
-                .msDuration = 100,
+                .msDuration = 70,
                 .fadeDecel = 0,
-                .fadeAccel = 400,
+                .fadeAccel = 300,
                 .instruction = auto_instruction_t::NONE
             };
             cmds[2] =
@@ -91,7 +91,7 @@ void buttonCommands::action (uint8_t count){
                 .msDuration = 10,
                 .fadeDecel = 800,
                 .fadeAccel = 1300,
-                .instruction = auto_instruction_t::NONE
+                .instruction = auto_instruction_t::SWITCH_JOYSTICK_MODE
             };
 
             //send commands to automatedArmchair command queue

main/control.cpp

@@ -125,11 +125,41 @@ void controlledArmchair::startHandleLoop() {
             case controlMode_t::AUTO:
                 vTaskDelay(20 / portTICK_PERIOD_MS);
                //generate commands
-               commands = armchair.generateCommands();
+               commands = armchair.generateCommands(&instruction);
                 //--- apply commands to motors ---
                 //TODO make motorctl.setTarget also accept motorcommand struct directly
-                motorRight->setTarget(commands.right.state, commands.right.duty); 
+               motorRight->setTarget(commands.right.state, commands.right.duty); 
-                motorLeft->setTarget(commands.left.state, commands.left.duty); 
+               motorLeft->setTarget(commands.left.state, commands.left.duty); 
+
+               //process received instruction
+               switch (instruction) {
+                   case auto_instruction_t::NONE:
+                       break;
+                   case auto_instruction_t::SWITCH_PREV_MODE:
+                       toggleMode(controlMode_t::AUTO);
+                       break;
+                   case auto_instruction_t::SWITCH_JOYSTICK_MODE:
+                       changeMode(controlMode_t::JOYSTICK);
+                       break;
+                   case auto_instruction_t::RESET_ACCEL_DECEL:
+                       //enable downfading (set to default value)
+                       motorLeft->setFade(fadeType_t::DECEL, true);
+                       motorRight->setFade(fadeType_t::DECEL, true);
+                       //set upfading to default value
+                       motorLeft->setFade(fadeType_t::ACCEL, true);
+                       motorRight->setFade(fadeType_t::ACCEL, true);
+                       break;
+                   case auto_instruction_t::RESET_ACCEL:
+                       //set upfading to default value
+                       motorLeft->setFade(fadeType_t::ACCEL, true);
+                       motorRight->setFade(fadeType_t::ACCEL, true);
+                       break;
+                   case auto_instruction_t::RESET_DECEL:
+                       //enable downfading (set to default value)
+                       motorLeft->setFade(fadeType_t::DECEL, true);
+                       motorRight->setFade(fadeType_t::DECEL, true);
+                       break;
+               }
                break;
 
 
@@ -274,7 +304,7 @@ void controlledArmchair::changeMode(controlMode_t modeNew) {
     }
 
     //copy previous mode
-    controlMode_t modePrevious = mode;
+    modePrevious = mode;
 
     ESP_LOGW(TAG, "=== changing mode from %s to %s ===", controlModeStr[(int)mode], controlModeStr[(int)modeNew]);
 
@@ -383,15 +413,8 @@ void controlledArmchair::changeMode(controlMode_t modeNew) {
 //-----------------------------------
 //function to toggle between IDLE and previous active mode
 void controlledArmchair::toggleIdle() {
-    if (mode == controlMode_t::IDLE){
+    //toggle between IDLE and previous mode
-        changeMode(modePrevious); //restore previous mode, or default if not switched yet
+    toggleMode(controlMode_t::IDLE);
-        buzzer->beep(2, 200, 100);
-        ESP_LOGW(TAG, "toggle idle: switched mode from IDLE to %s", controlModeStr[(int)mode]);
-    } else {
-        modePrevious = mode; //store current mode
-        changeMode(controlMode_t::IDLE); //set mode to IDLE
-        ESP_LOGW(TAG, "toggle idle: switched mode from %s to IDLE", controlModeStr[(int)mode]);
-    }
 }
 
 
@@ -415,3 +438,25 @@ void controlledArmchair::toggleModes(controlMode_t modePrimary, controlMode_t mo
         changeMode(modePrimary);
     }
 }
+
+
+
+//-----------------------------------
+//----------- toggleMode ------------
+//-----------------------------------
+//function that toggles between certain mode and previous mode
+void controlledArmchair::toggleMode(controlMode_t modePrimary){
+
+    //switch to previous mode when primary is already active
+    if (mode == modePrimary){
+        ESP_LOGW(TAG, "toggleMode: switching from primaryMode %s to previousMode %s", controlModeStr[(int)mode], controlModeStr[(int)modePrevious]);
+        //buzzer->beep(2,200,100);
+        changeMode(modePrevious); //switch to previous mode
+    } 
+    //switch to primary mode when any other mode is active
+    else {
+        ESP_LOGW(TAG, "toggleModes: switching from %s to primary mode %s", controlModeStr[(int)mode], controlModeStr[(int)modePrimary]);
+        //buzzer->beep(4,200,100);
+        changeMode(modePrimary);
+    }
+}

main/control.hpp

@@ -55,6 +55,8 @@ class controlledArmchair {
 
         //function that toggles between two modes, but prefers first argument if entirely different mode is currently active
         void toggleModes(controlMode_t modePrimary, controlMode_t modeSecondary);
+        //toggle between certain mode and previous mode
+        void toggleMode(controlMode_t modePrimary);
 
         //function that restarts timer which initiates the automatic timeout (switch to IDLE) after certain time of inactivity
         void resetTimeout();
@@ -91,6 +93,9 @@ class controlledArmchair {
 
         //variables for MASSAGE mode
         bool freezeInput = false;
+
+        //variables for AUTO mode
+        auto_instruction_t instruction = auto_instruction_t::NONE; //variable to receive instructions from automatedArmchair
         
         //variable to store button event
         uint8_t buttonCount = 0;