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
Download Patch File Download Diff File Expand all files Collapse all files

13
common/http.cpp
View File

@@ -178,23 +178,22 @@ esp_err_t httpJoystick::receiveHttpData(httpd_req_t *req){
//-------------------
//----- 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(){
//--- 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",
dataRead.x, dataRead.y, dataRead.radius, dataRead.angle);
timeLastData = esp_log_timestamp();
}
//--- 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 {
//send error message when last received data did NOT result in CENTER position
if (dataRead.position != joystickPos_t::CENTER) {
if (dataRead.position != joystickPos_t::CENTER && (esp_log_timestamp() - timeLastData) > config.timeoutMs) {
//change data to "joystick center" data to stop the motors
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;

3
common/http.hpp
View File

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

86
common/motorctl.cpp
View File

@@ -5,21 +5,20 @@
//tag for logging
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 ===========
//====================================
//task for handling the motors (ramp, current limit, driver)
void task_motorctl( void * task_motorctl_parameters ){
task_motorctl_parameters_t *objects = (task_motorctl_parameters_t *)task_motorctl_parameters;
ESP_LOGW(TAG, "Task-motorctl: starting handle loops for left and right motor...");
void task_motorctl( void * ptrControlledMotor ){
//get pointer to controlledMotor instance from task parameter
controlledMotor * motor = (controlledMotor *)ptrControlledMotor;
ESP_LOGW(TAG, "Task-motorctl [%s]: starting handle loop...", motor->getName());
while(1){
objects->motorRight->handle();
objects->motorLeft->handle();
vTaskDelay(15 / portTICK_PERIOD_MS);
motor->handle();
vTaskDelay(20 / portTICK_PERIOD_MS);
}
}
@@ -58,7 +57,10 @@ void controlledMotor::init(){
loadAccelDuration();
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
//--- 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);
state = commandReceive.state;
@@ -134,15 +136,44 @@ void controlledMotor::handle(){
}
}
//--- timeout, no data ---
//turn motors off if no data received for long time (e.g. no uart data / control offline)
//TODO no timeout when braking?
if ((esp_log_timestamp() - timestamp_commandReceived) > TIMEOUT_IDLE_WHEN_NO_COMMAND && !receiveTimeout){
receiveTimeout = true;
state = motorstate_t::IDLE;
dutyTarget = 0;
ESP_LOGE(TAG, "[%s] TIMEOUT, no target data received for more than %ds -> switch to IDLE", config.name, TIMEOUT_IDLE_WHEN_NO_COMMAND/1000);
}
//--- timeout, no data ---
// turn motors off if no data received for a long time (e.g. no uart data or control task offline)
if (dutyNow != 0 && esp_log_timestamp() - timestamp_commandReceived > TIMEOUT_IDLE_WHEN_NO_COMMAND && !receiveTimeout)
{
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);
receiveTimeout = true;
state = motorstate_t::IDLE;
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 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 -----
@@ -272,17 +299,18 @@ void controlledMotor::handle(){
//===============================
//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
void controlledMotor::setTarget(motorstate_t state_f, float duty_f){
commandSend = {
.state = state_f,
.duty = duty_f
};
void controlledMotor::setTarget(motorCommand_t commandSend){
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)
xQueueOverwrite( commandQueue, ( void * )&commandSend);
//xQueueSend( commandQueue, ( void * )&commandSend, ( TickType_t ) 0 );
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
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(motorCommand_t command);
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
@@ -42,6 +43,7 @@ class controlledMotor {
bool toggleFade(fadeType_t fadeType); //toggle acceleration or deceleration on/off
float getCurrentA() {return cSensor.read();}; //read current-sensor of this motor (Ampere)
char * getName() const {return config.name;};
//TODO set current limit method
@@ -78,6 +80,7 @@ class controlledMotor {
float dutyIncrementAccel;
float dutyIncrementDecel;
float dutyDelta;
uint32_t timeoutWaitForCommand = 0;
uint32_t msFadeAccel;
uint32_t msFadeDecel;
@@ -96,20 +99,10 @@ class controlledMotor {
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 ===========
//====================================
//task that inititialized the display, displays welcome message
//and releatedly updates the display with certain content
//note: pointer to required objects have to be provided as task-parameter
void task_motorctl( void * task_motorctl_parameters );
// note: pointer to a 'controlledMotor' object has to be provided as task-parameter
// runs handle method of certain motor repeatedly:
// 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 * controlledMotor );