Add boost outer tire, Add ratio-threshold, Fix motorctl timeout
Rework joystick command generation
Fix timeout no commands received in motorctl
Successfully tested this state on actual hardware:
turning behavior is significantly improved - does not get slower when turning anymore
joystick:
- add boost of inner tire when turning
- add threshold where ratio snaps to 1
- optimize structure, logging
control:
- rename maxDuty to maxDutyStraight to be more clear
- add methods to change and get new variable RelativeBoostPer for menu item
menu:
- add new item to set maxRelativeBoost config parameter
motorctl:
- fix timeout not working:
previously when not receiving commands for 15s the duty was set to 0 for 1 handle cycle only
This commit is contained in:
jonny_l480
parent
179c608638
commit
a6a630af44
@ -251,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 = 85,
|
||||
//-- 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
|
||||
};
|
||||
@ -570,11 +570,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 +589,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 +604,5 @@ void controlledArmchair::writeMaxDuty(float newValue){
|
||||
else
|
||||
ESP_LOGI(TAG, "nvs: successfully committed updates");
|
||||
// update variable
|
||||
joystickGenerateCommands_config.maxDuty = newValue;
|
||||
joystickGenerateCommands_config.maxDutyStraight = newValue;
|
||||
}
|
||||
@ -94,7 +94,10 @@ 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;};
|
||||
|
||||
|
||||
@ -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 #####
|
||||
//######################
|
||||
@ -550,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_statusScreen, item_maxDuty, item_accelLimit, item_decelLimit, item_motorControlMode, item_tractionControlSystem, item_reset, item_example, item_last};
|
||||
const int itemCount = 10;
|
||||
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;
|
||||
|
||||
|
||||
|
||||
|
||||
@ -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.15 // >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;
|
||||
}
|
||||
|
||||
|
||||
@ -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 -----
|
||||
//------------------------------------
|
||||
|
||||
@ -265,8 +265,11 @@ if ( dutyNow != 0 && esp_log_timestamp() - timestamp_commandReceived > TIMEOUT_I
|
||||
{
|
||||
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;
|
||||
}
|
||||
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user