Add chairAdjust task, pos tracking, move with encoder

board_single/main/button.cpp

@@ -23,7 +23,7 @@ void task_button(void *task_button_parameters)
     task_button_parameters_t *objects = (task_button_parameters_t *)task_button_parameters;
     ESP_LOGI(TAG, "Initializing command-button and starting handle loop");
     // create button instance
-    buttonCommands commandButton(objects->control, objects->joystick, objects->encoderQueue, objects->motorLeft, objects->motorRight, objects->buzzer);
+    buttonCommands commandButton(objects->control, objects->joystick, objects->encoderQueue, objects->motorLeft, objects->motorRight, objects->legRest, objects->backRest, objects->buzzer);
     // start handle loop
     commandButton.startHandleLoop();
 }
@@ -35,8 +35,10 @@ buttonCommands::buttonCommands(
     controlledArmchair *control_f,
     evaluatedJoystick *joystick_f,
     QueueHandle_t encoderQueue_f,
-    controlledMotor *motorLeft_f,
+    controlledMotor * motorLeft_f,
     controlledMotor *motorRight_f,
+    cControlledRest *legRest_f,
+    cControlledRest *backRest_f,
     buzzer_t *buzzer_f)
 {
     // copy object pointers
@@ -46,6 +48,8 @@ buttonCommands::buttonCommands(
     motorLeft = motorLeft_f;
     motorRight = motorRight_f;
     buzzer = buzzer_f;
+    legRest = legRest_f;
+    backRest = backRest_f;
     // TODO declare / configure evaluatedSwitch here instead of config (unnecessary that button object is globally available - only used here)?
 }
 
@@ -141,6 +145,12 @@ void buttonCommands::action (uint8_t count, bool lastPressLong){
             control->changeMode(controlMode_t::MASSAGE); //switch to MASSAGE mode
             break;
 
+        case 7:
+            legRest->setTargetPercent(100);
+            backRest->setTargetPercent(0);
+            ESP_LOGW(TAG, "7x TESTING: set leg/back rest to 100/0");
+            break;
+
         case 8:
         // ## toggle "sport-mode" ##
             //toggle deceleration fading between on and off
@@ -212,18 +222,25 @@ void buttonCommands::startHandleLoop()
                 isPressed = false; // rest stored state
                 break;
             case RE_ET_CHANGED: // scroll through status pages when simply rotating encoder
-                rotateCount++;
+
-                if (rotateCount >= 2) // at least two rotate-clicks necessary for one page switch
-                {
                 if (event.diff > 0)
-                        display_rotateStatusPage(true, true); // select NEXT status screen, stay at last element (dont rotate to first)
+                    legRest->setTargetPercent(legRest->getTargetPercent() + 10);
                 else
-                        display_rotateStatusPage(false, true); // select PREVIOUS status screen, stay at first element (dont rotate to last)
+                    legRest->setTargetPercent(legRest->getTargetPercent() - 10);
-                    rotateCount = 0;
+
-                    buzzer->beep(1, 90, 0);
+                //## switch status page with rotate - disabled
-                }
+                ///rotateCount++;
-                else
+                ///if (rotateCount >= 2) // at least two rotate-clicks necessary for one page switch
-                    buzzer->beep(1, 65, 0);
+                ///{
+                ///    if (event.diff > 0)
+                ///        display_rotateStatusPage(true, true); // select NEXT status screen, stay at last element (dont rotate to first)
+                ///    else
+                ///        display_rotateStatusPage(false, true); // select PREVIOUS status screen, stay at first element (dont rotate to last)
+                ///    rotateCount = 0;
+                ///    buzzer->beep(1, 90, 0);
+                ///}
+                ///else
+                ///    buzzer->beep(1, 65, 0);
                 break;
             case RE_ET_BTN_LONG_PRESSED:
             case RE_ET_BTN_CLICKED:
@@ -233,14 +250,15 @@ void buttonCommands::startHandleLoop()
         }
         else // timeout (no event received within TIMEOUT)
         {
+                //## switch status page with rotate - disabled
             // switch back to default status screen in case less than 2 rotate-clicks received
-            if (rotateCount != 0)
+            ///if (rotateCount != 0)
-            {
+            ///{
-                rotateCount = 0;
+            ///    rotateCount = 0;
-                display_selectStatusPage(STATUS_SCREEN_OVERVIEW);
+            ///    display_selectStatusPage(STATUS_SCREEN_OVERVIEW);
-                //TODO only change/beep if not already at STATUS_SCREEN_OVERVIEW
+            ///    //TODO only change/beep if not already at STATUS_SCREEN_OVERVIEW
-                //buzzer->beep(1, 100, 0);
+            ///    //buzzer->beep(1, 100, 0);
-            }
+            ///}
 
             // encoder was pressed
             if (count > 0)

board_single/main/button.hpp

@@ -6,6 +6,7 @@
 #include "motorctl.hpp"
 #include "auto.hpp"
 #include "joystick.hpp"
+#include "chairAdjust.hpp"
 
 
 
@@ -22,6 +23,8 @@ class buttonCommands {
             QueueHandle_t encoderQueue_f,
             controlledMotor * motorLeft_f,
             controlledMotor *motorRight_f,
+            cControlledRest *legRest_f,
+            cControlledRest *backRest_f,
             buzzer_t *buzzer_f);
 
         //--- functions ---
@@ -40,6 +43,8 @@ class buttonCommands {
         controlledMotor * motorRight;
         buzzer_t* buzzer;
         QueueHandle_t encoderQueue;
+        cControlledRest *legRest;
+        cControlledRest *backRest;
 
         //--- variables ---
         uint8_t count = 0;
@@ -62,6 +67,8 @@ typedef struct task_button_parameters_t
     QueueHandle_t encoderQueue;
     controlledMotor *motorLeft;
     controlledMotor *motorRight;
+    cControlledRest *legRest;
+    cControlledRest *backRest;
     buzzer_t *buzzer;
 } task_button_parameters_t;
 

board_single/main/main.cpp

@@ -162,8 +162,8 @@ void createObjects()
 
     // create objects for controlling the chair position
     //                       gpio_up, gpio_down, name
-    legRest = new cControlledRest(GPIO_NUM_2, GPIO_NUM_15, "legRest");
+    legRest = new cControlledRest(GPIO_NUM_2, GPIO_NUM_15, 14000, "legRest");
-    backRest = new cControlledRest(GPIO_NUM_16, GPIO_NUM_4, "backRest");
+    backRest = new cControlledRest(GPIO_NUM_16, GPIO_NUM_4, 12000, "backRest");
 
     // create control object (control.hpp)
     // with configuration from config.cpp
@@ -266,7 +266,7 @@ extern "C" void app_main(void) {
 	//--- create task for button ---
 	//------------------------------
 	//task that handles button/encoder events in any mode except 'MENU_SETTINGS' and 'MENU_MODE_SELECT' (e.g. switch modes by pressing certain count)
-	task_button_parameters_t button_param = {control, joystick, encoderQueue, motorLeft, motorRight, buzzer};
+	task_button_parameters_t button_param = {control, joystick, encoderQueue, motorLeft, motorRight, legRest, backRest, buzzer};
 	xTaskCreate(&task_button, "task_button", 4096, &button_param, 3, NULL);
 
 	//-----------------------------------
@@ -283,6 +283,13 @@ extern "C" void app_main(void) {
 	display_task_parameters_t display_param = {display_config, control, joystick, encoderQueue, motorLeft, motorRight, speedLeft, speedRight, buzzer, &nvsHandle};
 	xTaskCreate(&display_task, "display_task", 3*2048, &display_param, 3, NULL);
 	
+	//-------------------------------------
+	//-- create task for chairAdjustment --
+	//-------------------------------------
+	//task that stops chair-rest motors when they reach target
+	chairAdjust_task_parameters_t chairAdjust_param = {legRest, backRest};
+	xTaskCreate(&chairAdjust_task, "chairAdjust_task", 2048, &chairAdjust_param, 1, NULL);
+
 	vTaskDelay(200 / portTICK_PERIOD_MS); //wait for all tasks to finish initializing
 	printf("\n");
 

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);
+    }
+}
+
 
 
 //====================================

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;
+    const uint32_t travelDuration = 12000;
-    uint32_t timestamp_lastChange;
+    uint32_t timestamp_lastPosUpdate = 0;
-    float currentPosition = 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 ======
 //====================================