Added dynamic PID, read ISV57 servo state every cycle

Arduino/Esp32/Main/DiyActivePedal_types.cpp

@@ -53,6 +53,8 @@ void DAP_config_st::initialiseDefaults() {
   payLoadPedalConfig_.PID_i_gain = 50.0;
   payLoadPedalConfig_.PID_d_gain = 0.0;
 
+  payLoadPedalConfig_.control_strategy_b = 0;
+
   payLoadPedalConfig_.maxGameOutput = 100;
 
   payLoadPedalConfig_.kf_modelNoise = 128;

Arduino/Esp32/Main/DiyActivePedal_types.h

@@ -3,7 +3,7 @@
 #include <stdint.h>
 
 
-#define DAP_VERSION_CONFIG 106
+#define DAP_VERSION_CONFIG 107
 #define DAP_PAYLOAD_TYPE_CONFIG 100
 
 struct payloadHeader {
@@ -65,6 +65,8 @@ struct payloadPedalConfig {
   float PID_i_gain;
   float PID_d_gain;
 
+  uint8_t control_strategy_b;
+
   // controller settings
   uint8_t maxGameOutput;
 

Arduino/Esp32/Main/ForceCurve.cpp

@@ -32,8 +32,8 @@ float ForceCurve_Interpolated::EvalForceCubicSpline(const DAP_config_st* config_
   yOrig[4] = config_st->payLoadPedalConfig_.relativeForce_p080;
   yOrig[5] = config_st->payLoadPedalConfig_.relativeForce_p100;
 
-  double dx = 1.0f;
-  double t = (splineSegment_fl32 - (float)splineSegment_u8) / dx;
+  //double dx = 1.0f;
+  double t = (splineSegment_fl32 - (float)splineSegment_u8);// / dx;
   double y = (1 - t) * yOrig[splineSegment_u8] + t * yOrig[splineSegment_u8 + 1] + t * (1 - t) * (a * (1 - t) + b * t);
 
   y = calc_st->Force_Min + y / 100.0f * calc_st->Force_Range;
@@ -48,10 +48,12 @@ float ForceCurve_Interpolated::EvalForceCubicSpline(const DAP_config_st* config_
 /*                                                                                            */
 /**********************************************************************************************/
 
-float ForceCurve_Interpolated::EvalForceGradientCubicSpline(const DAP_config_st* config_st, const DAP_calculationVariables_st* calc_st, float fractionalPos)
+float ForceCurve_Interpolated::EvalForceGradientCubicSpline(const DAP_config_st* config_st, const DAP_calculationVariables_st* calc_st, float fractionalPos, bool normalized_b)
 {
 
-  float splineSegment_fl32 = fractionalPos * 5;
+  float fractionalPos_lcl = constrain(fractionalPos, 0, 1);
+
+  float splineSegment_fl32 = fractionalPos_lcl * 5;
   uint8_t splineSegment_u8 = (uint8_t)floor(splineSegment_fl32);
 
   if (splineSegment_u8 < 0){splineSegment_u8 = 0;}
@@ -69,17 +71,20 @@ float ForceCurve_Interpolated::EvalForceGradientCubicSpline(const DAP_config_st*
 
 
 
-  double Delta_x_orig = 100; 
-  double dx = Delta_x_orig / NUMBER_OF_SPLINE_SEGMENTS;
-  double t = (splineSegment_fl32 - (float)splineSegment_u8) / dx;
-  double dy = yOrig[splineSegment_u8 + 1] - yOrig[splineSegment_u8];
+  double Delta_x_orig = 100; // total horizontal range [0,100]
+  double dx = Delta_x_orig / NUMBER_OF_SPLINE_SEGMENTS; // spline segment horizontal range
+  double t = (splineSegment_fl32 - (float)splineSegment_u8); // relative position in spline segment [0, 1]
+  double dy = yOrig[splineSegment_u8 + 1] - yOrig[splineSegment_u8]; // spline segment vertical range
   double y_prime = dy / dx + (1 - 2 * t) * (a * (1 - t) + b * t) / dx + t * (1 - t) * (b - a) / dx;
 
   // when the spline was identified, x and y were givin in the unit of percent --> 0-100
-  // --> conversion of the gradient to the proper axis scling is performed
-  double d_y_scale = calc_st->Force_Range / 100.0;
-  double d_x_scale = 100.0 / calc_st->stepperPosRange;
-  y_prime *= d_x_scale * d_y_scale;
+  // --> conversion of the gradient to the proper axis scaling is performed
+  if (normalized_b == false)
+  {
+    double d_y_scale = calc_st->Force_Range / 100.0;
+    double d_x_scale = 100.0 / calc_st->stepperPosRange;
+    y_prime *= d_x_scale * d_y_scale;
+  }
 
   return y_prime;
 }

Arduino/Esp32/Main/ForceCurve.h

@@ -9,6 +9,6 @@ class ForceCurve_Interpolated {
 
 public:
   float EvalForceCubicSpline(const DAP_config_st* config_st, const DAP_calculationVariables_st* calc_st, float fractionalPos);
-  float EvalForceGradientCubicSpline(const DAP_config_st* config_st, const DAP_calculationVariables_st* calc_st, float fractionalPos);
+  float EvalForceGradientCubicSpline(const DAP_config_st* config_st, const DAP_calculationVariables_st* calc_st, float fractionalPos, bool normalized_b);
 
 };

Arduino/Esp32/Main/Main.ino

@@ -579,13 +579,15 @@ void pedalUpdateTask( void * pvParameters )
 
     // use interpolation to determine local linearized spring stiffness
     double stepperPosFraction = stepper->getCurrentPositionFraction();
+    //double stepperPosFraction2 = stepper->getCurrentPositionFractionFromExternalPos( -(int32_t)(isv57.servo_pos_given_p + isv57.servo_pos_error_p - isv57.getZeroPos()) );
     //int32_t Position_Next = MoveByInterpolatedStrategy(filteredReading, stepperPosFraction, &forceCurve, &dap_calculationVariables_st, &dap_config_st);
     int32_t Position_Next = MoveByPidStrategy(filteredReading, stepperPosFraction, stepper, &forceCurve, &dap_calculationVariables_st, &dap_config_st, absForceOffset_fl32);
 
+
     //#define DEBUG_STEPPER_POS
     #ifdef DEBUG_STEPPER_POS
-      static RTDebugOutput<int32_t, 3> rtDebugFilter({ "ESP_pos", "ESP_tar_pos", "ISV_pos"});
-      rtDebugFilter.offerData({ stepper->getCurrentPositionSteps(), Position_Next, -(int32_t)isv57.servo_pos_given_p});
+      static RTDebugOutput<int32_t, 5> rtDebugFilter({ "ESP_pos", "ESP_tar_pos", "ISV_pos", "frac1", "frac2"});
+      rtDebugFilter.offerData({ stepper->getCurrentPositionSteps(), Position_Next, -(int32_t)(isv57.servo_pos_given_p + isv57.servo_pos_error_p - isv57.getZeroPos()), (int32_t)(stepperPosFraction * 10000.), (int32_t)(stepperPosFraction2 * 10000.)});
     #endif
 
 
@@ -638,7 +640,8 @@ void pedalUpdateTask( void * pvParameters )
     if(semaphore_updateJoystick!=NULL)
     {
       if(xSemaphoreTake(semaphore_updateJoystick, (TickType_t)1)==pdTRUE) {
-        joystickNormalizedToInt32 = NormalizeControllerOutputValue(filteredReading, dap_calculationVariables_st.Force_Min, dap_calculationVariables_st.Force_Max, dap_config_st.payLoadPedalConfig_.maxGameOutput);
+        joystickNormalizedToInt32 = NormalizeControllerOutputValue(loadcellReading, dap_calculationVariables_st.Force_Min, dap_calculationVariables_st.Force_Max, dap_config_st.payLoadPedalConfig_.maxGameOutput);
+        //joystickNormalizedToInt32 = NormalizeControllerOutputValue(filteredReading, dap_calculationVariables_st.Force_Min, dap_calculationVariables_st.Force_Max, dap_config_st.payLoadPedalConfig_.maxGameOutput);
         xSemaphoreGive(semaphore_updateJoystick);
       }
     }
@@ -837,7 +840,8 @@ void servoCommunicationTask( void * pvParameters )
     if (isv57LifeSignal_b)
     {
 
-        delay(20);
+        //delay(5);
+        isv57.readServoStates();
 
 
         int32_t servo_offset_compensation_steps_local_i32 = 0;
@@ -853,7 +857,7 @@ void servoCommunicationTask( void * pvParameters )
 
         if (cond_2 == true)
         {
-          isv57.readServoStates();
+          //isv57.readServoStates();
           int16_t servoPos_now_i16 = isv57.servo_pos_given_p;
           timeNow_l = millis();
 

Arduino/Esp32/Main/Modbus.cpp

@@ -16,8 +16,8 @@ bool Modbus::init(int mode, bool en_log)
 {
      this->mode_ =  mode;
      this->log   =  en_log;
-     pinMode(mode_,OUTPUT);
-     digitalWrite(mode_, 0);  
+     //pinMode(mode_,OUTPUT);
+     //digitalWrite(mode_, 0);  
 
      return true;
 }
@@ -192,12 +192,12 @@ int Modbus::requestFrom(int slaveId, int type, int address,int nb)
             Serial.print("\t");
      }
 
-    digitalWrite(mode_,1);
-    delay(1);
+    //digitalWrite(mode_,1);
+    //delay(1);
     this->s->write(txout,8);
     this->s->flush();
-    digitalWrite(mode_,0);
-    delay(1);
+    //digitalWrite(mode_,0);
+    //delay(1);
     uint32_t t = millis();
     lenRx   = 0;
     datalen = 0;

Arduino/Esp32/Main/StepperMovementStrategy.h

@@ -15,6 +15,7 @@ float Setpoint, Input, Output;
 
 //Specify the links and initial tuning parameters
 double Kp=0.3, Ki=50.0, Kd=0.000;
+uint8_t control_strategy_u8 = 0;
 QuickPID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd,  /* OPTIONS */
                myPID.pMode::pOnError,                   /* pOnError, pOnMeas, pOnErrorMeas */
                myPID.dMode::dOnMeas,                    /* dOnError, dOnMeas */
@@ -32,7 +33,7 @@ bool pidWasInitialized = false;
 int32_t MoveByInterpolatedStrategy(float filteredLoadReadingKg, float stepperPosFraction, ForceCurve_Interpolated* forceCurve, const DAP_calculationVariables_st* calc_st, const DAP_config_st* config_st) {
   float spingStiffnessInv_lcl = calc_st->springStiffnesssInv;
   //float springStiffnessInterp = forceCurve->stiffnessAtPosition(stepperPosFraction);
-  float springStiffnessInterp = forceCurve->EvalForceGradientCubicSpline(config_st, calc_st, stepperPosFraction);
+  float springStiffnessInterp = forceCurve->EvalForceGradientCubicSpline(config_st, calc_st, stepperPosFraction, false);
 
 
   if (springStiffnessInterp > 0) {
@@ -56,6 +57,12 @@ int32_t MoveByInterpolatedStrategy(float filteredLoadReadingKg, float stepperPos
 
 void tunePidValues(DAP_config_st& config_st)
 {
+  Kp=config_st.payLoadPedalConfig_.PID_p_gain;
+  Ki=config_st.payLoadPedalConfig_.PID_i_gain;
+  Kd=config_st.payLoadPedalConfig_.PID_d_gain;
+
+  control_strategy_u8 = config_st.payLoadPedalConfig_.control_strategy_b;
+
   myPID.SetTunings(config_st.payLoadPedalConfig_.PID_p_gain, config_st.payLoadPedalConfig_.PID_i_gain, config_st.payLoadPedalConfig_.PID_d_gain);
 }
 
@@ -80,6 +87,36 @@ int32_t MoveByPidStrategy(float loadCellReadingKg, float stepperPosFraction, Ste
 
 
   float loadCellTargetKg = forceCurve->EvalForceCubicSpline(config_st, calc_st, stepperPosFraction);
+
+
+  if (control_strategy_u8 == 1) // dynamic PID parameters depending on force curve gradient
+  {
+    float gradient_orig_fl32 = forceCurve->EvalForceGradientCubicSpline(config_st, calc_st, stepperPosFraction, true); // determine gradient to modify the PID gain. The steeper the gradient, the less gain should be used
+
+    // normalize gradient
+    float gradient_fl32 = gradient_orig_fl32;
+    float gradient_abs_fl32 = fabs(gradient_fl32);
+
+    float gain_modifier_fl32 = 10.0;
+    if (gradient_abs_fl32 > 1e-5)
+    {
+       gain_modifier_fl32 = 1.0 / pow( gradient_abs_fl32 , 1);
+    }
+    gain_modifier_fl32 = constrain( gain_modifier_fl32, 0.1, 10);
+
+//#define PRINT_PID_DYNAMIC_PARAMETERS
+#ifdef PRINT_PID_DYNAMIC_PARAMETERS
+    {
+      static RTDebugOutput<float, 5> rtDebugFilter({ "gradient_orig_fl32", "gradient_fl32", "gain_modifier_fl32", "KP_default", "KP_mod"});
+      rtDebugFilter.offerData({ gradient_orig_fl32, gradient_fl32, gain_modifier_fl32, Kp, gain_modifier_fl32 * Kp}); 
+    }
+#endif
+
+    myPID.SetTunings(gain_modifier_fl32 * Kp, gain_modifier_fl32 * Ki, Kd);
+  }
+
+
+
   loadCellTargetKg -=absForceOffset_fl32;
 
   // ToDO

Arduino/Esp32/Main/StepperWithLimits.cpp

@@ -233,6 +233,10 @@ double StepperWithLimits::getCurrentPositionFraction() const {
   return double(getCurrentPositionSteps()) / getTravelSteps();
 }
 
+double StepperWithLimits::getCurrentPositionFractionFromExternalPos(int32_t extPos_i32) const {
+  return double(extPos_i32) / getTravelSteps();
+}
+
 int32_t StepperWithLimits::getTargetPositionSteps() const {
   return _stepper->getPositionAfterCommandsCompleted();
 }

Arduino/Esp32/Main/StepperWithLimits.h

@@ -3,7 +3,7 @@
 
 
 // these are physical properties of the stepper
-static const uint32_t MAXIMUM_STEPPER_RPM = 7000;     
+static const uint32_t MAXIMUM_STEPPER_RPM = 4000;     
 static const uint32_t STEPS_PER_MOTOR_REVOLUTION = 1600;
 static const uint32_t MAXIMUM_STEPPER_SPEED = (MAXIMUM_STEPPER_RPM * STEPS_PER_MOTOR_REVOLUTION) / 60;   // steps/s
 static const int32_t MAXIMUM_STEPPER_ACCELERATION = 1e10;                                                 // steps/s²
@@ -36,6 +36,7 @@ class StepperWithLimits {
 public:
   int32_t getCurrentPositionSteps() const;
   double getCurrentPositionFraction() const;
+  double getCurrentPositionFractionFromExternalPos(int32_t extPos_i32) const;
   int32_t getTargetPositionSteps() const;
 
 public:

SimHubPlugin/DataPluginDemo.cs

@@ -84,6 +84,8 @@ public struct payloadPedalConfig
     public float PID_i_gain;
     public float PID_d_gain;
 
+    public byte control_strategy_b;
+
     public byte maxGameOutput;
 
     // Kalman filter model noise
@@ -354,7 +356,7 @@ public void Init(PluginManager pluginManager)
 
 
             dap_config_initial_st.payloadHeader_.payloadType = 100;
-            dap_config_initial_st.payloadHeader_.version = 106;
+            dap_config_initial_st.payloadHeader_.version = 107;
             dap_config_initial_st.payloadHeader_.storeToEeprom = false;
             dap_config_initial_st.payloadPedalConfig_.pedalStartPosition = 35;
             dap_config_initial_st.payloadPedalConfig_.pedalEndPosition = 80;
@@ -394,6 +396,8 @@ public void Init(PluginManager pluginManager)
             dap_config_initial_st.payloadPedalConfig_.PID_i_gain = 50.0f;
             dap_config_initial_st.payloadPedalConfig_.PID_d_gain = 0.0f;
 
+            dap_config_initial_st.payloadPedalConfig_.control_strategy_b = 0;
+
 
         }
     }

SimHubPlugin/SettingsControlDemo.xaml

@@ -5,8 +5,7 @@
              xmlns:d="http://schemas.microsoft.com/expression/blend/2008" 
              xmlns:local="clr-namespace:User.PluginSdkDemo"
              xmlns:styles="clr-namespace:SimHub.Plugins.Styles;assembly=SimHub.Plugins" mc:Ignorable="d" 
-             xmlns:ui="clr-namespace:SimHub.Plugins.UI;assembly=SimHub.Plugins" 
-             d:DesignHeight="900" d:DesignWidth="1400">
+             xmlns:ui="clr-namespace:SimHub.Plugins.UI;assembly=SimHub.Plugins" Height="1016" Width="1296">
 
 
 
@@ -688,6 +687,37 @@
             <TextBox x:Name="TextBox2" Grid.Column="1" HorizontalAlignment="Left" Margin="7,36,0,0" Grid.Row="1" TextWrapping="Wrap" VerticalAlignment="Top" Width="373" AutomationProperties.Name="TextBox2" Height="46" Grid.RowSpan="2"/>
             <Button Content="Test-ABS/TC" Margin="27,54,0,0" Grid.Row="6" VerticalAlignment="Top" Click="TestAbs_click" Height="20" Background="#FF050550" HorizontalAlignment="Left" FontWeight="Bold" FontSize="9"/>
             <styles:SHButtonSecondary Click="DisconnectToPedal_click" HorizontalAlignment="Left" Grid.Row="2" Grid.Column="0" Width="120" Margin="200,35,0,29" FontSize="10" Background="Red" Content="Disconnect from Pedal" Grid.RowSpan="2" />
+            <TextBlock HorizontalAlignment="Left" 
+                VerticalAlignment="Top" 
+                Margin="4,132,0,0" 
+                Text="Control strategy:" 
+                Grid.Row="11" 
+                Grid.Column="0" 
+                Width="120" 
+                Foreground="White"/>
+            <Slider x:Name="PID_tuning_control_strategy_slider" 
+                HorizontalAlignment="Left" 
+                VerticalAlignment="Top" 
+                Grid.Row="11" 
+                Width="240" 
+                ValueChanged="PID_tuning_control_strategy_changed" 
+                AutoToolTipPlacement="TopLeft" 
+                AutoToolTipPrecision="0"
+                Minimum="0"
+                Maximum="1"
+                TickPlacement="BottomRight"
+                TickFrequency="1" RenderTransformOrigin="0.508,0.525"
+                Value="0"
+                IsSnapToTickEnabled="False" Margin="110,132,0,0" Height="16"
+            />
+            <TextBlock HorizontalAlignment="Left" 
+                VerticalAlignment="Top" 
+                Margin="124,148,0,0" 
+                Text="0: conventional PID, 1: dynamic PID" 
+                Grid.Row="11" 
+                Grid.Column="0" 
+                Width="257" 
+                Foreground="White" Height="37"/>
 
 
 

SimHubPlugin/SettingsControlDemo.xaml.cs

@@ -33,7 +33,7 @@ public partial class SettingsControlDemo : System.Windows.Controls.UserControl
     {
 
 
-        public uint pedalConfigPayload_version = 106;
+        public uint pedalConfigPayload_version = 107;
 
         public uint indexOfSelectedPedal_u = 1;
 
@@ -211,6 +211,8 @@ public SettingsControlDemo()
                 dap_config_st[indexOfSelectedPedal_u].payloadPedalConfig_.PID_i_gain = 50.0f;
                 dap_config_st[indexOfSelectedPedal_u].payloadPedalConfig_.PID_d_gain = 0.0f;
 
+                dap_config_st[indexOfSelectedPedal_u].payloadPedalConfig_.control_strategy_b = 0;
+
                 InitializeComponent();
 
             }
@@ -424,6 +426,8 @@ private void updateTheGuiFromConfig()
             PID_tuning_I_gain_slider.Value = (double)dap_config_st[indexOfSelectedPedal_u].payloadPedalConfig_.PID_i_gain;
             PID_tuning_D_gain_slider.Value = (double)dap_config_st[indexOfSelectedPedal_u].payloadPedalConfig_.PID_d_gain;
 
+            PID_tuning_control_strategy_slider.Value = (byte)dap_config_st[indexOfSelectedPedal_u].payloadPedalConfig_.control_strategy_b;
+
 
             maxGameOutput_slider.Value = dap_config_st[indexOfSelectedPedal_u].payloadPedalConfig_.maxGameOutput;
 
@@ -712,6 +716,14 @@ public void PID_tuning_D_gain_changed(object sender, RoutedPropertyChangedEventA
             dap_config_st[indexOfSelectedPedal_u].payloadPedalConfig_.PID_d_gain = (float)e.NewValue;
         }
 
+        public void PID_tuning_control_strategy_changed(object sender, RoutedPropertyChangedEventArgs<double> e)
+        {
+            dap_config_st[indexOfSelectedPedal_u].payloadPedalConfig_.control_strategy_b = (byte)e.NewValue;
+        }
+
+
+
+
 
         public void maxGameOutput_changed(object sender, RoutedPropertyChangedEventArgs<double> e)
         {