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

Add menu item to set brake-deceleration

Browse Source 
Also simplify motorctl by removing two variables
and using config struct instead
This commit is contained in:
jonny_l480 2024-05-20 09:08:10 +02:00
parent 3ebcd6ad3c
commit a839d61f65
3 changed files with 69 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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;

65
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;
@ -333,15 +334,15 @@ if ( dutyNow != 0 && esp_log_timestamp() - timestamp_commandReceived > TIMEOUT_I
dutyIncrementAccel = 0;
}
//- normal accel -
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 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 (deceleration) ---
//- sport mode -
if (msFadeDecel == 0){ //no decel limit (immediately reduce to 0)
if (config.msFadeDecel == 0){ //no decel limit (immediately reduce to 0)
dutyIncrementDecel = 100;
}
//- brake -
@ -364,7 +365,7 @@ if ( dutyNow != 0 && esp_log_timestamp() - timestamp_commandReceived > TIMEOUT_I
//- normal deceleration -
else {
// normal deceleration according to configured time
dutyIncrementDecel = (usPassed / ((float)msFadeDecel * 1000)) * 100;
dutyIncrementDecel = (usPassed / ((float)config.msFadeDecel * 1000)) * 100;
}
// reset braking state when start condition is no longer met (stick below threshold again)
@ -402,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;
@ -471,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
@ -586,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;
@ -602,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;
@ -623,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;
@ -663,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;
@ -695,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];
@ -705,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));
@ -721,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];
@ -732,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));
@ -748,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;
}
@ -760,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");
@ -770,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;
}
@ -789,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");
@ -799,5 +796,5 @@ void controlledMotor::writeDecelDuration(uint32_t newValue)
else
ESP_LOGI(TAG, "nvs: successfully committed updates");
// update variable
msFadeDecel = newValue;
config.msFadeDecel = newValue;
}

7
common/motorctl.hpp
View File

@ -47,6 +47,9 @@ class controlledMotor {
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
@ -85,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
@ -108,9 +112,6 @@ class controlledMotor {
float dutyDelta;
uint32_t timeoutWaitForCommand = 0;
uint32_t msFadeAccel;
uint32_t msFadeDecel;
uint32_t ramp;
int64_t timestampLastRunUs = 0;