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

Merge branch 'driving-behavior' into dev - brake works

Browse Source
This commit is contained in:
jonny 2024-05-24 23:40:27 +02:00
commit f94630711b
7 changed files with 140 additions and 48 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
board_single/main/config.cpp
View File

@ -32,7 +32,7 @@ void setLoglevels(void)
// esp_log_level_set("motordriver", ESP_LOG_DEBUG);
esp_log_level_set("motor-control", ESP_LOG_WARN);
// esp_log_level_set("evaluatedJoystick", ESP_LOG_DEBUG);
// esp_log_level_set("joystickCommands", ESP_LOG_DEBUG);
esp_log_level_set("joystickCommands", ESP_LOG_WARN);
esp_log_level_set("button", ESP_LOG_INFO);
esp_log_level_set("control", ESP_LOG_INFO);
// esp_log_level_set("fan-control", ESP_LOG_INFO);
@ -108,7 +108,9 @@ motorctl_config_t configMotorControlLeft = {
.currentMax = 30,
.currentInverted = true,
.currentSnapToZeroThreshold = 0.15,
.deadTimeMs = 0 // minimum time motor is off between direction change
.deadTimeMs = 0, // minimum time motor is off between direction change
.brakePauseBeforeResume = 1500,
.brakeDecel = 400,
};
//--- configure right motor (contol) ---
@ -124,7 +126,9 @@ motorctl_config_t configMotorControlRight = {
.currentMax = 30,
.currentInverted = false,
.currentSnapToZeroThreshold = 0.25,
.deadTimeMs = 0 // minimum time motor is off between direction change
.deadTimeMs = 0, // minimum time motor is off between direction change
.brakePauseBeforeResume = 1500,
.brakeDecel = 400,
};
//------------------------------

4
board_single/main/control.cpp
View File

@ -59,6 +59,10 @@ controlledArmchair::controlledArmchair(
// override default config value if maxDuty is found in nvs
loadMaxDuty();
// update brake start threshold with actual max duty for motorctl
ESP_LOGW(TAG, "setting brake start threshold for both motors to %.0f", joystickGenerateCommands_config.maxDutyStraight * BRAKE_START_STICK_PERCENTAGE / 100);
motorLeft->setBrakeStartThresholdDuty(joystickGenerateCommands_config.maxDutyStraight * BRAKE_START_STICK_PERCENTAGE / 100);
motorRight->setBrakeStartThresholdDuty(joystickGenerateCommands_config.maxDutyStraight * BRAKE_START_STICK_PERCENTAGE / 100);
// create semaphore for preventing race condition: mode-change operations while currently still executing certain mode
handleIteration_mutex = xSemaphoreCreateMutex();

8
board_single/main/control.hpp
View File

@ -13,6 +13,8 @@ extern "C"
#include "speedsensor.hpp"
#include "chairAdjust.hpp"
//percentage stick has to be moved in the opposite driving direction of current motor direction for braking to start
#define BRAKE_START_STICK_PERCENTAGE 95
//--------------------------------------------
//---- struct, enum, variable declarations ---
@ -93,7 +95,11 @@ class controlledArmchair {
bool toggleAltStickMapping();
// configure max dutycycle (in joystick or http mode)
void setMaxDuty(float maxDutyNew) { writeMaxDuty(maxDutyNew); };
void setMaxDuty(float maxDutyNew) {
writeMaxDuty(maxDutyNew);
motorLeft->setBrakeStartThresholdDuty(joystickGenerateCommands_config.maxDutyStraight * BRAKE_START_STICK_PERCENTAGE/100);
motorRight->setBrakeStartThresholdDuty(joystickGenerateCommands_config.maxDutyStraight * BRAKE_START_STICK_PERCENTAGE/100);
};
float getMaxDuty() const {return joystickGenerateCommands_config.maxDutyStraight; };
// configure max boost (in joystick or http mode)
void setMaxRelativeBoostPer(float newValue) { joystickGenerateCommands_config.maxRelativeBoostPercentOfMaxDuty = newValue; };

36
board_single/main/menu.cpp
View File

@ -370,6 +370,38 @@ menuItem_t item_decelLimit = {
};
// ######################
// ##### brakeDecel #####
// ######################
void item_brakeDecel_action(display_task_parameters_t * objects, SSD1306_t * display, int value)
{
objects->motorLeft->setBrakeDecel((uint32_t)value);
objects->motorRight->setBrakeDecel((uint32_t)value);
}
int item_brakeDecel_value(display_task_parameters_t * objects)
{
return objects->motorLeft->getBrakeDecel();
}
int item_brakeDecel_default(display_task_parameters_t * objects)
{
return objects->motorLeft->getBrakeDecelDefault();
}
menuItem_t item_brakeDecel = {
item_brakeDecel_action, // function action
item_brakeDecel_value, // function get initial value or NULL(show in line 2)
item_brakeDecel_default, // function get default value or NULL(dont set value, show msg)
0, // valueMin
10000, // valueMax
100, // valueIncrement
"Brake decel. ", // title
" Fade down time ", // line1 (above value)
"", // line2 <= showing "default = %d"
"", // line4 * (below value)
"", // line5 *
"milliseconds ", // line6
"from 100 to 0% ", // line7
};
//###############################
//### select motorControlMode ###
@ -578,8 +610,8 @@ menuItem_t item_last = {
//####################################################
//### store all configured menu items in one array ###
//####################################################
const menuItem_t menuItems[] = {item_centerJoystick, item_calibrateJoystick, item_debugJoystick, item_statusScreen, item_maxDuty, item_maxRelativeBoost, item_accelLimit, item_decelLimit, item_motorControlMode, item_tractionControlSystem, item_reset, item_example, item_last};
const int itemCount = 11;
const menuItem_t menuItems[] = {item_centerJoystick, item_calibrateJoystick, item_debugJoystick, item_statusScreen, item_maxDuty, item_maxRelativeBoost, item_accelLimit, item_decelLimit, item_brakeDecel, item_motorControlMode, item_tractionControlSystem, item_reset, item_example, item_last};
const int itemCount = 12;

113
common/motorctl.cpp
View File

@ -30,7 +30,8 @@ void task_motorctl( void * ptrControlledMotor ){
//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, 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),
configDefault(config_control){
//copy parameters for controlling the motor
config = config_control;
log = config.loggingEnabled;
@ -320,22 +321,62 @@ if ( dutyNow != 0 && esp_log_timestamp() - timestamp_commandReceived > TIMEOUT_I
//calculate passed time since last run
int64_t usPassed = esp_timer_get_time() - timestampLastRunUs;
//--- calculate increment ---
//--- calculate increment (acceleration) ---
//calculate increment for fading UP with passed time since last run and configured fade time
//- traction control -
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
//- recent braking -
//FIXME reset timeout when duty less
else if (isBraking && (esp_log_timestamp() - timestampBrakeStart) < config.brakePauseBeforeResume) // prevent immediate direction change when currently braking with timeout (eventually currently sliding)
{
if (log) ESP_LOGI(TAG, "pause after brake... -> accel = 0");
dutyIncrementAccel = 0;
}
//- normal accel -
else if (config.msFadeAccel > 0)
dutyIncrementAccel = (usPassed / ((float)config.msFadeAccel * 1000)) * 100; // TODO define maximum increment - first run after startup (or long) pause can cause a very large increment
//- sport mode -
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)
//--- calculate increment (deceleration) ---
//- sport mode -
if (config.msFadeDecel == 0){ //no decel limit (immediately reduce to 0)
dutyIncrementDecel = 100;
}
//- brake -
//detect when quicker brake response is desired (e.g. full speed forward, joystick suddenly is full reverse -> break fast)
#define NO_BRAKE_THRESHOLD_TOO_SLOW_DUTY 10 //TODO test/adjust this - dont brake when slow already (avoids starting full dead time)
else if (commandReceive.state != state && // direction differs
fabs(dutyNow) > NO_BRAKE_THRESHOLD_TOO_SLOW_DUTY && // not very slow already
fabs(dutyTarget) > brakeStartThreshold) // joystick above threshold
{
// set braking state and track start time (both for disabling acceleration for some time)
if (!isBraking) {
if (log) ESP_LOGW(TAG, "started braking...");
timestampBrakeStart = esp_log_timestamp();
isBraking = true;
}
// use brake deceleration instead of normal deceleration
dutyIncrementDecel = (usPassed / ((float)config.brakeDecel * 1000)) * 100;
if(log) ESP_LOGI(TAG, "braking (target duty >%.0f%% in other direction) -> using deceleration %dms", brakeStartThreshold, config.brakeDecel);
}
//- normal deceleration -
else {
// normal deceleration according to configured time
dutyIncrementDecel = (usPassed / ((float)config.msFadeDecel * 1000)) * 100;
}
//fade duty to target (up and down)
// reset braking state when start condition is no longer met (stick below threshold again)
if (isBraking &&
(fabs(dutyTarget) < brakeStartThreshold || commandReceive.state == state))
{
ESP_LOGW(TAG, "brake condition no longer met");
isBraking = false;
}
//--- 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)
if (dutyNow < 0) { //reverse, decelerating
@ -362,7 +403,7 @@ if ( dutyNow != 0 && esp_log_timestamp() - timestamp_commandReceived > TIMEOUT_I
float dutyOld = dutyNow;
//adaptive decrement:
//Note current exceeded twice -> twice as much decrement: TODO: decrement calc needs finetuning, currently random values
dutyIncrementDecel = (currentNow/config.currentMax) * ( usPassed / ((float)msFadeDecel * 1500) ) * 100;
dutyIncrementDecel = (currentNow/config.currentMax) * ( usPassed / ((float)config.msFadeDecel * 1500) ) * 100;
float currentLimitDecrement = ( (float)usPassed / ((float)1000 * 1000) ) * 100; //1000ms from 100 to 0
if (dutyNow < -currentLimitDecrement) {
dutyNow += currentLimitDecrement;
@ -431,7 +472,7 @@ if ( dutyNow != 0 && esp_log_timestamp() - timestamp_commandReceived > TIMEOUT_I
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
float dutyDecrement = (tcs_usPassed / ((float)config.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
@ -546,10 +587,10 @@ motorCommand_t controlledMotor::getStatus(){
uint32_t controlledMotor::getFade(fadeType_t fadeType){
switch(fadeType){
case fadeType_t::ACCEL:
return msFadeAccel;
return config.msFadeAccel;
break;
case fadeType_t::DECEL:
return msFadeDecel;
return config.msFadeDecel;
break;
}
return 0;
@ -562,10 +603,10 @@ uint32_t controlledMotor::getFade(fadeType_t fadeType){
uint32_t controlledMotor::getFadeDefault(fadeType_t fadeType){
switch(fadeType){
case fadeType_t::ACCEL:
return config.msFadeAccel;
return configDefault.msFadeAccel;
break;
case fadeType_t::DECEL:
return config.msFadeDecel;
return configDefault.msFadeDecel;
break;
}
return 0;
@ -583,11 +624,11 @@ void controlledMotor::setFade(fadeType_t fadeType, uint32_t msFadeNew){
//TODO: mutex for msFade variable also used in handle function
switch(fadeType){
case fadeType_t::ACCEL:
ESP_LOGW(TAG, "[%s] changed fade-up time from %d to %d", config.name, msFadeAccel, msFadeNew);
ESP_LOGW(TAG, "[%s] changed fade-up time from %d to %d", config.name, config.msFadeAccel, msFadeNew);
writeAccelDuration(msFadeNew);
break;
case fadeType_t::DECEL:
ESP_LOGW(TAG, "[%s] changed fade-down time from %d to %d",config.name, msFadeDecel, msFadeNew);
ESP_LOGW(TAG, "[%s] changed fade-down time from %d to %d",config.name, config.msFadeDecel, msFadeNew);
// write new value to nvs and update the variable
writeDecelDuration(msFadeNew);
break;
@ -623,16 +664,16 @@ bool controlledMotor::toggleFade(fadeType_t fadeType){
bool enabled = false;
switch(fadeType){
case fadeType_t::ACCEL:
if (msFadeAccel == 0){
msFadeNew = config.msFadeAccel;
if (config.msFadeAccel == 0){
msFadeNew = configDefault.msFadeAccel;
enabled = true;
} else {
msFadeNew = 0;
}
break;
case fadeType_t::DECEL:
if (msFadeDecel == 0){
msFadeNew = config.msFadeAccel;
if (config.msFadeDecel == 0){
msFadeNew = configDefault.msFadeAccel;
enabled = true;
} else {
msFadeNew = 0;
@ -655,8 +696,6 @@ bool controlledMotor::toggleFade(fadeType_t fadeType){
// load stored value from nvs if not successfull uses config default value
void controlledMotor::loadAccelDuration(void)
{
// load default value
msFadeAccel = config.msFadeAccel;
// read from nvs
uint32_t valueNew;
char key[15];
@ -665,11 +704,11 @@ void controlledMotor::loadAccelDuration(void)
switch (err)
{
case ESP_OK:
ESP_LOGW(TAG, "Successfully read value '%s' from nvs. Overriding default value %d with %d", key, config.msFadeAccel, valueNew);
msFadeAccel = valueNew;
ESP_LOGW(TAG, "Successfully read value '%s' from nvs. Overriding default value %d with %d", key, configDefault.msFadeAccel, valueNew);
config.msFadeAccel = valueNew;
break;
case ESP_ERR_NVS_NOT_FOUND:
ESP_LOGW(TAG, "nvs: the value '%s' is not initialized yet, keeping default value %d", key, msFadeAccel);
ESP_LOGW(TAG, "nvs: the value '%s' is not initialized yet, keeping default value %d", key, config.msFadeAccel);
break;
default:
ESP_LOGE(TAG, "Error (%s) reading nvs!", esp_err_to_name(err));
@ -681,8 +720,6 @@ void controlledMotor::loadAccelDuration(void)
//-----------------------------
void controlledMotor::loadDecelDuration(void)
{
// load default value
msFadeDecel = config.msFadeDecel;
// read from nvs
uint32_t valueNew;
char key[15];
@ -692,10 +729,10 @@ void controlledMotor::loadDecelDuration(void)
{
case ESP_OK:
ESP_LOGW(TAG, "Successfully read value '%s' from nvs. Overriding default value %d with %d", key, config.msFadeDecel, valueNew);
msFadeDecel = valueNew;
config.msFadeDecel = valueNew;
break;
case ESP_ERR_NVS_NOT_FOUND:
ESP_LOGW(TAG, "nvs: the value '%s' is not initialized yet, keeping default value %d", key, msFadeDecel);
ESP_LOGW(TAG, "nvs: the value '%s' is not initialized yet, keeping default value %d", key, config.msFadeDecel);
break;
default:
ESP_LOGE(TAG, "Error (%s) reading nvs!", esp_err_to_name(err));
@ -708,11 +745,11 @@ void controlledMotor::loadDecelDuration(void)
//------------------------------
//----- writeAccelDuration -----
//------------------------------
// write provided value to nvs to be persistent and update the local variable msFadeAccel
// write provided value to nvs to be persistent and update the local config
void controlledMotor::writeAccelDuration(uint32_t newValue)
{
// check if unchanged
if(msFadeAccel == newValue){
if(config.msFadeAccel == newValue){
ESP_LOGW(TAG, "value unchanged at %d, not writing to nvs", newValue);
return;
}
@ -720,7 +757,7 @@ void controlledMotor::writeAccelDuration(uint32_t newValue)
char key[15];
snprintf(key, 15, "m-%s-accel", config.name);
// update nvs value
ESP_LOGW(TAG, "[%s] updating nvs value '%s' from %d to %d", config.name, key, msFadeAccel, newValue);
ESP_LOGW(TAG, "[%s] updating nvs value '%s' from %d to %d", config.name, key, config.msFadeAccel, newValue);
esp_err_t err = nvs_set_u32(*nvsHandle, key, newValue);
if (err != ESP_OK)
ESP_LOGE(TAG, "nvs: failed writing");
@ -730,18 +767,18 @@ void controlledMotor::writeAccelDuration(uint32_t newValue)
else
ESP_LOGI(TAG, "nvs: successfully committed updates");
// update variable
msFadeAccel = newValue;
config.msFadeAccel = newValue;
}
//------------------------------
//----- writeDecelDuration -----
//------------------------------
// write provided value to nvs to be persistent and update the local variable msFadeDecel
// write provided value to nvs to be persistent and update the local config
// TODO: reduce duplicate code
void controlledMotor::writeDecelDuration(uint32_t newValue)
{
// check if unchanged
if(msFadeDecel == newValue){
if(config.msFadeDecel == newValue){
ESP_LOGW(TAG, "value unchanged at %d, not writing to nvs", newValue);
return;
}
@ -749,7 +786,7 @@ void controlledMotor::writeDecelDuration(uint32_t newValue)
char key[15];
snprintf(key, 15, "m-%s-decel", config.name);
// update nvs value
ESP_LOGW(TAG, "[%s] updating nvs value '%s' from %d to %d", config.name, key, msFadeDecel, newValue);
ESP_LOGW(TAG, "[%s] updating nvs value '%s' from %d to %d", config.name, key, config.msFadeDecel, newValue);
esp_err_t err = nvs_set_u32(*nvsHandle, key, newValue);
if (err != ESP_OK)
ESP_LOGE(TAG, "nvs: failed writing");
@ -759,5 +796,5 @@ void controlledMotor::writeDecelDuration(uint32_t newValue)
else
ESP_LOGI(TAG, "nvs: successfully committed updates");
// update variable
msFadeDecel = newValue;
config.msFadeDecel = newValue;
}

13
common/motorctl.hpp
View File

@ -46,6 +46,10 @@ class controlledMotor {
void disableTractionControlSystem() {config.tractionControlSystemEnabled = false; tcs_isExceeded = false;};
bool getTractionControlSystemStatus() {return config.tractionControlSystemEnabled;};
void setControlMode(motorControlMode_t newMode) {mode = newMode;};
void setBrakeStartThresholdDuty(float duty) {brakeStartThreshold = duty;};
void setBrakeDecel(uint32_t msFadeBrake) {config.brakeDecel = msFadeBrake;};
uint32_t getBrakeDecel() {return config.brakeDecel;}; //todo store and load from nvs
uint32_t getBrakeDecelDefault() {return configDefault.brakeDecel;};
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
@ -84,6 +88,7 @@ class controlledMotor {
//TODO add name for logging?
//struct for storing control specific parameters
motorctl_config_t config;
const motorctl_config_t configDefault; //backup default configuration (unchanged)
bool log = false;
motorstate_t state = motorstate_t::IDLE;
motorControlMode_t mode = motorControlMode_t::DUTY; //default control mode
@ -107,9 +112,6 @@ class controlledMotor {
float dutyDelta;
uint32_t timeoutWaitForCommand = 0;
uint32_t msFadeAccel;
uint32_t msFadeDecel;
uint32_t ramp;
int64_t timestampLastRunUs = 0;
@ -129,6 +131,11 @@ class controlledMotor {
uint32_t tcs_usExceeded = 0; //sum up time
bool tcs_isExceeded = false; //is currently too fast
int64_t tcs_timestampLastRun = 0;
//brake (decel boost)
uint32_t timestampBrakeStart = 0;
bool isBraking = false;
float brakeStartThreshold = 60;
};
//====================================

2
common/types.hpp
View File

@ -53,6 +53,8 @@ typedef struct motorctl_config_t {
bool currentInverted;
float currentSnapToZeroThreshold;
uint32_t deadTimeMs; //time motor stays in IDLE before direction change
uint32_t brakePauseBeforeResume;
uint32_t brakeDecel;
} motorctl_config_t;
//enum fade type (acceleration, deceleration)