Add chairAdjust task, pos tracking, move with encoder

common/chairAdjust.cpp

@@ -24,7 +24,7 @@ static const char * TAG = "chair-adjustment";
 //=============================
 //======== constructor ========
 //=============================
-cControlledRest::cControlledRest(gpio_num_t gpio_up_f, gpio_num_t gpio_down_f, const char * name_f){
+cControlledRest::cControlledRest(gpio_num_t gpio_up_f, gpio_num_t gpio_down_f, uint32_t travelDurationMs, const char * name_f):travelDuration(travelDurationMs){
     strcpy(name, name_f);
     gpio_up = gpio_up_f;
     gpio_down = gpio_down_f;
@@ -53,6 +53,43 @@ void cControlledRest::init()
 
 
 
+//==========================
+//===== 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 += timeRan / travelDuration * 100;
+            break;
+        case REST_DOWN:
+            positionNow -= timeRan / travelDuration * 100;
+            break;
+        case REST_OFF:
+            //no change
+            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 =========
 //============================
@@ -64,31 +101,103 @@ void cControlledRest::setState(restState_t targetState)
         return;
     }
 
+    // when switching direction without stop: update position first
+    if (state != REST_OFF)
+        updatePosition();
+
     //apply new state
     ESP_LOGI(TAG, "[%s] switching from state '%s' to '%s'", name, restStateStr[state], restStateStr[targetState]);
     state = targetState;
-    timestamp_lastChange = esp_log_timestamp(); //TODO use this to estimate position
     switch (state)
     {
     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();
         break;
     }
 }
 
-    void setTargetPercent(float targetPercent);
-    void handle(){
-        
+
+
+//==========================
+//==== setTargetPercent ====
+//==========================
+void cControlledRest::setTargetPercent(float targetPercent){
+    positionTarget = targetPercent;
+    // 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);
+    else if (positionTarget < positionNow || positionTarget <= 0)
+        setState(REST_DOWN);
+    else // already at exact position
+        setState(REST_OFF);
+}
+
+
+
+//======================
+//======= handle =======
+//======================
+// handle automatic stop when target position is reached, should be run repeatedly in a task
+#define TRAVEL_TIME_LIMIT_ADDITION_MS 3000 // traveling longer into limit compensates inaccuracies in time based position tracking
+void cControlledRest::handle(){
+
+    // nothing to do when not running atm
+    // TODO: turn on automatically when position != target?
+    if (state == REST_OFF)
+        return;
+
+    // calculate time already running and needed time to reach target
+    uint32_t timeRan = esp_log_timestamp() - timestamp_lastPosUpdate;
+    uint32_t timeTarget = travelDuration * fabs(positionTarget - positionNow) / 100;
+
+    // intentionally travel longer into limit - compensates inaccuracies in time based position tracking
+    if (positionTarget == 0 || positionTarget == 100)
+        timeTarget += TRAVEL_TIME_LIMIT_ADDITION_MS;
+
+    // target reached
+    if (timeRan >= timeTarget){
+        ESP_LOGI(TAG, "[%s] reached target run-time (%dms/%dms) for position %.2f%% -> stopping", name, timeRan, timeTarget, positionTarget);
+        setState(REST_OFF);
     }
+}
+
+
+
+//============================
+//===== chairAdjust_task =====
+//============================
+#define CHAIR_ADJUST_HANDLE_TASK_DELAY 500
+void chairAdjust_task( void * pvParameters )
+{
+	ESP_LOGW(TAG, "Starting chairAdjust task...");
+	//get struct with pointers to all needed global objects from task parameter
+	chairAdjust_task_parameters_t *objects = (chairAdjust_task_parameters_t *)pvParameters;
+
+	// repeatedly update display with content depending on current mode
+	while (1)
+	{
+        //TODO mutex
+        //TODO add queue so task only runs when at least one rest is actually moving
+        objects->legRest->handle();
+        objects->backRest->handle();
+        vTaskDelay(CHAIR_ADJUST_HANDLE_TASK_DELAY / portTICK_PERIOD_MS);
+    }
+}
+
 
 
 //====================================
@@ -116,4 +225,4 @@ void controlChairAdjustment(joystickData_t data, cControlledRest * legRest, cCon
     if (data.y > stickThreshold) backRest->setState(REST_UP);
     else if (data.y < -stickThreshold) backRest->setState(REST_DOWN);
     else backRest->setState(REST_OFF);
-}
+}

common/chairAdjust.hpp

@@ -18,25 +18,44 @@ extern const char* restStateStr[];
 //2 instances will be created one for back and one for leg rest
 class cControlledRest {
 public:
-    cControlledRest(gpio_num_t gpio_up, gpio_num_t gpio_down, const char *name);
+    cControlledRest(gpio_num_t gpio_up, gpio_num_t gpio_down, uint32_t travelDurationMs, const char *name);
     void setState(restState_t targetState);
-    float getPercent();
+    float getPercent(); //TODO update position first
     void setTargetPercent(float targetPercent);
+    float getTargetPercent() const {return positionTarget;};
     void handle();
-    void stop();
 
 private:
     void init();
+    void updatePosition();
 
     char name[32];
     gpio_num_t gpio_up;
     gpio_num_t gpio_down;
     restState_t state;
-    const uint32_t travelDuration = 5000;
-    uint32_t timestamp_lastChange;
-    float currentPosition = 0;
+    const uint32_t travelDuration = 12000;
+    uint32_t timestamp_lastPosUpdate = 0;
+    float positionTarget = 0;
+    float positionNow = 0;
+
 };
 
+// struct with variables passed to task from main()
+typedef struct chairAdjust_task_parameters_t {
+    cControlledRest * legRest;
+    cControlledRest * backRest;
+    //buzzer_t *buzzer;
+} chairAdjust_task_parameters_t;
+
+
+
+//===========================
+//==== chairAdjust_task =====
+//===========================
+void chairAdjust_task( void * chairAdjust_task_parameters_t );
+
+
+
 //====================================
 //====== controlChairAdjustment ======
 //====================================