Rework cControlledRest (for MIN_TIME_ON, MIN_TIME_OFF)

- split up in more simple methods
- clearer structure

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);
+            legRest->requestStateChange(REST_OFF);
-            backRest->setState(REST_OFF);
+            backRest->requestStateChange(REST_OFF);
             break;
         }
 

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(){
+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)
 {
-    // lock the mutex before accessing shared variables
+    uint32_t now = esp_log_timestamp();
-    if (xSemaphoreTakeRecursive(mutex, MUTEX_TIMEOUT) == pdTRUE)
+    uint32_t timeRan = now - timestamp_lastPosUpdate;
+    timestamp_lastPosUpdate = now;
+    float positionOld = positionNow;
+
+    // calculate new percentage
+    switch (state)
     {
-        // TODO: drop this section?
+    case REST_UP:
-        // check if actually changed
+        positionNow += (float)timeRan / travelDuration * 100;
-        if (targetState == state)
+        break;
-        {
+    case REST_DOWN:
-            // update anyways when target is 0 or 100, to trigger movement threshold in case of position tracking is out of sync
+        positionNow -= (float)timeRan / travelDuration * 100;
-            if (positionTarget == 0 || positionTarget == 100)
+        break;
-                ESP_LOGD(TAG, "[%s] state already at '%s', but updating anyway to trigger move to limit addition", name, restStateStr[state]);
+    case REST_OFF:
-            else
+        // no change
-            {
+        ESP_LOGW(TAG, "updatePosition() unknown direction - cant update position when state is REST_OFF");
-                ESP_LOGV(TAG, "[%s] state already at '%s', nothing to do", name, restStateStr[state]);
+        return;
-                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();
     }
+
+    // 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);
+            ESP_LOGW(TAG, "[%s] reached target! run-time (%dms/%dms) for position %.2f%% -> requesting stop", name, timeRan, timeTarget, positionTarget);
-            setState(REST_OFF);
+            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;
+    cControlledRest *rest = (cControlledRest *)pvParameter;
-	ESP_LOGW(TAG, "Starting task for controlling %s...", rest->getName());
+    ESP_LOGW(TAG, "Starting task for controlling %s...", rest->getName());
+    // provide taskHandle to rest object for wakeup
     rest->taskHandle = xTaskGetCurrentTaskHandle();
 
-	while (1)
+    while (1)
-	{
+    {
-        ulTaskNotifyTake(pdTRUE, portMAX_DELAY); // wait for wakeup by setState() (rest-motor turned on)
+        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){
+        // running while 1. motor running  or  2. not in target state yet
-            rest->handle();
+        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);
 }

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;