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

make control-modes and TCS work [proof of concept]

Browse Source 
Tested new control modes on actual hardware.
Adjust new code so the modes kind of work now, as proof of concept.
Still needs major optimization and fixes though.

motorctl:
    - add config option to disable logging for particular instance
    - add some definitions to finetune control modes
    - rework current and speed mode, so they actually kind of work
    - fix TCS to not cause deadlock motors off

menu:
    - add item set motorControlMode (select DUTY, CURRENT, SPEED)
    - fix missing item maxDuty

speedsensor: fix return type
This commit is contained in:
jonny_l480 2024-02-28 23:00:38 +01:00
parent 2abeefde07
commit 5d1d17915d
6 changed files with 148 additions and 50 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
board_single/main/config.cpp
View File

@ -48,7 +48,7 @@ void setLoglevels(void)
esp_log_level_set("TESTING", ESP_LOG_VERBOSE); esp_log_level_set("TESTING", ESP_LOG_ERROR);
@ -98,6 +98,7 @@ sabertooth2x60_config_t sabertoothConfig = {
//--- configure left motor (contol) --- //--- configure left motor (contol) ---
motorctl_config_t configMotorControlLeft = { motorctl_config_t configMotorControlLeft = {
.name = "left", .name = "left",
.loggingEnabled = true,
.msFadeAccel = 1500, // acceleration of the motor (ms it takes from 0% to 100%) .msFadeAccel = 1500, // acceleration of the motor (ms it takes from 0% to 100%)
.msFadeDecel = 1000, // deceleration of the motor (ms it takes from 100% to 0%) .msFadeDecel = 1000, // deceleration of the motor (ms it takes from 100% to 0%)
.currentLimitEnabled = false, .currentLimitEnabled = false,
@ -113,6 +114,7 @@ motorctl_config_t configMotorControlLeft = {
//--- configure right motor (contol) --- //--- configure right motor (contol) ---
motorctl_config_t configMotorControlRight = { motorctl_config_t configMotorControlRight = {
.name = "right", .name = "right",
.loggingEnabled = false,
.msFadeAccel = 1500, // acceleration of the motor (ms it takes from 0% to 100%) .msFadeAccel = 1500, // acceleration of the motor (ms it takes from 0% to 100%)
.msFadeDecel = 1000, // deceleration of the motor (ms it takes from 100% to 0%) .msFadeDecel = 1000, // deceleration of the motor (ms it takes from 100% to 0%)
.currentLimitEnabled = false, .currentLimitEnabled = false,

55
board_single/main/menu.cpp
View File

@ -343,6 +343,49 @@ menuItem_t item_decelLimit = {
}; };
//###############################
//### select motorControlMode ###
//###############################
void item_motorControlMode_action(display_task_parameters_t *objects, SSD1306_t *display, int value)
{
switch (value)
{
case 1:
default:
objects->motorLeft->setControlMode(motorControlMode_t::DUTY);
objects->motorRight->setControlMode(motorControlMode_t::DUTY);
break;
case 2:
objects->motorLeft->setControlMode(motorControlMode_t::CURRENT);
objects->motorRight->setControlMode(motorControlMode_t::CURRENT);
break;
case 3:
objects->motorLeft->setControlMode(motorControlMode_t::SPEED);
objects->motorRight->setControlMode(motorControlMode_t::SPEED);
break;
}
}
int item_motorControlMode_value(display_task_parameters_t *objects)
{
return 1; // initial value shown / changed from //TODO get actual mode
}
menuItem_t item_motorControlMode = {
item_motorControlMode_action, // function action
item_motorControlMode_value, // function get initial value or NULL(show in line 2)
NULL, // function get default value or NULL(dont set value, show msg)
1, // valueMin
3, // valueMax
1, // valueIncrement
"Control mode ", // title
" sel. motor ", // line1 (above value)
" control mode ", // line2 (above value)
"1: DUTY (defaul)", // line4 * (below value)
"2: CURRENT", // line5 *
"3: SPEED", // line6
"", // line7
};
//################################### //###################################
//##### Traction Control System ##### //##### Traction Control System #####
//################################### //###################################
@ -372,10 +415,10 @@ menuItem_t item_tractionControlSystem = {
"TCS / ASR ", // title "TCS / ASR ", // title
"Traction Control", // line1 (above value) "Traction Control", // line1 (above value)
" System ", // line2 (above value) " System ", // line2 (above value)
"", // line4 * (below value) "1: enable ", // line4 * (below value)
"", // line5 * "0: disable ", // line5 *
"1: enable ", // line6 "note: requires ", // line6
"0: disable ", // line7 "speed ctl-mode ", // line7
}; };
@ -508,8 +551,8 @@ menuItem_t item_last = {
//#################################################### //####################################################
//### store all configured menu items in one array ### //### store all configured menu items in one array ###
//#################################################### //####################################################
const menuItem_t menuItems[] = {item_centerJoystick, item_calibrateJoystick, item_debugJoystick, item_statusScreen, item_accelLimit, item_decelLimit, item_tractionControlSystem, item_reset, item_example, item_last}; const menuItem_t menuItems[] = {item_centerJoystick, item_calibrateJoystick, item_debugJoystick, item_statusScreen, item_maxDuty, item_accelLimit, item_decelLimit, item_motorControlMode, item_tractionControlSystem, item_reset, item_example, item_last};
const int itemCount = 9; const int itemCount = 10;

128
common/motorctl.cpp
View File

@ -33,6 +33,7 @@ controlledMotor::controlledMotor(motorSetCommandFunc_t setCommandFunc, motorctl
cSensor(config_control.currentSensor_adc, config_control.currentSensor_ratedCurrent, config_control.currentSnapToZeroThreshold, config_control.currentInverted) { cSensor(config_control.currentSensor_adc, config_control.currentSensor_ratedCurrent, config_control.currentSnapToZeroThreshold, config_control.currentInverted) {
//copy parameters for controlling the motor //copy parameters for controlling the motor
config = config_control; config = config_control;
log = config.loggingEnabled;
//pointer to update motot dury method //pointer to update motot dury method
motorSetCommand = setCommandFunc; motorSetCommand = setCommandFunc;
//pointer to nvs handle //pointer to nvs handle
@ -113,7 +114,7 @@ void controlledMotor::handle(){
//--- RECEIVE DATA FROM QUEUE --- //--- RECEIVE DATA FROM QUEUE ---
if( xQueueReceive( commandQueue, &commandReceive, timeoutWaitForCommand / portTICK_PERIOD_MS ) ) //wait time is always 0 except when at target duty already if( xQueueReceive( commandQueue, &commandReceive, timeoutWaitForCommand / portTICK_PERIOD_MS ) ) //wait time is always 0 except when at target duty already
{ {
ESP_LOGV(TAG, "[%s] Read command from queue: state=%s, duty=%.2f", config.name, motorstateStr[(int)commandReceive.state], commandReceive.duty); if(log) ESP_LOGV(TAG, "[%s] Read command from queue: state=%s, duty=%.2f", config.name, motorstateStr[(int)commandReceive.state], commandReceive.duty);
state = commandReceive.state; state = commandReceive.state;
dutyTarget = commandReceive.duty; dutyTarget = commandReceive.duty;
receiveTimeout = false; receiveTimeout = false;
@ -127,9 +128,6 @@ void controlledMotor::handle(){
// ----- EXPERIMENTAL, DIFFERENT MODES ----- // ----- EXPERIMENTAL, DIFFERENT MODES -----
// define target duty differently depending on current contro-mode // define target duty differently depending on current contro-mode
#define CURRENT_CONTROL_ALLOWED_AMPERE_DIFF 2
#define SPEED_CONTROL_MAX_SPEED_KMH 9
#define SPEED_CONTROL_ALLOWED_KMH_DIFF 1
//declare variables used inside switch //declare variables used inside switch
float ampereNow, ampereTarget, ampereDiff; float ampereNow, ampereTarget, ampereDiff;
float speedDiff; float speedDiff;
@ -160,18 +158,27 @@ float speedDiff;
} }
break; break;
#define CURRENT_CONTROL_ALLOWED_AMPERE_DIFF 1 //difference from target where no change is made yet
#define CURRENT_CONTROL_MIN_AMPERE 0.7 //current where motor is turned off
//TODO define different, fixed fading configuration in current mode, fade down can be significantly less (500/500ms fade up worked fine)
case motorControlMode_t::CURRENT: // regulate to desired current flow case motorControlMode_t::CURRENT: // regulate to desired current flow
ampereNow = cSensor.read(); ampereNow = cSensor.read();
ampereTarget = config.currentMax * commandReceive.duty / 100; // TODO ensure input data is 0-100 (no duty max), add currentMax to menu/config ampereTarget = config.currentMax * commandReceive.duty / 100; // TODO ensure input data is 0-100 (no duty max), add currentMax to menu/config
if (commandReceive.state == motorstate_t::REV) ampereTarget = - ampereTarget; //target is negative when driving reverse
ampereDiff = ampereTarget - ampereNow; ampereDiff = ampereTarget - ampereNow;
ESP_LOGV("TESTING", "CURRENT-CONTROL: ampereNow=%.2f, ampereTarget=%.2f, diff=%.2f", ampereNow, ampereTarget, ampereDiff); // todo handle brake if(log) ESP_LOGV("TESTING", "[%s] CURRENT-CONTROL: ampereNow=%.2f, ampereTarget=%.2f, diff=%.2f", config.name, ampereNow, ampereTarget, ampereDiff); // todo handle brake
if (fabs(ampereDiff) > CURRENT_CONTROL_ALLOWED_AMPERE_DIFF)
//--- when IDLE to keep the current at target zero motor needs to be on for some duty (to compensate generator current)
if (commandReceive.duty == 0 && fabs(ampereNow) < CURRENT_CONTROL_MIN_AMPERE){ //stop motors completely when current is very low already
dutyTarget = 0;
}
else if (fabs(ampereDiff) > CURRENT_CONTROL_ALLOWED_AMPERE_DIFF || commandReceive.duty == 0) //#### BOOST BY 1 A
{ {
if (ampereDiff > 0 && commandReceive.state == motorstate_t::FWD) // forward need to increase current if (ampereDiff > 0 && commandReceive.state != motorstate_t::REV) // forward need to increase current
{ {
dutyTarget = 100; // todo add custom fading depending on diff? currently very dependent of fade times dutyTarget = 100; // todo add custom fading depending on diff? currently very dependent of fade times
} }
else if (ampereDiff < 0 && commandReceive.state == motorstate_t::REV) // backward need to increase current (more negative) else if (ampereDiff < 0 && commandReceive.state != motorstate_t::FWD) // backward need to increase current (more negative)
{ {
dutyTarget = -100; dutyTarget = -100;
} }
@ -179,44 +186,71 @@ float speedDiff;
{ {
dutyTarget = 0; dutyTarget = 0;
} }
ESP_LOGV("TESTING", "CURRENT-CONTROL: set target to %.0f%%", dutyTarget); if(log) ESP_LOGV("TESTING", "[%s] CURRENT-CONTROL: set target to %.0f%%", config.name, dutyTarget);
} }
else else
{ {
dutyTarget = dutyNow; // target current reached dutyTarget = dutyNow; // target current reached
ESP_LOGD("TESTING", "CURRENT-CONTROL: target current %.3f reached", dutyTarget); if(log) ESP_LOGD("TESTING", "[%s] CURRENT-CONTROL: target current %.3f reached", config.name, dutyTarget);
} }
break; break;
#define SPEED_CONTROL_MAX_SPEED_KMH 10
#define SPEED_CONTROL_ALLOWED_KMH_DIFF 0.6
#define SPEED_CONTROL_MIN_SPEED 0.7 //" start from standstill" always accelerate to this speed, ignoring speedsensor data
case motorControlMode_t::SPEED: // regulate to desired speed case motorControlMode_t::SPEED: // regulate to desired speed
speedNow = sSensor->getKmph(); speedNow = sSensor->getKmph();
//caculate target speed from input
speedTarget = SPEED_CONTROL_MAX_SPEED_KMH * commandReceive.duty / 100; // TODO add maxSpeed to config speedTarget = SPEED_CONTROL_MAX_SPEED_KMH * commandReceive.duty / 100; // TODO add maxSpeed to config
// target speed negative when driving reverse // target speed negative when driving reverse
if (commandReceive.state == motorstate_t::REV) if (commandReceive.state == motorstate_t::REV)
speedTarget = -speedTarget; speedTarget = -speedTarget;
if (sSensor->getTimeLastUpdate() != timestamp_speedLastUpdate ){ //only modify duty when new speed data available
timestamp_speedLastUpdate = sSensor->getTimeLastUpdate(); //TODO get time only once
speedDiff = speedTarget - speedNow; speedDiff = speedTarget - speedNow;
ESP_LOGV("TESTING", "SPEED-CONTROL: target-speed=%.2f, current-speed=%.2f, diff=%.3f", speedTarget, speedNow, speedDiff); } else {
if (fabs(speedDiff) > SPEED_CONTROL_ALLOWED_KMH_DIFF) if(log) ESP_LOGV("TESTING", "[%s] SPEED-CONTROL: no new speed data, not changing duty", config.name);
speedDiff = 0;
}
if(log) ESP_LOGV("TESTING", "[%s] SPEED-CONTROL: target-speed=%.2f, current-speed=%.2f, diff=%.3f", config.name, speedTarget, speedNow, speedDiff);
//stop when target is 0
if (commandReceive.duty == 0) { //TODO add IDLE, BRAKE state
if(log) ESP_LOGV("TESTING", "[%s] SPEED-CONTROL: OFF, target is 0... current-speed=%.2f, diff=%.3f", config.name, speedNow, speedDiff);
dutyTarget = 0;
}
else if (fabs(speedNow) < SPEED_CONTROL_MIN_SPEED){ //start from standstill or too slow (not enough speedsensor data)
if (log)
ESP_LOGV("TESTING", "[%s] SPEED-CONTROL: starting from standstill -> increase duty... target-speed=%.2f, current-speed=%.2f, diff=%.3f", config.name, speedTarget, speedNow, speedDiff);
if (commandReceive.state == motorstate_t::FWD)
dutyTarget = 100;
else if (commandReceive.state == motorstate_t::REV)
dutyTarget = -100;
}
else if (fabs(speedDiff) > SPEED_CONTROL_ALLOWED_KMH_DIFF) //speed too fast/slow
{ {
if (speedDiff > 0 && commandReceive.state == motorstate_t::FWD) // forward need to increase speed if (speedDiff > 0 && commandReceive.state != motorstate_t::REV) // forward need to increase speed
{ {
// TODO retain max duty here // TODO retain max duty here
dutyTarget = 100; // todo add custom fading depending on diff? currently very dependent of fade times dutyTarget = 100; // todo add custom fading depending on diff? currently very dependent of fade times
if(log) ESP_LOGV("TESTING", "[%s] SPEED-CONTROL: speed to low (fwd), diff=%.2f, increasing set target from %.1f%% to %.1f%%", config.name, speedDiff, dutyNow, dutyTarget);
} }
else if (speedDiff < 0 && commandReceive.state == motorstate_t::REV) // backward need to increase speed (more negative) else if (speedDiff < 0 && commandReceive.state != motorstate_t::FWD) // backward need to increase speed (more negative)
{ {
dutyTarget = -100; dutyTarget = -100;
if(log) ESP_LOGV("TESTING", "[%s] SPEED-CONTROL: speed to low (rev), diff=%.2f, increasing set target from %.1f%% to %.1f%%", config.name, speedDiff, dutyNow, dutyTarget);
} }
else // fwd too much, rev too much -> decrease else // fwd too much, rev too much -> decrease
{ {
dutyTarget = 0; dutyTarget = 0;
if(log) ESP_LOGV("TESTING", "[%s] SPEED-CONTROL: speed to high, diff=%.2f, decreasing set target from %.1f%% to %.1f%%", config.name, speedDiff, dutyNow, dutyTarget);
} }
ESP_LOGV("TESTING", "CURRENT-CONTROL: set target to %.0f%%", dutyTarget);
} }
else else
{ {
dutyTarget = dutyNow; // target current reached dutyTarget = dutyNow; // target speed reached
ESP_LOGD("TESTING", "SPEED-CONTROL: target speed %.3f reached", speedTarget); if(log) ESP_LOGD("TESTING", "[%s] SPEED-CONTROL: target speed %.3f reached", config.name, speedTarget);
} }
break; break;
@ -227,9 +261,9 @@ float speedDiff;
//--- TIMEOUT NO DATA --- //--- TIMEOUT NO DATA ---
// turn motors off if no data received for a long time (e.g. no uart data or control task offline) // 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) 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); if(log) 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; receiveTimeout = true;
state = motorstate_t::IDLE; state = motorstate_t::IDLE;
dutyTarget = 0; // todo put this in else section of queue (no data received) and add control mode "timeout"? dutyTarget = 0; // todo put this in else section of queue (no data received) and add control mode "timeout"?
@ -245,12 +279,14 @@ if (dutyNow != 0 && esp_log_timestamp() - timestamp_commandReceived > TIMEOUT_ID
//--- DETECT ALREADY AT TARGET --- //--- DETECT ALREADY AT TARGET ---
// when already at exact target duty there is no need to run very fast to handle fading // 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 //-> slow down loop by waiting significantly longer for new commands to arrive
if ((dutyDelta == 0 && !config.currentLimitEnabled && !config.tractionControlSystemEnabled) || (dutyTarget == 0 && dutyNow == 0)) //when current limit or tcs enabled only slow down when duty is 0 if (mode != motorControlMode_t::CURRENT //dont slow down when in CURRENT mode at all
&& ((dutyDelta == 0 && !config.currentLimitEnabled && !config.tractionControlSystemEnabled && mode != motorControlMode_t::SPEED) //when neither of current-limit, tractioncontrol or speed-mode is enabled slow down when target reached
|| (dutyTarget == 0 && dutyNow == 0))) //otherwise only slow down when when actually off
{ {
//increase queue timeout when duty is the same (once) //increase queue timeout when duty is the same (once)
if (timeoutWaitForCommand == 0) if (timeoutWaitForCommand == 0)
{ // TODO verify if state matches too? { // TODO verify if state matches too?
ESP_LOGI(TAG, "[%s] already at target duty %.2f, slowing down...", config.name, dutyTarget); if(log) ESP_LOGI(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 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 vTaskDelay(20 / portTICK_PERIOD_MS); // add small additional delay overall, in case the same commands get spammed
@ -259,7 +295,7 @@ if (dutyNow != 0 && esp_log_timestamp() - timestamp_commandReceived > TIMEOUT_ID
else if (timeoutWaitForCommand != 0) else if (timeoutWaitForCommand != 0)
{ {
timeoutWaitForCommand = 0; // dont wait additional time for new commands, handle fading fast timeoutWaitForCommand = 0; // dont wait additional time for new commands, handle fading fast
ESP_LOGI(TAG, "[%s] duty changed to %.2f, resuming at full speed", config.name, dutyTarget); if(log) ESP_LOGI(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 // 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 timestampLastRunUs = esp_timer_get_time() - 20*1000; //subtract approx 1 cycle delay
} }
@ -269,9 +305,9 @@ if (dutyNow != 0 && esp_log_timestamp() - timestamp_commandReceived > TIMEOUT_ID
//--- BRAKE --- //--- BRAKE ---
//brake immediately, update state, duty and exit this cycle of handle function //brake immediately, update state, duty and exit this cycle of handle function
if (state == motorstate_t::BRAKE){ if (state == motorstate_t::BRAKE){
ESP_LOGD(TAG, "braking - skip fading"); if(log) ESP_LOGD(TAG, "braking - skip fading");
motorSetCommand({motorstate_t::BRAKE, dutyTarget}); motorSetCommand({motorstate_t::BRAKE, dutyTarget});
ESP_LOGD(TAG, "[%s] Set Motordriver: state=%s, duty=%.2f - Measurements: current=%.2f, speed=N/A", config.name, motorstateStr[(int)state], dutyNow, currentNow); if(log) ESP_LOGD(TAG, "[%s] Set Motordriver: state=%s, duty=%.2f - Measurements: current=%.2f, speed=N/A", config.name, motorstateStr[(int)state], dutyNow, currentNow);
//dutyNow = 0; //dutyNow = 0;
return; //no need to run the fade algorithm return; //no need to run the fade algorithm
} }
@ -330,7 +366,7 @@ if (dutyNow != 0 && esp_log_timestamp() - timestamp_commandReceived > TIMEOUT_ID
} else if (dutyNow > currentLimitDecrement) { } else if (dutyNow > currentLimitDecrement) {
dutyNow -= currentLimitDecrement; dutyNow -= currentLimitDecrement;
} }
ESP_LOGW(TAG, "[%s] current limit exceeded! now=%.3fA max=%.1fA => decreased duty from %.3f to %.3f", config.name, currentNow, config.currentMax, dutyOld, dutyNow); if(log) ESP_LOGW(TAG, "[%s] current limit exceeded! now=%.3fA max=%.1fA => decreased duty from %.3f to %.3f", config.name, currentNow, config.currentMax, dutyOld, dutyNow);
} }
} }
@ -340,7 +376,10 @@ if (dutyNow != 0 && esp_log_timestamp() - timestamp_commandReceived > TIMEOUT_ID
//TODO only run this when speed sensors actually updated //TODO only run this when speed sensors actually updated
//handle tcs when enabled and new speed sensor data is available TODO: currently assumes here that speed sensor data of other motor updated as well //handle tcs when enabled and new speed sensor data is available TODO: currently assumes here that speed sensor data of other motor updated as well
#define TCS_MAX_ALLOWED_RATIO_DIFF 0.1 //when motor speed ratio differs more than that, one motor is slowed down #define TCS_MAX_ALLOWED_RATIO_DIFF 0.1 //when motor speed ratio differs more than that, one motor is slowed down
if (config.tractionControlSystemEnabled && sSensor->getTimeLastUpdate() != tcs_timestampLastSpeedUpdate){ #define TCS_NO_SPEED_DATA_TIMEOUT_US 200*1000
#define TCS_MIN_SPEED_KMH 1 //must be at least that fast for TCS to be enabled
//TODO rework this: clearer structure (less nested if statements)
if (config.tractionControlSystemEnabled && mode == motorControlMode_t::SPEED && sSensor->getTimeLastUpdate() != tcs_timestampLastSpeedUpdate && (esp_timer_get_time() - tcs_timestampLastRun < TCS_NO_SPEED_DATA_TIMEOUT_US)){
//update last speed update received //update last speed update received
tcs_timestampLastSpeedUpdate = sSensor->getTimeLastUpdate(); //TODO: re-use tcs_timestampLastRun in if statement, instead of having additional variable SpeedUpdate tcs_timestampLastSpeedUpdate = sSensor->getTimeLastUpdate(); //TODO: re-use tcs_timestampLastRun in if statement, instead of having additional variable SpeedUpdate
@ -349,8 +388,8 @@ if (dutyNow != 0 && esp_log_timestamp() - timestamp_commandReceived > TIMEOUT_ID
tcs_timestampLastRun = esp_timer_get_time(); tcs_timestampLastRun = esp_timer_get_time();
//get motor stats //get motor stats
float speedNowThis = sSensor->getRpm(); float speedNowThis = sSensor->getKmph();
float speedNowOther = (*ppOtherMotor)->getDuty(); float speedNowOther = (*ppOtherMotor)->getCurrentSpeed();
float speedTargetThis = speedTarget; float speedTargetThis = speedTarget;
float speedTargetOther = (*ppOtherMotor)->getTargetSpeed(); float speedTargetOther = (*ppOtherMotor)->getTargetSpeed();
float dutyTargetOther = (*ppOtherMotor)->getTargetDuty(); float dutyTargetOther = (*ppOtherMotor)->getTargetDuty();
@ -368,26 +407,30 @@ if (dutyNow != 0 && esp_log_timestamp() - timestamp_commandReceived > TIMEOUT_ID
//calculate unexpected difference //calculate unexpected difference
float ratioDiff = ratioSpeedNow - ratioSpeedTarget; float ratioDiff = ratioSpeedNow - ratioSpeedTarget;
ESP_LOGD("TESTING", "[%s] TCS: ratioSpeedTarget=%.3f, ratioSpeedNow=%.3f, ratioDutyNow=%.3f, diff=%.3f", config.name, ratioSpeedTarget, ratioSpeedNow, ratioDutyNow, ratioDiff); if(log) ESP_LOGD("TESTING", "[%s] TCS: speedThis=%.3f, speedOther=%.3f, ratioSpeedTarget=%.3f, ratioSpeedNow=%.3f, ratioDutyNow=%.3f, diff=%.3f", config.name, speedNowThis, speedNowOther, ratioSpeedTarget, ratioSpeedNow, ratioDutyNow, ratioDiff);
//-- handle rotating faster than expected -- //-- handle rotating faster than expected --
//TODO also increase duty when other motor is slipping? (diff negative) //TODO also increase duty when other motor is slipping? (diff negative)
if (ratioDiff > TCS_MAX_ALLOWED_RATIO_DIFF) // motor turns too fast compared to expected target ratio if (speedNowThis < TCS_MIN_SPEED_KMH) { //disable / turn off TCS when currently too slow (danger of deadlock)
tcs_isExceeded = false;
tcs_usExceeded = 0;
}
else if (ratioDiff > TCS_MAX_ALLOWED_RATIO_DIFF ) // motor turns too fast compared to expected target ratio
{ {
if (!tcs_isExceeded) // just started being too fast if (!tcs_isExceeded) // just started being too fast
{ {
tcs_timestampBeginExceeded = esp_timer_get_time(); tcs_timestampBeginExceeded = esp_timer_get_time();
tcs_isExceeded = true; //also blocks further acceleration (fade) tcs_isExceeded = true; //also blocks further acceleration (fade)
ESP_LOGW("TESTING", "[%s] TCS: now exceeding max allowed ratio diff! diff=%.2f max=%.2f", config.name, ratioDiff, TCS_MAX_ALLOWED_RATIO_DIFF); if(log) ESP_LOGW("TESTING", "[%s] TCS: now exceeding max allowed ratio diff! diff=%.2f max=%.2f", config.name, ratioDiff, TCS_MAX_ALLOWED_RATIO_DIFF);
} }
else else
{ // too fast for more than 2 cycles already { // too fast for more than 2 cycles already
tcs_usExceeded = esp_timer_get_time() - tcs_timestampBeginExceeded; //time too fast already tcs_usExceeded = esp_timer_get_time() - tcs_timestampBeginExceeded; //time too fast already
ESP_LOGI("TESTING", "[%s] TCS: faster than expected since %dms, current ratioDiff=%.2f -> slowing down", config.name, tcs_usExceeded/1000, ratioDiff); if(log) ESP_LOGI("TESTING", "[%s] TCS: faster than expected since %dms, current ratioDiff=%.2f -> slowing down", config.name, tcs_usExceeded/1000, ratioDiff);
// calculate amount duty gets decreased // calculate amount duty gets decreased
float dutyDecrement = (tcs_usPassed / ((float)msFadeDecel * 1000)) * 100; //TODO optimize dynamic increment: P:scale with ratio-difference, I: scale with duration exceeded float dutyDecrement = (tcs_usPassed / ((float)msFadeDecel * 1000)) * 100; //TODO optimize dynamic increment: P:scale with ratio-difference, I: scale with duration exceeded
// decrease duty // decrease duty
ESP_LOGI("TESTING", "[%s] TCS: msPassed=%.3f, reducing duty by %.3f%%", config.name, (float)tcs_usPassed/1000, dutyDecrement); if(log) ESP_LOGI("TESTING", "[%s] TCS: msPassed=%.3f, reducing duty by %.3f%%", config.name, (float)tcs_usPassed/1000, dutyDecrement);
fade(&dutyNow, 0, -dutyDecrement); //reduce duty but not less than 0 fade(&dutyNow, 0, -dutyDecrement); //reduce duty but not less than 0
} }
} }
@ -397,6 +440,11 @@ if (dutyNow != 0 && esp_log_timestamp() - timestamp_commandReceived > TIMEOUT_ID
tcs_usExceeded = 0; tcs_usExceeded = 0;
} }
} }
else // TCS mode not active or timed out
{ // not exceeded
tcs_isExceeded = false;
tcs_usExceeded = 0;
}
@ -415,10 +463,10 @@ if (dutyNow != 0 && esp_log_timestamp() - timestamp_commandReceived > TIMEOUT_ID
( state == motorstate_t::FWD && (esp_log_timestamp() - timestampsModeLastActive[(int)motorstate_t::REV] < config.deadTimeMs)) ( state == motorstate_t::FWD && (esp_log_timestamp() - timestampsModeLastActive[(int)motorstate_t::REV] < config.deadTimeMs))
|| (state == motorstate_t::REV && (esp_log_timestamp() - timestampsModeLastActive[(int)motorstate_t::FWD] < config.deadTimeMs)) || (state == motorstate_t::REV && (esp_log_timestamp() - timestampsModeLastActive[(int)motorstate_t::FWD] < config.deadTimeMs))
){ ){
ESP_LOGD(TAG, "waiting dead-time... dir change %s -> %s", motorstateStr[(int)statePrev], motorstateStr[(int)state]); if(log) ESP_LOGD(TAG, "waiting dead-time... dir change %s -> %s", motorstateStr[(int)statePrev], motorstateStr[(int)state]);
if (!deadTimeWaiting){ //log start if (!deadTimeWaiting){ //log start
deadTimeWaiting = true; deadTimeWaiting = true;
ESP_LOGI(TAG, "starting dead-time... %s -> %s", motorstateStr[(int)statePrev], motorstateStr[(int)state]); if(log) ESP_LOGI(TAG, "starting dead-time... %s -> %s", motorstateStr[(int)statePrev], motorstateStr[(int)state]);
} }
//force IDLE state during wait //force IDLE state during wait
state = motorstate_t::IDLE; state = motorstate_t::IDLE;
@ -426,9 +474,9 @@ if (dutyNow != 0 && esp_log_timestamp() - timestamp_commandReceived > TIMEOUT_ID
} else { } else {
if (deadTimeWaiting){ //log end if (deadTimeWaiting){ //log end
deadTimeWaiting = false; deadTimeWaiting = false;
ESP_LOGI(TAG, "dead-time ended - continue with %s", motorstateStr[(int)state]); if(log) ESP_LOGI(TAG, "dead-time ended - continue with %s", motorstateStr[(int)state]);
} }
ESP_LOGV(TAG, "deadtime: no change below deadtime detected... dir=%s, duty=%.1f", motorstateStr[(int)state], dutyNow); if(log) ESP_LOGV(TAG, "deadtime: no change below deadtime detected... dir=%s, duty=%.1f", motorstateStr[(int)state], dutyNow);
} }
} }
@ -442,7 +490,7 @@ if (dutyNow != 0 && esp_log_timestamp() - timestamp_commandReceived > TIMEOUT_ID
//--- apply new target to motor --- //--- apply new target to motor ---
motorSetCommand({state, (float)fabs(dutyNow)}); motorSetCommand({state, (float)fabs(dutyNow)});
ESP_LOGI(TAG, "[%s] Set Motordriver: state=%s, duty=%.2f - Measurements: current=%.2f, speed=N/A", config.name, motorstateStr[(int)state], dutyNow, currentNow); if(log) ESP_LOGI(TAG, "[%s] Set Motordriver: state=%s, duty=%.2f - Measurements: current=%.2f, speed=N/A", config.name, motorstateStr[(int)state], dutyNow, currentNow);
//note: BRAKE state is handled earlier //note: BRAKE state is handled earlier
@ -458,11 +506,11 @@ if (dutyNow != 0 && esp_log_timestamp() - timestamp_commandReceived > TIMEOUT_ID
//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(motorCommand_t commandSend){ 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); if(log) 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"); if(log) ESP_LOGD(TAG, "finished inserting new command");
} }
// accept target state and duty as separate agrguments: // accept target state and duty as separate agrguments:

7
common/motorctl.hpp
View File

@ -24,6 +24,7 @@ extern "C"
typedef void (*motorSetCommandFunc_t)(motorCommand_t cmd); typedef void (*motorSetCommandFunc_t)(motorCommand_t cmd);
enum class motorControlMode_t {DUTY, CURRENT, SPEED};
//=================================== //===================================
//====== controlledMotor class ====== //====== controlledMotor class ======
@ -40,9 +41,11 @@ class controlledMotor {
float getDuty() {return dutyNow;}; float getDuty() {return dutyNow;};
float getTargetDuty() {return dutyTarget;}; float getTargetDuty() {return dutyTarget;};
float getTargetSpeed() {return speedTarget;}; float getTargetSpeed() {return speedTarget;};
float getCurrentSpeed() {return sSensor->getKmph();};
void enableTractionControlSystem() {config.tractionControlSystemEnabled = true;}; void enableTractionControlSystem() {config.tractionControlSystemEnabled = true;};
void disableTractionControlSystem() {config.tractionControlSystemEnabled = false; tcs_isExceeded = false;}; void disableTractionControlSystem() {config.tractionControlSystemEnabled = false; tcs_isExceeded = false;};
bool getTractionControlSystemStatus() {return config.tractionControlSystemEnabled;}; bool getTractionControlSystemStatus() {return config.tractionControlSystemEnabled;};
void setControlMode(motorControlMode_t newMode) {mode = newMode;};
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
uint32_t getFadeDefault(fadeType_t fadeType); //get acceleration or deceleration fading time from config uint32_t getFadeDefault(fadeType_t fadeType); //get acceleration or deceleration fading time from config
@ -81,8 +84,9 @@ class controlledMotor {
//TODO add name for logging? //TODO add name for logging?
//struct for storing control specific parameters //struct for storing control specific parameters
motorctl_config_t config; motorctl_config_t config;
bool log = false;
motorstate_t state = motorstate_t::IDLE; motorstate_t state = motorstate_t::IDLE;
motorControlMode_t mode = motorControlMode_t::DUTY; motorControlMode_t mode = motorControlMode_t::DUTY; //default control mode
//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;
@ -92,6 +96,7 @@ class controlledMotor {
//speed mode //speed mode
float speedTarget = 0; float speedTarget = 0;
float speedNow = 0; float speedNow = 0;
uint32_t timestamp_speedLastUpdate = 0;
float dutyTarget = 0; float dutyTarget = 0;

2
common/speedsensor.hpp
View File

@ -33,7 +33,7 @@ public:
float getKmph(); //kilometers per hour float getKmph(); //kilometers per hour
float getMps(); //meters per second float getMps(); //meters per second
float getRpm(); //rotations per minute float getRpm(); //rotations per minute
float getTimeLastUpdate() {return timeLastUpdate;}; uint32_t getTimeLastUpdate() {return timeLastUpdate;};
//variables for handling the encoder (public because ISR needs access) //variables for handling the encoder (public because ISR needs access)
speedSensor_config_t config; speedSensor_config_t config;

2
common/types.hpp
View File

@ -22,7 +22,6 @@ enum class motorstate_t {IDLE, FWD, REV, BRAKE};
//definition of string array to be able to convert state enum to readable string (defined in motordrivers.cpp) //definition of string array to be able to convert state enum to readable string (defined in motordrivers.cpp)
extern const char* motorstateStr[4]; extern const char* motorstateStr[4];
enum class motorControlMode_t {DUTY, CURRENT, SPEED};
//=========================== //===========================
@ -43,6 +42,7 @@ typedef struct motorCommands_t {
//struct with all config parameters for a motor regarding ramp and current limit //struct with all config parameters for a motor regarding ramp and current limit
typedef struct motorctl_config_t { typedef struct motorctl_config_t {
char * name; //name for unique nvs storage-key prefix and logging char * name; //name for unique nvs storage-key prefix and logging
bool loggingEnabled; //enable/disable ALL log output (mostly better to debug only one instance)
uint32_t msFadeAccel; //acceleration of the motor (ms it takes from 0% to 100%) uint32_t msFadeAccel; //acceleration of the motor (ms it takes from 0% to 100%)
uint32_t msFadeDecel; //deceleration of the motor (ms it takes from 100% to 0%) uint32_t msFadeDecel; //deceleration of the motor (ms it takes from 100% to 0%)
bool currentLimitEnabled; bool currentLimitEnabled;