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

Control: Rework timeout to work standalone (untested)

Browse Source 
rework and enable timeout feature to work independent of handledmotor
object now uses generated commands instead of acutal motor duty
not testet yet
This commit is contained in:
jonny_jr9 2023-08-30 19:34:41 +02:00
parent 446c246f43
commit 455b6f0456
2 changed files with 58 additions and 71 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

124
board_control/main/control.cpp
View File

@ -64,11 +64,9 @@ const char* controlModeStr[7] = {"IDLE", "JOYSTICK", "MASSAGE", "HTTP", "MQTT",
break; break;
case controlMode_t::IDLE: case controlMode_t::IDLE:
//copy preset commands for idling both motors //send both motors idle command to motorctl pcb
commands = cmds_bothMotorsIdle; uart_sendStruct<motorCommands_t>(cmds_bothMotorsIdle);
uart_sendStruct<motorCommands_t>(commands); commands_now = cmds_bothMotorsIdle;
//motorRight->setTarget(commands.right.state, commands.right.duty);
//motorLeft->setTarget(commands.left.state, commands.left.duty);
vTaskDelay(200 / portTICK_PERIOD_MS); vTaskDelay(200 / portTICK_PERIOD_MS);
#ifdef JOYSTICK_LOG_IN_IDLE #ifdef JOYSTICK_LOG_IN_IDLE
//get joystick data here (without using it) //get joystick data here (without using it)
@ -79,18 +77,15 @@ const char* controlModeStr[7] = {"IDLE", "JOYSTICK", "MASSAGE", "HTTP", "MQTT",
case controlMode_t::JOYSTICK: case controlMode_t::JOYSTICK:
vTaskDelay(20 / portTICK_PERIOD_MS); vTaskDelay(50 / portTICK_PERIOD_MS);
//get current joystick data with getData method of evaluatedJoystick //get current joystick data with getData method of evaluatedJoystick
stickData = joystick_l->getData(); stickData = joystick_l->getData();
//additionaly scale coordinates (more detail in slower area) //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.35); //TODO: add scaling parameters to config
//generate motor commands //generate motor commands
commands = joystick_generateCommandsDriving(stickData, altStickMapping); commands_now = joystick_generateCommandsDriving(stickData, altStickMapping);
//apply motor commands //apply motor commands
uart_sendStruct<motorCommands_t>(commands); uart_sendStruct<motorCommands_t>(commands_now);
//motorRight->setTarget(commands.right.state, commands.right.duty);
//motorLeft->setTarget(commands.left.state, commands.left.duty);
//TODO make motorctl.setTarget also accept motorcommand struct directly
break; break;
@ -103,11 +98,9 @@ const char* controlModeStr[7] = {"IDLE", "JOYSTICK", "MASSAGE", "HTTP", "MQTT",
} }
//--- generate motor commands --- //--- generate motor commands ---
//pass joystick data from getData method of evaluatedJoystick to generateCommandsShaking function //pass joystick data from getData method of evaluatedJoystick to generateCommandsShaking function
commands = joystick_generateCommandsShaking(stickData); commands_now = joystick_generateCommandsShaking(stickData);
//apply motor commands //apply motor commands
uart_sendStruct<motorCommands_t>(commands); uart_sendStruct<motorCommands_t>(commands_now);
//motorRight->setTarget(commands.right.state, commands.right.duty);
//motorLeft->setTarget(commands.left.state, commands.left.duty);
break; break;
@ -121,25 +114,23 @@ const char* controlModeStr[7] = {"IDLE", "JOYSTICK", "MASSAGE", "HTTP", "MQTT",
ESP_LOGD(TAG, "generating commands from x=%.3f y=%.3f radius=%.3f angle=%.3f", stickData.x, stickData.y, stickData.radius, stickData.angle); 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 --- //--- generate motor commands ---
//Note: timeout (no data received) is handled in getData method //Note: timeout (no data received) is handled in getData method
commands = joystick_generateCommandsDriving(stickData, altStickMapping); commands_now = joystick_generateCommandsDriving(stickData, altStickMapping);
//--- apply commands to motors --- //--- apply commands to motors ---
//TODO make motorctl.setTarget also accept motorcommand struct directly uart_sendStruct<motorCommands_t>(commands_now);
uart_sendStruct<motorCommands_t>(commands);
//motorRight->setTarget(commands.right.state, commands.right.duty);
//motorLeft->setTarget(commands.left.state, commands.left.duty);
break; break;
case controlMode_t::AUTO: case controlMode_t::AUTO:
//FIXME auto mode currently not supported, needs rework
vTaskDelay(20 / portTICK_PERIOD_MS); vTaskDelay(20 / portTICK_PERIOD_MS);
// //generate commands // //generate commands
// commands = armchair.generateCommands(&instruction); // commands_now = armchair.generateCommands(&instruction);
// //--- apply commands to motors --- // //--- apply commands to motors ---
// //TODO make motorctl.setTarget also accept motorcommand struct directly // //TODO make motorctl.setTarget also accept motorcommand struct directly
// uart_sendStruct<motorCommands_t>(commands); // uart_sendStruct<motorCommands_t>(commands_now);
// //motorRight->setTarget(commands.right.state, commands.right.duty); // //motorRight->setTarget(commands_now.right.state, commands_now.right.duty);
// //motorLeft->setTarget(commands.left.state, commands.left.duty); // //motorLeft->setTarget(commands_now.left.state, commands_now.left.duty);
// //
// //process received instruction // //process received instruction
// switch (instruction) { // switch (instruction) {
@ -172,7 +163,6 @@ const char* controlModeStr[7] = {"IDLE", "JOYSTICK", "MASSAGE", "HTTP", "MQTT",
// } // }
break; break;
//TODO: add other modes here //TODO: add other modes here
} }
@ -180,11 +170,14 @@ const char* controlModeStr[7] = {"IDLE", "JOYSTICK", "MASSAGE", "HTTP", "MQTT",
//--- run actions based on received button button event --- //--- run actions based on received button button event ---
//note: buttonCount received by sendButtonEvent method called from button.cpp //note: buttonCount received by sendButtonEvent method called from button.cpp
//TODO: what if variable gets set from other task during this code? -> mutex around this code //TODO: what if variable gets set from other task during this code? -> mutex around this code
//TODO add methods and move below code to button file possible?
switch (buttonCount) { switch (buttonCount) {
case 1: //define joystick center or freeze input case 1: //define joystick center or freeze input
if (mode == controlMode_t::JOYSTICK){ if (mode == controlMode_t::JOYSTICK){
//joystick mode: calibrate joystick //joystick mode: calibrate joystick
joystick_l->defineCenter(); joystick_l->defineCenter();
buzzer->beep(2, 50, 30);
buzzer->beep(1, 200, 25);
} else if (mode == controlMode_t::MASSAGE){ } else if (mode == controlMode_t::MASSAGE){
//massage mode: toggle freeze of input (lock joystick at current values) //massage mode: toggle freeze of input (lock joystick at current values)
freezeInput = !freezeInput; freezeInput = !freezeInput;
@ -268,35 +261,30 @@ const char* controlModeStr[7] = {"IDLE", "JOYSTICK", "MASSAGE", "HTTP", "MQTT",
} }
} }
//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 //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.
//FIXME rework timout to work via uart duy or control input only //has to be run repeatedly in a *slow interval* so change between current and last duty is detectable
void controlledArmchair::handleTimeout(){ void controlledArmchair::handleTimeout(){
// //check for timeout only when not idling already //check for timeout only when not idling already
// if (mode != controlMode_t::IDLE) { if (mode != controlMode_t::IDLE) {
// //get current duty from controlled motor objects //activity detected between current and last generated motor commands
// float dutyLeftNow = motorLeft->getStatus().duty; if (validateActivity(commands_lastActivityCheck.left.duty, commands_now.left.duty, inactivityTolerance)
// float dutyRightNow = motorRight->getStatus().duty; || validateActivity(commands_lastActivityCheck.right.duty, commands_now.right.duty, inactivityTolerance)
// ){
// //activity detected on any of the two motors ESP_LOGD(TAG, "timeout check: [activity] detected since last check -> reset");
// if (validateActivity(dutyLeft_lastActivity, dutyLeftNow, inactivityTolerance) //reset last commands and timestamp
// || validateActivity(dutyRight_lastActivity, dutyRightNow, inactivityTolerance) commands_lastActivityCheck = commands_now;
// ){ resetTimeout();
// ESP_LOGD(TAG, "timeout check: [activity] detected since last check -> reset"); }
// //reset last duty and timestamp //no activity on any motor and msTimeout exceeded
// dutyLeft_lastActivity = dutyLeftNow; else if (esp_log_timestamp() - timestamp_lastActivity > config.timeoutMs){
// dutyRight_lastActivity = dutyRightNow; ESP_LOGW(TAG, "timeout check: [TIMEOUT], no activity for more than %.ds -> switch to idle", config.timeoutMs/1000);
// resetTimeout(); //toggle to idle mode
// } toggleIdle();
// //no activity on any motor and msTimeout exceeded }
// else if (esp_log_timestamp() - timestamp_lastActivity > config.timeoutMs){ else {
// ESP_LOGI(TAG, "timeout check: [TIMEOUT], no activity for more than %.ds -> switch to idle", config.timeoutMs/1000); ESP_LOGD(TAG, "timeout check: [inactive], last activity %.1f s ago, timeout after %d s", (float)(esp_log_timestamp() - timestamp_lastActivity)/1000, config.timeoutMs/1000);
// //toggle to idle mode }
// toggleIdle(); }
// }
// else {
// ESP_LOGD(TAG, "timeout check: [inactive], last activity %.1f s ago, timeout after %d s", (float)(esp_log_timestamp() - timestamp_lastActivity)/1000, config.timeoutMs/1000);
// }
// }
} }
@ -335,21 +323,21 @@ const char* controlModeStr[7] = {"IDLE", "JOYSTICK", "MASSAGE", "HTTP", "MQTT",
break; break;
#endif #endif
// case controlMode_t::HTTP: case controlMode_t::HTTP:
// ESP_LOGW(TAG, "switching from http mode -> disabling http and wifi"); ESP_LOGW(TAG, "switching from http mode -> disabling http and wifi");
// //stop http server //stop http server
// ESP_LOGI(TAG, "disabling http server..."); ESP_LOGI(TAG, "disabling http server...");
// http_stop_server(); http_stop_server();
//
// //FIXME: make wifi function work here - currently starting wifi at startup (see notes main.cpp) //FIXME: make wifi function work here - currently starting wifi at startup (see notes main.cpp)
// //stop wifi //stop wifi
// //TODO: decide whether ap or client is currently used - which has to be disabled? //TODO: decide whether ap or client is currently used - which has to be disabled?
// //ESP_LOGI(TAG, "deinit wifi..."); //ESP_LOGI(TAG, "deinit wifi...");
// //wifi_deinit_client(); //wifi_deinit_client();
// //wifi_deinit_ap(); //wifi_deinit_ap();
// ESP_LOGI(TAG, "done stopping http mode"); ESP_LOGI(TAG, "done stopping http mode");
// break; break;
//
// case controlMode_t::MASSAGE: // case controlMode_t::MASSAGE:
// ESP_LOGW(TAG, "switching from MASSAGE mode -> restoring fading, reset frozen input"); // ESP_LOGW(TAG, "switching from MASSAGE mode -> restoring fading, reset frozen input");
// //TODO: fix issue when downfading was disabled before switching to massage mode - currently it gets enabled again here... // //TODO: fix issue when downfading was disabled before switching to massage mode - currently it gets enabled again here...

5
board_control/main/control.hpp
View File

@ -78,7 +78,7 @@
//---variables --- //---variables ---
//struct for motor commands returned by generate functions of each mode //struct for motor commands returned by generate functions of each mode
motorCommands_t commands; motorCommands_t commands_now;
//struct with config parameters //struct with config parameters
control_config_t config; control_config_t config;
@ -118,9 +118,8 @@
uint32_t timestamp_SlowLoopLastRun = 0; uint32_t timestamp_SlowLoopLastRun = 0;
//variables for detecting timeout (switch to idle, after inactivity) //variables for detecting timeout (switch to idle, after inactivity)
float dutyLeft_lastActivity = 0;
float dutyRight_lastActivity = 0;
uint32_t timestamp_lastActivity = 0; uint32_t timestamp_lastActivity = 0;
motorCommands_t commands_lastActivityCheck;
}; };