diff --git a/Arduino/Esp32/Main/DiyActivePedal_types.cpp b/Arduino/Esp32/Main/DiyActivePedal_types.cpp index 633287b3..77366a0c 100644 --- a/Arduino/Esp32/Main/DiyActivePedal_types.cpp +++ b/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; diff --git a/Arduino/Esp32/Main/DiyActivePedal_types.h b/Arduino/Esp32/Main/DiyActivePedal_types.h index 900c55d8..87ff6dbe 100644 --- a/Arduino/Esp32/Main/DiyActivePedal_types.h +++ b/Arduino/Esp32/Main/DiyActivePedal_types.h @@ -3,7 +3,7 @@ #include -#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; diff --git a/Arduino/Esp32/Main/ForceCurve.cpp b/Arduino/Esp32/Main/ForceCurve.cpp index 1ef87dfa..c58471f0 100644 --- a/Arduino/Esp32/Main/ForceCurve.cpp +++ b/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; } diff --git a/Arduino/Esp32/Main/ForceCurve.h b/Arduino/Esp32/Main/ForceCurve.h index e971bfee..c2251d97 100644 --- a/Arduino/Esp32/Main/ForceCurve.h +++ b/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); }; diff --git a/Arduino/Esp32/Main/Main.ino b/Arduino/Esp32/Main/Main.ino index ca905c77..9fc11452 100644 --- a/Arduino/Esp32/Main/Main.ino +++ 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 rtDebugFilter({ "ESP_pos", "ESP_tar_pos", "ISV_pos"}); - rtDebugFilter.offerData({ stepper->getCurrentPositionSteps(), Position_Next, -(int32_t)isv57.servo_pos_given_p}); + static RTDebugOutput 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(); diff --git a/Arduino/Esp32/Main/Modbus.cpp b/Arduino/Esp32/Main/Modbus.cpp index b0e9fa8b..04bf6513 100644 --- a/Arduino/Esp32/Main/Modbus.cpp +++ b/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; diff --git a/Arduino/Esp32/Main/StepperMovementStrategy.h b/Arduino/Esp32/Main/StepperMovementStrategy.h index 1c4b0f93..f7c9840f 100644 --- a/Arduino/Esp32/Main/StepperMovementStrategy.h +++ b/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 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 diff --git a/Arduino/Esp32/Main/StepperWithLimits.cpp b/Arduino/Esp32/Main/StepperWithLimits.cpp index 8923da9d..2a388afe 100644 --- a/Arduino/Esp32/Main/StepperWithLimits.cpp +++ b/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(); } diff --git a/Arduino/Esp32/Main/StepperWithLimits.h b/Arduino/Esp32/Main/StepperWithLimits.h index cbc01096..ebee0727 100644 --- a/Arduino/Esp32/Main/StepperWithLimits.h +++ b/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: diff --git a/SimHubPlugin/.vs/User.PluginSdkDemo/FileContentIndex/62866acf-4e5e-46d1-961c-bfc288b4c7fd.vsidx b/SimHubPlugin/.vs/User.PluginSdkDemo/FileContentIndex/62866acf-4e5e-46d1-961c-bfc288b4c7fd.vsidx new file mode 100644 index 00000000..910e58ed Binary files /dev/null and b/SimHubPlugin/.vs/User.PluginSdkDemo/FileContentIndex/62866acf-4e5e-46d1-961c-bfc288b4c7fd.vsidx differ diff --git a/SimHubPlugin/.vs/User.PluginSdkDemo/FileContentIndex/77078788-67ee-401b-b352-c82fe0bf1301.vsidx b/SimHubPlugin/.vs/User.PluginSdkDemo/FileContentIndex/77078788-67ee-401b-b352-c82fe0bf1301.vsidx deleted file mode 100644 index 47332ba0..00000000 Binary files a/SimHubPlugin/.vs/User.PluginSdkDemo/FileContentIndex/77078788-67ee-401b-b352-c82fe0bf1301.vsidx and /dev/null differ diff --git a/SimHubPlugin/.vs/User.PluginSdkDemo/FileContentIndex/a8cf2515-f953-40c8-ab2f-7d6ac18ca14f.vsidx b/SimHubPlugin/.vs/User.PluginSdkDemo/FileContentIndex/8e783dfc-0a32-4ca8-a149-d7796c782bbb.vsidx similarity index 99% rename from SimHubPlugin/.vs/User.PluginSdkDemo/FileContentIndex/a8cf2515-f953-40c8-ab2f-7d6ac18ca14f.vsidx rename to SimHubPlugin/.vs/User.PluginSdkDemo/FileContentIndex/8e783dfc-0a32-4ca8-a149-d7796c782bbb.vsidx index 16601aef..5ccc1548 100644 Binary files a/SimHubPlugin/.vs/User.PluginSdkDemo/FileContentIndex/a8cf2515-f953-40c8-ab2f-7d6ac18ca14f.vsidx and b/SimHubPlugin/.vs/User.PluginSdkDemo/FileContentIndex/8e783dfc-0a32-4ca8-a149-d7796c782bbb.vsidx differ diff --git a/SimHubPlugin/.vs/User.PluginSdkDemo/FileContentIndex/35bed095-b612-41be-9039-4de3dc19343a.vsidx b/SimHubPlugin/.vs/User.PluginSdkDemo/FileContentIndex/9e4d46b7-b8db-4f19-8986-6a57f5d15c71.vsidx similarity index 74% rename from SimHubPlugin/.vs/User.PluginSdkDemo/FileContentIndex/35bed095-b612-41be-9039-4de3dc19343a.vsidx rename to SimHubPlugin/.vs/User.PluginSdkDemo/FileContentIndex/9e4d46b7-b8db-4f19-8986-6a57f5d15c71.vsidx index 7bdbf10c..b50e1031 100644 Binary files a/SimHubPlugin/.vs/User.PluginSdkDemo/FileContentIndex/35bed095-b612-41be-9039-4de3dc19343a.vsidx and b/SimHubPlugin/.vs/User.PluginSdkDemo/FileContentIndex/9e4d46b7-b8db-4f19-8986-6a57f5d15c71.vsidx differ diff --git a/SimHubPlugin/.vs/User.PluginSdkDemo/FileContentIndex/c1243db2-c8ff-4845-972b-a62d6ef74dcd.vsidx b/SimHubPlugin/.vs/User.PluginSdkDemo/FileContentIndex/c1243db2-c8ff-4845-972b-a62d6ef74dcd.vsidx new file mode 100644 index 00000000..244a401d Binary files /dev/null and b/SimHubPlugin/.vs/User.PluginSdkDemo/FileContentIndex/c1243db2-c8ff-4845-972b-a62d6ef74dcd.vsidx differ diff --git a/SimHubPlugin/.vs/User.PluginSdkDemo/FileContentIndex/ca20e1fe-1d78-4f73-bbe3-b7fef0d33a6f.vsidx b/SimHubPlugin/.vs/User.PluginSdkDemo/FileContentIndex/ca20e1fe-1d78-4f73-bbe3-b7fef0d33a6f.vsidx deleted file mode 100644 index ca4804f3..00000000 Binary files a/SimHubPlugin/.vs/User.PluginSdkDemo/FileContentIndex/ca20e1fe-1d78-4f73-bbe3-b7fef0d33a6f.vsidx and /dev/null differ diff --git a/SimHubPlugin/.vs/User.PluginSdkDemo/v17/.suo b/SimHubPlugin/.vs/User.PluginSdkDemo/v17/.suo index 5d523a32..3f2388ed 100644 Binary files a/SimHubPlugin/.vs/User.PluginSdkDemo/v17/.suo and b/SimHubPlugin/.vs/User.PluginSdkDemo/v17/.suo differ diff --git a/SimHubPlugin/DataPluginDemo.cs b/SimHubPlugin/DataPluginDemo.cs index 23712a53..852d2f66 100644 --- a/SimHubPlugin/DataPluginDemo.cs +++ b/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; + } } diff --git a/SimHubPlugin/SettingsControlDemo.xaml b/SimHubPlugin/SettingsControlDemo.xaml index db53d524..db7466ab 100644 --- a/SimHubPlugin/SettingsControlDemo.xaml +++ b/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 @@