Optimize motorctl: slow down task when target reached
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
parent
fab4d442e6
commit
bc014befb7
@ -58,32 +58,30 @@ controlledArmchair::controlledArmchair(
|
||||
|
||||
// override default config value if maxDuty is found in nvs
|
||||
loadMaxDuty();
|
||||
|
||||
//TODO declare / configure controlled motors here instead of config (unnecessary that button object is globally available - only used here)?
|
||||
}
|
||||
|
||||
|
||||
//=======================================
|
||||
//============ control task =============
|
||||
//=======================================
|
||||
//task that controls the armchair modes and initiates commands generation and applies them to driver
|
||||
// task that controls the armchair modes
|
||||
// generates commands depending on current mode and sends those to corresponding task
|
||||
// parameter: pointer to controlledArmchair object
|
||||
void task_control( void * pvParameters ){
|
||||
//control_task_parameters_t * objects = (control_task_parameters_t *)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)
|
||||
//objects->control->startHandleLoop();
|
||||
control->startHandleLoop();
|
||||
}
|
||||
|
||||
|
||||
//----------------------------------
|
||||
//---------- Handle loop -----------
|
||||
//----------------------------------
|
||||
//function that repeatedly generates motor commands depending on the current mode
|
||||
//also handles fading and current-limit
|
||||
void controlledArmchair::startHandleLoop() {
|
||||
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) {
|
||||
default:
|
||||
@ -91,31 +89,40 @@ void controlledArmchair::startHandleLoop() {
|
||||
break;
|
||||
|
||||
case controlMode_t::IDLE:
|
||||
//copy preset commands for idling both motors
|
||||
commands = cmds_bothMotorsIdle;
|
||||
motorRight->setTarget(commands.right.state, commands.right.duty);
|
||||
motorLeft->setTarget(commands.left.state, commands.left.duty);
|
||||
vTaskDelay(300 / portTICK_PERIOD_MS);
|
||||
//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);
|
||||
#ifdef JOYSTICK_LOG_IN_IDLE
|
||||
//get joystick data here (without using it)
|
||||
//since loglevel is DEBUG, calculateion details is output
|
||||
joystick_l->getData(); //get joystick data here
|
||||
//since loglevel is DEBUG, calculation details are output
|
||||
joystick_l->getData();
|
||||
#endif
|
||||
break;
|
||||
|
||||
|
||||
case controlMode_t::JOYSTICK:
|
||||
vTaskDelay(20 / portTICK_PERIOD_MS);
|
||||
vTaskDelay(50 / portTICK_PERIOD_MS);
|
||||
//get current joystick data with getData method of evaluatedJoystick
|
||||
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
|
||||
// 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)
|
||||
{
|
||||
commands = joystick_generateCommandsDriving(stickData, &joystickGenerateCommands_config);
|
||||
// apply motor commands
|
||||
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
|
||||
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;
|
||||
|
||||
|
||||
@ -130,27 +137,32 @@ void controlledArmchair::startHandleLoop() {
|
||||
//pass joystick data from getData method of evaluatedJoystick to generateCommandsShaking function
|
||||
commands = joystick_generateCommandsShaking(stickData);
|
||||
//apply motor commands
|
||||
motorRight->setTarget(commands.right.state, commands.right.duty);
|
||||
motorLeft->setTarget(commands.left.state, commands.left.duty);
|
||||
motorRight->setTarget(commands.right);
|
||||
motorLeft->setTarget(commands.left);
|
||||
break;
|
||||
|
||||
|
||||
case controlMode_t::HTTP:
|
||||
//--- get joystick data from queue ---
|
||||
//Note this function waits several seconds (httpconfig.timeoutMs) for data to arrive, otherwise Center data or NULL is returned
|
||||
//TODO: as described above, when changing modes it might delay a few seconds for the change to apply
|
||||
stickData = httpJoystickMain_l->getData();
|
||||
stickDataLast = stickData;
|
||||
stickData = httpJoystickMain_l->getData(); //get last stored data from receive queue (waits up to 500ms for new event to arrive)
|
||||
//scale coordinates additionally (more detail in slower area)
|
||||
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);
|
||||
//--- 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 ){
|
||||
// Note: timeout (no data received) is handled in getData method
|
||||
commands = joystick_generateCommandsDriving(stickData, &joystickGenerateCommands_config);
|
||||
|
||||
//--- apply commands to motors ---
|
||||
//TODO make motorctl.setTarget also accept motorcommand struct directly
|
||||
motorRight->setTarget(commands.right.state, commands.right.duty);
|
||||
motorLeft->setTarget(commands.left.state, commands.left.duty);
|
||||
motorRight->setTarget(commands.right);
|
||||
motorLeft->setTarget(commands.left);
|
||||
}
|
||||
else
|
||||
{
|
||||
ESP_LOGD(TAG, "http joystick data unchanged at %s not updating commands", joystickPosStr[(int)stickData.position]);
|
||||
}
|
||||
break;
|
||||
|
||||
|
||||
@ -159,9 +171,8 @@ void controlledArmchair::startHandleLoop() {
|
||||
//generate commands
|
||||
commands = automatedArmchair->generateCommands(&instruction);
|
||||
//--- apply commands to motors ---
|
||||
//TODO make motorctl.setTarget also accept motorcommand struct directly
|
||||
motorRight->setTarget(commands.right.state, commands.right.duty);
|
||||
motorLeft->setTarget(commands.left.state, commands.left.duty);
|
||||
motorRight->setTarget(commands.right);
|
||||
motorLeft->setTarget(commands.left);
|
||||
|
||||
//process received instruction
|
||||
switch (instruction) {
|
||||
@ -200,9 +211,9 @@ void controlledArmchair::startHandleLoop() {
|
||||
//--- read joystick ---
|
||||
stickData = joystick_l->getData();
|
||||
//--- idle motors ---
|
||||
commands = cmds_bothMotorsIdle;
|
||||
motorRight->setTarget(commands.right.state, commands.right.duty);
|
||||
motorLeft->setTarget(commands.left.state, commands.left.duty);
|
||||
//commands = cmds_bothMotorsIdle; - now done once at mode change
|
||||
//motorRight->setTarget(commands.right.state, commands.right.duty);
|
||||
//motorLeft->setTarget(commands.left.state, commands.left.duty);
|
||||
//--- control armchair position with joystick input ---
|
||||
controlChairAdjustment(joystick_l->getData(), legRest, backRest);
|
||||
break;
|
||||
@ -212,9 +223,9 @@ void controlledArmchair::startHandleLoop() {
|
||||
vTaskDelay(1000 / portTICK_PERIOD_MS);
|
||||
//nothing to do here, display task handles the menu
|
||||
//--- idle motors ---
|
||||
commands = cmds_bothMotorsIdle;
|
||||
motorRight->setTarget(commands.right.state, commands.right.duty);
|
||||
motorLeft->setTarget(commands.left.state, commands.left.duty);
|
||||
//commands = cmds_bothMotorsIdle; - now done once at mode change
|
||||
//motorRight->setTarget(commands.right.state, commands.right.duty);
|
||||
//motorLeft->setTarget(commands.left.state, commands.left.duty);
|
||||
break;
|
||||
|
||||
//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);
|
||||
timestamp_SlowLoopLastRun = esp_log_timestamp();
|
||||
|
||||
//run function which detects timeout (switch to idle)
|
||||
//run function that detects timeout (switch to idle)
|
||||
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 -----------
|
||||
@ -426,6 +445,8 @@ void controlledArmchair::changeMode(controlMode_t modeNew) {
|
||||
break;
|
||||
|
||||
case controlMode_t::IDLE:
|
||||
ESP_LOGW(TAG, "switching to IDLE mode: turning both motors off, beep");
|
||||
idleBothMotors();
|
||||
buzzer->beep(1, 1000, 0);
|
||||
#ifdef JOYSTICK_LOG_IN_IDLE
|
||||
esp_log_level_set("evaluatedJoystick", ESP_LOG_DEBUG);
|
||||
@ -433,10 +454,15 @@ void controlledArmchair::changeMode(controlMode_t modeNew) {
|
||||
break;
|
||||
|
||||
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");
|
||||
idleBothMotors();
|
||||
buzzer->beep(4, 200, 100);
|
||||
break;
|
||||
|
||||
case controlMode_t::MENU:
|
||||
idleBothMotors();
|
||||
break;
|
||||
|
||||
case controlMode_t::MASSAGE:
|
||||
ESP_LOGW(TAG, "switching to MASSAGE mode -> reducing fading");
|
||||
uint32_t shake_msFadeAccel = 500; //TODO: move this to config
|
||||
|
||||
@ -105,6 +105,8 @@ class controlledArmchair {
|
||||
void loadMaxDuty(); //load stored value for maxDuty from nvs
|
||||
void writeMaxDuty(float newMaxDuty); //write new value for maxDuty to nvs
|
||||
|
||||
void idleBothMotors(); //turn both motors off
|
||||
|
||||
//--- objects ---
|
||||
buzzer_t* buzzer;
|
||||
controlledMotor* motorLeft;
|
||||
@ -118,14 +120,33 @@ class controlledArmchair {
|
||||
//handle for using the nvs flash (persistent config variables)
|
||||
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 ---
|
||||
//struct for motor commands returned by generate functions of each mode
|
||||
motorCommands_t commands;
|
||||
motorCommands_t commands = cmds_bothMotorsIdle;
|
||||
//struct with config parameters
|
||||
control_config_t config;
|
||||
|
||||
//store joystick data
|
||||
joystickData_t stickData;
|
||||
joystickData_t stickData = joystickData_center;
|
||||
joystickData_t stickDataLast = joystickData_center;
|
||||
|
||||
//variables for http mode
|
||||
uint32_t http_timestamp_lastData = 0;
|
||||
@ -145,16 +166,6 @@ class controlledArmchair {
|
||||
//variable to store mode when toggling IDLE 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
|
||||
uint32_t timestamp_SlowLoopLastRun = 0;
|
||||
|
||||
|
||||
@ -251,9 +251,10 @@ extern "C" void app_main(void) {
|
||||
//----------------------------------------------
|
||||
//--- create task for controlling the motors ---
|
||||
//----------------------------------------------
|
||||
//task that receives commands, handles ramp and current limit and executes commands using the motordriver function
|
||||
task_motorctl_parameters_t motorctl_param = {motorLeft, motorRight};
|
||||
xTaskCreate(&task_motorctl, "task_motor-control", 2*4096, &motorctl_param, 6, NULL);
|
||||
//task for each motor that handles to following:
|
||||
//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_ctl-left-motor", 2*4096, motorLeft, 6, NULL);
|
||||
xTaskCreate(&task_motorctl, "task_ctl-right-motor", 2*4096, motorRight, 6, NULL);
|
||||
|
||||
//------------------------------
|
||||
//--- create task for buzzer ---
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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;
|
||||
};
|
||||
@ -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,6 +57,9 @@ void controlledMotor::init(){
|
||||
loadAccelDuration();
|
||||
loadDecelDuration();
|
||||
|
||||
// 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;
|
||||
@ -135,15 +137,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){
|
||||
// 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;
|
||||
ESP_LOGE(TAG, "[%s] TIMEOUT, no target data received for more than %ds -> switch to IDLE", config.name, TIMEOUT_IDLE_WHEN_NO_COMMAND/1000);
|
||||
}
|
||||
|
||||
//--- 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
|
||||
int64_t usPassed = esp_timer_get_time() - timestampLastRunUs;
|
||||
@ -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});
|
||||
}
|
||||
|
||||
|
||||
|
||||
@ -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 );
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user