From 89cab00acc60d0a8d1d1d75504c8b476631f9fc9 Mon Sep 17 00:00:00 2001 From: jonny Date: Fri, 6 Sep 2024 11:05:31 +0200 Subject: [PATCH] Rework cControlledRest (for MIN_TIME_ON, MIN_TIME_OFF) - split up in more simple methods - clearer structure --- board_single/main/control.cpp | 4 +- common/chairAdjust.cpp | 339 +++++++++++++++++++++------------- common/chairAdjust.hpp | 16 +- 3 files changed, 223 insertions(+), 136 deletions(-) diff --git a/board_single/main/control.cpp b/board_single/main/control.cpp index 8c8c411..4a05547 100644 --- a/board_single/main/control.cpp +++ b/board_single/main/control.cpp @@ -514,8 +514,8 @@ void controlledArmchair::changeMode(controlMode_t modeNew, bool noBeep) case controlMode_t::ADJUST_CHAIR: ESP_LOGW(TAG, "switching from ADJUST_CHAIR mode => turning off adjustment motors..."); // prevent motors from being always on in case of mode switch while joystick is not in center thus motors currently moving - legRest->setState(REST_OFF); - backRest->setState(REST_OFF); + legRest->requestStateChange(REST_OFF); + backRest->requestStateChange(REST_OFF); break; } diff --git a/common/chairAdjust.cpp b/common/chairAdjust.cpp index defba8a..ea5c312 100644 --- a/common/chairAdjust.cpp +++ b/common/chairAdjust.cpp @@ -9,6 +9,9 @@ extern "C" #define MUTEX_TIMEOUT (10000 / portTICK_PERIOD_MS) +#define MIN_TIME_ON 1000 +#define MIN_TIME_OFF 2000 + //--- gloabl variables --- // strings for logging the rest state const char* restStateStr[] = {"REST_OFF", "REST_DOWN", "REST_UP"}; @@ -51,107 +54,39 @@ void cControlledRest::init() state = REST_OFF; } - - //========================== //===== updatePosition ===== //========================== // calculate and update position in percent based of time running in current direction -void cControlledRest::updatePosition(){ - uint32_t now = esp_log_timestamp(); - uint32_t timeRan = now - timestamp_lastPosUpdate; - timestamp_lastPosUpdate = now; - float positionOld = positionNow; - - // calculate new percentage - switch (state) - { - case REST_UP: - positionNow += (float)timeRan / travelDuration * 100; - break; - case REST_DOWN: - positionNow -= (float)timeRan / travelDuration * 100; - break; - case REST_OFF: - //no change - ESP_LOGW(TAG, "updatePosition() unknown direction - cant update position when state is REST_OFF"); - break; - } - - // clip to 0-100 (because cant actually happen due to limit switches) - if (positionNow < 0) - positionNow = 0; - else if (positionNow > 100) - positionNow = 100; - - - ESP_LOGD(TAG, "[%s] state='%s' - update pos from %.2f%% to %.2f%% (time ran %dms)", name, restStateStr[state], positionOld, positionNow, timeRan); - -} - - - -//============================ -//========= setState ========= -//============================ -void cControlledRest::setState(restState_t targetState) +void cControlledRest::updatePosition() { - // lock the mutex before accessing shared variables - if (xSemaphoreTakeRecursive(mutex, MUTEX_TIMEOUT) == pdTRUE) + uint32_t now = esp_log_timestamp(); + uint32_t timeRan = now - timestamp_lastPosUpdate; + timestamp_lastPosUpdate = now; + float positionOld = positionNow; + + // calculate new percentage + switch (state) { - // TODO: drop this section? - // check if actually changed - if (targetState == state) - { - // update anyways when target is 0 or 100, to trigger movement threshold in case of position tracking is out of sync - if (positionTarget == 0 || positionTarget == 100) - ESP_LOGD(TAG, "[%s] state already at '%s', but updating anyway to trigger move to limit addition", name, restStateStr[state]); - else - { - ESP_LOGV(TAG, "[%s] state already at '%s', nothing to do", name, restStateStr[state]); - return; - } - } - - // when switching direction without stop: update position first - if (state != REST_OFF) - updatePosition(); - - // activate handle task when turning on (previous state is off) - if (state == REST_OFF) - xTaskNotifyGive(taskHandle); // activate handle task that stops the rest-motor again - - // apply new state - ESP_LOGI(TAG, "[%s] switching from state '%s' to '%s'", name, restStateStr[state], restStateStr[targetState]); - switch (targetState) - { - case REST_UP: - gpio_set_level(gpio_down, 0); - gpio_set_level(gpio_up, 1); - timestamp_lastPosUpdate = esp_log_timestamp(); - break; - case REST_DOWN: - gpio_set_level(gpio_down, 1); - gpio_set_level(gpio_up, 0); - timestamp_lastPosUpdate = esp_log_timestamp(); - break; - case REST_OFF: - gpio_set_level(gpio_down, 0); - gpio_set_level(gpio_up, 0); - updatePosition(); - positionTarget = positionNow; // disable resuming - no unexpected pos when incrementing - break; - } - state = targetState; - - // Release the mutex - xSemaphoreGiveRecursive(mutex); - } - else - { - ESP_LOGE(TAG, "mutex timeout in setState() -> RESTART"); - esp_restart(); + case REST_UP: + positionNow += (float)timeRan / travelDuration * 100; + break; + case REST_DOWN: + positionNow -= (float)timeRan / travelDuration * 100; + break; + case REST_OFF: + // no change + ESP_LOGW(TAG, "updatePosition() unknown direction - cant update position when state is REST_OFF"); + return; } + + // clip to 0-100 (because cant actually happen due to limit switches) + if (positionNow < 0) + positionNow = 0; + else if (positionNow > 100) + positionNow = 100; + + ESP_LOGD(TAG, "[%s] state='%s' - update pos from %.2f%% to %.2f%% (time ran %dms)", name, restStateStr[state], positionOld, positionNow, timeRan); } //========================== @@ -162,7 +97,6 @@ void cControlledRest::setTargetPercent(float targetPercent) // lock the mutex before accessing shared variables if (xSemaphoreTakeRecursive(mutex, MUTEX_TIMEOUT) == pdTRUE) { - positionTarget = targetPercent; float positionTargetPrev = positionTarget; positionTarget = targetPercent; @@ -172,23 +106,21 @@ void cControlledRest::setTargetPercent(float targetPercent) else if (positionTarget < 0) positionTarget = 0; - // ignore if unchanged - // if (positionTarget == positionTargetPrev){ - // ESP_LOGI(TAG, "[%s] Target position unchanged at %.2f%%", name, positionTarget); - // return; - //} - ESP_LOGI(TAG, "[%s] changed Target position from %.2f%% to %.2f%%", name, positionTargetPrev, positionTarget); + // update actual position positionNow first when already running + if (state != REST_OFF) + updatePosition(); + // start rest in required direction // TODO always run this check in handle()? // note: when already at 0/100 start anyways (runs for certain threshold in case tracked position out of sync) if (positionTarget > positionNow || positionTarget >= 100) - setState(REST_UP); + requestStateChange(REST_UP); else if (positionTarget < positionNow || positionTarget <= 0) - setState(REST_DOWN); + requestStateChange(REST_DOWN); else // already at exact position - setState(REST_OFF); + requestStateChange(REST_OFF); // Release the mutex xSemaphoreGiveRecursive(mutex); @@ -200,20 +132,171 @@ void cControlledRest::setTargetPercent(float targetPercent) } } -//====================== -//======= handle ======= -//====================== + + +//============================ +//==== requestStateChange ==== +//============================ +// queue state change that is executed when valid (respecting min thresholds) +void cControlledRest::requestStateChange(restState_t targetState) +{ + // lock the mutex before accessing shared variables + if (xSemaphoreTakeRecursive(mutex, MUTEX_TIMEOUT) == pdTRUE) + { + ESP_LOGV(TAG, "[%s] requesting change to state '%s'", name, restStateStr[targetState]); + nextState = targetState; + + // activate handle to process the request when on running already + if (state == REST_OFF) + xTaskNotifyGive(taskHandle); // activate handle task that stops the rest-motor again + // Release the mutex + xSemaphoreGiveRecursive(mutex); + } + else + { + ESP_LOGE(TAG, "mutex timeout while waiting in requestStateChange -> RESTART"); + esp_restart(); + } +} + + + +//=========================== +//==== handleStateChange ==== +//=========================== +// ensure MIN_TIME_ON and MIN_TIME_OFF has passed between doing the requested state change +// repeatedly run by task if state is requested +void cControlledRest::handleStateChange() +{ + // lock the mutex before accessing shared variables + if (xSemaphoreTakeRecursive(mutex, MUTEX_TIMEOUT) == pdTRUE) + { + uint32_t now = esp_log_timestamp(); + + // already at target state + if (state == nextState) + { + // exit, nothing todo + xSemaphoreGiveRecursive(mutex); + return; + } + + // turn off requested + else if (nextState == REST_OFF) + { + // exit if not on long enough + if (now - timestamp_lastStateChange < MIN_TIME_ON) + { + positionTarget = positionNow; // disable resuming - no unexpected pos when incrementing + ESP_LOGV(TAG, "SC: not on long enough, not turning off yet"); + xSemaphoreGiveRecursive(mutex); + return; + } + } + + // turn on requested + else if (state == REST_OFF && nextState != REST_OFF) + { + // exit if not off long enough + if (now - timestamp_lastStateChange < MIN_TIME_OFF) + { + ESP_LOGV(TAG, "SC: not OFF long enough, not turning on yet"); + xSemaphoreGiveRecursive(mutex); + return; + } + } + + // direction change requested + else if (state != REST_OFF && nextState != REST_OFF) + { + // exit if not on long enough + if (now - timestamp_lastStateChange < MIN_TIME_ON) + { + ESP_LOGV(TAG, "SC: dir change detected: not ON long enough, not turning off yet"); + xSemaphoreGiveRecursive(mutex); + return; + } + // no immediate dir change, turn off first + else + { + ESP_LOGD(TAG, "SC: dir change detected: turning off first"); + changeState(REST_OFF); + xSemaphoreGiveRecursive(mutex); + return; + } + } + + // not exited by now = no reason to prevent the state change -> update state! + ESP_LOGV(TAG, "SC: change is allowed now -> applying new state '%s'", restStateStr[nextState]); + changeState(nextState); + + // Release the mutex + xSemaphoreGiveRecursive(mutex); + } + else + { + ESP_LOGE(TAG, "mutex timeout while waiting in handleStateChange -> RESTART"); + esp_restart(); + } +} + + + +//========================= +//====== changeState ====== +//========================= +// change state (relays, timestamp) without any validation whether change is allowed +void cControlledRest::changeState(restState_t newState) +{ + // check if actually changed + if (newState == state) + { + ESP_LOGV(TAG, "[%s] state already at '%s', nothing to do", name, restStateStr[state]); + return; + } + + // apply new state to relays + ESP_LOGI(TAG, "[%s] switching Relays from state '%s' to '%s'", name, restStateStr[state], restStateStr[newState]); + switch (newState) + { + case REST_UP: + gpio_set_level(gpio_down, 0); + gpio_set_level(gpio_up, 1); + break; + case REST_DOWN: + gpio_set_level(gpio_down, 1); + gpio_set_level(gpio_up, 0); + break; + case REST_OFF: + gpio_set_level(gpio_down, 0); + gpio_set_level(gpio_up, 0); + break; + } + + // apply new state to variables + timestamp_lastStateChange = esp_log_timestamp(); + updatePosition(); + state = newState; +} + + + +//=============================== +//=== handle StopAtPosReached === +//=============================== // handle automatic stop when target position is reached, should be run repeatedly in a task #define TRAVEL_TIME_LIMIT_ADDITION_MS 2000 // traveling longer into limit compensates inaccuracies in time based position tracking -void cControlledRest::handle() +void cControlledRest::handleStopAtPosReached() { // lock the mutex before accessing shared variables if (xSemaphoreTakeRecursive(mutex, MUTEX_TIMEOUT) == pdTRUE) { // nothing to do when not running atm - // TODO: turn on automatically when position != target? if (state == REST_OFF) + { + xSemaphoreGiveRecursive(mutex); return; + } // calculate time already running and needed time to reach target uint32_t timeRan = esp_log_timestamp() - timestamp_lastPosUpdate; @@ -226,16 +309,15 @@ void cControlledRest::handle() // target reached if (timeRan >= timeTarget) { - ESP_LOGW(TAG, "[%s] handle: reached target run-time (%dms/%dms) for position %.2f%% -> stopping", name, timeRan, timeTarget, positionTarget); - setState(REST_OFF); + ESP_LOGW(TAG, "[%s] reached target! run-time (%dms/%dms) for position %.2f%% -> requesting stop", name, timeRan, timeTarget, positionTarget); + requestStateChange(REST_OFF); } - // Release the mutex xSemaphoreGiveRecursive(mutex); } else { - ESP_LOGE(TAG, "mutex timeout while waiting in handle() -> RESTART"); + ESP_LOGE(TAG, "mutex timeout while waiting in handleStopAtPosReached -> RESTART"); esp_restart(); } } @@ -244,21 +326,25 @@ void cControlledRest::handle() //===== chairAdjust_task ===== //============================ #define CHAIR_ADJUST_HANDLE_TASK_DELAY 100 -void chairAdjust_task( void * pvParameter ) +void chairAdjust_task(void *pvParameter) { - cControlledRest * rest = (cControlledRest *)pvParameter; - ESP_LOGW(TAG, "Starting task for controlling %s...", rest->getName()); + cControlledRest *rest = (cControlledRest *)pvParameter; + ESP_LOGW(TAG, "Starting task for controlling %s...", rest->getName()); + // provide taskHandle to rest object for wakeup rest->taskHandle = xTaskGetCurrentTaskHandle(); - while (1) - { - ulTaskNotifyTake(pdTRUE, portMAX_DELAY); // wait for wakeup by setState() (rest-motor turned on) + while (1) + { + ulTaskNotifyTake(pdTRUE, portMAX_DELAY); // wait for wakeup by changeState() (rest-motor turned on) ESP_LOGW(TAG, "task %s: received notification -> activating task!", rest->getName()); - while (rest->getState() != REST_OFF){ - rest->handle(); + // running while 1. motor running or 2. not in target state yet + while ((rest->getState() != REST_OFF) || (rest->getNextState() != rest->getState())) + { + rest->handleStateChange(); + rest->handleStopAtPosReached(); vTaskDelay(CHAIR_ADJUST_HANDLE_TASK_DELAY / portTICK_PERIOD_MS); } - ESP_LOGW(TAG, "task %s: rest turned off -> sleeping task", rest->getName()); + ESP_LOGW(TAG, "task %s: motor-off and at target state -> sleeping task", rest->getName()); } } @@ -269,12 +355,7 @@ void chairAdjust_task( void * pvParameter ) //==================================== //function that controls the two rests according to joystick data (applies threshold, defines direction) //TODO: -// - add separate task that controls chair adjustment -// - timeout -// - track position -// - auto-adjust: move to position while driving // - control via app -// - add delay betweem direction change void controlChairAdjustment(joystickData_t data, cControlledRest * legRest, cControlledRest * backRest){ //--- variables --- float stickThreshold = 0.3; //min coordinate for motor to start @@ -283,10 +364,10 @@ void controlChairAdjustment(joystickData_t data, cControlledRest * legRest, cCon //leg rest (x-axis) if (data.x > stickThreshold) legRest->setTargetPercent(100); else if (data.x < -stickThreshold) legRest->setTargetPercent(0); - else legRest->setState(REST_OFF); + else legRest->requestStateChange(REST_OFF); //back rest (y-axis) if (data.y > stickThreshold) backRest->setTargetPercent(100); else if (data.y < -stickThreshold) backRest->setTargetPercent(0); - else backRest->setState(REST_OFF); + else backRest->requestStateChange(REST_OFF); } \ No newline at end of file diff --git a/common/chairAdjust.hpp b/common/chairAdjust.hpp index 0479978..32d92de 100644 --- a/common/chairAdjust.hpp +++ b/common/chairAdjust.hpp @@ -26,18 +26,22 @@ extern const char* restStateStr[]; class cControlledRest { public: cControlledRest(gpio_num_t gpio_up, gpio_num_t gpio_down, uint32_t travelDurationMs, const char *name, float defaultPosition = 0); - void setState(restState_t targetState); + void requestStateChange(restState_t targetState); //mutex restState_t getState() const {return state;}; + restState_t getNextState() const {return nextState;}; float getPercent(); //TODO update position first - void setTargetPercent(float targetPercent); + void setTargetPercent(float targetPercent); //mutex float getTargetPercent() const {return positionTarget;}; - void handle(); + + void handleStopAtPosReached(); //mutex + void handleStateChange(); //mutex const char * getName() const {return name;}; - TaskHandle_t taskHandle = NULL; //task that repeatedly runs the handle() method + TaskHandle_t taskHandle = NULL; //task that repeatedly runs the handle() method, is assigned at task creation private: void init(); void updatePosition(); + void changeState(restState_t newState); SemaphoreHandle_t mutex; @@ -46,7 +50,9 @@ private: const gpio_num_t gpio_down; const uint32_t travelDuration = 12000; - restState_t state; + restState_t state = REST_OFF; + restState_t nextState = REST_OFF; + uint32_t timestamp_lastStateChange = 0; uint32_t timestamp_lastPosUpdate = 0; float positionTarget = 0; float positionNow = 0;