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

Merge branch 'driving-behavior' into dev - turning, experimental ctl-modes

Browse Source
This commit is contained in:
jonny 2024-03-26 16:36:47 +01:00
commit a65f6205f6
14 changed files with 795 additions and 362 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
board_single/main/button.cpp
View File

@ -75,7 +75,10 @@ void buttonCommands::action (uint8_t count, bool lastPressLong){
if (lastPressLong) if (lastPressLong)
{ {
control->changeMode(controlMode_t::MENU); control->changeMode(controlMode_t::MENU);
ESP_LOGW(TAG, "1x long press -> change to menu mode"); ESP_LOGW(TAG, "1x long press -> clear encoder queue and change to menu mode");
// clear encoder event queue (prevent menu from exiting immediately due to long press event just happend)
rotary_encoder_event_t ev;
while (xQueueReceive(encoderQueue, &ev, 0) == pdPASS);
buzzer->beep(20, 20, 10); buzzer->beep(20, 20, 10);
vTaskDelay(500 / portTICK_PERIOD_MS); vTaskDelay(500 / portTICK_PERIOD_MS);
} }
@ -156,7 +159,7 @@ void buttonCommands::action (uint8_t count, bool lastPressLong){
// when not in MENU mode, repeatedly receives events from encoder button // when not in MENU mode, repeatedly receives events from encoder button
// and takes the corresponding action // and takes the corresponding action
// this function has to be started once in a separate task // this function has to be started once in a separate task
#define INPUT_TIMEOUT 700 // duration of no button events, after which action is run (implicitly also is 'long-press' time) #define INPUT_TIMEOUT 500 // duration of no button events, after which action is run (implicitly also is 'long-press' time)
void buttonCommands::startHandleLoop() void buttonCommands::startHandleLoop()
{ {
//-- variables -- //-- variables --

29
board_single/main/config.cpp
View File

@ -45,6 +45,13 @@ void setLoglevels(void)
esp_log_level_set("chair-adjustment", ESP_LOG_INFO); esp_log_level_set("chair-adjustment", ESP_LOG_INFO);
esp_log_level_set("menu", ESP_LOG_INFO); esp_log_level_set("menu", ESP_LOG_INFO);
esp_log_level_set("encoder", ESP_LOG_INFO); esp_log_level_set("encoder", ESP_LOG_INFO);
esp_log_level_set("TESTING", ESP_LOG_ERROR);
} }
//================================== //==================================
@ -91,9 +98,11 @@ 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,
.tractionControlSystemEnabled = false,
.currentSensor_adc = ADC1_CHANNEL_4, // GPIO32 .currentSensor_adc = ADC1_CHANNEL_4, // GPIO32
.currentSensor_ratedCurrent = 50, .currentSensor_ratedCurrent = 50,
.currentMax = 30, .currentMax = 30,
@ -105,9 +114,11 @@ 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,
.tractionControlSystemEnabled = false,
.currentSensor_adc = ADC1_CHANNEL_5, // GPIO33 .currentSensor_adc = ADC1_CHANNEL_5, // GPIO33
.currentSensor_ratedCurrent = 50, .currentSensor_ratedCurrent = 50,
.currentMax = 30, .currentMax = 30,
@ -240,7 +251,19 @@ rotary_encoder_t encoder_config = {
//----------------------------------- //-----------------------------------
//configure parameters for motor command generation from joystick data //configure parameters for motor command generation from joystick data
joystickGenerateCommands_config_t joystickGenerateCommands_config{ joystickGenerateCommands_config_t joystickGenerateCommands_config{
.maxDuty = 100, //-- maxDuty --
.dutyOffset = 5, // duty at which motors start immediately // max duty when both motors are at equal ratio e.g. driving straight forward
.altStickMapping = false, // better to be set less than 100% to have some reserve for boosting the outer tire when turning
.maxDutyStraight = 75,
//-- maxBoost --
// boost is amount of duty added to maxDutyStraight to outer tire while turning
// => turning: inner tire gets slower, outer tire gets faster
// 0: boost = 0 (disabled)
// 100: boost = maxDutyStraight (e.g. when maxDuty is 50, outer motor can still reach 100 (50+50))
.maxRelativeBoostPercentOfMaxDuty = 60,
// 60: when maxDuty is set above 62% (equals 0.6*62 = 38% boost) the outer tire can still reach 100% - below 62 maxDuty the boosted speed is also reduced.
// => setting this value lower prevents desired low max duty configuration from being way to fast in curves.
.dutyOffset = 5, // duty at which motors start immediately
.ratioSnapToOneThreshold = 0.9, // threshold ratio snaps to 1 to have some area of max turning before entering X-Axis-full-rotate mode
.altStickMapping = false // invert reverse direction
}; };

472
board_single/main/control.cpp
View File

@ -59,6 +59,9 @@ controlledArmchair::controlledArmchair(
// override default config value if maxDuty is found in nvs // override default config value if maxDuty is found in nvs
loadMaxDuty(); loadMaxDuty();
// create semaphore for preventing race condition: mode-change operations while currently still executing certain mode
handleIteration_mutex = xSemaphoreCreateMutex();
} }
@ -75,188 +78,205 @@ void task_control( void * pvParameters ){
} }
//---------------------------------- //----------------------------------
//---------- Handle loop ----------- //---------- Handle loop -----------
//---------------------------------- //----------------------------------
//function that repeatedly generates motor commands depending on the current mode // start endless loop that repeatedly calls handle() and handleTimeout() methods
void controlledArmchair::startHandleLoop() { void controlledArmchair::startHandleLoop()
while (1){ {
ESP_LOGV(TAG, "control loop executing... mode=%s", controlModeStr[(int)mode]); while (1)
{
// mutex to prevent race condition with actions beeing run at mode change and previous mode still beeing executed
if (xSemaphoreTake(handleIteration_mutex, portMAX_DELAY) == pdTRUE)
{
//--- handle current mode ---
ESP_LOGV(TAG, "control loop executing... mode='%s'", controlModeStr[(int)mode]);
handle();
switch(mode) { xSemaphoreGive(handleIteration_mutex);
default: } // end mutex
mode = controlMode_t::IDLE;
break;
case controlMode_t::IDLE: //--- slow loop ---
//copy preset commands for idling both motors - now done once at mode change // this section is run approx every 5s (+500ms)
//commands = cmds_bothMotorsIdle; if (esp_log_timestamp() - timestamp_SlowLoopLastRun > 5000)
//motorRight->setTarget(commands.right.state, commands.right.duty); {
//motorLeft->setTarget(commands.left.state, commands.left.duty); ESP_LOGV(TAG, "running slow loop... time since last run: %.1fs", (float)(esp_log_timestamp() - timestamp_SlowLoopLastRun) / 1000);
vTaskDelay(500 / portTICK_PERIOD_MS);
#ifdef JOYSTICK_LOG_IN_IDLE
// get joystick data and log it
joystickData_t data joystick_l->getData();
ESP_LOGI("JOYSTICK_LOG_IN_IDLE", "x=%.3f, y=%.3f, radius=%.3f, angle=%.3f, pos=%s, adcx=%d, adcy=%d",
data.x, data.y, data.radius, data.angle,
joystickPosStr[(int)data.position],
objects->joystick->getRawX(), objects->joystick->getRawY());
#endif
break;
//------- handle JOYSTICK mode -------
case controlMode_t::JOYSTICK:
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)
{
resetTimeout(); //user input -> reset switch to IDLE timeout
commands = joystick_generateCommandsDriving(stickData, &joystickGenerateCommands_config);
// apply motor commands
motorRight->setTarget(commands.right);
motorLeft->setTarget(commands.left);
}
else
{
vTaskDelay(20 / portTICK_PERIOD_MS);
ESP_LOGV(TAG, "analog joystick data unchanged at %s not updating commands", joystickPosStr[(int)stickData.position]);
}
break;
//------- handle MASSAGE mode -------
case controlMode_t::MASSAGE:
vTaskDelay(10 / portTICK_PERIOD_MS);
//--- read joystick ---
// only update joystick data when input not frozen
stickDataLast = stickData;
if (!freezeInput)
stickData = joystick_l->getData();
//--- 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)
{
resetTimeout(); // user input -> reset switch to IDLE timeout
// pass joystick data from getData method of evaluatedJoystick to generateCommandsShaking function
commands = joystick_generateCommandsShaking(stickData);
// apply motor commands
motorRight->setTarget(commands.right);
motorLeft->setTarget(commands.left);
}
break;
//------- handle HTTP mode -------
case controlMode_t::HTTP:
//--- get joystick data from queue ---
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 ){
resetTimeout(); // user input -> reset switch to IDLE timeout
// Note: timeout (no data received) is handled in getData method
commands = joystick_generateCommandsDriving(stickData, &joystickGenerateCommands_config);
//--- apply commands to motors ---
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;
//------- handle AUTO mode -------
case controlMode_t::AUTO:
vTaskDelay(20 / portTICK_PERIOD_MS);
//generate commands
commands = automatedArmchair->generateCommands(&instruction);
//--- apply commands to motors ---
motorRight->setTarget(commands.right);
motorLeft->setTarget(commands.left);
//process received instruction
switch (instruction) {
case auto_instruction_t::NONE:
break;
case auto_instruction_t::SWITCH_PREV_MODE:
toggleMode(controlMode_t::AUTO);
break;
case auto_instruction_t::SWITCH_JOYSTICK_MODE:
changeMode(controlMode_t::JOYSTICK);
break;
case auto_instruction_t::RESET_ACCEL_DECEL:
//enable downfading (set to default value)
motorLeft->setFade(fadeType_t::DECEL, true);
motorRight->setFade(fadeType_t::DECEL, true);
//set upfading to default value
motorLeft->setFade(fadeType_t::ACCEL, true);
motorRight->setFade(fadeType_t::ACCEL, true);
break;
case auto_instruction_t::RESET_ACCEL:
//set upfading to default value
motorLeft->setFade(fadeType_t::ACCEL, true);
motorRight->setFade(fadeType_t::ACCEL, true);
break;
case auto_instruction_t::RESET_DECEL:
//enable downfading (set to default value)
motorLeft->setFade(fadeType_t::DECEL, true);
motorRight->setFade(fadeType_t::DECEL, true);
break;
}
break;
//------- handle ADJUST_CHAIR mode -------
case controlMode_t::ADJUST_CHAIR:
vTaskDelay(100 / portTICK_PERIOD_MS);
//--- read joystick ---
stickDataLast = stickData;
stickData = joystick_l->getData();
//--- control armchair position with joystick input ---
// dont update when stick data did not change
if (stickData.x != stickDataLast.x || stickData.y != stickDataLast.y)
{
resetTimeout(); // user input -> reset switch to IDLE timeout
controlChairAdjustment(joystick_l->getData(), legRest, backRest);
}
break;
//------- handle MENU mode -------
case controlMode_t::MENU:
//nothing to do here, display task handles the menu
vTaskDelay(1000 / portTICK_PERIOD_MS);
break;
//TODO: add other modes here
}
//-----------------------
//------ slow loop ------
//-----------------------
//this section is run about every 5s (+500ms)
if (esp_log_timestamp() - timestamp_SlowLoopLastRun > 5000) {
ESP_LOGV(TAG, "running slow loop... time since last run: %.1fs", (float)(esp_log_timestamp() - timestamp_SlowLoopLastRun)/1000);
timestamp_SlowLoopLastRun = esp_log_timestamp(); timestamp_SlowLoopLastRun = esp_log_timestamp();
//run function that detects timeout (switch to idle, or notify "forgot to turn off") //--- handle timeouts ---
// run function that detects timeouts (switch to idle, or notify "forgot to turn off")
handleTimeout(); handleTimeout();
} }
vTaskDelay(5 / portTICK_PERIOD_MS); // small delay necessary to give modeChange() a chance to take the mutex
// TODO: move mode specific delays from handle() to here, to prevent unnecessary long mutex lock
}
}
}//end while(1)
}//end startHandleLoop
//-------------------------------------
//---------- Handle control -----------
//-------------------------------------
// function that repeatedly generates motor commands and runs actions depending on the current mode
void controlledArmchair::handle()
{
switch (mode)
{
default:
//switch to IDLE mode when current mode is not implemented
changeMode(controlMode_t::IDLE);
break;
//------- handle IDLE -------
case controlMode_t::IDLE:
vTaskDelay(500 / portTICK_PERIOD_MS);
// TODO repeatedly set motors to idle, in case driver bugs? Currently 15s motorctl timeout would have to pass
#ifdef JOYSTICK_LOG_IN_IDLE
// get joystick data and log it
joystickData_t data joystick_l->getData();
ESP_LOGI("JOYSTICK_LOG_IN_IDLE", "x=%.3f, y=%.3f, radius=%.3f, angle=%.3f, pos=%s, adcx=%d, adcy=%d",
data.x, data.y, data.radius, data.angle,
joystickPosStr[(int)data.position],
objects->joystick->getRawX(), objects->joystick->getRawY());
#endif
break;
//------- handle JOYSTICK mode -------
case controlMode_t::JOYSTICK:
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.7, 0.45); // 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)
{
resetTimeout(); // user input -> reset switch to IDLE timeout
commands = joystick_generateCommandsDriving(stickData, &joystickGenerateCommands_config);
// apply motor commands
motorRight->setTarget(commands.right);
motorLeft->setTarget(commands.left);
}
else
{
vTaskDelay(20 / portTICK_PERIOD_MS);
ESP_LOGV(TAG, "analog joystick data unchanged at %s not updating commands", joystickPosStr[(int)stickData.position]);
}
break;
//------- handle MASSAGE mode -------
case controlMode_t::MASSAGE:
vTaskDelay(10 / portTICK_PERIOD_MS);
//--- read joystick ---
// only update joystick data when input not frozen
stickDataLast = stickData;
if (!freezeInput)
stickData = joystick_l->getData();
// reset timeout when joystick data changed
if (stickData.x != stickDataLast.x || stickData.y != stickDataLast.y)
resetTimeout(); // user input -> reset switch to IDLE timeout
//--- generate motor commands ---
// pass joystick data from getData method of evaluatedJoystick to generateCommandsShaking function
commands = joystick_generateCommandsShaking(stickData);
// apply motor commands
motorRight->setTarget(commands.right);
motorLeft->setTarget(commands.left);
break;
//------- handle HTTP mode -------
case controlMode_t::HTTP:
//--- get joystick data from queue ---
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)
{
resetTimeout(); // user input -> reset switch to IDLE timeout
// Note: timeout (no data received) is handled in getData method
commands = joystick_generateCommandsDriving(stickData, &joystickGenerateCommands_config);
//--- apply commands to motors ---
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;
//------- handle AUTO mode -------
case controlMode_t::AUTO:
vTaskDelay(20 / portTICK_PERIOD_MS);
// generate commands
commands = automatedArmchair->generateCommands(&instruction);
//--- apply commands to motors ---
motorRight->setTarget(commands.right);
motorLeft->setTarget(commands.left);
// process received instruction
switch (instruction)
{
case auto_instruction_t::NONE:
break;
case auto_instruction_t::SWITCH_PREV_MODE:
toggleMode(controlMode_t::AUTO);
break;
case auto_instruction_t::SWITCH_JOYSTICK_MODE:
changeMode(controlMode_t::JOYSTICK);
break;
case auto_instruction_t::RESET_ACCEL_DECEL:
// enable downfading (set to default value)
motorLeft->setFade(fadeType_t::DECEL, true);
motorRight->setFade(fadeType_t::DECEL, true);
// set upfading to default value
motorLeft->setFade(fadeType_t::ACCEL, true);
motorRight->setFade(fadeType_t::ACCEL, true);
break;
case auto_instruction_t::RESET_ACCEL:
// set upfading to default value
motorLeft->setFade(fadeType_t::ACCEL, true);
motorRight->setFade(fadeType_t::ACCEL, true);
break;
case auto_instruction_t::RESET_DECEL:
// enable downfading (set to default value)
motorLeft->setFade(fadeType_t::DECEL, true);
motorRight->setFade(fadeType_t::DECEL, true);
break;
}
break;
//------- handle ADJUST_CHAIR mode -------
case controlMode_t::ADJUST_CHAIR:
vTaskDelay(100 / portTICK_PERIOD_MS);
//--- read joystick ---
stickDataLast = stickData;
stickData = joystick_l->getData();
//--- control armchair position with joystick input ---
// dont update when stick data did not change
if (stickData.x != stickDataLast.x || stickData.y != stickDataLast.y)
{
resetTimeout(); // user input -> reset switch to IDLE timeout
controlChairAdjustment(joystick_l->getData(), legRest, backRest);
}
break;
//------- handle MENU mode -------
case controlMode_t::MENU:
// nothing to do here, display task handles the menu
vTaskDelay(1000 / portTICK_PERIOD_MS);
break;
// TODO: add other modes here
}
} // end - handle method
@ -383,35 +403,43 @@ void controlledArmchair::handleTimeout()
//----------- changeMode ------------ //----------- changeMode ------------
//----------------------------------- //-----------------------------------
//function to change to a specified control mode //function to change to a specified control mode
void controlledArmchair::changeMode(controlMode_t modeNew) { void controlledArmchair::changeMode(controlMode_t modeNew)
{
//exit if target mode is already active // exit if target mode is already active
if (mode == modeNew) { if (mode == modeNew)
{
ESP_LOGE(TAG, "changeMode: Already in target mode '%s' -> nothing to change", controlModeStr[(int)mode]); ESP_LOGE(TAG, "changeMode: Already in target mode '%s' -> nothing to change", controlModeStr[(int)mode]);
return; return;
} }
//copy previous mode // mutex to wait for current handle iteration (control-task) to finish
modePrevious = mode; // prevents race conditions where operations when changing mode are run but old mode gets handled still
//store time changed (needed for timeout) ESP_LOGI(TAG, "changeMode: waiting for current handle() iteration to finish...");
timestamp_lastModeChange = esp_log_timestamp(); if (xSemaphoreTake(handleIteration_mutex, portMAX_DELAY) == pdTRUE)
{
// copy previous mode
modePrevious = mode;
// store time changed (needed for timeout)
timestamp_lastModeChange = esp_log_timestamp();
ESP_LOGW(TAG, "=== changing mode from %s to %s ===", controlModeStr[(int)mode], controlModeStr[(int)modeNew]); ESP_LOGW(TAG, "=== changing mode from %s to %s ===", controlModeStr[(int)mode], controlModeStr[(int)modeNew]);
//========== commands change FROM mode ========== //========== commands change FROM mode ==========
//run functions when changing FROM certain mode // run functions when changing FROM certain mode
switch(modePrevious){ switch (modePrevious)
default: {
ESP_LOGI(TAG, "noting to execute when changing FROM this mode"); default:
break; ESP_LOGI(TAG, "noting to execute when changing FROM this mode");
break;
case controlMode_t::IDLE: case controlMode_t::IDLE:
#ifdef JOYSTICK_LOG_IN_IDLE #ifdef JOYSTICK_LOG_IN_IDLE
ESP_LOGI(TAG, "disabling debug output for 'evaluatedJoystick'"); ESP_LOGI(TAG, "disabling debug output for 'evaluatedJoystick'");
esp_log_level_set("evaluatedJoystick", ESP_LOG_WARN); //FIXME: loglevel from config esp_log_level_set("evaluatedJoystick", ESP_LOG_WARN); // FIXME: loglevel from config
#endif #endif
buzzer->beep(1,200,100); buzzer->beep(1, 200, 100);
break; break;
case controlMode_t::HTTP: case controlMode_t::HTTP:
ESP_LOGW(TAG, "switching from HTTP mode -> stopping wifi-ap"); ESP_LOGW(TAG, "switching from HTTP mode -> stopping wifi-ap");
@ -420,41 +448,40 @@ void controlledArmchair::changeMode(controlMode_t modeNew) {
case controlMode_t::MASSAGE: case controlMode_t::MASSAGE:
ESP_LOGW(TAG, "switching from MASSAGE mode -> restoring fading, reset frozen input"); ESP_LOGW(TAG, "switching from MASSAGE mode -> restoring fading, reset frozen input");
//TODO: fix issue when downfading was disabled before switching to massage mode - currently it gets enabled again here... // TODO: fix issue when downfading was disabled before switching to massage mode - currently it gets enabled again here...
//enable downfading (set to default value) // enable downfading (set to default value)
motorLeft->setFade(fadeType_t::DECEL, true); motorLeft->setFade(fadeType_t::DECEL, true);
motorRight->setFade(fadeType_t::DECEL, true); motorRight->setFade(fadeType_t::DECEL, true);
//set upfading to default value // set upfading to default value
motorLeft->setFade(fadeType_t::ACCEL, true); motorLeft->setFade(fadeType_t::ACCEL, true);
motorRight->setFade(fadeType_t::ACCEL, true); motorRight->setFade(fadeType_t::ACCEL, true);
//reset frozen input state // reset frozen input state
freezeInput = false; freezeInput = false;
break; break;
case controlMode_t::AUTO: case controlMode_t::AUTO:
ESP_LOGW(TAG, "switching from AUTO mode -> restoring fading to default"); ESP_LOGW(TAG, "switching from AUTO mode -> restoring fading to default");
//TODO: fix issue when downfading was disabled before switching to auto mode - currently it gets enabled again here... // TODO: fix issue when downfading was disabled before switching to auto mode - currently it gets enabled again here...
//enable downfading (set to default value) // enable downfading (set to default value)
motorLeft->setFade(fadeType_t::DECEL, true); motorLeft->setFade(fadeType_t::DECEL, true);
motorRight->setFade(fadeType_t::DECEL, true); motorRight->setFade(fadeType_t::DECEL, true);
//set upfading to default value // set upfading to default value
motorLeft->setFade(fadeType_t::ACCEL, true); motorLeft->setFade(fadeType_t::ACCEL, true);
motorRight->setFade(fadeType_t::ACCEL, true); motorRight->setFade(fadeType_t::ACCEL, true);
break; break;
case controlMode_t::ADJUST_CHAIR: case controlMode_t::ADJUST_CHAIR:
ESP_LOGW(TAG, "switching from ADJUST_CHAIR mode => turning off adjustment motors..."); ESP_LOGW(TAG, "switching from ADJUST_CHAIR mode => turning off adjustment motors...");
//prevent motors from being always on in case of mode switch while joystick is not in center thus motors currently moving // prevent motors from being always on in case of mode switch while joystick is not in center thus motors currently moving
legRest->setState(REST_OFF); legRest->setState(REST_OFF);
backRest->setState(REST_OFF); backRest->setState(REST_OFF);
break; break;
}
} //========== commands change TO mode ==========
// run functions when changing TO certain mode
switch (modeNew)
//========== commands change TO mode ========== {
//run functions when changing TO certain mode
switch(modeNew){
default: default:
ESP_LOGI(TAG, "noting to execute when changing TO this mode"); ESP_LOGI(TAG, "noting to execute when changing TO this mode");
break; break;
@ -482,24 +509,25 @@ void controlledArmchair::changeMode(controlMode_t modeNew) {
case controlMode_t::MASSAGE: case controlMode_t::MASSAGE:
ESP_LOGW(TAG, "switching to MASSAGE mode -> reducing fading"); ESP_LOGW(TAG, "switching to MASSAGE mode -> reducing fading");
uint32_t shake_msFadeAccel = 500; //TODO: move this to config uint32_t shake_msFadeAccel = 500; // TODO: move this to config
//disable downfading (max. deceleration) // disable downfading (max. deceleration)
motorLeft->setFade(fadeType_t::DECEL, false); motorLeft->setFade(fadeType_t::DECEL, false);
motorRight->setFade(fadeType_t::DECEL, false); motorRight->setFade(fadeType_t::DECEL, false);
//reduce upfading (increase acceleration) // reduce upfading (increase acceleration)
motorLeft->setFade(fadeType_t::ACCEL, shake_msFadeAccel); motorLeft->setFade(fadeType_t::ACCEL, shake_msFadeAccel);
motorRight->setFade(fadeType_t::ACCEL, shake_msFadeAccel); motorRight->setFade(fadeType_t::ACCEL, shake_msFadeAccel);
break; break;
}
} //--- update mode to new mode ---
mode = modeNew;
//--- update mode to new mode --- // unlock mutex for control task to continue handling modes
//TODO: add mutex xSemaphoreGive(handleIteration_mutex);
mode = modeNew; } // end mutex
} }
//TODO simplify the following 3 functions? can be replaced by one? //TODO simplify the following 3 functions? can be replaced by one?
//----------------------------------- //-----------------------------------
@ -526,7 +554,7 @@ void controlledArmchair::toggleModes(controlMode_t modePrimary, controlMode_t mo
} }
//switch to primary mode when any other mode is active //switch to primary mode when any other mode is active
else { else {
ESP_LOGW(TAG, "toggleModes: switching from %s to primary mode %s", controlModeStr[(int)mode], controlModeStr[(int)modePrimary]); ESP_LOGW(TAG, "toggleModes: switching from '%s' to primary mode '%s'", controlModeStr[(int)mode], controlModeStr[(int)modePrimary]);
//buzzer->beep(4,200,100); //buzzer->beep(4,200,100);
changeMode(modePrimary); changeMode(modePrimary);
} }
@ -542,13 +570,13 @@ void controlledArmchair::toggleMode(controlMode_t modePrimary){
//switch to previous mode when primary is already active //switch to previous mode when primary is already active
if (mode == modePrimary){ if (mode == modePrimary){
ESP_LOGW(TAG, "toggleMode: switching from primaryMode %s to previousMode %s", controlModeStr[(int)mode], controlModeStr[(int)modePrevious]); ESP_LOGW(TAG, "toggleMode: switching from primaryMode '%s' to previousMode '%s'", controlModeStr[(int)mode], controlModeStr[(int)modePrevious]);
//buzzer->beep(2,200,100); //buzzer->beep(2,200,100);
changeMode(modePrevious); //switch to previous mode changeMode(modePrevious); //switch to previous mode
} }
//switch to primary mode when any other mode is active //switch to primary mode when any other mode is active
else { else {
ESP_LOGW(TAG, "toggleModes: switching from %s to primary mode %s", controlModeStr[(int)mode], controlModeStr[(int)modePrimary]); ESP_LOGW(TAG, "toggleModes: switching from '%s' to primary mode '%s'", controlModeStr[(int)mode], controlModeStr[(int)modePrimary]);
//buzzer->beep(4,200,100); //buzzer->beep(4,200,100);
changeMode(modePrimary); changeMode(modePrimary);
} }
@ -570,11 +598,11 @@ void controlledArmchair::loadMaxDuty(void)
switch (err) switch (err)
{ {
case ESP_OK: case ESP_OK:
ESP_LOGW(TAG, "Successfully read value '%s' from nvs. Overriding default value %.2f with %.2f", "c-maxDuty", joystickGenerateCommands_config.maxDuty, valueRead/100.0); ESP_LOGW(TAG, "Successfully read value '%s' from nvs. Overriding default value %.2f with %.2f", "c-maxDuty", joystickGenerateCommands_config.maxDutyStraight, valueRead/100.0);
joystickGenerateCommands_config.maxDuty = (float)(valueRead/100.0); joystickGenerateCommands_config.maxDutyStraight = (float)(valueRead/100.0);
break; break;
case ESP_ERR_NVS_NOT_FOUND: case ESP_ERR_NVS_NOT_FOUND:
ESP_LOGW(TAG, "nvs: the value '%s' is not initialized yet, keeping default value %.2f", "c-maxDuty", joystickGenerateCommands_config.maxDuty); ESP_LOGW(TAG, "nvs: the value '%s' is not initialized yet, keeping default value %.2f", "c-maxDuty", joystickGenerateCommands_config.maxDutyStraight);
break; break;
default: default:
ESP_LOGE(TAG, "Error (%s) reading nvs!", esp_err_to_name(err)); ESP_LOGE(TAG, "Error (%s) reading nvs!", esp_err_to_name(err));
@ -589,12 +617,12 @@ void controlledArmchair::loadMaxDuty(void)
// note: duty percentage gets stored as uint with factor 100 (to get more precision) // note: duty percentage gets stored as uint with factor 100 (to get more precision)
void controlledArmchair::writeMaxDuty(float newValue){ void controlledArmchair::writeMaxDuty(float newValue){
// check if unchanged // check if unchanged
if(joystickGenerateCommands_config.maxDuty == newValue){ if(joystickGenerateCommands_config.maxDutyStraight == newValue){
ESP_LOGW(TAG, "value unchanged at %.2f, not writing to nvs", newValue); ESP_LOGW(TAG, "value unchanged at %.2f, not writing to nvs", newValue);
return; return;
} }
// update nvs value // update nvs value
ESP_LOGW(TAG, "updating nvs value '%s' from %.2f to %.2f", "c-maxDuty", joystickGenerateCommands_config.maxDuty, newValue) ; ESP_LOGW(TAG, "updating nvs value '%s' from %.2f to %.2f", "c-maxDuty", joystickGenerateCommands_config.maxDutyStraight, newValue) ;
esp_err_t err = nvs_set_u16(*nvsHandle, "c-maxDuty", (uint16_t)(newValue*100)); esp_err_t err = nvs_set_u16(*nvsHandle, "c-maxDuty", (uint16_t)(newValue*100));
if (err != ESP_OK) if (err != ESP_OK)
ESP_LOGE(TAG, "nvs: failed writing"); ESP_LOGE(TAG, "nvs: failed writing");
@ -604,5 +632,5 @@ void controlledArmchair::writeMaxDuty(float newValue){
else else
ESP_LOGI(TAG, "nvs: successfully committed updates"); ESP_LOGI(TAG, "nvs: successfully committed updates");
// update variable // update variable
joystickGenerateCommands_config.maxDuty = newValue; joystickGenerateCommands_config.maxDutyStraight = newValue;
} }

15
board_single/main/control.hpp
View File

@ -37,7 +37,7 @@ typedef struct control_config_t {
//======================================= //=======================================
//task that controls the armchair modes and initiates commands generation and applies them to driver //task that controls the armchair modes and initiates commands generation and applies them to driver
//parameter: pointer to controlledArmchair object //parameter: pointer to controlledArmchair object
void task_control( void * pvParameters ); void task_control( void * controlledArmchair );
@ -64,7 +64,7 @@ class controlledArmchair {
); );
//--- functions --- //--- functions ---
//task that repeatedly generates motor commands depending on the current mode //endless loop that repeatedly calls handle() and handleTimeout() methods respecting mutex
void startHandleLoop(); void startHandleLoop();
//function that changes to a specified control mode //function that changes to a specified control mode
@ -94,13 +94,19 @@ class controlledArmchair {
// configure max dutycycle (in joystick or http mode) // configure max dutycycle (in joystick or http mode)
void setMaxDuty(float maxDutyNew) { writeMaxDuty(maxDutyNew); }; void setMaxDuty(float maxDutyNew) { writeMaxDuty(maxDutyNew); };
float getMaxDuty() const {return joystickGenerateCommands_config.maxDuty; }; float getMaxDuty() const {return joystickGenerateCommands_config.maxDutyStraight; };
// configure max boost (in joystick or http mode)
void setMaxRelativeBoostPer(float newValue) { joystickGenerateCommands_config.maxRelativeBoostPercentOfMaxDuty = newValue; };
float getMaxRelativeBoostPer() const {return joystickGenerateCommands_config.maxRelativeBoostPercentOfMaxDuty; };
uint32_t getInactivityDurationMs() {return esp_log_timestamp() - timestamp_lastActivity;}; uint32_t getInactivityDurationMs() {return esp_log_timestamp() - timestamp_lastActivity;};
private: private:
//--- functions --- //--- functions ---
//generate motor commands or run actions depending on the current mode
void handle();
//function that evaluates whether there is no activity/change on the motor duty for a certain time, if so a switch to IDLE is issued. - has to be run repeatedly in a slow interval //function that evaluates whether there is no activity/change on the motor duty for a certain time, if so a switch to IDLE is issued. - has to be run repeatedly in a slow interval
void handleTimeout(); void handleTimeout();
@ -146,6 +152,9 @@ class controlledArmchair {
//struct with config parameters //struct with config parameters
control_config_t config; control_config_t config;
//mutex to prevent race condition between handle() and changeMode()
SemaphoreHandle_t handleIteration_mutex;
//store joystick data //store joystick data
joystickData_t stickData = joystickData_center; joystickData_t stickData = joystickData_center;
joystickData_t stickDataLast = joystickData_center; joystickData_t stickDataLast = joystickData_center;

10
board_single/main/main.cpp
View File

@ -140,16 +140,16 @@ void createObjects()
// with configuration above // with configuration above
//sabertoothDriver = new sabertooth2x60a(sabertoothConfig); //sabertoothDriver = new sabertooth2x60a(sabertoothConfig);
// create controlled motor instances (motorctl.hpp)
// with configurations from config.cpp
motorLeft = new controlledMotor(setLeftFunc, configMotorControlLeft, &nvsHandle);
motorRight = new controlledMotor(setRightFunc, configMotorControlRight, &nvsHandle);
// create speedsensor instances // create speedsensor instances
// with configurations from config.cpp // with configurations from config.cpp
speedLeft = new speedSensor(speedLeft_config); speedLeft = new speedSensor(speedLeft_config);
speedRight = new speedSensor(speedRight_config); speedRight = new speedSensor(speedRight_config);
// create controlled motor instances (motorctl.hpp)
// with configurations from config.cpp
motorLeft = new controlledMotor(setLeftFunc, configMotorControlLeft, &nvsHandle, speedLeft, &motorRight); //note: ptr to ptr of controlledMotor since it isnt defined yet
motorRight = new controlledMotor(setRightFunc, configMotorControlRight, &nvsHandle, speedRight, &motorLeft);
// create joystick instance (joystick.hpp) // create joystick instance (joystick.hpp)
joystick = new evaluatedJoystick(configJoystick, &nvsHandle); joystick = new evaluatedJoystick(configJoystick, &nvsHandle);

111
board_single/main/menu.cpp
View File

@ -276,6 +276,34 @@ menuItem_t item_maxDuty = {
}; };
//##################################
//##### set max relative boost #####
//##################################
void maxRelativeBoost_action(display_task_parameters_t * objects, SSD1306_t * display, int value)
{
objects->control->setMaxRelativeBoostPer(value);
}
int maxRelativeBoost_currentValue(display_task_parameters_t * objects)
{
return (int)objects->control->getMaxRelativeBoostPer();
}
menuItem_t item_maxRelativeBoost = {
maxRelativeBoost_action, // function action
maxRelativeBoost_currentValue, // function get initial value or NULL(show in line 2)
NULL, // function get default value or NULL(dont set value, show msg)
0, // valueMin
150, // valueMax
1, // valueIncrement
"Set max Boost ", // title
"Set max Boost % ", // line1 (above value)
"for outer tire ", // line2 (above value)
"", // line4 * (below value)
"", // line5 *
" % of max duty ", // line6
"added on turning", // line7
};
//###################### //######################
//##### accelLimit ##### //##### accelLimit #####
//###################### //######################
@ -342,6 +370,85 @@ 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 #####
//###################################
void tractionControlSystem_action(display_task_parameters_t * objects, SSD1306_t * display, int value)
{
if (value == 1){
objects->motorLeft->enableTractionControlSystem();
objects->motorRight->enableTractionControlSystem();
ESP_LOGW(TAG, "enabled Traction Control System");
} else {
objects->motorLeft->disableTractionControlSystem();
objects->motorRight->disableTractionControlSystem();
ESP_LOGW(TAG, "disabled Traction Control System");
}
}
int tractionControlSystem_currentValue(display_task_parameters_t * objects)
{
return (int)objects->motorLeft->getTractionControlSystemStatus();
}
menuItem_t item_tractionControlSystem = {
tractionControlSystem_action, // function action
tractionControlSystem_currentValue, // function get initial value or NULL(show in line 2)
NULL, // function get default value or NULL(dont set value, show msg)
0, // valueMin
1, // valueMax
1, // valueIncrement
"TCS / ASR ", // title
"Traction Control", // line1 (above value)
" System ", // line2 (above value)
"1: enable ", // line4 * (below value)
"0: disable ", // line5 *
"note: requires ", // line6
"speed ctl-mode ", // line7
};
//##################### //#####################
//####### RESET ####### //####### RESET #######
//##################### //#####################
@ -471,8 +578,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_maxDuty, item_accelLimit, item_decelLimit, item_statusScreen, item_reset, item_example, item_last}; const menuItem_t menuItems[] = {item_centerJoystick, item_calibrateJoystick, item_debugJoystick, item_statusScreen, item_maxDuty, item_maxRelativeBoost, item_accelLimit, item_decelLimit, item_motorControlMode, item_tractionControlSystem, item_reset, item_example, item_last};
const int itemCount = 8; const int itemCount = 11;

2
board_single/sdkconfig
View File

@ -1252,7 +1252,7 @@ CONFIG_WPA_MBEDTLS_CRYPTO=y
# #
CONFIG_RE_MAX=1 CONFIG_RE_MAX=1
CONFIG_RE_INTERVAL_US=1000 CONFIG_RE_INTERVAL_US=1000
CONFIG_RE_BTN_DEAD_TIME_US=10000 CONFIG_RE_BTN_DEAD_TIME_US=40000
CONFIG_RE_BTN_PRESSED_LEVEL_0=y CONFIG_RE_BTN_PRESSED_LEVEL_0=y
# CONFIG_RE_BTN_PRESSED_LEVEL_1 is not set # CONFIG_RE_BTN_PRESSED_LEVEL_1 is not set
CONFIG_RE_BTN_LONG_PRESS_TIME_US=500000 CONFIG_RE_BTN_LONG_PRESS_TIME_US=500000

85
common/joystick.cpp
View File

@ -306,30 +306,38 @@ joystickPos_t joystick_evaluatePosition(float x, float y){
//function that generates commands for both motors from the joystick data //function that generates commands for both motors from the joystick data
motorCommands_t joystick_generateCommandsDriving(joystickData_t data, joystickGenerateCommands_config_t * config){ motorCommands_t joystick_generateCommandsDriving(joystickData_t data, joystickGenerateCommands_config_t * config){
//struct with current data of the joystick //--- interpret config parameters ---
//typedef struct joystickData_t { float dutyOffset = config->dutyOffset; // immediately starts with this duty
// joystickPos_t position; float dutyRange = config->maxDutyStraight - config->dutyOffset; //duty at max radius
// float x; // calculate configured boost duty (added when turning)
// float y; float dutyBoost = config->maxDutyStraight * config->maxRelativeBoostPercentOfMaxDuty/100;
// float radius; // limit to maximum possible duty
// float angle; float dutyAvailable = 100 - config->maxDutyStraight;
//} joystickData_t; if (dutyBoost > dutyAvailable) dutyBoost = dutyAvailable;
//--- variables ---
motorCommands_t commands;
float dutyOffset = 5; //immediately starts with this duty, TODO add this to config
float dutyRange = config->maxDuty - config->dutyOffset;
float ratio = fabs(data.angle) / 90; //90degree = x=0 || 0degree = y=0
//--- snap ratio to max at angle threshold ---
//(-> more joystick area where inner wheel is off when turning) //--- calculate paramaters with current data ---
/* motorCommands_t commands; // store new motor commands
//FIXME works, but armchair unsusable because of current bug with motor driver (inner motor freezes after turn)
float ratioClipThreshold = 0.3; // -- calculate ratio --
if (ratio < ratioClipThreshold) ratio = 0; // get current ratio from stick angle
else if (ratio > 1-ratioClipThreshold) ratio = 1; float ratioActual = fabs(data.angle) / 90; //x=0 -> 90deg -> ratio=1 || y=0 -> 0deg -> ratio=0
//TODO subtract this clip threshold from available joystick range at ratio usage ratioActual = 1 - ratioActual; // invert ratio
*/ // scale and clip ratio according to configured tolerance
// to have some joystick area at max ratio before reaching X-Axis-full-turn-mode
float ratio = ratioActual / (config->ratioSnapToOneThreshold); //0->0 threshold->1
// limit to 1 when above threshold (inside area max ratio)
if (ratio > 1) ratio = 1; // >threshold -> 1
// -- calculate outer tire boost --
#define BOOST_RATIO_MANIPULATION_SCALE 1.05 // >1 to apply boost slightly faster, this slightly compensates that available boost is most times less than reduction of inner duty, so for small turns the total speed feels more equal
float boostAmountOuter = data.radius*dutyBoost* ratio *BOOST_RATIO_MANIPULATION_SCALE;
// limit to max amount
if (boostAmountOuter > dutyBoost) boostAmountOuter = dutyBoost;
// -- calculate inner tire reduction --
float reductionAmountInner = (data.radius * dutyRange + dutyOffset) * ratio;
//--- experimental alternative control mode --- //--- experimental alternative control mode ---
if (config->altStickMapping == true){ if (config->altStickMapping == true){
@ -380,36 +388,43 @@ motorCommands_t joystick_generateCommandsDriving(joystickData_t data, joystickGe
case joystickPos_t::TOP_RIGHT: case joystickPos_t::TOP_RIGHT:
commands.left.state = motorstate_t::FWD; commands.left.state = motorstate_t::FWD;
commands.right.state = motorstate_t::FWD; commands.right.state = motorstate_t::FWD;
commands.left.duty = data.radius * dutyRange + dutyOffset; commands.left.duty = data.radius * dutyRange + boostAmountOuter + dutyOffset;
commands.right.duty = data.radius * dutyRange - (data.radius*dutyRange + dutyOffset)*(1-ratio) + dutyOffset; commands.right.duty = data.radius * dutyRange - reductionAmountInner + dutyOffset;
break; break;
case joystickPos_t::TOP_LEFT: case joystickPos_t::TOP_LEFT:
commands.left.state = motorstate_t::FWD; commands.left.state = motorstate_t::FWD;
commands.right.state = motorstate_t::FWD; commands.right.state = motorstate_t::FWD;
commands.left.duty = data.radius * dutyRange - (data.radius*dutyRange + dutyOffset)*(1-ratio) + dutyOffset; commands.left.duty = data.radius * dutyRange - reductionAmountInner + dutyOffset;
commands.right.duty = data.radius * dutyRange + dutyOffset; commands.right.duty = data.radius * dutyRange + boostAmountOuter + dutyOffset;
break; break;
case joystickPos_t::BOTTOM_LEFT: case joystickPos_t::BOTTOM_LEFT:
commands.left.state = motorstate_t::REV; commands.left.state = motorstate_t::REV;
commands.right.state = motorstate_t::REV; commands.right.state = motorstate_t::REV;
commands.left.duty = data.radius * dutyRange + dutyOffset; commands.left.duty = data.radius * dutyRange + boostAmountOuter + dutyOffset;
commands.right.duty = data.radius * dutyRange - (data.radius*dutyRange + dutyOffset)*(1-ratio) + dutyOffset; commands.right.duty = data.radius * dutyRange - reductionAmountInner + dutyOffset;
break; break;
case joystickPos_t::BOTTOM_RIGHT: case joystickPos_t::BOTTOM_RIGHT:
commands.left.state = motorstate_t::REV; commands.left.state = motorstate_t::REV;
commands.right.state = motorstate_t::REV; commands.right.state = motorstate_t::REV;
commands.left.duty = data.radius * dutyRange - (data.radius*dutyRange + dutyOffset)*(1-ratio) + dutyOffset; commands.left.duty = data.radius * dutyRange - reductionAmountInner + dutyOffset;
commands.right.duty = data.radius * dutyRange + dutyOffset; commands.right.duty = data.radius * dutyRange + boostAmountOuter + dutyOffset;
break; break;
} }
ESP_LOGI(TAG_CMD, "generated commands from data: state=%s, angle=%.3f, ratio=%.3f/%.3f, radius=%.2f, x=%.2f, y=%.2f", // log input data
joystickPosStr[(int)data.position], data.angle, ratio, (1-ratio), data.radius, data.x, data.y); ESP_LOGD(TAG_CMD, "in: pos='%s', angle=%.3f, ratioActual/Scaled=%.2f/%.2f, r=%.2f, x=%.2f, y=%.2f",
ESP_LOGI(TAG_CMD, "motor left: state=%s, duty=%.3f", motorstateStr[(int)commands.left.state], commands.left.duty); joystickPosStr[(int)data.position], data.angle, ratioActual, ratio, data.radius, data.x, data.y);
ESP_LOGI(TAG_CMD, "motor right: state=%s, duty=%.3f", motorstateStr[(int)commands.right.state], commands.right.duty); // log generation details
ESP_LOGI(TAG_CMD, "left=%.2f, right=%.2f -- BoostOuter=%.1f, ReductionInner=%.1f, maxDuty=%.0f, maxBoost=%.0f, dutyOffset=%.0f",
commands.left.duty, commands.right.duty,
boostAmountOuter, reductionAmountInner,
config->maxDutyStraight, dutyBoost, dutyOffset);
// log generated motor commands
ESP_LOGD(TAG_CMD, "motor left: state=%s, duty=%.3f", motorstateStr[(int)commands.left.state], commands.left.duty);
ESP_LOGD(TAG_CMD, "motor right: state=%s, duty=%.3f", motorstateStr[(int)commands.right.state], commands.right.duty);
return commands; return commands;
} }

14
common/joystick.hpp
View File

@ -69,15 +69,17 @@ typedef struct joystickData_t {
float angle; float angle;
} joystickData_t; } joystickData_t;
// struct with parameters provided to joystick_GenerateCommandsDriving() // struct with parameters provided to joystick_GenerateCommandsDriving()
typedef struct joystickGenerateCommands_config_t { typedef struct joystickGenerateCommands_config_t
float maxDuty; {
float dutyOffset; float maxDutyStraight; // max duty applied when driving with ratio=1 (when turning it might increase by Boost)
bool altStickMapping; float maxRelativeBoostPercentOfMaxDuty; // max duty percent added to outer tire when turning (max actual is 100-maxDutyStraight) - set 0 to disable
// note: to be able to reduce the overall driving speed boost has to be limited as well otherwise outer tire when turning would always be 100% no matter of maxDuty
float dutyOffset; // motors immediately start with this duty (duty movement starts)
float ratioSnapToOneThreshold; // have some area around X-Axis where inner tire is completely off - set 1 to disable
bool altStickMapping; // swap reverse direction
} joystickGenerateCommands_config_t; } joystickGenerateCommands_config_t;
//------------------------------------ //------------------------------------
//----- evaluatedJoystick class ----- //----- evaluatedJoystick class -----
//------------------------------------ //------------------------------------

373
common/motorctl.cpp
View File

@ -28,14 +28,20 @@ void task_motorctl( void * ptrControlledMotor ){
//======== constructor ======== //======== constructor ========
//============================= //=============================
//constructor, simultaniously initialize instance of motor driver 'motor' and current sensor 'cSensor' with provided config (see below lines after ':') //constructor, simultaniously initialize instance of motor driver 'motor' and current sensor 'cSensor' with provided config (see below lines after ':')
controlledMotor::controlledMotor(motorSetCommandFunc_t setCommandFunc, motorctl_config_t config_control, nvs_handle_t * nvsHandle_f): controlledMotor::controlledMotor(motorSetCommandFunc_t setCommandFunc, motorctl_config_t config_control, nvs_handle_t * nvsHandle_f, speedSensor * speedSensor_f, controlledMotor ** otherMotor_f):
//create current sensor
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
nvsHandle = nvsHandle_f; nvsHandle = nvsHandle_f;
//pointer to other motor object
ppOtherMotor = otherMotor_f;
//pointer to speed sensor
sSensor = speedSensor_f;
//create queue, initialize config values //create queue, initialize config values
init(); init();
@ -105,61 +111,185 @@ void controlledMotor::handle(){
//TODO: History: skip fading when motor was running fast recently / alternatively add rot-speed sensor //TODO: History: skip fading when motor was running fast recently / alternatively add rot-speed sensor
//--- receive commands from queue --- //--- RECEIVE 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;
timestamp_commandReceived = esp_log_timestamp(); timestamp_commandReceived = esp_log_timestamp();
//--- convert duty ---
//define target duty (-100 to 100) from provided duty and motorstate
//this value is more suitable for the fading algorithm
switch(commandReceive.state){
case motorstate_t::BRAKE:
//update state
state = motorstate_t::BRAKE;
//dutyTarget = 0;
dutyTarget = fabs(commandReceive.duty);
break;
case motorstate_t::IDLE:
dutyTarget = 0;
break;
case motorstate_t::FWD:
dutyTarget = fabs(commandReceive.duty);
break;
case motorstate_t::REV:
dutyTarget = - fabs(commandReceive.duty);
break;
}
} }
//--- 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)
// ----- EXPERIMENTAL, DIFFERENT MODES -----
// define target duty differently depending on current contro-mode
//declare variables used inside switch
float ampereNow, ampereTarget, ampereDiff;
float speedDiff;
switch (mode)
{ {
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); case motorControlMode_t::DUTY: // regulate to desired duty (as originally)
receiveTimeout = true; //--- convert duty ---
state = motorstate_t::IDLE; // define target duty (-100 to 100) from provided duty and motorstate
dutyTarget = 0; // this value is more suitable for t
// todo scale target input with DUTY-MAX here instead of in joysick cmd generationhe fading algorithm
switch (commandReceive.state)
{
case motorstate_t::BRAKE:
// update state
state = motorstate_t::BRAKE;
// dutyTarget = 0;
dutyTarget = fabs(commandReceive.duty);
break;
case motorstate_t::IDLE:
dutyTarget = 0;
break;
case motorstate_t::FWD:
dutyTarget = fabs(commandReceive.duty);
break;
case motorstate_t::REV:
dutyTarget = -fabs(commandReceive.duty);
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
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;
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::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::FWD) // backward need to increase current (more negative)
{
dutyTarget = -100;
}
else // fwd too much, rev too much -> decrease
{
dutyTarget = 0;
}
if(log) ESP_LOGV("TESTING", "[%s] CURRENT-CONTROL: set target to %.0f%%", config.name, dutyTarget);
}
else
{
dutyTarget = dutyNow; // target current reached
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;
} 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::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::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);
}
}
else
{
dutyTarget = dutyNow; // target speed reached
if(log) ESP_LOGD("TESTING", "[%s] SPEED-CONTROL: target speed %.3f reached", config.name, speedTarget);
}
break;
} }
//--- calculate difference ---
dutyDelta = dutyTarget - dutyNow;
//--- 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(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;
// set target and last command to IDLE
state = motorstate_t::IDLE;
commandReceive.state = motorstate_t::IDLE;
dutyTarget = 0; // todo put this in else section of queue (no data received) and add control mode "timeout"?
commandReceive.duty = 0;
}
//--- CALCULATE DUTY-DIFF ---
dutyDelta = dutyTarget - dutyNow;
//positive: need to increase by that value //positive: need to increase by that value
//negative: need to decrease //negative: need to decrease
//--- 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) || (dutyTarget == 0 && dutyNow == 0)) //when current limit 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 timeout once 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
@ -168,44 +298,43 @@ void controlledMotor::handle(){
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
} }
//TODO skip rest of the handle function below using return? Some regular driver updates sound useful though //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;
if (msFadeAccel > 0){
dutyIncrementAccel = ( usPassed / ((float)msFadeAccel * 1000) ) * 100; //TODO define maximum increment - first run after startup (or long) pause can cause a very large increment
} else {
dutyIncrementAccel = 100;
}
//calculate increment for fading DOWN with passed time since last run and configured fade time
if (msFadeDecel > 0){
dutyIncrementDecel = ( usPassed / ((float)msFadeDecel * 1000) ) * 100;
} else {
dutyIncrementDecel = 100;
}
//--- 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
} }
//----- FADING ----- //----- FADING -----
//calculate passed time since last run
int64_t usPassed = esp_timer_get_time() - timestampLastRunUs;
//--- calculate increment ---
//calculate increment for fading UP with passed time since last run and configured fade time
if (tcs_isExceeded) // disable acceleration when slippage is currently detected
dutyIncrementAccel = 0;
else if (msFadeAccel > 0)
dutyIncrementAccel = (usPassed / ((float)msFadeAccel * 1000)) * 100; // TODO define maximum increment - first run after startup (or long) pause can cause a very large increment
else //no accel limit (immediately set to 100)
dutyIncrementAccel = 100;
//calculate increment for fading DOWN with passed time since last run and configured fade time
if (msFadeDecel > 0)
dutyIncrementDecel = ( usPassed / ((float)msFadeDecel * 1000) ) * 100;
else //no decel limit (immediately reduce to 0)
dutyIncrementDecel = 100;
//fade duty to target (up and down) //fade duty to target (up and down)
//TODO: this needs optimization (can be more clear and/or simpler) //TODO: this needs optimization (can be more clear and/or simpler)
if (dutyDelta > 0) { //difference positive -> increasing duty (-100 -> 100) if (dutyDelta > 0) { //difference positive -> increasing duty (-100 -> 100)
@ -226,7 +355,7 @@ void controlledMotor::handle(){
} }
//----- CURRENT LIMIT ----- //----- CURRENT LIMIT -----
currentNow = cSensor.read(); currentNow = cSensor.read();
if ((config.currentLimitEnabled) && (dutyDelta != 0)){ if ((config.currentLimitEnabled) && (dutyDelta != 0)){
if (fabs(currentNow) > config.currentMax){ if (fabs(currentNow) > config.currentMax){
@ -240,11 +369,89 @@ void controlledMotor::handle(){
} 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);
} }
} }
//----- TRACTION CONTROL -----
//reduce duty when turning faster than expected
//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
#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
//calculate time passed since last run
uint32_t tcs_usPassed = esp_timer_get_time() - tcs_timestampLastRun; // passed time since last time handled
tcs_timestampLastRun = esp_timer_get_time();
//get motor stats
float speedNowThis = sSensor->getKmph();
float speedNowOther = (*ppOtherMotor)->getCurrentSpeed();
float speedTargetThis = speedTarget;
float speedTargetOther = (*ppOtherMotor)->getTargetSpeed();
float dutyTargetOther = (*ppOtherMotor)->getTargetDuty();
float dutyTargetThis = dutyTarget;
float dutyNowOther = (*ppOtherMotor)->getDuty();
float dutyNowThis = dutyNow;
//calculate expected ratio
float ratioSpeedTarget = speedTargetThis / speedTargetOther;
//calculate current ratio of actual measured rotational speed
float ratioSpeedNow = speedNowThis / speedNowOther;
//calculate current duty ration (logging only)
float ratioDutyNow = dutyNowThis / dutyNowOther;
//calculate unexpected difference
float ratioDiff = ratioSpeedNow - ratioSpeedTarget;
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 (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)
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
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
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
}
}
else
{ // not exceeded
tcs_isExceeded = false;
tcs_usExceeded = 0;
}
}
else // TCS mode not active or timed out
{ // not exceeded
tcs_isExceeded = false;
tcs_usExceeded = 0;
}
//--- define new motorstate --- (-100 to 100 => direction) //--- define new motorstate --- (-100 to 100 => direction)
state=getStateFromDuty(dutyNow); state=getStateFromDuty(dutyNow);
@ -254,25 +461,27 @@ void controlledMotor::handle(){
//FWD -> IDLE -> FWD continue without waiting //FWD -> IDLE -> FWD continue without waiting
//FWD -> IDLE -> REV wait for dead-time in IDLE //FWD -> IDLE -> REV wait for dead-time in IDLE
//TODO check when changed only? //TODO check when changed only?
if ( //not enough time between last direction state if (config.deadTimeMs > 0) { //deadTime is enabled
( state == motorstate_t::FWD && (esp_log_timestamp() - timestampsModeLastActive[(int)motorstate_t::REV] < config.deadTimeMs)) if ( //not enough time between last direction state
|| (state == motorstate_t::REV && (esp_log_timestamp() - timestampsModeLastActive[(int)motorstate_t::FWD] < 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))
ESP_LOGD(TAG, "waiting dead-time... dir change %s -> %s", motorstateStr[(int)statePrev], motorstateStr[(int)state]); ){
if (!deadTimeWaiting){ //log start if(log) ESP_LOGD(TAG, "waiting dead-time... dir change %s -> %s", motorstateStr[(int)statePrev], motorstateStr[(int)state]);
deadTimeWaiting = true; if (!deadTimeWaiting){ //log start
ESP_LOGI(TAG, "starting dead-time... %s -> %s", motorstateStr[(int)statePrev], motorstateStr[(int)state]); deadTimeWaiting = true;
} 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; //force IDLE state during wait
dutyNow = 0; state = motorstate_t::IDLE;
} else { dutyNow = 0;
if (deadTimeWaiting){ //log end } else {
deadTimeWaiting = false; if (deadTimeWaiting){ //log end
ESP_LOGI(TAG, "dead-time ended - continue with %s", motorstateStr[(int)state]); deadTimeWaiting = false;
} 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);
}
}
//--- save current actual motorstate and timestamp --- //--- save current actual motorstate and timestamp ---
@ -284,7 +493,7 @@ void controlledMotor::handle(){
//--- 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
@ -300,11 +509,11 @@ void controlledMotor::handle(){
//function to set the target mode and duty of a motor //function to set the target mode and duty of a motor
//puts the provided command in a queue for the handle function running in another task //puts the provided command in a queue for the handle function running in another task
void controlledMotor::setTarget(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:

34
common/motorctl.hpp
View File

@ -13,6 +13,7 @@ extern "C"
#include "motordrivers.hpp" #include "motordrivers.hpp"
#include "currentsensor.hpp" #include "currentsensor.hpp"
#include "speedsensor.hpp"
//======================================= //=======================================
@ -23,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 ======
@ -30,11 +32,20 @@ typedef void (*motorSetCommandFunc_t)(motorCommand_t cmd);
class controlledMotor { class controlledMotor {
public: public:
//--- functions --- //--- functions ---
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 //TODO move speedsensor object creation in this class to (pass through / wrap methods)
controlledMotor(motorSetCommandFunc_t setCommandFunc, motorctl_config_t config_control, nvs_handle_t * nvsHandle, speedSensor * speedSensor, controlledMotor ** otherMotor); //constructor with structs for configuring motordriver and parameters for control TODO: add configuration for currentsensor
void handle(); //controls motor duty with fade and current limiting feature (has to be run frequently by another task) void handle(); //controls motor duty with fade and current limiting feature (has to be run frequently by another task)
void setTarget(motorstate_t state_f, float duty_f = 0); //adds target command to queue for handle function void setTarget(motorstate_t state_f, float duty_f = 0); //adds target command to queue for handle function
void setTarget(motorCommand_t command); void setTarget(motorCommand_t command);
motorCommand_t getStatus(); //get current status of the motor (returns struct with state and duty) motorCommand_t getStatus(); //get current status of the motor (returns struct with state and duty)
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 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
@ -60,6 +71,11 @@ class controlledMotor {
QueueHandle_t commandQueue = NULL; QueueHandle_t commandQueue = NULL;
//current sensor //current sensor
currentSensor cSensor; currentSensor cSensor;
//speed sensor
speedSensor * sSensor;
//other motor (needed for traction control)
controlledMotor ** ppOtherMotor; //ptr to ptr of controlledMotor (because not created at initialization yet)
//function pointer that sets motor duty (driver) //function pointer that sets motor duty (driver)
motorSetCommandFunc_t motorSetCommand; motorSetCommandFunc_t motorSetCommand;
@ -68,15 +84,24 @@ 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; //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;
float currentMax; float currentMax;
float currentNow; float currentNow;
//speed mode
float speedTarget = 0;
float speedNow = 0;
uint32_t timestamp_speedLastUpdate = 0;
float dutyTarget = 0; float dutyTarget = 0;
float dutyNow = 0; float dutyNow = 0;
float dutyIncrementAccel; float dutyIncrementAccel;
float dutyIncrementDecel; float dutyIncrementDecel;
float dutyDelta; float dutyDelta;
@ -97,6 +122,13 @@ class controlledMotor {
uint32_t timestamp_commandReceived = 0; uint32_t timestamp_commandReceived = 0;
bool receiveTimeout = false; bool receiveTimeout = false;
//traction control system
uint32_t tcs_timestampLastSpeedUpdate = 0; //track speedsensor update
int64_t tcs_timestampBeginExceeded = 0; //track start of event
uint32_t tcs_usExceeded = 0; //sum up time
bool tcs_isExceeded = false; //is currently too fast
int64_t tcs_timestampLastRun = 0;
}; };
//==================================== //====================================

1
common/speedsensor.cpp
View File

@ -93,6 +93,7 @@ void IRAM_ATTR onEncoderRising(void *arg)
// calculate rotational speed // calculate rotational speed
uint64_t pulseSum = pulse1 + pulse2 + pulse3; uint64_t pulseSum = pulse1 + pulse2 + pulse3;
sensor->currentRpm = direction * (sensor->config.degreePerGroup / 360.0 * 60.0 / ((double)pulseSum / 1000000.0)); sensor->currentRpm = direction * (sensor->config.degreePerGroup / 360.0 * 60.0 / ((double)pulseSum / 1000000.0));
sensor->timeLastUpdate = currentTime;
} }
} }

2
common/speedsensor.hpp
View File

@ -33,6 +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
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;
@ -45,6 +46,7 @@ public:
int debugCount = 0; int debugCount = 0;
uint32_t debug_countIgnoredSequencesTooShort = 0; uint32_t debug_countIgnoredSequencesTooShort = 0;
double currentRpm = 0; double currentRpm = 0;
uint32_t timeLastUpdate = 0;
private: private:
static bool isrIsInitialized; // default false due to static static bool isrIsInitialized; // default false due to static

2
common/types.hpp
View File

@ -42,9 +42,11 @@ 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;
bool tractionControlSystemEnabled;
adc1_channel_t currentSensor_adc; adc1_channel_t currentSensor_adc;
float currentSensor_ratedCurrent; float currentSensor_ratedCurrent;
float currentMax; float currentMax;