Add example command, Improvements

- automaticArmchair:
  - add method addCommands for adding an array of
    commands to queue
  - add keys to simpleCommand struct
    - fadeDecel (not used yet)
    - fadeAcel (not used yet)
    - instructions (new enum for running other commands in control task
      , not used yet)

- button.cpp
  - add example command to 1x button press (comment out previous cmd
    temporarily)

- control.cpp
  - update changeMode function: dont do anything when current mode is
    already target mode

main/auto.cpp

@@ -21,15 +21,15 @@ motorCommands_t automatedArmchair::generateCommands() {
     //check if previous command is finished
     if ( esp_log_timestamp() > timestampCmdFinished ) {
         //get next command from queue
-        if( xQueueReceive( commandQueue, &cmdCurrent, pdMS_TO_TICKS(0) ) ) {
+        if( xQueueReceive( commandQueue, &cmdCurrent, pdMS_TO_TICKS(500) ) ) {
             ESP_LOGI(TAG, "running next command from queue...");
             //calculate timestamp the command is finished
             timestampCmdFinished = esp_log_timestamp() + cmdCurrent.msDuration;
             //copy the new commands
-            motorCommands = cmdCurrent.commands;
+            motorCommands = cmdCurrent.motorCmds;
         } else { //queue empty
             ESP_LOGD(TAG, "no new command in queue -> set motors to IDLE");
-            motorCommands = cmds_bothMotorsIdle;
+            motorCommands = motorCmds_bothMotorsIdle;
         }
     } else { //previous command still running
         ESP_LOGD(TAG, "command still running -> no change");
@@ -53,6 +53,12 @@ void automatedArmchair::addCommand(commandSimple_t command) {
      }
 }
 
+void automatedArmchair::addCommands(commandSimple_t commands[], size_t count) {
+    for (int i = 0; i < count; i++) {
+         ESP_LOGI(TAG, "Reading command no. %d from provided array", i);
+        addCommand(commands[i]);
+    }
+}
 
 
 //===============================
@@ -65,6 +71,7 @@ motorCommands_t automatedArmchair::clearCommands() {
     xQueueReset( commandQueue );
     ESP_LOGW(TAG, "command queue was successfully emptied");
     //return commands for idling both motors
-    return cmds_bothMotorsIdle;
+    motorCommands = motorCmds_bothMotorsIdle;
+    return motorCmds_bothMotorsIdle;
 }
 

main/auto.hpp

@@ -17,11 +17,17 @@ extern "C"
 //--------------------------------------------
 //---- struct, enum, variable declarations ---
 //--------------------------------------------
+//enum for special instructions / commands to be run in control task
+enum class instructions_t { NONE, SWITCH_PREV_MODE, RESET_ACCEL, RESET_DECEL };
+
 //struct for a simple command
 //e.g. put motors in a certain state for certain time
-typedef struct {
-    motorCommands_t commands;
+typedef struct commandSimple_t{
+    motorCommands_t motorCmds;
     uint32_t msDuration;
+    uint32_t fadeDecel;
+    uint32_t fadeAcel;
+    instructions_t instructions;
 } commandSimple_t;
 
 
@@ -36,11 +42,16 @@ class automatedArmchair {
         automatedArmchair(void);
         //function to generate motor commands
         //can be also seen as handle function 
-        //TODO: maybe create separate task for handling at mode switch and communicate with queue?
+        //TODO: go with other approach: separate task for handling auto mode
+        //  - 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 adds a basic command to the queue
         void addCommand(commandSimple_t command);
+        //function that adds an array of basic commands to queue
+        void addCommands(commandSimple_t commands[], size_t count);
 
         //function that deletes all pending/queued commands
         motorCommands_t clearCommands();
@@ -61,13 +72,13 @@ class automatedArmchair {
         motorCommands_t motorCommands;
 
         //command preset for idling motors
-        const motorCommand_t cmd_motorIdle = {
+        const motorCommand_t motorCmd_motorIdle = {
             .state = motorstate_t::IDLE,
             .duty = 0
         };
-        const motorCommands_t cmds_bothMotorsIdle = {
-            .left = cmd_motorIdle,
-            .right = cmd_motorIdle
+        const motorCommands_t motorCmds_bothMotorsIdle = {
+            .left = motorCmd_motorIdle,
+            .right = motorCmd_motorIdle
         };
 };
 

main/button.cpp

@@ -38,6 +38,7 @@ buttonCommands::buttonCommands(gpio_evaluatedSwitch * button_f, controlledArmcha
 void buttonCommands::action (uint8_t count){
     //--- variable declarations ---
     bool decelEnabled; //for different beeping when toggling
+    commandSimple_t cmds[8]; //array for commands for automatedArmchair
 
     //--- actions based on count ---
     switch (count){
@@ -54,9 +55,50 @@ void buttonCommands::action (uint8_t count){
             esp_restart();
 
         case 1:
-            ESP_LOGW(TAG, "cmd %d: sending button event to control task", count);
-            //-> define joystick center or toggle freeze input (executed in control task)
-            control->sendButtonEvent(count); //TODO: always send button event to control task (not just at count=1) -> control.cpp has to be changed
+            ////////ESP_LOGW(TAG, "cmd %d: sending button event to control task", count);
+            //////////-> define joystick center or toggle freeze input (executed in control task)
+            ////////control->sendButtonEvent(count); //TODO: always send button event to control task (not just at count=1) -> control.cpp has to be changed
+            //====== TESTING: send cmd to auto mode =====
+            //define commands
+            cmds[1] =
+            {
+                .motorCmds = {
+                    .left = {motorstate_t::FWD, 40},
+                    .right = {motorstate_t::FWD, 40}
+                },
+                .msDuration = 2000,
+                .fadeDecel = 800,
+                .fadeAcel = 1300,
+                .instructions = instructions_t::NONE
+            };
+            cmds[2] =
+            {
+                .motorCmds = {
+                    .left = {motorstate_t::IDLE, 0},
+                    .right = {motorstate_t::IDLE, 0}
+                },
+                .msDuration = 1000,
+                .fadeDecel = 800,
+                .fadeAcel = 1300,
+                .instructions = instructions_t::NONE
+            };
+            cmds[3] =
+            {
+                .motorCmds = {
+                    .left = {motorstate_t::REV, 40},
+                    .right = {motorstate_t::REV, 40}
+                },
+                .msDuration = 1000,
+                .fadeDecel = 800,
+                .fadeAcel = 1300,
+                .instructions = instructions_t::NONE
+            };
+
+            //send commands to automatedArmchair command queue
+            armchair.addCommands(cmds, 3);
+
+            //change mode to AUTO
+            control->changeMode(controlMode_t::AUTO);
             break;
 
         case 3:

main/button.hpp

@@ -4,6 +4,8 @@
 #include "buzzer.hpp"
 #include "control.hpp"
 #include "motorctl.hpp"
+#include "auto.hpp"
+#include "config.hpp"
 
 
 

main/control.cpp

@@ -263,6 +263,12 @@ void controlledArmchair::changeMode(controlMode_t modeNew) {
     //reset timeout timer
     resetTimeout();
 
+    //exit if target mode is already active
+    if (mode == modeNew) {
+        ESP_LOGE(TAG, "changeMode: Already in target mode '%s' -> nothing to change", controlModeStr[(int)mode]);
+        return;
+    }
+
     //copy previous mode
     controlMode_t modePrevious = mode;