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)
{
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);
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
// and takes the corresponding action
// 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()
{
//-- variables --

27
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("menu", 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) ---
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,
.tractionControlSystemEnabled = false,
.currentSensor_adc = ADC1_CHANNEL_4, // GPIO32
.currentSensor_ratedCurrent = 50,
.currentMax = 30,
@ -105,9 +114,11 @@ 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,
.tractionControlSystemEnabled = false,
.currentSensor_adc = ADC1_CHANNEL_5, // GPIO33
.currentSensor_ratedCurrent = 50,
.currentMax = 30,
@ -240,7 +251,19 @@ rotary_encoder_t encoder_config = {
//-----------------------------------
//configure parameters for motor command generation from joystick data
joystickGenerateCommands_config_t joystickGenerateCommands_config{
.maxDuty = 100,
//-- maxDuty --
// max duty when both motors are at equal ratio e.g. driving straight forward
// 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
.altStickMapping = false,
.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
};

136
board_single/main/control.cpp
View File

@ -59,6 +59,9 @@ controlledArmchair::controlledArmchair(
// override default config value if maxDuty is found in nvs
loadMaxDuty();
// create semaphore for preventing race condition: mode-change operations while currently still executing certain mode
handleIteration_mutex = xSemaphoreCreateMutex();
}
@ -75,25 +78,59 @@ void task_control( void * pvParameters ){
}
//----------------------------------
//---------- Handle loop -----------
//----------------------------------
//function that repeatedly generates motor commands depending on the current mode
void controlledArmchair::startHandleLoop() {
while (1){
ESP_LOGV(TAG, "control loop executing... mode=%s", controlModeStr[(int)mode]);
// start endless loop that repeatedly calls handle() and handleTimeout() methods
void controlledArmchair::startHandleLoop()
{
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);
} // end mutex
//--- slow loop ---
// this section is run approx 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();
//--- handle timeouts ---
// run function that detects timeouts (switch to idle, or notify "forgot to turn off")
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
}
}
//-------------------------------------
//---------- Handle control -----------
//-------------------------------------
// function that repeatedly generates motor commands and runs actions depending on the current mode
void controlledArmchair::handle()
{
switch (mode)
{
default:
mode = controlMode_t::IDLE;
//switch to IDLE mode when current mode is not implemented
changeMode(controlMode_t::IDLE);
break;
//------- handle IDLE -------
case controlMode_t::IDLE:
//copy preset commands for idling both motors - now done once at mode change
//commands = cmds_bothMotorsIdle;
//motorRight->setTarget(commands.right.state, commands.right.duty);
//motorLeft->setTarget(commands.left.state, commands.left.duty);
vTaskDelay(500 / portTICK_PERIOD_MS);
// 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();
@ -103,6 +140,7 @@ void controlledArmchair::startHandleLoop() {
objects->joystick->getRawX(), objects->joystick->getRawY());
#endif
break;
//------- handle JOYSTICK mode -------
case controlMode_t::JOYSTICK:
vTaskDelay(50 / portTICK_PERIOD_MS);
@ -110,7 +148,7 @@ void controlledArmchair::startHandleLoop() {
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
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)
@ -128,7 +166,6 @@ void controlledArmchair::startHandleLoop() {
}
break;
//------- handle MASSAGE mode -------
case controlMode_t::MASSAGE:
vTaskDelay(10 / portTICK_PERIOD_MS);
@ -137,20 +174,17 @@ void controlledArmchair::startHandleLoop() {
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)
// 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 ---
@ -161,7 +195,8 @@ void controlledArmchair::startHandleLoop() {
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 ){
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);
@ -176,7 +211,6 @@ void controlledArmchair::startHandleLoop() {
}
break;
//------- handle AUTO mode -------
case controlMode_t::AUTO:
vTaskDelay(20 / portTICK_PERIOD_MS);
@ -187,7 +221,8 @@ void controlledArmchair::startHandleLoop() {
motorLeft->setTarget(commands.left);
// process received instruction
switch (instruction) {
switch (instruction)
{
case auto_instruction_t::NONE:
break;
case auto_instruction_t::SWITCH_PREV_MODE:
@ -217,7 +252,6 @@ void controlledArmchair::startHandleLoop() {
}
break;
//------- handle ADJUST_CHAIR mode -------
case controlMode_t::ADJUST_CHAIR:
vTaskDelay(100 / portTICK_PERIOD_MS);
@ -233,7 +267,6 @@ void controlledArmchair::startHandleLoop() {
}
break;
//------- handle MENU mode -------
case controlMode_t::MENU:
// nothing to do here, display task handles the menu
@ -243,20 +276,7 @@ void controlledArmchair::startHandleLoop() {
// 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();
//run function that detects timeout (switch to idle, or notify "forgot to turn off")
handleTimeout();
}
}//end while(1)
}//end startHandleLoop
} // end - handle method
@ -383,14 +403,21 @@ void controlledArmchair::handleTimeout()
//----------- changeMode ------------
//-----------------------------------
//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
if (mode == modeNew) {
if (mode == modeNew)
{
ESP_LOGE(TAG, "changeMode: Already in target mode '%s' -> nothing to change", controlModeStr[(int)mode]);
return;
}
// mutex to wait for current handle iteration (control-task) to finish
// prevents race conditions where operations when changing mode are run but old mode gets handled still
ESP_LOGI(TAG, "changeMode: waiting for current handle() iteration to finish...");
if (xSemaphoreTake(handleIteration_mutex, portMAX_DELAY) == pdTRUE)
{
// copy previous mode
modePrevious = mode;
// store time changed (needed for timeout)
@ -400,7 +427,8 @@ void controlledArmchair::changeMode(controlMode_t modeNew) {
//========== commands change FROM mode ==========
// run functions when changing FROM certain mode
switch(modePrevious){
switch (modePrevious)
{
default:
ESP_LOGI(TAG, "noting to execute when changing FROM this mode");
break;
@ -448,13 +476,12 @@ void controlledArmchair::changeMode(controlMode_t modeNew) {
legRest->setState(REST_OFF);
backRest->setState(REST_OFF);
break;
}
//========== commands change TO mode ==========
// run functions when changing TO certain mode
switch(modeNew){
switch (modeNew)
{
default:
ESP_LOGI(TAG, "noting to execute when changing TO this mode");
break;
@ -491,14 +518,15 @@ void controlledArmchair::changeMode(controlMode_t modeNew) {
motorLeft->setFade(fadeType_t::ACCEL, shake_msFadeAccel);
motorRight->setFade(fadeType_t::ACCEL, shake_msFadeAccel);
break;
}
//--- update mode to new mode ---
//TODO: add mutex
mode = modeNew;
}
// unlock mutex for control task to continue handling modes
xSemaphoreGive(handleIteration_mutex);
} // end mutex
}
//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
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);
changeMode(modePrimary);
}
@ -542,13 +570,13 @@ void controlledArmchair::toggleMode(controlMode_t modePrimary){
//switch to previous mode when primary is already active
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);
changeMode(modePrevious); //switch to previous mode
}
//switch to primary mode when any other mode is active
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);
changeMode(modePrimary);
}
@ -570,11 +598,11 @@ void controlledArmchair::loadMaxDuty(void)
switch (err)
{
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);
joystickGenerateCommands_config.maxDuty = (float)(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.maxDutyStraight = (float)(valueRead/100.0);
break;
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;
default:
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)
void controlledArmchair::writeMaxDuty(float newValue){
// 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);
return;
}
// 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));
if (err != ESP_OK)
ESP_LOGE(TAG, "nvs: failed writing");
@ -604,5 +632,5 @@ void controlledArmchair::writeMaxDuty(float newValue){
else
ESP_LOGI(TAG, "nvs: successfully committed updates");
// 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
//parameter: pointer to controlledArmchair object
void task_control( void * pvParameters );
void task_control( void * controlledArmchair );
@ -64,7 +64,7 @@ class controlledArmchair {
);
//--- 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();
//function that changes to a specified control mode
@ -94,13 +94,19 @@ class controlledArmchair {
// configure max dutycycle (in joystick or http mode)
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;};
private:
//--- 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
void handleTimeout();
@ -146,6 +152,9 @@ class controlledArmchair {
//struct with config parameters
control_config_t config;
//mutex to prevent race condition between handle() and changeMode()
SemaphoreHandle_t handleIteration_mutex;
//store joystick data
joystickData_t stickData = joystickData_center;
joystickData_t stickDataLast = joystickData_center;

10
board_single/main/main.cpp
View File

@ -140,16 +140,16 @@ void createObjects()
// with configuration above
//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
// with configurations from config.cpp
speedLeft = new speedSensor(speedLeft_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)
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 #####
//######################
@ -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 #######
//#####################
@ -471,8 +578,8 @@ menuItem_t item_last = {
//####################################################
//### 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 int itemCount = 8;
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 = 11;

2
board_single/sdkconfig
View File

@ -1252,7 +1252,7 @@ CONFIG_WPA_MBEDTLS_CRYPTO=y
#
CONFIG_RE_MAX=1
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_1 is not set
CONFIG_RE_BTN_LONG_PRESS_TIME_US=500000

83
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
motorCommands_t joystick_generateCommandsDriving(joystickData_t data, joystickGenerateCommands_config_t * config){
//struct with current data of the joystick
//typedef struct joystickData_t {
// joystickPos_t position;
// float x;
// float y;
// float radius;
// float angle;
//} joystickData_t;
//--- interpret config parameters ---
float dutyOffset = config->dutyOffset; // immediately starts with this duty
float dutyRange = config->maxDutyStraight - config->dutyOffset; //duty at max radius
// calculate configured boost duty (added when turning)
float dutyBoost = config->maxDutyStraight * config->maxRelativeBoostPercentOfMaxDuty/100;
// limit to maximum possible duty
float dutyAvailable = 100 - config->maxDutyStraight;
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)
/*
//FIXME works, but armchair unsusable because of current bug with motor driver (inner motor freezes after turn)
float ratioClipThreshold = 0.3;
if (ratio < ratioClipThreshold) ratio = 0;
else if (ratio > 1-ratioClipThreshold) ratio = 1;
//TODO subtract this clip threshold from available joystick range at ratio usage
*/
//--- calculate paramaters with current data ---
motorCommands_t commands; // store new motor commands
// -- calculate ratio --
// get current ratio from stick angle
float ratioActual = fabs(data.angle) / 90; //x=0 -> 90deg -> ratio=1 || y=0 -> 0deg -> ratio=0
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 ---
if (config->altStickMapping == true){
@ -380,36 +388,43 @@ motorCommands_t joystick_generateCommandsDriving(joystickData_t data, joystickGe
case joystickPos_t::TOP_RIGHT:
commands.left.state = motorstate_t::FWD;
commands.right.state = motorstate_t::FWD;
commands.left.duty = data.radius * dutyRange + dutyOffset;
commands.right.duty = data.radius * dutyRange - (data.radius*dutyRange + dutyOffset)*(1-ratio) + dutyOffset;
commands.left.duty = data.radius * dutyRange + boostAmountOuter + dutyOffset;
commands.right.duty = data.radius * dutyRange - reductionAmountInner + dutyOffset;
break;
case joystickPos_t::TOP_LEFT:
commands.left.state = motorstate_t::FWD;
commands.right.state = motorstate_t::FWD;
commands.left.duty = data.radius * dutyRange - (data.radius*dutyRange + dutyOffset)*(1-ratio) + dutyOffset;
commands.right.duty = data.radius * dutyRange + dutyOffset;
commands.left.duty = data.radius * dutyRange - reductionAmountInner + dutyOffset;
commands.right.duty = data.radius * dutyRange + boostAmountOuter + dutyOffset;
break;
case joystickPos_t::BOTTOM_LEFT:
commands.left.state = motorstate_t::REV;
commands.right.state = motorstate_t::REV;
commands.left.duty = data.radius * dutyRange + dutyOffset;
commands.right.duty = data.radius * dutyRange - (data.radius*dutyRange + dutyOffset)*(1-ratio) + dutyOffset;
commands.left.duty = data.radius * dutyRange + boostAmountOuter + dutyOffset;
commands.right.duty = data.radius * dutyRange - reductionAmountInner + dutyOffset;
break;
case joystickPos_t::BOTTOM_RIGHT:
commands.left.state = motorstate_t::REV;
commands.right.state = motorstate_t::REV;
commands.left.duty = data.radius * dutyRange - (data.radius*dutyRange + dutyOffset)*(1-ratio) + dutyOffset;
commands.right.duty = data.radius * dutyRange + dutyOffset;
commands.left.duty = data.radius * dutyRange - reductionAmountInner + dutyOffset;
commands.right.duty = data.radius * dutyRange + boostAmountOuter + dutyOffset;
break;
}
ESP_LOGI(TAG_CMD, "generated commands from data: state=%s, angle=%.3f, ratio=%.3f/%.3f, radius=%.2f, x=%.2f, y=%.2f",
joystickPosStr[(int)data.position], data.angle, ratio, (1-ratio), data.radius, data.x, data.y);
ESP_LOGI(TAG_CMD, "motor left: state=%s, duty=%.3f", motorstateStr[(int)commands.left.state], commands.left.duty);
ESP_LOGI(TAG_CMD, "motor right: state=%s, duty=%.3f", motorstateStr[(int)commands.right.state], commands.right.duty);
// log input data
ESP_LOGD(TAG_CMD, "in: pos='%s', angle=%.3f, ratioActual/Scaled=%.2f/%.2f, r=%.2f, x=%.2f, y=%.2f",
joystickPosStr[(int)data.position], data.angle, ratioActual, ratio, data.radius, data.x, data.y);
// 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;
}

14
common/joystick.hpp
View File

@ -69,15 +69,17 @@ typedef struct joystickData_t {
float angle;
} joystickData_t;
// struct with parameters provided to joystick_GenerateCommandsDriving()
typedef struct joystickGenerateCommands_config_t {
float maxDuty;
float dutyOffset;
bool altStickMapping;
typedef struct joystickGenerateCommands_config_t
{
float maxDutyStraight; // max duty applied when driving with ratio=1 (when turning it might increase by Boost)
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;
//------------------------------------
//----- evaluatedJoystick class -----
//------------------------------------

295
common/motorctl.cpp
View File

@ -28,14 +28,20 @@ void task_motorctl( void * ptrControlledMotor ){
//======== constructor ========
//=============================
//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) {
//copy parameters for controlling the motor
config = config_control;
log = config.loggingEnabled;
//pointer to update motot dury method
motorSetCommand = setCommandFunc;
//pointer to nvs handle
nvsHandle = nvsHandle_f;
//pointer to other motor object
ppOtherMotor = otherMotor_f;
//pointer to speed sensor
sSensor = speedSensor_f;
//create queue, initialize config values
init();
@ -105,19 +111,35 @@ void controlledMotor::handle(){
//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
{
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;
timestamp_commandReceived = esp_log_timestamp();
}
// ----- 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)
{
case motorControlMode_t::DUTY: // regulate to desired duty (as originally)
//--- 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){
// 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;
@ -134,32 +156,140 @@ void controlledMotor::handle(){
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);
}
//--- timeout, no data ---
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)
{
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;
// set target and last command to IDLE
state = motorstate_t::IDLE;
dutyTarget = 0;
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 difference ---
//--- CALCULATE DUTY-DIFF ---
dutyDelta = dutyTarget - dutyNow;
//positive: need to increase by that value
//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
//-> 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)
{ // 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
@ -168,44 +298,43 @@ void controlledMotor::handle(){
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
}
//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 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
}
//----- 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)
//TODO: this needs optimization (can be more clear and/or simpler)
if (dutyDelta > 0) { //difference positive -> increasing duty (-100 -> 100)
@ -240,11 +369,89 @@ void controlledMotor::handle(){
} 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);
}
}
//----- 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)
state=getStateFromDuty(dutyNow);
@ -254,14 +461,15 @@ void controlledMotor::handle(){
//FWD -> IDLE -> FWD continue without waiting
//FWD -> IDLE -> REV wait for dead-time in IDLE
//TODO check when changed only?
if (config.deadTimeMs > 0) { //deadTime is enabled
if ( //not enough time between last direction state
( 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;
@ -269,9 +477,10 @@ void controlledMotor::handle(){
} 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]);
}
if(log) ESP_LOGV(TAG, "deadtime: no change below deadtime detected... dir=%s, duty=%.1f", motorstateStr[(int)state], dutyNow);
}
ESP_LOGV(TAG, "deadtime: no change below deadtime detected... dir=%s, duty=%.1f", motorstateStr[(int)state], dutyNow);
}
@ -284,7 +493,7 @@ void controlledMotor::handle(){
//--- 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
@ -300,11 +509,11 @@ void controlledMotor::handle(){
//function to set the target mode and duty of a motor
//puts the provided command in a queue for the handle function running in another task
void controlledMotor::setTarget(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:

34
common/motorctl.hpp
View File

@ -13,6 +13,7 @@ extern "C"
#include "motordrivers.hpp"
#include "currentsensor.hpp"
#include "speedsensor.hpp"
//=======================================
@ -23,6 +24,7 @@ extern "C"
typedef void (*motorSetCommandFunc_t)(motorCommand_t cmd);
enum class motorControlMode_t {DUTY, CURRENT, SPEED};
//===================================
//====== controlledMotor class ======
@ -30,11 +32,20 @@ typedef void (*motorSetCommandFunc_t)(motorCommand_t cmd);
class controlledMotor {
public:
//--- 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 setTarget(motorstate_t state_f, float duty_f = 0); //adds target command to queue for handle function
void setTarget(motorCommand_t command);
motorCommand_t getStatus(); //get current status of the motor (returns struct with state and duty)
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
@ -60,6 +71,11 @@ class controlledMotor {
QueueHandle_t commandQueue = NULL;
//current sensor
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)
motorSetCommandFunc_t motorSetCommand;
@ -68,15 +84,24 @@ 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; //default control mode
//handle for using the nvs flash (persistent config variables)
nvs_handle_t * nvsHandle;
float currentMax;
float currentNow;
//speed mode
float speedTarget = 0;
float speedNow = 0;
uint32_t timestamp_speedLastUpdate = 0;
float dutyTarget = 0;
float dutyNow = 0;
float dutyIncrementAccel;
float dutyIncrementDecel;
float dutyDelta;
@ -97,6 +122,13 @@ class controlledMotor {
uint32_t timestamp_commandReceived = 0;
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
uint64_t pulseSum = pulse1 + pulse2 + pulse3;
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 getMps(); //meters per second
float getRpm(); //rotations per minute
uint32_t getTimeLastUpdate() {return timeLastUpdate;};
//variables for handling the encoder (public because ISR needs access)
speedSensor_config_t config;
@ -45,6 +46,7 @@ public:
int debugCount = 0;
uint32_t debug_countIgnoredSequencesTooShort = 0;
double currentRpm = 0;
uint32_t timeLastUpdate = 0;
private:
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
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;
bool tractionControlSystemEnabled;
adc1_channel_t currentSensor_adc;
float currentSensor_ratedCurrent;
float currentMax;