j/armchair_fw
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Fix race condition causing bug: motors stay on after mode-change

Browse Source 
Add mutex to fix bug motors stay on when mode-change while driving
due to race condition when handle() still executing while mode change

Change joystick scaling parameters

control:
    - add mutex to handle() and changemode() to prevent race condition
    - outsource handle() method from createHandleLoop()
    - change joystick scaling (reduce 'more detail in slower area')
    - comments, formatting
This commit is contained in:
jonny 2024-03-06 10:59:21 +01:00
parent a6a630af44
commit a5544adeb6
2 changed files with 254 additions and 218 deletions
board_single/main
control.cppcontrol.hpp

118
board_single/main/control.cpp

@ -59,6 +59,9 @@ controlledArmchair::controlledArmchair(
// override default config value if maxDuty is found in nvs
loadMaxDuty();
// create semaphore for preventing race condition: mode-change operations while currently still executing certain mode
handleIteration_mutex = xSemaphoreCreateMutex();
}
@ -75,25 +78,59 @@ void task_control( void * pvParameters ){
}
//----------------------------------
//---------- Handle loop -----------
//----------------------------------
//function that repeatedly generates motor commands depending on the current mode
void controlledArmchair::startHandleLoop() {
while (1){
ESP_LOGV(TAG, "control loop executing... mode=%s", controlModeStr[(int)mode]);
// start endless loop that repeatedly calls handle() and handleTimeout() methods
void controlledArmchair::startHandleLoop()
{
while (1)
{
// mutex to prevent race condition with actions beeing run at mode change and previous mode still beeing executed
if (xSemaphoreTake(handleIteration_mutex, portMAX_DELAY) == pdTRUE)
{
//--- handle current mode ---
ESP_LOGV(TAG, "control loop executing... mode='%s'", controlModeStr[(int)mode]);
handle();
switch(mode) {
xSemaphoreGive(handleIteration_mutex);
} // end mutex
//--- slow loop ---
// this section is run approx every 5s (+500ms)
if (esp_log_timestamp() - timestamp_SlowLoopLastRun > 5000)
{
ESP_LOGV(TAG, "running slow loop... time since last run: %.1fs", (float)(esp_log_timestamp() - timestamp_SlowLoopLastRun) / 1000);
timestamp_SlowLoopLastRun = esp_log_timestamp();
//--- handle timeouts ---
// run function that detects timeouts (switch to idle, or notify "forgot to turn off")
handleTimeout();
}
vTaskDelay(5 / portTICK_PERIOD_MS); // small delay necessary to give modeChange() a chance to take the mutex
// TODO: move mode specific delays from handle() to here, to prevent unnecessary long mutex lock
}
}
//-------------------------------------
//---------- Handle control -----------
//-------------------------------------
// function that repeatedly generates motor commands and runs actions depending on the current mode
void controlledArmchair::handle()
{
switch (mode)
{
default:
mode = controlMode_t::IDLE;
//switch to IDLE mode when current mode is not implemented
changeMode(controlMode_t::IDLE);
break;
//------- handle IDLE -------
case controlMode_t::IDLE:
//copy preset commands for idling both motors - now done once at mode change
//commands = cmds_bothMotorsIdle;
//motorRight->setTarget(commands.right.state, commands.right.duty);
//motorLeft->setTarget(commands.left.state, commands.left.duty);
vTaskDelay(500 / portTICK_PERIOD_MS);
// TODO repeatedly set motors to idle, in case driver bugs? Currently 15s motorctl timeout would have to pass
#ifdef JOYSTICK_LOG_IN_IDLE
// get joystick data and log it
joystickData_t data joystick_l->getData();
@ -103,6 +140,7 @@ void controlledArmchair::startHandleLoop() {
objects->joystick->getRawX(), objects->joystick->getRawY());
#endif
break;
//------- handle JOYSTICK mode -------
case controlMode_t::JOYSTICK:
vTaskDelay(50 / portTICK_PERIOD_MS);
@ -110,7 +148,7 @@ void controlledArmchair::startHandleLoop() {
stickDataLast = stickData;
stickData = joystick_l->getData();
// additionaly scale coordinates (more detail in slower area)
joystick_scaleCoordinatesLinear(&stickData, 0.6, 0.35); //TODO: add scaling parameters to config
joystick_scaleCoordinatesLinear(&stickData, 0.6, 0.5); // TODO: add scaling parameters to config
// generate motor commands
// only generate when the stick data actually changed (e.g. stick stayed in center)
if (stickData.x != stickDataLast.x || stickData.y != stickDataLast.y)
@ -128,7 +166,6 @@ void controlledArmchair::startHandleLoop() {
}
break;
//------- handle MASSAGE mode -------
case controlMode_t::MASSAGE:
vTaskDelay(10 / portTICK_PERIOD_MS);
@ -150,7 +187,6 @@ void controlledArmchair::startHandleLoop() {
}
break;
//------- handle HTTP mode -------
case controlMode_t::HTTP:
//--- get joystick data from queue ---
@ -161,7 +197,8 @@ void controlledArmchair::startHandleLoop() {
ESP_LOGD(TAG, "generating commands from x=%.3f y=%.3f radius=%.3f angle=%.3f", stickData.x, stickData.y, stickData.radius, stickData.angle);
//--- generate motor commands ---
// only generate when the stick data actually changed (e.g. no new data recevied via http)
if (stickData.x != stickDataLast.x || stickData.y != stickDataLast.y ){
if (stickData.x != stickDataLast.x || stickData.y != stickDataLast.y)
{
resetTimeout(); // user input -> reset switch to IDLE timeout
// Note: timeout (no data received) is handled in getData method
commands = joystick_generateCommandsDriving(stickData, &joystickGenerateCommands_config);
@ -176,7 +213,6 @@ void controlledArmchair::startHandleLoop() {
}
break;
//------- handle AUTO mode -------
case controlMode_t::AUTO:
vTaskDelay(20 / portTICK_PERIOD_MS);
@ -187,7 +223,8 @@ void controlledArmchair::startHandleLoop() {
motorLeft->setTarget(commands.left);
// process received instruction
switch (instruction) {
switch (instruction)
{
case auto_instruction_t::NONE:
break;
case auto_instruction_t::SWITCH_PREV_MODE:
@ -217,7 +254,6 @@ void controlledArmchair::startHandleLoop() {
}
break;
//------- handle ADJUST_CHAIR mode -------
case controlMode_t::ADJUST_CHAIR:
vTaskDelay(100 / portTICK_PERIOD_MS);
@ -233,7 +269,6 @@ void controlledArmchair::startHandleLoop() {
}
break;
//------- handle MENU mode -------
case controlMode_t::MENU:
// nothing to do here, display task handles the menu
@ -243,20 +278,7 @@ void controlledArmchair::startHandleLoop() {
// TODO: add other modes here
}
//-----------------------
//------ slow loop ------
//-----------------------
//this section is run about every 5s (+500ms)
if (esp_log_timestamp() - timestamp_SlowLoopLastRun > 5000) {
ESP_LOGV(TAG, "running slow loop... time since last run: %.1fs", (float)(esp_log_timestamp() - timestamp_SlowLoopLastRun)/1000);
timestamp_SlowLoopLastRun = esp_log_timestamp();
//run function that detects timeout (switch to idle, or notify "forgot to turn off")
handleTimeout();
}
}//end while(1)
}//end startHandleLoop
} // end - handle method
@ -383,14 +405,21 @@ void controlledArmchair::handleTimeout()
//----------- changeMode ------------
//-----------------------------------
//function to change to a specified control mode
void controlledArmchair::changeMode(controlMode_t modeNew) {
void controlledArmchair::changeMode(controlMode_t modeNew)
{
// exit if target mode is already active
if (mode == modeNew) {
if (mode == modeNew)
{
ESP_LOGE(TAG, "changeMode: Already in target mode '%s' -> nothing to change", controlModeStr[(int)mode]);
return;
}
// mutex to wait for current handle iteration (control-task) to finish
// prevents race conditions where operations when changing mode are run but old mode gets handled still
ESP_LOGI(TAG, "changeMode: waiting for current handle() iteration to finish...");
if (xSemaphoreTake(handleIteration_mutex, portMAX_DELAY) == pdTRUE)
{
// copy previous mode
modePrevious = mode;
// store time changed (needed for timeout)
@ -400,7 +429,8 @@ void controlledArmchair::changeMode(controlMode_t modeNew) {
//========== commands change FROM mode ==========
// run functions when changing FROM certain mode
switch(modePrevious){
switch (modePrevious)
{
default:
ESP_LOGI(TAG, "noting to execute when changing FROM this mode");
break;
@ -448,13 +478,12 @@ void controlledArmchair::changeMode(controlMode_t modeNew) {
legRest->setState(REST_OFF);
backRest->setState(REST_OFF);
break;
}
//========== commands change TO mode ==========
// run functions when changing TO certain mode
switch(modeNew){
switch (modeNew)
{
default:
ESP_LOGI(TAG, "noting to execute when changing TO this mode");
break;
@ -491,14 +520,15 @@ void controlledArmchair::changeMode(controlMode_t modeNew) {
motorLeft->setFade(fadeType_t::ACCEL, shake_msFadeAccel);
motorRight->setFade(fadeType_t::ACCEL, shake_msFadeAccel);
break;
}
//--- update mode to new mode ---
//TODO: add mutex
mode = modeNew;
}
// unlock mutex for control task to continue handling modes
xSemaphoreGive(handleIteration_mutex);
} // end mutex
}
//TODO simplify the following 3 functions? can be replaced by one?
@ -526,7 +556,7 @@ void controlledArmchair::toggleModes(controlMode_t modePrimary, controlMode_t mo
}
//switch to primary mode when any other mode is active
else {
ESP_LOGW(TAG, "toggleModes: switching from %s to primary mode %s", controlModeStr[(int)mode], controlModeStr[(int)modePrimary]);
ESP_LOGW(TAG, "toggleModes: switching from '%s' to primary mode '%s'", controlModeStr[(int)mode], controlModeStr[(int)modePrimary]);
//buzzer->beep(4,200,100);
changeMode(modePrimary);
}
@ -542,13 +572,13 @@ void controlledArmchair::toggleMode(controlMode_t modePrimary){
//switch to previous mode when primary is already active
if (mode == modePrimary){
ESP_LOGW(TAG, "toggleMode: switching from primaryMode %s to previousMode %s", controlModeStr[(int)mode], controlModeStr[(int)modePrevious]);
ESP_LOGW(TAG, "toggleMode: switching from primaryMode '%s' to previousMode '%s'", controlModeStr[(int)mode], controlModeStr[(int)modePrevious]);
//buzzer->beep(2,200,100);
changeMode(modePrevious); //switch to previous mode
}
//switch to primary mode when any other mode is active
else {
ESP_LOGW(TAG, "toggleModes: switching from %s to primary mode %s", controlModeStr[(int)mode], controlModeStr[(int)modePrimary]);
ESP_LOGW(TAG, "toggleModes: switching from '%s' to primary mode '%s'", controlModeStr[(int)mode], controlModeStr[(int)modePrimary]);
//buzzer->beep(4,200,100);
changeMode(modePrimary);
}

10
board_single/main/control.hpp

@ -37,7 +37,7 @@ typedef struct control_config_t {
//=======================================
//task that controls the armchair modes and initiates commands generation and applies them to driver
//parameter: pointer to controlledArmchair object
void task_control( void * pvParameters );
void task_control( void * controlledArmchair );
@ -64,7 +64,7 @@ class controlledArmchair {
);
//--- functions ---
//task that repeatedly generates motor commands depending on the current mode
//endless loop that repeatedly calls handle() and handleTimeout() methods respecting mutex
void startHandleLoop();
//function that changes to a specified control mode
@ -104,6 +104,9 @@ class controlledArmchair {
private:
//--- functions ---
//generate motor commands or run actions depending on the current mode
void handle();
//function that evaluates whether there is no activity/change on the motor duty for a certain time, if so a switch to IDLE is issued. - has to be run repeatedly in a slow interval
void handleTimeout();
@ -149,6 +152,9 @@ class controlledArmchair {
//struct with config parameters
control_config_t config;
//mutex to prevent race condition between handle() and changeMode()
SemaphoreHandle_t handleIteration_mutex;
//store joystick data
joystickData_t stickData = joystickData_center;
joystickData_t stickDataLast = joystickData_center;