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

Optimize motorctl: slow down task when target reached

Browse Source 
Major changes in motorctl and control to optimize performance
by freeing unnecessary cpu usage by motorctl task
Needs testing on actual hardware!

motorctl:
    - slow down handle loop when duty is at target (wait for new command)
    - create separate task for each motor
    - setTarget method also accepts motorCommand directly now

control.cpp:
    - redurce stress on motorctl by removing unnecessary commands
        - set motors to idle at mode change only, instead of every iteration (IDLE, MENU, ADJUST)
        - HTTP, JOYSTICK: only update motors when stick data actually changed
    - simplify code
        - add method for idling both motors
        - use motorcommands directly in setTarget()

http:cpp:
    - dont block control task with getData() method
    - handle timeout independent of one queue event
    - prevents unresponsive system for http-timeout when changing mode from HTTP
This commit is contained in:
jonny_jr9 2024-02-23 23:57:21 +01:00
parent fab4d442e6
commit bc014befb7
7 changed files with 176 additions and 117 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

128
board_single/main/control.cpp
View File

@ -58,32 +58,30 @@ controlledArmchair::controlledArmchair(
// override default config value if maxDuty is found in nvs // override default config value if maxDuty is found in nvs
loadMaxDuty(); loadMaxDuty();
//TODO declare / configure controlled motors here instead of config (unnecessary that button object is globally available - only used here)?
} }
//======================================= //=======================================
//============ control task ============= //============ control task =============
//======================================= //=======================================
//task that controls the armchair modes and initiates commands generation and applies them to driver // task that controls the armchair modes
//parameter: pointer to controlledArmchair object // generates commands depending on current mode and sends those to corresponding task
// parameter: pointer to controlledArmchair object
void task_control( void * pvParameters ){ void task_control( void * pvParameters ){
//control_task_parameters_t * objects = (control_task_parameters_t *)pvParameters;
controlledArmchair * control = (controlledArmchair *)pvParameters; controlledArmchair * control = (controlledArmchair *)pvParameters;
ESP_LOGI(TAG, "Initializing controlledArmchair and starting handle loop"); ESP_LOGW(TAG, "Initializing controlledArmchair and starting handle loop");
//start handle loop (control object declared in config.hpp) //start handle loop (control object declared in config.hpp)
//objects->control->startHandleLoop();
control->startHandleLoop(); control->startHandleLoop();
} }
//---------------------------------- //----------------------------------
//---------- Handle loop ----------- //---------- Handle loop -----------
//---------------------------------- //----------------------------------
//function that repeatedly generates motor commands depending on the current mode //function that repeatedly generates motor commands depending on the current mode
//also handles fading and current-limit
void controlledArmchair::startHandleLoop() { void controlledArmchair::startHandleLoop() {
while (1){ while (1){
ESP_LOGV(TAG, "control task executing... mode=%s", controlModeStr[(int)mode]); ESP_LOGV(TAG, "control loop executing... mode=%s", controlModeStr[(int)mode]);
switch(mode) { switch(mode) {
default: default:
@ -91,31 +89,40 @@ void controlledArmchair::startHandleLoop() {
break; break;
case controlMode_t::IDLE: case controlMode_t::IDLE:
//copy preset commands for idling both motors //copy preset commands for idling both motors - now done once at mode change
commands = cmds_bothMotorsIdle; //commands = cmds_bothMotorsIdle;
motorRight->setTarget(commands.right.state, commands.right.duty); //motorRight->setTarget(commands.right.state, commands.right.duty);
motorLeft->setTarget(commands.left.state, commands.left.duty); //motorLeft->setTarget(commands.left.state, commands.left.duty);
vTaskDelay(300 / portTICK_PERIOD_MS); vTaskDelay(500 / 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)
//since loglevel is DEBUG, calculateion details is output //since loglevel is DEBUG, calculation details are output
joystick_l->getData(); //get joystick data here joystick_l->getData();
#endif #endif
break; break;
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
stickDataLast = stickData;
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, &joystickGenerateCommands_config); // only generate when the stick data actually changed (e.g. stick stayed in center)
//apply motor commands if (stickData.x != stickDataLast.x || stickData.y != stickDataLast.y)
motorRight->setTarget(commands.right.state, commands.right.duty); {
motorLeft->setTarget(commands.left.state, commands.left.duty); commands = joystick_generateCommandsDriving(stickData, &joystickGenerateCommands_config);
//TODO make motorctl.setTarget also accept motorcommand struct directly // apply motor commands
motorRight->setTarget(commands.right);
motorLeft->setTarget(commands.left);
}
else
{
vTaskDelay(20 / portTICK_PERIOD_MS);
ESP_LOGD(TAG, "analog joystick data unchanged at %s not updating commands", joystickPosStr[(int)stickData.position]);
}
break; break;
@ -130,28 +137,33 @@ void controlledArmchair::startHandleLoop() {
//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 = joystick_generateCommandsShaking(stickData);
//apply motor commands //apply motor commands
motorRight->setTarget(commands.right.state, commands.right.duty); motorRight->setTarget(commands.right);
motorLeft->setTarget(commands.left.state, commands.left.duty); motorLeft->setTarget(commands.left);
break; break;
case controlMode_t::HTTP: case controlMode_t::HTTP:
//--- get joystick data from queue --- //--- get joystick data from queue ---
//Note this function waits several seconds (httpconfig.timeoutMs) for data to arrive, otherwise Center data or NULL is returned stickDataLast = stickData;
//TODO: as described above, when changing modes it might delay a few seconds for the change to apply stickData = httpJoystickMain_l->getData(); //get last stored data from receive queue (waits up to 500ms for new event to arrive)
stickData = httpJoystickMain_l->getData();
//scale coordinates additionally (more detail in slower area) //scale coordinates additionally (more detail in slower area)
joystick_scaleCoordinatesLinear(&stickData, 0.6, 0.4); //TODO: add scaling parameters to config joystick_scaleCoordinatesLinear(&stickData, 0.6, 0.4); //TODO: add scaling parameters to config
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 //only generate when the stick data actually changed (e.g. no new data recevied via http)
commands = joystick_generateCommandsDriving(stickData, &joystickGenerateCommands_config); if (stickData.x != stickDataLast.x || stickData.y != stickDataLast.y ){
// Note: timeout (no data received) is handled in getData method
commands = joystick_generateCommandsDriving(stickData, &joystickGenerateCommands_config);
//--- apply commands to motors --- //--- apply commands to motors ---
//TODO make motorctl.setTarget also accept motorcommand struct directly motorRight->setTarget(commands.right);
motorRight->setTarget(commands.right.state, commands.right.duty); motorLeft->setTarget(commands.left);
motorLeft->setTarget(commands.left.state, commands.left.duty); }
break; else
{
ESP_LOGD(TAG, "http joystick data unchanged at %s not updating commands", joystickPosStr[(int)stickData.position]);
}
break;
case controlMode_t::AUTO: case controlMode_t::AUTO:
@ -159,9 +171,8 @@ void controlledArmchair::startHandleLoop() {
//generate commands //generate commands
commands = automatedArmchair->generateCommands(&instruction); commands = automatedArmchair->generateCommands(&instruction);
//--- apply commands to motors --- //--- apply commands to motors ---
//TODO make motorctl.setTarget also accept motorcommand struct directly motorRight->setTarget(commands.right);
motorRight->setTarget(commands.right.state, commands.right.duty); motorLeft->setTarget(commands.left);
motorLeft->setTarget(commands.left.state, commands.left.duty);
//process received instruction //process received instruction
switch (instruction) { switch (instruction) {
@ -200,9 +211,9 @@ void controlledArmchair::startHandleLoop() {
//--- read joystick --- //--- read joystick ---
stickData = joystick_l->getData(); stickData = joystick_l->getData();
//--- idle motors --- //--- idle motors ---
commands = cmds_bothMotorsIdle; //commands = cmds_bothMotorsIdle; - now done once at mode change
motorRight->setTarget(commands.right.state, commands.right.duty); //motorRight->setTarget(commands.right.state, commands.right.duty);
motorLeft->setTarget(commands.left.state, commands.left.duty); //motorLeft->setTarget(commands.left.state, commands.left.duty);
//--- control armchair position with joystick input --- //--- control armchair position with joystick input ---
controlChairAdjustment(joystick_l->getData(), legRest, backRest); controlChairAdjustment(joystick_l->getData(), legRest, backRest);
break; break;
@ -212,9 +223,9 @@ void controlledArmchair::startHandleLoop() {
vTaskDelay(1000 / portTICK_PERIOD_MS); vTaskDelay(1000 / portTICK_PERIOD_MS);
//nothing to do here, display task handles the menu //nothing to do here, display task handles the menu
//--- idle motors --- //--- idle motors ---
commands = cmds_bothMotorsIdle; //commands = cmds_bothMotorsIdle; - now done once at mode change
motorRight->setTarget(commands.right.state, commands.right.duty); //motorRight->setTarget(commands.right.state, commands.right.duty);
motorLeft->setTarget(commands.left.state, commands.left.duty); //motorLeft->setTarget(commands.left.state, commands.left.duty);
break; break;
//TODO: add other modes here //TODO: add other modes here
@ -228,7 +239,7 @@ void controlledArmchair::startHandleLoop() {
ESP_LOGV(TAG, "running slow loop... time since last run: %.1fs", (float)(esp_log_timestamp() - timestamp_SlowLoopLastRun)/1000); ESP_LOGV(TAG, "running slow loop... time since last run: %.1fs", (float)(esp_log_timestamp() - timestamp_SlowLoopLastRun)/1000);
timestamp_SlowLoopLastRun = esp_log_timestamp(); timestamp_SlowLoopLastRun = esp_log_timestamp();
//run function which detects timeout (switch to idle) //run function that detects timeout (switch to idle)
handleTimeout(); handleTimeout();
} }
@ -290,6 +301,14 @@ bool controlledArmchair::toggleAltStickMapping()
} }
//-----------------------------------
//--------- idleBothMotors ----------
//-----------------------------------
// turn both motors off
void controlledArmchair::idleBothMotors(){
motorRight->setTarget(cmd_motorIdle);
motorLeft->setTarget(cmd_motorIdle);
}
//----------------------------------- //-----------------------------------
//---------- resetTimeout ----------- //---------- resetTimeout -----------
@ -425,16 +444,23 @@ void controlledArmchair::changeMode(controlMode_t modeNew) {
ESP_LOGI(TAG, "noting to execute when changing TO this mode"); ESP_LOGI(TAG, "noting to execute when changing TO this mode");
break; break;
case controlMode_t::IDLE: case controlMode_t::IDLE:
buzzer->beep(1, 1000, 0); ESP_LOGW(TAG, "switching to IDLE mode: turning both motors off, beep");
idleBothMotors();
buzzer->beep(1, 1000, 0);
#ifdef JOYSTICK_LOG_IN_IDLE #ifdef JOYSTICK_LOG_IN_IDLE
esp_log_level_set("evaluatedJoystick", ESP_LOG_DEBUG); esp_log_level_set("evaluatedJoystick", ESP_LOG_DEBUG);
#endif #endif
break; break;
case controlMode_t::ADJUST_CHAIR: case controlMode_t::ADJUST_CHAIR:
ESP_LOGW(TAG, "switching to ADJUST_CHAIR mode -> beep"); ESP_LOGW(TAG, "switching to ADJUST_CHAIR mode: turning both motors off, beep");
buzzer->beep(4,200,100); idleBothMotors();
buzzer->beep(4, 200, 100);
break;
case controlMode_t::MENU:
idleBothMotors();
break; break;
case controlMode_t::MASSAGE: case controlMode_t::MASSAGE:

35
board_single/main/control.hpp
View File

@ -105,6 +105,8 @@ class controlledArmchair {
void loadMaxDuty(); //load stored value for maxDuty from nvs void loadMaxDuty(); //load stored value for maxDuty from nvs
void writeMaxDuty(float newMaxDuty); //write new value for maxDuty to nvs void writeMaxDuty(float newMaxDuty); //write new value for maxDuty to nvs
void idleBothMotors(); //turn both motors off
//--- objects --- //--- objects ---
buzzer_t* buzzer; buzzer_t* buzzer;
controlledMotor* motorLeft; controlledMotor* motorLeft;
@ -118,14 +120,33 @@ class controlledArmchair {
//handle for using the nvs flash (persistent config variables) //handle for using the nvs flash (persistent config variables)
nvs_handle_t * nvsHandle; nvs_handle_t * nvsHandle;
//--- constants ---
//command preset for idling motors
const motorCommand_t cmd_motorIdle = {
.state = motorstate_t::IDLE,
.duty = 0
};
const motorCommands_t cmds_bothMotorsIdle = {
.left = cmd_motorIdle,
.right = cmd_motorIdle
};
const joystickData_t joystickData_center = {
.position = joystickPos_t::CENTER,
.x = 0,
.y = 0,
.radius = 0,
.angle = 0
};
//---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 = cmds_bothMotorsIdle;
//struct with config parameters //struct with config parameters
control_config_t config; control_config_t config;
//store joystick data //store joystick data
joystickData_t stickData; joystickData_t stickData = joystickData_center;
joystickData_t stickDataLast = joystickData_center;
//variables for http mode //variables for http mode
uint32_t http_timestamp_lastData = 0; uint32_t http_timestamp_lastData = 0;
@ -145,16 +166,6 @@ class controlledArmchair {
//variable to store mode when toggling IDLE mode //variable to store mode when toggling IDLE mode
controlMode_t modePrevious; //default mode controlMode_t modePrevious; //default mode
//command preset for idling motors
const motorCommand_t cmd_motorIdle = {
.state = motorstate_t::IDLE,
.duty = 0
};
const motorCommands_t cmds_bothMotorsIdle = {
.left = cmd_motorIdle,
.right = cmd_motorIdle
};
//variable for slow loop //variable for slow loop
uint32_t timestamp_SlowLoopLastRun = 0; uint32_t timestamp_SlowLoopLastRun = 0;

7
board_single/main/main.cpp
View File

@ -251,9 +251,10 @@ extern "C" void app_main(void) {
//---------------------------------------------- //----------------------------------------------
//--- create task for controlling the motors --- //--- create task for controlling the motors ---
//---------------------------------------------- //----------------------------------------------
//task that receives commands, handles ramp and current limit and executes commands using the motordriver function //task for each motor that handles to following:
task_motorctl_parameters_t motorctl_param = {motorLeft, motorRight}; //receives commands from control via queue, handle ramp and current, apply new duty by passing it to method of motordriver (ptr)
xTaskCreate(&task_motorctl, "task_motor-control", 2*4096, &motorctl_param, 6, NULL); xTaskCreate(&task_motorctl, "task_ctl-left-motor", 2*4096, motorLeft, 6, NULL);
xTaskCreate(&task_motorctl, "task_ctl-right-motor", 2*4096, motorRight, 6, NULL);
//------------------------------ //------------------------------
//--- create task for buzzer --- //--- create task for buzzer ---

13
common/http.cpp
View File

@ -178,23 +178,22 @@ esp_err_t httpJoystick::receiveHttpData(httpd_req_t *req){
//------------------- //-------------------
//----- getData ----- //----- getData -----
//------------------- //-------------------
//wait for and return joystick data from queue, if timeout return NULL //wait for and return joystick data from queue, return last data if nothing received within 500ms, return center data when timeout exceeded
joystickData_t httpJoystick::getData(){ joystickData_t httpJoystick::getData(){
//--- get joystick data from queue --- //--- get joystick data from queue ---
if( xQueueReceive( joystickDataQueue, &dataRead, pdMS_TO_TICKS(config.timeoutMs) ) ) { if( xQueueReceive( joystickDataQueue, &dataRead, pdMS_TO_TICKS(500) ) ) { //dont wait longer than 500ms to not block the control loop for too long
ESP_LOGD(TAG, "getData: received data (from queue): x=%.3f y=%.3f radius=%.3f angle=%.3f", ESP_LOGD(TAG, "getData: received data (from queue): x=%.3f y=%.3f radius=%.3f angle=%.3f",
dataRead.x, dataRead.y, dataRead.radius, dataRead.angle); dataRead.x, dataRead.y, dataRead.radius, dataRead.angle);
timeLastData = esp_log_timestamp();
} }
//--- timeout --- //--- timeout ---
//no new data received within configured timeout // send error message when last received data did NOT result in CENTER position and timeout exceeded
else { else {
//send error message when last received data did NOT result in CENTER position if (dataRead.position != joystickPos_t::CENTER && (esp_log_timestamp() - timeLastData) > config.timeoutMs) {
if (dataRead.position != joystickPos_t::CENTER) {
//change data to "joystick center" data to stop the motors //change data to "joystick center" data to stop the motors
dataRead = dataCenter; dataRead = dataCenter;
ESP_LOGE(TAG, "TIMEOUT - no data received for 3s -> set to center"); ESP_LOGE(TAG, "TIMEOUT - no data received for %dms -> set to center", config.timeoutMs);
} }
} }
return dataRead; return dataRead;

3
common/http.hpp
View File

@ -70,7 +70,7 @@ class httpJoystick{
httpJoystick_config_t config; httpJoystick_config_t config;
QueueHandle_t joystickDataQueue = xQueueCreate( 1, sizeof( struct joystickData_t ) ); QueueHandle_t joystickDataQueue = xQueueCreate( 1, sizeof( struct joystickData_t ) );
//struct for receiving data from http function, and storing data of last update //struct for receiving data from http function, and storing data of last update
joystickData_t dataRead; uint32_t timeLastData = 0;
const joystickData_t dataCenter = { const joystickData_t dataCenter = {
.position = joystickPos_t::CENTER, .position = joystickPos_t::CENTER,
.x = 0, .x = 0,
@ -78,4 +78,5 @@ class httpJoystick{
.radius = 0, .radius = 0,
.angle = 0 .angle = 0
}; };
joystickData_t dataRead = dataCenter;
}; };

86
common/motorctl.cpp
View File

@ -5,21 +5,20 @@
//tag for logging //tag for logging
static const char * TAG = "motor-control"; static const char * TAG = "motor-control";
#define TIMEOUT_IDLE_WHEN_NO_COMMAND 8000 #define TIMEOUT_IDLE_WHEN_NO_COMMAND 15000 // turn motor off when still on and no new command received within that time
#define TIMEOUT_QUEUE_WHEN_AT_TARGET 5000 // time waited for new command when motors at target duty (e.g. IDLE) (no need to handle fading in fast loop)
//==================================== //====================================
//========== motorctl task =========== //========== motorctl task ===========
//==================================== //====================================
//task for handling the motors (ramp, current limit, driver) //task for handling the motors (ramp, current limit, driver)
void task_motorctl( void * task_motorctl_parameters ){ void task_motorctl( void * ptrControlledMotor ){
task_motorctl_parameters_t *objects = (task_motorctl_parameters_t *)task_motorctl_parameters; //get pointer to controlledMotor instance from task parameter
ESP_LOGW(TAG, "Task-motorctl: starting handle loops for left and right motor..."); controlledMotor * motor = (controlledMotor *)ptrControlledMotor;
ESP_LOGW(TAG, "Task-motorctl [%s]: starting handle loop...", motor->getName());
while(1){ while(1){
objects->motorRight->handle(); motor->handle();
objects->motorLeft->handle(); vTaskDelay(20 / portTICK_PERIOD_MS);
vTaskDelay(15 / portTICK_PERIOD_MS);
} }
} }
@ -58,7 +57,10 @@ void controlledMotor::init(){
loadAccelDuration(); loadAccelDuration();
loadDecelDuration(); loadDecelDuration();
//cSensor.calibrateZeroAmpere(); //currently done in currentsensor constructor TODO do this regularly e.g. in idle? // turn motor off initially
motorSetCommand({motorstate_t::IDLE, 0.00});
//cSensor.calibrateZeroAmpere(); //currently done in currentsensor constructor TODO do this regularly e.g. in idle?
} }
@ -104,7 +106,7 @@ void controlledMotor::handle(){
//TODO: History: skip fading when motor was running fast recently / alternatively add rot-speed sensor //TODO: History: skip fading when motor was running fast recently / alternatively add rot-speed sensor
//--- receive commands from queue --- //--- receive commands from queue ---
if( xQueueReceive( commandQueue, &commandReceive, ( TickType_t ) 0 ) ) if( xQueueReceive( commandQueue, &commandReceive, timeoutWaitForCommand / portTICK_PERIOD_MS ) ) //wait time is always 0 except when at target duty already
{ {
ESP_LOGD(TAG, "[%s] Read command from queue: state=%s, duty=%.2f", config.name, motorstateStr[(int)commandReceive.state], commandReceive.duty); ESP_LOGD(TAG, "[%s] Read command from queue: state=%s, duty=%.2f", config.name, motorstateStr[(int)commandReceive.state], commandReceive.duty);
state = commandReceive.state; state = commandReceive.state;
@ -134,15 +136,44 @@ void controlledMotor::handle(){
} }
} }
//--- timeout, no data --- //--- timeout, no data ---
//turn motors off if no data received for long time (e.g. no uart data / control offline) // turn motors off if no data received for a long time (e.g. no uart data or control task offline)
//TODO no timeout when braking? if (dutyNow != 0 && esp_log_timestamp() - timestamp_commandReceived > TIMEOUT_IDLE_WHEN_NO_COMMAND && !receiveTimeout)
if ((esp_log_timestamp() - timestamp_commandReceived) > TIMEOUT_IDLE_WHEN_NO_COMMAND && !receiveTimeout){ {
receiveTimeout = true; ESP_LOGE(TAG, "[%s] TIMEOUT, motor active, but no target data received for more than %ds -> switch from duty=%.2f to IDLE", config.name, TIMEOUT_IDLE_WHEN_NO_COMMAND / 1000, dutyTarget);
state = motorstate_t::IDLE; receiveTimeout = true;
dutyTarget = 0; state = motorstate_t::IDLE;
ESP_LOGE(TAG, "[%s] TIMEOUT, no target data received for more than %ds -> switch to IDLE", config.name, TIMEOUT_IDLE_WHEN_NO_COMMAND/1000); dutyTarget = 0;
} }
//--- calculate difference ---
dutyDelta = dutyTarget - dutyNow;
//positive: need to increase by that value
//negative: need to decrease
//--- already at target ---
// when already at exact target duty there is no need to run very fast to handle fading
//-> slow down loop by waiting significantly longer for new commands to arrive
if ((dutyDelta == 0 && !config.currentLimitEnabled) || (dutyTarget == 0 && dutyNow == 0)) //when current limit enabled only slow down when duty is 0
{
//increase timeout once when duty is the same (once)
if (timeoutWaitForCommand == 0)
{ // TODO verify if state matches too?
ESP_LOGW(TAG, "[%s] already at target duty %.2f, slowing down...", config.name, dutyTarget);
timeoutWaitForCommand = TIMEOUT_QUEUE_WHEN_AT_TARGET; // wait in queue very long, for new command to arrive
}
vTaskDelay(20 / portTICK_PERIOD_MS); // add small additional delay overall, in case the same commands get spammed
}
//reset timeout when duty differs again (once)
else if (timeoutWaitForCommand != 0)
{
timeoutWaitForCommand = 0; // dont wait additional time for new commands, handle fading fast
ESP_LOGW(TAG, "[%s] duty changed to %.2f, resuming at full speed", config.name, dutyTarget);
// adjust lastRun timestamp to not mess up fading, due to much time passed but with no actual duty change
timestampLastRunUs = esp_timer_get_time() - 20*1000; //subtract approx 1 cycle delay
}
//TODO skip rest of the handle function below using return? Some regular driver updates sound useful though
//--- calculate increment --- //--- calculate increment ---
//calculate increment for fading UP with passed time since last run and configured fade time //calculate increment for fading UP with passed time since last run and configured fade time
@ -172,10 +203,6 @@ void controlledMotor::handle(){
} }
//--- calculate difference ---
dutyDelta = dutyTarget - dutyNow;
//positive: need to increase by that value
//negative: need to decrease
//----- FADING ----- //----- FADING -----
@ -272,17 +299,18 @@ void controlledMotor::handle(){
//=============================== //===============================
//function to set the target mode and duty of a motor //function to set the target mode and duty of a motor
//puts the provided command in a queue for the handle function running in another task //puts the provided command in a queue for the handle function running in another task
void controlledMotor::setTarget(motorstate_t state_f, float duty_f){ void controlledMotor::setTarget(motorCommand_t commandSend){
commandSend = {
.state = state_f,
.duty = duty_f
};
ESP_LOGI(TAG, "[%s] setTarget: Inserting command to queue: state='%s'(%d), duty=%.2f", config.name, motorstateStr[(int)commandSend.state], (int)commandSend.state, commandSend.duty); ESP_LOGI(TAG, "[%s] setTarget: Inserting command to queue: state='%s'(%d), duty=%.2f", config.name, motorstateStr[(int)commandSend.state], (int)commandSend.state, commandSend.duty);
//send command to queue (overwrite if an old command is still in the queue and not processed) //send command to queue (overwrite if an old command is still in the queue and not processed)
xQueueOverwrite( commandQueue, ( void * )&commandSend); xQueueOverwrite( commandQueue, ( void * )&commandSend);
//xQueueSend( commandQueue, ( void * )&commandSend, ( TickType_t ) 0 ); //xQueueSend( commandQueue, ( void * )&commandSend, ( TickType_t ) 0 );
ESP_LOGD(TAG, "finished inserting new command"); ESP_LOGD(TAG, "finished inserting new command");
}
// accept target state and duty as separate agrguments:
void controlledMotor::setTarget(motorstate_t state_f, float duty_f){
// create motorCommand struct from the separate parameters, and run the method to insert that new command
setTarget({state_f, duty_f});
} }

21
common/motorctl.hpp
View File

@ -33,6 +33,7 @@ class controlledMotor {
controlledMotor(motorSetCommandFunc_t setCommandFunc, motorctl_config_t config_control, nvs_handle_t * nvsHandle); //constructor with structs for configuring motordriver and parameters for control TODO: add configuration for currentsensor controlledMotor(motorSetCommandFunc_t setCommandFunc, motorctl_config_t config_control, nvs_handle_t * nvsHandle); //constructor with structs for configuring motordriver and parameters for control TODO: add configuration for currentsensor
void handle(); //controls motor duty with fade and current limiting feature (has to be run frequently by another task) void handle(); //controls motor duty with fade and current limiting feature (has to be run frequently by another task)
void setTarget(motorstate_t state_f, float duty_f = 0); //adds target command to queue for handle function void setTarget(motorstate_t state_f, float duty_f = 0); //adds target command to queue for handle function
void setTarget(motorCommand_t command);
motorCommand_t getStatus(); //get current status of the motor (returns struct with state and duty) motorCommand_t getStatus(); //get current status of the motor (returns struct with state and duty)
uint32_t getFade(fadeType_t fadeType); //get currently set acceleration or deceleration fading time uint32_t getFade(fadeType_t fadeType); //get currently set acceleration or deceleration fading time
@ -42,6 +43,7 @@ class controlledMotor {
bool toggleFade(fadeType_t fadeType); //toggle acceleration or deceleration on/off bool toggleFade(fadeType_t fadeType); //toggle acceleration or deceleration on/off
float getCurrentA() {return cSensor.read();}; //read current-sensor of this motor (Ampere) float getCurrentA() {return cSensor.read();}; //read current-sensor of this motor (Ampere)
char * getName() const {return config.name;};
//TODO set current limit method //TODO set current limit method
@ -78,6 +80,7 @@ class controlledMotor {
float dutyIncrementAccel; float dutyIncrementAccel;
float dutyIncrementDecel; float dutyIncrementDecel;
float dutyDelta; float dutyDelta;
uint32_t timeoutWaitForCommand = 0;
uint32_t msFadeAccel; uint32_t msFadeAccel;
uint32_t msFadeDecel; uint32_t msFadeDecel;
@ -96,20 +99,10 @@ class controlledMotor {
bool receiveTimeout = false; bool receiveTimeout = false;
}; };
// struct with variables passed to task from main
typedef struct task_motorctl_parameters_t {
controlledMotor * motorLeft;
controlledMotor * motorRight;
} task_motorctl_parameters_t;
//==================================== //====================================
//========== motorctl task =========== //========== motorctl task ===========
//==================================== //====================================
//task that inititialized the display, displays welcome message // note: pointer to a 'controlledMotor' object has to be provided as task-parameter
//and releatedly updates the display with certain content // runs handle method of certain motor repeatedly:
//note: pointer to required objects have to be provided as task-parameter // receives commands from control via queue, handle ramp and current, apply new duty by passing it to method of motordriver (ptr)
void task_motorctl( void * task_motorctl_parameters ); void task_motorctl( void * controlledMotor );