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;