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

board_single/main/config.cpp

@@ -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 --
-    .dutyOffset = 5, // duty at which motors start immediately
+    // max duty when both motors are at equal ratio e.g. driving straight forward
-    .altStickMapping = false,
+    // better to be set less than 100% to have some reserve for boosting the outer tire when turning
+    .maxDutyStraight = 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
+    .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
 };

board_single/main/control.cpp

@@ -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);
+        ESP_LOGW(TAG, "Successfully read value '%s' from nvs. Overriding default value %.2f with %.2f", "c-maxDuty", joystickGenerateCommands_config.maxDutyStraight, valueRead/100.0);
-        joystickGenerateCommands_config.maxDuty = (float)(valueRead/100.0);
+        joystickGenerateCommands_config.maxDutyStraight = (float)(valueRead/100.0);
         break;
     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;
 }

board_single/main/control.hpp

@@ -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;};
 

board_single/main/menu.cpp

@@ -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 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 = 10;
+const int itemCount = 11;
 
 
 

common/joystick.cpp

@@ -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
+	//--- interpret config parameters ---
-    //typedef struct joystickData_t {
+    float dutyOffset = config->dutyOffset; // immediately starts with this duty
-    //    joystickPos_t position;
+    float dutyRange = config->maxDutyStraight - config->dutyOffset; //duty at max radius
-    //    float x;
+    // calculate configured boost duty (added when turning)
-    //    float y;
+    float dutyBoost = config->maxDutyStraight * config->maxRelativeBoostPercentOfMaxDuty/100;
-    //    float radius;
+    // limit to maximum possible duty
-    //    float angle;
+    float dutyAvailable = 100 - config->maxDutyStraight;
-    //} joystickData_t;
+    if (dutyBoost > dutyAvailable) dutyBoost = dutyAvailable;
-
-	//--- variables ---
-    motorCommands_t commands;
-    float dutyOffset = 5; //immediately starts with this duty, TODO add this to config
-    float dutyRange = config->maxDuty - config->dutyOffset;
-    float ratio = fabs(data.angle) / 90; //90degree = x=0 || 0degree = y=0
 	
-	//--- snap ratio to max at angle threshold --- 
+
-	//(-> more joystick area where inner wheel is off when turning)
+    //--- calculate paramaters with current data ---
-	/*
+    motorCommands_t commands; // store new motor commands
-	//FIXME works, but armchair unsusable because of current bug with motor driver (inner motor freezes after turn)
+
-	float ratioClipThreshold = 0.3;
+    // -- calculate ratio --
-	if (ratio < ratioClipThreshold) ratio = 0;
+    // get current ratio from stick angle
-	else if (ratio > 1-ratioClipThreshold) ratio = 1;
+    float ratioActual = fabs(data.angle) / 90; //x=0 -> 90deg -> ratio=1 || y=0 -> 0deg -> ratio=0
-	//TODO subtract this clip threshold from available joystick range at ratio usage
+    ratioActual = 1 - ratioActual; // invert ratio
-	*/
+    // scale and clip ratio according to configured tolerance 
+    // to have some joystick area at max ratio before reaching X-Axis-full-turn-mode
+    float ratio = ratioActual / (config->ratioSnapToOneThreshold); //0->0  threshold->1
+    // limit to 1 when above threshold (inside area max ratio)
+    if (ratio > 1) ratio = 1; // >threshold -> 1
+
+    // -- calculate outer tire boost --
+    #define BOOST_RATIO_MANIPULATION_SCALE 1.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.left.duty = data.radius * dutyRange + boostAmountOuter + dutyOffset;
-            commands.right.duty = data.radius * dutyRange - (data.radius*dutyRange + dutyOffset)*(1-ratio) + dutyOffset;
+            commands.right.duty = data.radius * dutyRange - reductionAmountInner + dutyOffset;
             break;
 
         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.left.duty = data.radius * dutyRange - reductionAmountInner + dutyOffset;
-            commands.right.duty = data.radius * dutyRange + 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.left.duty = data.radius * dutyRange + boostAmountOuter + dutyOffset;
-            commands.right.duty = data.radius * dutyRange - (data.radius*dutyRange + dutyOffset)*(1-ratio) + dutyOffset;
+            commands.right.duty = data.radius * dutyRange - reductionAmountInner + dutyOffset;
             break;
 
         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.left.duty = data.radius * dutyRange - reductionAmountInner + dutyOffset;
-            commands.right.duty =  data.radius * dutyRange + 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",
+    // log input data
-            joystickPosStr[(int)data.position], data.angle, ratio, (1-ratio), data.radius, data.x, data.y);
+    ESP_LOGD(TAG_CMD, "in: pos='%s', angle=%.3f, ratioActual/Scaled=%.2f/%.2f, r=%.2f, x=%.2f, y=%.2f",
-    ESP_LOGI(TAG_CMD, "motor left: state=%s, duty=%.3f", motorstateStr[(int)commands.left.state], commands.left.duty);
+            joystickPosStr[(int)data.position], data.angle, ratioActual, ratio, data.radius, data.x, data.y);
-    ESP_LOGI(TAG_CMD, "motor right: state=%s, duty=%.3f", motorstateStr[(int)commands.right.state], commands.right.duty);
+    // log generation details
+    ESP_LOGI(TAG_CMD, "left=%.2f, right=%.2f -- BoostOuter=%.1f, ReductionInner=%.1f, maxDuty=%.0f, maxBoost=%.0f, dutyOffset=%.0f",
+             commands.left.duty, commands.right.duty, 
+             boostAmountOuter, reductionAmountInner, 
+             config->maxDutyStraight, dutyBoost, dutyOffset);
+    // log generated motor commands
+    ESP_LOGD(TAG_CMD, "motor left: state=%s, duty=%.3f", motorstateStr[(int)commands.left.state], commands.left.duty);
+    ESP_LOGD(TAG_CMD, "motor right: state=%s, duty=%.3f", motorstateStr[(int)commands.right.state], commands.right.duty);
     return commands;
 }
 

common/joystick.hpp

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

common/motorctl.cpp

@@ -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;
 }