Velocity Closed Loop Dynamic error correction
Dependencies: mbed QEI PID DmTftLibraryEx
Diff: main.cpp
- Revision:
- 33:f77aa3ecf87d
- Parent:
- 32:1be3d79ff4db
- Child:
- 34:0522cebfe489
--- a/main.cpp Fri Apr 08 05:27:20 2022 +0000 +++ b/main.cpp Mon Apr 11 06:01:01 2022 +0000 @@ -17,25 +17,78 @@ #include "DisplayDriver.h" +const float cf_SCOPeriod_s = 0.00025000; // 250us +const float cf_PWMPeriod_s = 0.01000000; // 10ms +const float cf_MOTPeriod_s = 0.01000000; // 10ms + +//const int64_t ci64_TargetPOS = 240; // +const int64_t ci64_TargetPOS = 3200; // + +// LA: LCM_ShowTactics +// =============== +// void LCM_ShowTactics( - int32_t i32_Pulses, - int32_t i32_ATVSpeed, - float f_ai0000_Aux, - float f_ai0001_Aux, - float f_ai0002_Aux, - float f_ai0003_Aux, - float f_ai0004_Aux, - float f_ai0005_Aux + int64_t i64_Pulses, + int32_t i32_ATVSpeed, + // + float f_ai0000_Aux, + float f_ai0001_Aux, + float f_ai0002_Aux, + float f_ai0003_Aux, + float f_ai0004_Aux, + float f_ai0005_Aux, + // + int32_t i32_Velocity, + int32_t i32_Acceleration, + int32_t i32_Jerk + ); -static void SampleAndStore (void); -Ticker SampleTimer; // LA: To Sample 1AI any ms +// LA: SampleAndStore +// LA: SampleTimer == +// ============== +// +static void SampleAndStore (void); +Ticker SampleTimer; // LA: To Sample 1AI any 'x'ms + float af_PlotSamples[240]; // LA: "Horiz" Plot Array uint16_t aui16_PlotSamples[240]; uint16_t aui16_PlotClears_Lo[240]; uint16_t aui16_PlotClears_Hi[240]; +int32_t ai32_POS2VelGraph[4000]; +uint16_t aui16_PlotPOS2VelSamples[240]; +uint16_t aui16_PlotPOS2VelClears_Lo[240]; +uint16_t aui16_PlotPOS2VelClears_Hi[240]; + +int32_t ai32_POS2AccGraph[4000]; +int32_t ai32_POS2JrkGraph[4000]; + +// LA: MotionHandler +// LA: MotionTimer = +// ============= +// +static void MotionHandler (void); +Ticker MotionTimer; // LA: Gestisce il rilevamento (RT) di Velocità, Accelerazione e Jerk + // Esegue il Movimento Programmato + +float f_PWMPercent; // Deprecated + +float fVelocity; +float fAcceleration; +float fJerk; +float fTorque; + +int64_t i64_Position_Prec; +int32_t i32_Velocity; +int32_t i32_Velocity_Prec; +int32_t i32_Acceleration; +int32_t i32_Acceleration_Prec; +int32_t i32_Jerk; + +// LA: Motion +// ====== // LA: Theory of Operation // =================== // @@ -104,16 +157,16 @@ // ======= // int main (void){ -const float cf_PWMPeriod_s = 0.010; rDIR_FWD = true; // LA: Actuator Extends rENA_Off = true; // LA: Drive Power is Off + // + in_PosizionatoreSW.b_ServoLock = false; + in_PosizionatoreSW.rtServoLock_Q = false; EepromInit(); // LA: Inizializza la EEProm TimersInit(); // LA: Parte il Timer a 1ms - SampleTimer.attach_us(&SampleAndStore, 250); - // LA: FactoryReset se "userButton" premuto all'avvio // if (userButton == 0) { @@ -121,8 +174,11 @@ } DisplayDriverInit(); - PWM_PB3.period(cf_PWMPeriod_s); // LA: Avvia il PWM con TimeBase x [s] - PWM_PB3.write((float) 0.0); // Set to 0% +// SampleTimer.attach_us(&SampleAndStore, 250); // LA: Scope has its own times + SampleTimer.attach(&SampleAndStore, cf_SCOPeriod_s); // LA: Avvia l'OscilloScopio con TimeBase x [s] + MotionTimer.attach(&MotionHandler, cf_MOTPeriod_s); // LA: Avvia il Motion con TimeBase x [s] + PWM_PB3.period(cf_PWMPeriod_s); // LA: Avvia il PWM con TimeBase x [s] + PWM_PB3.write((float) 0.0); // Set to 0% // LA: Motion (1st) Setup // @@ -134,19 +190,24 @@ // POS Mode // ======== // - in_PosizionatoreSW.b_ServoLock = true; - in_PosizionatoreSW.rtServoLock_Q = false; +// in_PosizionatoreSW.b_ServoLock = true; +// in_PosizionatoreSW.rtServoLock_Q = false; // - in_PosizionatoreSW.i64_TargetPosition = 3200; // [ui] + in_PosizionatoreSW.i64_TargetPosition = ci64_TargetPOS; // [ui] in_PosizionatoreSW.i64_ActualPosition = Stabilus322699.getPulses(); // - in_PosizionatoreSW.i64_AccelerationWindow = 64; // LA: Spazio concesso all'accelerazione. - in_PosizionatoreSW.i64_DecelerationWindow = 512; // Spazio concesso alla decelerazione, è prioritario rispetto all'accelerazione. - in_PosizionatoreSW.i64_diToleranceWindow = 10; // Finestra di Tolleranza +// in_PosizionatoreSW.i64_AccelerationWindow = 32; // LA: Spazio concesso all'accelerazione. +// in_PosizionatoreSW.i64_DecelerationWindow = 256; // Spazio concesso alla decelerazione, è prioritario rispetto all'accelerazione. + in_PosizionatoreSW.i64_AccelerationWindow = 2048; // LA: Spazio concesso all'accelerazione. + in_PosizionatoreSW.i64_DecelerationWindow = 2048; // Spazio concesso alla decelerazione, è prioritario rispetto all'accelerazione. + in_PosizionatoreSW.i64_diToleranceWindow = 16; // Finestra di Tolleranza // in_PosizionatoreSW.f_MaximumSpeed_x100_FW = 16.0; // % of "i32_Max_Speed" in_PosizionatoreSW.f_MaximumSpeed_x100_BW = 60.0; // - in_PosizionatoreSW.f_ServoLockSpeed_x100_FW = 4.8; // Riferimento di velocità minima a cui (appena) si muove l'asse [verso FW] - in_PosizionatoreSW.f_ServoLockSpeed_x100_BW = 18.0; // Riferimento di velocità minima a cui (appena) si muove l'asse [verso BW] +// in_PosizionatoreSW.f_ServoLockSpeed_x100_FW = 4.8; // Riferimento di velocità minima a cui (appena) si muove l'asse [verso FW] +// in_PosizionatoreSW.f_ServoLockSpeed_x100_BW = 18.0; // Riferimento di velocità minima a cui (appena) si muove l'asse [verso BW] + in_PosizionatoreSW.f_ServoLockSpeed_x100_FW = 5.2; // Riferimento di velocità minima a cui (appena) si muove l'asse [verso FW] +// in_PosizionatoreSW.f_ServoLockSpeed_x100_FW = 4.8; // Riferimento di velocità minima a cui (appena) si muove l'asse [verso FW] + in_PosizionatoreSW.f_ServoLockSpeed_x100_BW = 22.0; // Riferimento di velocità minima a cui (appena) si muove l'asse [verso BW] // JOG Mode // ======== @@ -160,7 +221,6 @@ in_PosizionatoreSW.f_JogSpeed_x100_FW = (in_PosizionatoreSW.f_MaximumSpeed_x100_FW/ 2); // LA: JOG's the Half of Max POS's Speed in_PosizionatoreSW.f_JogSpeed_x100_BW = (in_PosizionatoreSW.f_MaximumSpeed_x100_BW/ 2); // - // LA: Color RGB Component(s) // ====================== // @@ -194,36 +254,6 @@ in_PosizionatoreSW.rtServoLock_Q = true; while (1) { - float f_PWMPercent; - - in_PosizionatoreSW.i64_ActualPosition = Stabilus322699.getPulses(); - // - PosizionatoreSW (in_PosizionatoreSW, out_PosizionatoreSW); - in_PosizionatoreSW.rtServoLock_Q = false; - - // int64_t i64_StartPosition; - // int64_t i64_Distance; - // bool b_Accelerating; // LA: bACPos_Accelerating - // bool b_MaxSpeedReached; - // bool b_Decelerating; // bACPos_Decelerating - // bool b_InPosition; - // bool b_InToleranceFW; - // bool b_InToleranceBW; - - // int32_t i32_ATVSpeed; - f_PWMPercent = ((float)out_PosizionatoreSW.i32_ATVSpeed)/ (float)in_PosizionatoreSW.i32_Max_Speed; // LA: In Range (float) 0.. 1 - PWM_PB3.write((float) 1.0- f_PWMPercent); // Set to x% - - // bool b_ATVDirectionFW; - rDIR_FWD = out_PosizionatoreSW.b_ATVDirectionFW; - - // bool b_ATVDirectionBW; - - // bool b_STW1_On; - // bool b_STW1_NoCStop; - // bool b_STW1_NoQStop; - // bool b_STW1_Enable; - rENA_Off = !out_PosizionatoreSW.b_STW1_Enable; // LA: Scope, Theory of operation. // =========================== @@ -242,37 +272,51 @@ ADC12_IN9.read(), // 8 ADC12_IN15.read(), // 9 - (f_PWMPercent* 100) // 10 + (f_PWMPercent* 100), // 10 + + i32_Velocity, // 11 + i32_Acceleration, // 12 + i32_Jerk // 13 + ); - LCM_PlotVector ( + LCM_PlotScope ( Scale2RGBColor (0, 0, 0), // Back: Black Scale2RGBColor (31, 0, 0) // Fore: Red ); + LCM_PlotSpeed ( + Scale2RGBColor (0, 0, 0), // Back: Black + Scale2RGBColor (31, 0, 0) // Fore: Red + ); if (out_PosizionatoreSW.b_InPosition) if (in_PosizionatoreSW.i64_TargetPosition > 0) in_PosizionatoreSW.i64_TargetPosition = 0; else - in_PosizionatoreSW.i64_TargetPosition = 3200; + in_PosizionatoreSW.i64_TargetPosition = ci64_TargetPOS; } } void LCM_ShowTactics( - int32_t i32_Pulses, + int64_t i64_Pulses, int32_t i32_ATVSpeed, - float f_ai0000_Aux, - float f_ai0001_Aux, - float f_ai0002_Aux, - float f_ai0003_Aux, - float f_ai0004_Aux, - float f_ai0005_Aux + // + float f_ai0000_Aux, + float f_ai0001_Aux, + float f_ai0002_Aux, + float f_ai0003_Aux, + float f_ai0004_Aux, + float f_ai0005_Aux, + // + int32_t i32_Velocity, + int32_t i32_Acceleration, + int32_t i32_Jerk ) { char StringText[MAX_CHAR_PER_LINE+ 1]; // don't forget the /0 (end of string) -static int32_t Pulses_Prec; +static int64_t i64_Pulses_Prec; static uint32_t ms_0002_prec; static float f_ai0000_prec; @@ -282,13 +326,18 @@ static float f_ai0004_prec; static float f_ai0005_prec; - if (i32_Pulses != Pulses_Prec) { +static uint32_t i32_Velocity_prec; +static uint32_t i32_Acceleration_prec; +static uint32_t i32_Jerk_prec; + + if (i64_Pulses != i64_Pulses_Prec) { sprintf (StringText, - "Pulses: %d ", i32_Pulses); + "Pulses: %d ", (int32_t)i64_Pulses); LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 1), StringText); - Pulses_Prec = i32_Pulses; + i64_Pulses_Prec = i64_Pulses; } +/* if (i32_ATVSpeed != ms_0002_prec) { sprintf (StringText, "Speed[ui]: %d ", i32_ATVSpeed); @@ -337,13 +386,30 @@ LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 10), StringText); f_ai0005_prec = f_ai0005_Aux; } +*/ + if (i32_Velocity != i32_Velocity_prec) { + sprintf (StringText, + "Vel[ui/10ms]: %d ", i32_Velocity); //, fVelocity); + LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 11), StringText); + i32_Velocity_prec = i32_Velocity; + } + if (i32_Acceleration != i32_Acceleration_prec) { + sprintf (StringText, + "Acc[ui/10ms^2]: %d ", i32_Acceleration); //, fAcceleration); + LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 12), StringText); + i32_Acceleration_prec = i32_Acceleration; + } + if (i32_Jerk != i32_Jerk_prec) { + sprintf (StringText, + "Jerk: %d ", i32_Jerk); //, fJerk); + LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 13), StringText); + i32_Jerk_prec = i32_Jerk; + } } -//static void SampleAndStore (void) { -void SampleAndStore (void) { -//static int32_t i32_Pulses; +static void SampleAndStore (void) { int16_t i16_SampleIndex; -//float f_SampleAux; +int64_t i64_SampleIndex; // af_PlotSamples[240- 1] = ADC12_IN9.read(); af_PlotSamples[240- 1] = (float) Stabilus322699.getChannelA() * 0.33f; @@ -351,7 +417,103 @@ for (i16_SampleIndex = 0; i16_SampleIndex < (240- 1); i16_SampleIndex++) af_PlotSamples[i16_SampleIndex] = af_PlotSamples[i16_SampleIndex+ 1]; -// i32_Pulses++; + // LA: Position's Graph Section + // ======================== + // + i64_SampleIndex = Stabilus322699.getPulses(); + ai32_POS2VelGraph[i64_SampleIndex] = i32_Velocity; + ai32_POS2AccGraph[i64_SampleIndex] = i32_Acceleration; + ai32_POS2JrkGraph[i64_SampleIndex] = i32_Jerk; } -// getDmTft().setPixel (0,0,1); +static void MotionHandler (void) { +static int16_t i16_Index = 0; + + // LA: Retrieve Actual Position + // + in_PosizionatoreSW.i64_ActualPosition = Stabilus322699.getPulses(); + + // LA: Execute Motion + // + PosizionatoreSW (in_PosizionatoreSW, out_PosizionatoreSW); + in_PosizionatoreSW.rtServoLock_Q = false; + + // LA: Handle PostServo + // + + // int64_t i64_StartPosition; + // int64_t i64_Distance; + // bool b_Accelerating; // LA: bACPos_Accelerating + // bool b_MaxSpeedReached; + // bool b_Decelerating; // bACPos_Decelerating + // bool b_InPosition; + // bool b_InToleranceFW; + // bool b_InToleranceBW; + + // int32_t i32_ATVSpeed; + f_PWMPercent = ((float)out_PosizionatoreSW.i32_ATVSpeed)/ (float)in_PosizionatoreSW.i32_Max_Speed; // LA: In Range (float) 0.. 1 + PWM_PB3.write((float) 1.0- f_PWMPercent); // Set to x% + + // bool b_ATVDirectionFW; + rDIR_FWD = out_PosizionatoreSW.b_ATVDirectionFW; + + // bool b_ATVDirectionBW; + + // bool b_STW1_On; + // bool b_STW1_NoCStop; + // bool b_STW1_NoQStop; + // bool b_STW1_Enable; + rENA_Off = !out_PosizionatoreSW.b_STW1_Enable; + + // LA: Update Motion Dynamic References + // ================================ + // + // Per avere maggiore granularità delle misure, l'acquisizione viene fatta ogni 10 interventi + // Se il motion gira a 10ms, i riferimenti dinamici saranno calcolati ogni 100 + // + if (i16_Index == 0) { + static uint32_t ui32_PreviousStep_ms; + uint32_t ui32_ActualStepSampled_ms; + uint32_t ui32_PassedActual_ms; + // + float fPassedActual_sxs; + + // LA: Generazione del millisecondo Attuale + // ==================================== + // + // Invoca il timer di sistema (TimersTimerValue) e lo confronta col suo precedente. + // Una volta elaborato e "scevrato" l'eventuale "Rollover" la sezione ritorna "ui32_PassedActual_ms_Local". + // "ui32_PassedActual_ms_Local" rappresenta i [ms] passati tra una istanza e l'altra + // + ui32_ActualStepSampled_ms = TimersTimerValue(); // Freezes the Actual Sample. + if (ui32_ActualStepSampled_ms >= ui32_PreviousStep_ms) + ui32_PassedActual_ms = (ui32_ActualStepSampled_ms- ui32_PreviousStep_ms); // Result => Actual- Previous + else + ui32_PassedActual_ms = ui32_ActualStepSampled_ms+ (0x7fffffff- ui32_PreviousStep_ms); // Result => Actual+ (Rollover- Previous) + ui32_PreviousStep_ms = ui32_ActualStepSampled_ms; // Store(s)&Hold(s) actual msSample + fPassedActual_sxs = ((float) 1000.0/ (float) ui32_PassedActual_ms); // Steps Any [s] + + i32_Velocity = (int32_t) (in_PosizionatoreSW.i64_ActualPosition- i64_Position_Prec); // LA: Velocity in [ui/10ms] + i64_Position_Prec = in_PosizionatoreSW.i64_ActualPosition; + i32_Acceleration = (i32_Velocity- i32_Velocity_Prec); // LA: Acceleration in [ui/10ms^2] + i32_Velocity_Prec = i32_Velocity; + i32_Jerk = (i32_Acceleration- i32_Acceleration_Prec); // LA: Jerk + i32_Acceleration_Prec = i32_Acceleration; + + fVelocity = (float) i32_Velocity * fPassedActual_sxs; // Velocity in [ui/s] + fAcceleration = (float) i32_Acceleration * fPassedActual_sxs; // Acceleration in [ui/s^2] + fJerk = (float) i32_Jerk * fPassedActual_sxs; // Jerk + } + i16_Index++; + if (i16_Index >= 10) + i16_Index = 0; + +/* + // LA: Position's Graph Section + // ======================== + // + ai32_POS2VelGraph[in_PosizionatoreSW.i64_ActualPosition] = i32_Velocity; + ai32_POS2AccGraph[in_PosizionatoreSW.i64_ActualPosition] = i32_Acceleration; + ai32_POS2JrkGraph[in_PosizionatoreSW.i64_ActualPosition] = i32_Jerk; +*/ +}