Fix race condition causing bug: motors stay on after mode-change

Add mutex to fix bug motors stay on when mode-change while driving
due to race condition when handle() still executing while mode change

Change joystick scaling parameters

control:
    - add mutex to handle() and changemode() to prevent race condition
    - outsource handle() method from createHandleLoop()
    - change joystick scaling (reduce 'more detail in slower area')
    - comments, formatting

board_single/main/control.cpp

@@ -59,6 +59,9 @@ controlledArmchair::controlledArmchair(
     
     // override default config value if maxDuty is found in nvs
     loadMaxDuty();
+
+    // create semaphore for preventing race condition: mode-change operations while currently still executing certain mode
+    handleIteration_mutex = xSemaphoreCreateMutex();
 }
 
 
@@ -75,25 +78,59 @@ void task_control( void * pvParameters ){
 }
 
 
+
 //----------------------------------
 //---------- Handle loop -----------
 //----------------------------------
-//function that repeatedly generates motor commands depending on the current mode
+// start endless loop that repeatedly calls handle() and handleTimeout() methods
-void controlledArmchair::startHandleLoop() {
+void controlledArmchair::startHandleLoop()
-    while (1){
+{
-        ESP_LOGV(TAG, "control loop executing... mode=%s", controlModeStr[(int)mode]);
+    while (1)
+    {
+        // mutex to prevent race condition with actions beeing run at mode change and previous mode still beeing executed
+        if (xSemaphoreTake(handleIteration_mutex, portMAX_DELAY) == pdTRUE)
+        {
+            //--- handle current mode ---
+            ESP_LOGV(TAG, "control loop executing... mode='%s'", controlModeStr[(int)mode]);
+            handle();
 
-        switch(mode) {
+            xSemaphoreGive(handleIteration_mutex);
+        } // end mutex
+
+        //--- slow loop ---
+        // this section is run approx every 5s (+500ms)
+        if (esp_log_timestamp() - timestamp_SlowLoopLastRun > 5000)
+        {
+            ESP_LOGV(TAG, "running slow loop... time since last run: %.1fs", (float)(esp_log_timestamp() - timestamp_SlowLoopLastRun) / 1000);
+            timestamp_SlowLoopLastRun = esp_log_timestamp();
+            //--- handle timeouts ---
+            // run function that detects timeouts (switch to idle, or notify "forgot to turn off")
+            handleTimeout();
+        }
+        vTaskDelay(5 / portTICK_PERIOD_MS); // small delay necessary to give modeChange() a chance to take the mutex
+        // TODO: move mode specific delays from handle() to here, to prevent unnecessary long mutex lock
+    }
+}
+
+
+//-------------------------------------
+//---------- Handle control -----------
+//-------------------------------------
+// function that repeatedly generates motor commands and runs actions depending on the current mode
+void controlledArmchair::handle()
+{
+
+    switch (mode)
+    {
     default:
-                mode = controlMode_t::IDLE;
+        //switch to IDLE mode when current mode is not implemented
+        changeMode(controlMode_t::IDLE);
         break;
 
+    //------- handle IDLE -------
     case controlMode_t::IDLE:
-                //copy preset commands for idling both motors - now done once at mode change
-                //commands = cmds_bothMotorsIdle;
-                //motorRight->setTarget(commands.right.state, commands.right.duty); 
-                //motorLeft->setTarget(commands.left.state, commands.left.duty); 
         vTaskDelay(500 / portTICK_PERIOD_MS);
+        // TODO repeatedly set motors to idle, in case driver bugs? Currently 15s motorctl timeout would have to pass
 #ifdef JOYSTICK_LOG_IN_IDLE
         // get joystick data and log it
         joystickData_t data joystick_l->getData();
@@ -103,6 +140,7 @@ void controlledArmchair::startHandleLoop() {
                  objects->joystick->getRawX(), objects->joystick->getRawY());
 #endif
         break;
+
     //------- handle JOYSTICK mode -------
     case controlMode_t::JOYSTICK:
         vTaskDelay(50 / portTICK_PERIOD_MS);
@@ -110,7 +148,7 @@ void controlledArmchair::startHandleLoop() {
         stickDataLast = stickData;
         stickData = joystick_l->getData();
         // additionaly scale coordinates (more detail in slower area)
-                joystick_scaleCoordinatesLinear(&stickData, 0.6, 0.35); //TODO: add scaling parameters to config
+        joystick_scaleCoordinatesLinear(&stickData, 0.6, 0.5); // TODO: add scaling parameters to config
         // generate motor commands
         // only generate when the stick data actually changed (e.g. stick stayed in center)
         if (stickData.x != stickDataLast.x || stickData.y != stickDataLast.y)
@@ -128,7 +166,6 @@ void controlledArmchair::startHandleLoop() {
         }
         break;
 
-
     //------- handle MASSAGE mode -------
     case controlMode_t::MASSAGE:
         vTaskDelay(10 / portTICK_PERIOD_MS);
@@ -150,7 +187,6 @@ void controlledArmchair::startHandleLoop() {
         }
         break;
 
-
     //------- handle HTTP mode -------
     case controlMode_t::HTTP:
         //--- get joystick data from queue ---
@@ -161,7 +197,8 @@ void controlledArmchair::startHandleLoop() {
         ESP_LOGD(TAG, "generating commands from x=%.3f  y=%.3f  radius=%.3f  angle=%.3f", stickData.x, stickData.y, stickData.radius, stickData.angle);
         //--- generate motor commands ---
         // only generate when the stick data actually changed (e.g. no new data recevied via http)
-                if (stickData.x != stickDataLast.x || stickData.y != stickDataLast.y ){
+        if (stickData.x != stickDataLast.x || stickData.y != stickDataLast.y)
+        {
             resetTimeout(); // user input -> reset switch to IDLE timeout
             // Note: timeout (no data received) is handled in getData method
             commands = joystick_generateCommandsDriving(stickData, &joystickGenerateCommands_config);
@@ -176,7 +213,6 @@ void controlledArmchair::startHandleLoop() {
         }
         break;
 
-
     //------- handle AUTO mode -------
     case controlMode_t::AUTO:
         vTaskDelay(20 / portTICK_PERIOD_MS);
@@ -187,7 +223,8 @@ void controlledArmchair::startHandleLoop() {
         motorLeft->setTarget(commands.left);
 
         // process received instruction
-               switch (instruction) {
+        switch (instruction)
+        {
         case auto_instruction_t::NONE:
             break;
         case auto_instruction_t::SWITCH_PREV_MODE:
@@ -217,7 +254,6 @@ void controlledArmchair::startHandleLoop() {
         }
         break;
 
-
     //------- handle ADJUST_CHAIR mode -------
     case controlMode_t::ADJUST_CHAIR:
         vTaskDelay(100 / portTICK_PERIOD_MS);
@@ -233,7 +269,6 @@ void controlledArmchair::startHandleLoop() {
         }
         break;
 
-
     //------- handle MENU mode -------
     case controlMode_t::MENU:
         // nothing to do here, display task handles the menu
@@ -243,20 +278,7 @@ void controlledArmchair::startHandleLoop() {
         // TODO: add other modes here
     }
 
-        //-----------------------
+} // end - handle method
-        //------ slow loop ------
-        //-----------------------
-        //this section is run about every 5s (+500ms)
-        if (esp_log_timestamp() - timestamp_SlowLoopLastRun > 5000) {
-            ESP_LOGV(TAG, "running slow loop... time since last run: %.1fs", (float)(esp_log_timestamp() - timestamp_SlowLoopLastRun)/1000);
-            timestamp_SlowLoopLastRun = esp_log_timestamp();
-            //run function that detects timeout (switch to idle, or notify "forgot to turn off")
-            handleTimeout();
-        }
-
-    }//end while(1)
-}//end startHandleLoop
-
 
 
 
@@ -383,14 +405,21 @@ void controlledArmchair::handleTimeout()
 //----------- changeMode ------------
 //-----------------------------------
 //function to change to a specified control mode
-void controlledArmchair::changeMode(controlMode_t modeNew) {
+void controlledArmchair::changeMode(controlMode_t modeNew)
+{
 
     // exit if target mode is already active
-    if (mode == modeNew) {
+    if (mode == modeNew)
+    {
         ESP_LOGE(TAG, "changeMode: Already in target mode '%s' -> nothing to change", controlModeStr[(int)mode]);
         return;
     }
 
+    // mutex to wait for current handle iteration (control-task) to finish
+    // prevents race conditions where operations when changing mode are run but old mode gets handled still
+    ESP_LOGI(TAG, "changeMode: waiting for current handle() iteration to finish...");
+    if (xSemaphoreTake(handleIteration_mutex, portMAX_DELAY) == pdTRUE)
+    {
         // copy previous mode
         modePrevious = mode;
         // store time changed (needed for timeout)
@@ -400,7 +429,8 @@ void controlledArmchair::changeMode(controlMode_t modeNew) {
 
         //========== commands change FROM mode ==========
         // run functions when changing FROM certain mode
-	switch(modePrevious){
+        switch (modePrevious)
+        {
         default:
             ESP_LOGI(TAG, "noting to execute when changing FROM this mode");
             break;
@@ -448,13 +478,12 @@ void controlledArmchair::changeMode(controlMode_t modeNew) {
             legRest->setState(REST_OFF);
             backRest->setState(REST_OFF);
             break;
-
         }
 
-
         //========== commands change TO mode ==========
         // run functions when changing TO certain mode
-    switch(modeNew){
+        switch (modeNew)
+        {
         default:
             ESP_LOGI(TAG, "noting to execute when changing TO this mode");
             break;
@@ -491,14 +520,15 @@ void controlledArmchair::changeMode(controlMode_t modeNew) {
             motorLeft->setFade(fadeType_t::ACCEL, shake_msFadeAccel);
             motorRight->setFade(fadeType_t::ACCEL, shake_msFadeAccel);
             break;
-
         }
 
         //--- update mode to new mode ---
-    //TODO: add mutex
         mode = modeNew;
-}
 
+        // unlock mutex for control task to continue handling modes
+        xSemaphoreGive(handleIteration_mutex);
+    } // end mutex
+}
 
 //TODO simplify the following 3 functions? can be replaced by one?
 
@@ -526,7 +556,7 @@ void controlledArmchair::toggleModes(controlMode_t modePrimary, controlMode_t mo
     } 
     //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]);
+        ESP_LOGW(TAG, "toggleModes: switching from '%s' to primary mode '%s'", controlModeStr[(int)mode], controlModeStr[(int)modePrimary]);
         //buzzer->beep(4,200,100);
         changeMode(modePrimary);
     }
@@ -542,13 +572,13 @@ 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]);
+        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]);
+        ESP_LOGW(TAG, "toggleModes: switching from '%s' to primary mode '%s'", controlModeStr[(int)mode], controlModeStr[(int)modePrimary]);
         //buzzer->beep(4,200,100);
         changeMode(modePrimary);
     }

board_single/main/control.hpp

@@ -37,7 +37,7 @@ typedef struct control_config_t {
 //=======================================
 //task that controls the armchair modes and initiates commands generation and applies them to driver
 //parameter: pointer to controlledArmchair object 
-void task_control( void * pvParameters );
+void task_control( void * controlledArmchair );
 
 
 
@@ -64,7 +64,7 @@ class controlledArmchair {
                 );
 
         //--- functions ---
-        //task that repeatedly generates motor commands depending on the current mode
+        //endless loop that repeatedly calls handle() and handleTimeout() methods respecting mutex
         void startHandleLoop();
 
         //function that changes to a specified control mode
@@ -104,6 +104,9 @@ class controlledArmchair {
     private:
 
         //--- functions ---
+        //generate motor commands or run actions depending on the current mode
+        void handle();
+
         //function that evaluates whether there is no activity/change on the motor duty for a certain time, if so a switch to IDLE is issued. - has to be run repeatedly in a slow interval
         void handleTimeout();
 
@@ -149,6 +152,9 @@ class controlledArmchair {
         //struct with config parameters
         control_config_t config;
 
+        //mutex to prevent race condition between handle() and changeMode()
+        SemaphoreHandle_t handleIteration_mutex;
+
         //store joystick data
         joystickData_t stickData = joystickData_center;
         joystickData_t stickDataLast = joystickData_center;