diff --git a/board_single/main/control.cpp b/board_single/main/control.cpp index 4a05547..9cde36e 100644 --- a/board_single/main/control.cpp +++ b/board_single/main/control.cpp @@ -120,21 +120,55 @@ void controlledArmchair::startHandleLoop() { while (1) { + // === handle current mode === // mutex to prevent race condition with actions beeing run at mode change and previous mode still beeing executed + ESP_LOGV(TAG, "handle(): requesting mutex..."); if (xSemaphoreTake(handleIteration_mutex, MUTEX_TIMEOUT / portTICK_PERIOD_MS) == pdTRUE) { + ESP_LOGV(TAG, "handle(): got mutex!"); //--- handle current mode --- ESP_LOGV(TAG, "control loop executing... mode='%s'", controlModeStr[(int)mode]); handle(); + ESP_LOGV(TAG, "handle(): releasing mutex"); xSemaphoreGive(handleIteration_mutex); } // end mutex - else { + else + { ESP_LOGE(TAG, "mutex timeout - stuck in changeMode? -> RESTART"); esp_restart(); } - //--- slow loop --- + // ==== run mode specific delay === + // outsourced here to not block the mutex by just waiting + switch (mode) + { + default: + case controlMode_t::IDLE: + vTaskDelay(500 / portTICK_PERIOD_MS); + break; + case controlMode_t::JOYSTICK: + vTaskDelay(50 / portTICK_PERIOD_MS); + break; + case controlMode_t::MASSAGE: + vTaskDelay(10 / portTICK_PERIOD_MS); + break; + case controlMode_t::HTTP: + // has 500ms timeout waiting for new events, thus blocks mutex... + break; + case controlMode_t::AUTO: + vTaskDelay(20 / portTICK_PERIOD_MS); + break; + case controlMode_t::ADJUST_CHAIR: + vTaskDelay(100 / portTICK_PERIOD_MS); + break; + case controlMode_t::MENU_SETTINGS: + case controlMode_t::MENU_MODE_SELECT: + vTaskDelay(500 / portTICK_PERIOD_MS); + break; + } + + //=== slow loop, timeout === // this section is run approx every 5s (+500ms) if (esp_log_timestamp() - timestamp_SlowLoopLastRun > 5000) { @@ -150,12 +184,14 @@ void controlledArmchair::startHandleLoop() } + //------------------------------------- //---------- Handle control ----------- //------------------------------------- // function that repeatedly generates motor commands and runs actions depending on the current mode void controlledArmchair::handle() { + //note: mode specific delays are outsourced to startHandleLoop() to not block the mutex switch (mode) { @@ -166,7 +202,6 @@ void controlledArmchair::handle() //------- handle IDLE ------- case controlMode_t::IDLE: - 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 @@ -180,7 +215,6 @@ void controlledArmchair::handle() //------- handle JOYSTICK mode ------- case controlMode_t::JOYSTICK: - vTaskDelay(50 / portTICK_PERIOD_MS); // get current joystick data with getData method of evaluatedJoystick stickDataLast = stickData; stickData = joystick_l->getData(); @@ -205,7 +239,6 @@ void controlledArmchair::handle() //------- handle MASSAGE mode ------- case controlMode_t::MASSAGE: - vTaskDelay(10 / portTICK_PERIOD_MS); //--- read joystick --- // only update joystick data when input not frozen stickDataLast = stickData; @@ -250,7 +283,6 @@ void controlledArmchair::handle() //------- handle AUTO mode ------- case controlMode_t::AUTO: - vTaskDelay(20 / portTICK_PERIOD_MS); // generate commands commands = automatedArmchair->generateCommands(&instruction); //--- apply commands to motors --- @@ -291,7 +323,6 @@ void controlledArmchair::handle() //------- handle ADJUST_CHAIR mode ------- case controlMode_t::ADJUST_CHAIR: - vTaskDelay(100 / portTICK_PERIOD_MS); //--- read joystick --- stickDataLast = stickData; stickData = joystick_l->getData(); @@ -308,7 +339,6 @@ void controlledArmchair::handle() case controlMode_t::MENU_SETTINGS: case controlMode_t::MENU_MODE_SELECT: // nothing to do here, display task handles the menu - vTaskDelay(500 / portTICK_PERIOD_MS); break; // TODO: add other modes here @@ -457,8 +487,10 @@ void controlledArmchair::changeMode(controlMode_t modeNew, bool noBeep) // 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..."); + ESP_LOGV(TAG, "changemode(): requesting mutex..."); if (xSemaphoreTake(handleIteration_mutex, MUTEX_TIMEOUT / portTICK_PERIOD_MS) == pdTRUE) { + ESP_LOGV(TAG, "changemode(): got mutex!"); // copy previous mode modePrevious = mode; // store time changed (needed for timeout) @@ -569,6 +601,7 @@ void controlledArmchair::changeMode(controlMode_t modeNew, bool noBeep) mode = modeNew; // unlock mutex for control task to continue handling modes + ESP_LOGV(TAG, "changemode(): releasing mutex"); xSemaphoreGive(handleIteration_mutex); } // end mutex else