make control-modes and TCS work [proof of concept]
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
parent
2abeefde07
commit
5d1d17915d
@ -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) ---
|
||||
motorctl_config_t configMotorControlLeft = {
|
||||
.name = "left",
|
||||
.loggingEnabled = true,
|
||||
.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%)
|
||||
.currentLimitEnabled = false,
|
||||
@ -113,6 +114,7 @@ motorctl_config_t configMotorControlLeft = {
|
||||
//--- configure right motor (contol) ---
|
||||
motorctl_config_t configMotorControlRight = {
|
||||
.name = "right",
|
||||
.loggingEnabled = false,
|
||||
.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%)
|
||||
.currentLimitEnabled = false,
|
||||
|
||||
@ -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 #####
|
||||
//###################################
|
||||
@ -372,10 +415,10 @@ menuItem_t item_tractionControlSystem = {
|
||||
"TCS / ASR ", // title
|
||||
"Traction Control", // line1 (above value)
|
||||
" System ", // line2 (above value)
|
||||
"", // line4 * (below value)
|
||||
"", // line5 *
|
||||
"1: enable ", // line6
|
||||
"0: disable ", // line7
|
||||
"1: enable ", // line4 * (below value)
|
||||
"0: disable ", // line5 *
|
||||
"note: requires ", // line6
|
||||
"speed ctl-mode ", // line7
|
||||
};
|
||||
|
||||
|
||||
@ -508,8 +551,8 @@ menuItem_t item_last = {
|
||||
//####################################################
|
||||
//### 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 int itemCount = 9;
|
||||
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 = 10;
|
||||
|
||||
|
||||
|
||||
|
||||
@ -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) {
|
||||
//copy parameters for controlling the motor
|
||||
config = config_control;
|
||||
log = config.loggingEnabled;
|
||||
//pointer to update motot dury method
|
||||
motorSetCommand = setCommandFunc;
|
||||
//pointer to nvs handle
|
||||
@ -113,7 +114,7 @@ void controlledMotor::handle(){
|
||||
//--- RECEIVE DATA FROM QUEUE ---
|
||||
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;
|
||||
dutyTarget = commandReceive.duty;
|
||||
receiveTimeout = false;
|
||||
@ -127,9 +128,6 @@ void controlledMotor::handle(){
|
||||
|
||||
// ----- EXPERIMENTAL, DIFFERENT MODES -----
|
||||
// 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
|
||||
float ampereNow, ampereTarget, ampereDiff;
|
||||
float speedDiff;
|
||||
@ -160,18 +158,27 @@ float speedDiff;
|
||||
}
|
||||
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
|
||||
ampereNow = cSensor.read();
|
||||
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;
|
||||
ESP_LOGV("TESTING", "CURRENT-CONTROL: ampereNow=%.2f, ampereTarget=%.2f, diff=%.2f", ampereNow, ampereTarget, ampereDiff); // todo handle brake
|
||||
if (fabs(ampereDiff) > CURRENT_CONTROL_ALLOWED_AMPERE_DIFF)
|
||||
if(log) ESP_LOGV("TESTING", "[%s] CURRENT-CONTROL: ampereNow=%.2f, ampereTarget=%.2f, diff=%.2f", config.name, ampereNow, ampereTarget, ampereDiff); // todo handle brake
|
||||
|
||||
//--- 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
|
||||
}
|
||||
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;
|
||||
}
|
||||
@ -179,44 +186,71 @@ float speedDiff;
|
||||
{
|
||||
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
|
||||
{
|
||||
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;
|
||||
|
||||
#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
|
||||
speedNow = sSensor->getKmph();
|
||||
|
||||
//caculate target speed from input
|
||||
speedTarget = SPEED_CONTROL_MAX_SPEED_KMH * commandReceive.duty / 100; // TODO add maxSpeed to config
|
||||
// target speed negative when driving reverse
|
||||
if (commandReceive.state == motorstate_t::REV)
|
||||
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;
|
||||
ESP_LOGV("TESTING", "SPEED-CONTROL: target-speed=%.2f, current-speed=%.2f, diff=%.3f", speedTarget, speedNow, speedDiff);
|
||||
if (fabs(speedDiff) > SPEED_CONTROL_ALLOWED_KMH_DIFF)
|
||||
} else {
|
||||
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
|
||||
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;
|
||||
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
|
||||
{
|
||||
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
|
||||
{
|
||||
dutyTarget = dutyNow; // target current reached
|
||||
ESP_LOGD("TESTING", "SPEED-CONTROL: target speed %.3f reached", speedTarget);
|
||||
dutyTarget = dutyNow; // target speed reached
|
||||
if(log) ESP_LOGD("TESTING", "[%s] SPEED-CONTROL: target speed %.3f reached", config.name, speedTarget);
|
||||
}
|
||||
|
||||
break;
|
||||
@ -227,9 +261,9 @@ float speedDiff;
|
||||
|
||||
//--- 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)
|
||||
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;
|
||||
state = motorstate_t::IDLE;
|
||||
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 ---
|
||||
// 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 && !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)
|
||||
if (timeoutWaitForCommand == 0)
|
||||
{ // 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
|
||||
}
|
||||
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)
|
||||
{
|
||||
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
|
||||
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 immediately, update state, duty and exit this cycle of handle function
|
||||
if (state == motorstate_t::BRAKE){
|
||||
ESP_LOGD(TAG, "braking - skip fading");
|
||||
if(log) ESP_LOGD(TAG, "braking - skip fading");
|
||||
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;
|
||||
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) {
|
||||
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
|
||||
//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
|
||||
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
|
||||
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();
|
||||
|
||||
//get motor stats
|
||||
float speedNowThis = sSensor->getRpm();
|
||||
float speedNowOther = (*ppOtherMotor)->getDuty();
|
||||
float speedNowThis = sSensor->getKmph();
|
||||
float speedNowOther = (*ppOtherMotor)->getCurrentSpeed();
|
||||
float speedTargetThis = speedTarget;
|
||||
float speedTargetOther = (*ppOtherMotor)->getTargetSpeed();
|
||||
float dutyTargetOther = (*ppOtherMotor)->getTargetDuty();
|
||||
@ -368,26 +407,30 @@ if (dutyNow != 0 && esp_log_timestamp() - timestamp_commandReceived > TIMEOUT_ID
|
||||
|
||||
//calculate unexpected difference
|
||||
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 --
|
||||
//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
|
||||
{
|
||||
tcs_timestampBeginExceeded = esp_timer_get_time();
|
||||
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
|
||||
{ // too fast for more than 2 cycles 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
|
||||
float dutyDecrement = (tcs_usPassed / ((float)msFadeDecel * 1000)) * 100; //TODO optimize dynamic increment: P:scale with ratio-difference, I: scale with duration exceeded
|
||||
// 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
|
||||
}
|
||||
}
|
||||
@ -397,6 +440,11 @@ if (dutyNow != 0 && esp_log_timestamp() - timestamp_commandReceived > TIMEOUT_ID
|
||||
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::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
|
||||
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
|
||||
state = motorstate_t::IDLE;
|
||||
@ -426,9 +474,9 @@ if (dutyNow != 0 && esp_log_timestamp() - timestamp_commandReceived > TIMEOUT_ID
|
||||
} else {
|
||||
if (deadTimeWaiting){ //log end
|
||||
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 ---
|
||||
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
|
||||
|
||||
|
||||
@ -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
|
||||
//puts the provided command in a queue for the handle function running in another task
|
||||
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)
|
||||
xQueueOverwrite( commandQueue, ( void * )&commandSend);
|
||||
//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:
|
||||
|
||||
@ -24,6 +24,7 @@ extern "C"
|
||||
|
||||
typedef void (*motorSetCommandFunc_t)(motorCommand_t cmd);
|
||||
|
||||
enum class motorControlMode_t {DUTY, CURRENT, SPEED};
|
||||
|
||||
//===================================
|
||||
//====== controlledMotor class ======
|
||||
@ -40,9 +41,11 @@ class controlledMotor {
|
||||
float getDuty() {return dutyNow;};
|
||||
float getTargetDuty() {return dutyTarget;};
|
||||
float getTargetSpeed() {return speedTarget;};
|
||||
float getCurrentSpeed() {return sSensor->getKmph();};
|
||||
void enableTractionControlSystem() {config.tractionControlSystemEnabled = true;};
|
||||
void disableTractionControlSystem() {config.tractionControlSystemEnabled = false; tcs_isExceeded = false;};
|
||||
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 getFadeDefault(fadeType_t fadeType); //get acceleration or deceleration fading time from config
|
||||
@ -81,8 +84,9 @@ class controlledMotor {
|
||||
//TODO add name for logging?
|
||||
//struct for storing control specific parameters
|
||||
motorctl_config_t config;
|
||||
bool log = false;
|
||||
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)
|
||||
nvs_handle_t * nvsHandle;
|
||||
|
||||
@ -92,6 +96,7 @@ class controlledMotor {
|
||||
//speed mode
|
||||
float speedTarget = 0;
|
||||
float speedNow = 0;
|
||||
uint32_t timestamp_speedLastUpdate = 0;
|
||||
|
||||
|
||||
float dutyTarget = 0;
|
||||
|
||||
@ -33,7 +33,7 @@ public:
|
||||
float getKmph(); //kilometers per hour
|
||||
float getMps(); //meters per second
|
||||
float getRpm(); //rotations per minute
|
||||
float getTimeLastUpdate() {return timeLastUpdate;};
|
||||
uint32_t getTimeLastUpdate() {return timeLastUpdate;};
|
||||
|
||||
//variables for handling the encoder (public because ISR needs access)
|
||||
speedSensor_config_t config;
|
||||
|
||||
@ -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)
|
||||
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
|
||||
typedef struct motorctl_config_t {
|
||||
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 msFadeDecel; //deceleration of the motor (ms it takes from 100% to 0%)
|
||||
bool currentLimitEnabled;
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user