Velocity Closed Loop Dynamic error correction
Dependencies: mbed QEI PID DmTftLibraryEx
main.cpp
- Committer:
- lex9296
- Date:
- 2022-04-15
- Revision:
- 39:be7055a0e9a4
- Parent:
- 37:5fc7f2f435e8
File content as of revision 39:be7055a0e9a4:
//Warning: Incompatible redefinition of macro "MBED_RAM_SIZE" in "tmp/HU5Hqj", Line: 39, Col: 10 #ifndef MBED_RAM_SIZE #define MBED_RAM_SIZE 0x00018000 #endif // =========================================================== // =========================================================== // LA: Stage 01 cleared, Il posizionatore funziona CORRETTAMENTE // =========================================================== // =========================================================== // // Commit & Publish del 11 Aprile 2022, il Pozizionamento è corretto e testato. // La velocità imposta all'asse, tuttavia, NON E' retroazionata. // // Occorrerebbe un anello chiuso sulla velocità, che si muova "almeno" alla velocità di scansione del PWM. // Se la velocità non è ancora raggiunta, la tensione deve venire adeguata conseguentemente agendo sul PWM. // // LA: 11 Aprile 2022, Aggiungo il PID // =============================== // // La retroazione sulla Velocità (Calcolata ogni "cf_MOTPeriod_s") è eseguita, nello stesso intervallo, da "PID_VelocityClosedLoop" // Il parametro di Riferimento è la Velocity Calcolata // Il Fattore di correzione è il PWM in uscita // Con la versione Pubblicata 0022 Il Doppio PID è correttamente funzionante. // // LA: 12 Aprile 2022, Introduco la retroazione ad anello Avanzata // =========================================================== // // Provo ad escludere il PID (I PID, attualmente son due) a favore di un controllo di retroazione più incisivo. // L'attuale PID non tiene conto del verso di correzione, mentre nel moto attuale mi aspetto maggiori difficoltà a frenare che ad // accelerare. // PQM introduco un controllo (in retroazione di proporzionalità all'errore) che possa diversificarsi tra Accelerazione e FRENO. // i.e. deve essere più incisivo quando frena, meno quando accelera. // // 15/4: Retroazione Avanzata e correzione dinamica dell'errore. // // In base all'errore calcolato sulla velocità, opero una retroazione semplice per far tornare il sistema in stabilità. // Ad ogni "chiamata" del motion correggo l'uscita nel verso suggerito dall'errore di un fattore ad esso proporzionale. // Ad oggi è il risultato migliore. // // Committo // Forco // Pubblico la 0030 // #include "QEI.h" #include "SWPos.h" #include "Timers.h" #include "Eeprom.h" #include "math.h" #define MAX_CHAR_PER_LINE 28 #define TEXT_ROW_SPACING 16 #define FONT_CHAR_WIDTH 8 #define FONT_CHAR_HEIGHT 16 #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 int64_t ci64_TargetPOS = 3096; // Used also Outside // LA: LCM_ShowTactics // =============== // /* void LCM_ShowTactics( int64_t i64_Pulses, float f_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 ); */ // 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; // float F_VelocityLoopFB_FW; // LA: Correzione Dinamica FW float F_VelocityLoopFB_BW; // LA: Correzione Dinamica BW //float fPID_Error; // float fMOT_VelocityAct; // 0.0= 0%, 1.0= 100% float fMOT_VelocityReq; // 0.0= 0%, 1.0= 100% float fMOT_VelocityError; // 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 // =================== // // Il PWM funziona da sè in Interrupt // Il QEI funziona da sè in Interrupt // Se si creano dei Ticker (Che sono a loro volta interrupt(s)) è possibile che PWM e QEI perdano correlazione con l'HW. // // PQM // // Il rinfresco del Display e la gestione del motion vanno fatte il più frequentemente possibile ma fuori dal loop dei Ticker. // Con qst versione (LA0005, che termina un FORK (il successivo è LA0010) quanto detto sopra è FUNZIONANTE. // Questo messaggio è incluso nel "commitment" /*! * \brief Define IO for Unused Pin */ //DigitalOut F_CS (D6); // MBED description of pin //DigitalOut SD_CS (D8); // MBED description of pin DigitalIn userButton (USER_BUTTON); // DigitalOut rENA_Off (PC_0); // CN7.38 - Power Enable Relay, Power Disabled when true DigitalOut rDIR_FWD (PC_1); // CN7.36 - Move Direction Relay Bridge, Move FW(Extends) when true AnalogIn adc_temp (ADC_TEMP); AnalogIn adc_vref (ADC_VREF); AnalogIn adc_vbat (ADC_VBAT); AnalogIn ADC12_IN9 (PA_4); // STM32 PA4 AnalogIn ADC12_IN15 (PB_0); // STM32 PB0 // PWM // === // PwmOut PWM_PB3(PWM_OUT); // LA: PWM_OUT = D3 = PB_3 // QEI // === // QEI Stabilus322699 (PA_1, PA_0, NC, 100, QEI::X4_ENCODING); //DigitalIn Hall_A (PA_1); //DigitalIn Hall_B (PA_0); // Motion // ====== // //Ticker POS_MotionScan; // LA: Non uso un Ticker. Agisce sotto Interrupt e falsa la lettura QEI e la sincronicità del PWM // in_sPosizionatoreSW in_PosizionatoreSW; out_sPosizionatoreSW out_PosizionatoreSW; // LCD Display // =========== // //Ticker LCD_RefreshViews; // LA: Non uso un Ticker. Agisce sotto Interrupt e falsa la lettura QEI e la sincronicità del PWM void FactoryReset (void) { EepromFactoryReset( ); HAL_NVIC_SystemReset( ); } void LCM_ShowTactics( int64_t i64_Pulses, float f_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 ) { char StringText[MAX_CHAR_PER_LINE+ 1]; // don't forget the /0 (end of string) static int64_t i64_Pulses_Prec; static float f_ai0005_prec; static float f_ai0006_prec; //static float f_ai0007_prec; static float f_ai0008_prec; static uint32_t i32_Velocity_prec; static float f_ATVSpeed_prec; if (i64_Pulses != i64_Pulses_Prec) { sprintf (StringText, "Pulses: %d ", (int32_t)i64_Pulses); LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 1), StringText); i64_Pulses_Prec = i64_Pulses; } if (i32_Velocity != i32_Velocity_prec) { sprintf (StringText, "Velocity[ui/ms]: %d ", i32_Velocity); LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 2), StringText); i32_Velocity_prec = i32_Velocity; } if (fMOT_VelocityAct != f_ATVSpeed_prec) { sprintf (StringText, "VAct[0..1]: %f ", fMOT_VelocityAct); LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 3), StringText); f_ATVSpeed_prec = fMOT_VelocityAct; } if (fMOT_VelocityReq != f_ai0008_prec) { sprintf (StringText, "VReq[0..1]: %f ", fMOT_VelocityReq); LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 4), StringText); f_ai0008_prec = fMOT_VelocityReq; } if (f_PWMPercent != f_ai0005_prec) { sprintf (StringText, "PWM [0..1]: %f ", f_PWMPercent); LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 5), StringText); f_ai0005_prec = f_PWMPercent; } if (fMOT_VelocityError != f_ai0006_prec) { sprintf (StringText, "Err [0..1): %f ", fMOT_VelocityError); LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 6), StringText); f_ai0006_prec = fMOT_VelocityError; } /* if (F_VelocityLoopFB != f_ai0007_prec) { sprintf (StringText, "Corr[0..1]: %f ", F_VelocityLoopFB); //, fJerk); LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 7), StringText); f_ai0007_prec = F_VelocityLoopFB; } */ } static void SampleAndStore (void) { int16_t i16_SampleIndex; int64_t i64_SampleIndex; // af_PlotSamples[240- 1] = ADC12_IN9.read(); af_PlotSamples[240- 1] = (float) Stabilus322699.getChannelA() * 0.33f; for (i16_SampleIndex = 0; i16_SampleIndex < (240- 1); i16_SampleIndex++) af_PlotSamples[i16_SampleIndex] = af_PlotSamples[i16_SampleIndex+ 1]; // 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; } static void MotionHandler (void) { static uint32_t ui32_PreviousStep_ms; uint32_t ui32_ActualStepSampled_ms; uint32_t ui32_PassedActual_ms; // float fPassedActual_sxs; // 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 // ================================ // 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/ms] i64_Position_Prec = in_PosizionatoreSW.i64_ActualPosition; i32_Acceleration = (i32_Velocity- i32_Velocity_Prec); // LA: Acceleration in [ui/ms^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 // LA: Elaborazione dell'Uscita di Controllo della velocità // ==================================================== // // Verifica della velocità raggiunta. // Calcolo dell'ERRORE. // Elaborazione e applicazione della correzione. // // LA: i32_Velocity, Velocità Attuale (Reale, rilevata) // LA: out_PosizionatoreSW.i32_ATVSpeed, Velocità Richiesta (Elaborata in base al profilo) // // Si suppone che la velocità massima sia raggiungibile al massimo valore di f_PWMPercent (-> 1.0) // Si suppone che la velocità minima sia raggiungibile al minimo valore di f_PWMPercent (-> 0.0) // // LA: Calcolo dell'Errore scalato 0..1 // fMOT_VelocityAct = ((float)abs(i32_Velocity)/ in_PosizionatoreSW.f_Max_Speed); // 0.0= 0%, 1.0= 100% fMOT_VelocityReq = (out_PosizionatoreSW.f_ATVSpeed/ in_PosizionatoreSW.f_Max_Speed); // 0.0= 0%, 1.0= 100% fMOT_VelocityError = fMOT_VelocityReq- fMOT_VelocityAct; // // f_PWMPercent = out_PosizionatoreSW.f_ATVSpeed/ in_PosizionatoreSW.f_Max_Speed; // LA: In Range (float) 0.. 1 if (out_PosizionatoreSW.b_ATVDirectionFW) { F_VelocityLoopFB_BW = 0.0f; f_PWMPercent = (out_PosizionatoreSW.f_ATVSpeed/ in_PosizionatoreSW.f_Max_Speed)/ 4.0f; // FW is about 4:1 BW f_PWMPercent += (fMOT_VelocityError* 0.1f); /* if (fMOT_VelocityError > 0.001f) F_VelocityLoopFB_FW += 0.0002f; if (fMOT_VelocityError < -0.001f) F_VelocityLoopFB_FW -= 0.0002f; */ F_VelocityLoopFB_FW += (0.004f* fMOT_VelocityError); f_PWMPercent += F_VelocityLoopFB_FW; } else { F_VelocityLoopFB_FW = 0.0f; f_PWMPercent = (out_PosizionatoreSW.f_ATVSpeed/ in_PosizionatoreSW.f_Max_Speed)/ 1.4f; f_PWMPercent += (fMOT_VelocityError* 0.1f); /* if (fMOT_VelocityError > 0.001f) F_VelocityLoopFB_BW += 0.0005f; if (fMOT_VelocityError < -0.001f) F_VelocityLoopFB_BW -= 0.0005f; */ F_VelocityLoopFB_BW += (0.01f* fMOT_VelocityError); f_PWMPercent += F_VelocityLoopFB_BW; } if (f_PWMPercent > 1.0f) f_PWMPercent = 1.0f; if (f_PWMPercent < 0.0f) f_PWMPercent = 0.0f; PWM_PB3.write((float) 1.0- f_PWMPercent); // Set to x% } // ======= // ======= // Main(s) // ======= // ======= // int main (void){ 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 // LA: FactoryReset se "userButton" premuto all'avvio // if (userButton == 0) { FactoryReset(); } DisplayDriverInit(); 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(0.0f); // Set to 0% // LA: Ok // LA: Motion (1st) Setup // in_PosizionatoreSW.b_AxisPowered = true; in_PosizionatoreSW.b_ACPos_Homed = true; in_PosizionatoreSW.f_Max_Speed = 20.0f; // [ui/ms] in_PosizionatoreSW.f_ZeroSpeed = 0.0f; // // POS Mode // ======== // // in_PosizionatoreSW.b_ServoLock = true; // in_PosizionatoreSW.rtServoLock_Q = false; // in_PosizionatoreSW.i64_TargetPosition = ci64_TargetPOS; // [ui] in_PosizionatoreSW.i64_ActualPosition = Stabilus322699.getPulses(); // in_PosizionatoreSW.i64_AccelerationWindow = 64; // LA: Spazio concesso alla rampa di Accelerazione in_PosizionatoreSW.i64_DecelerationWindow = 1024; // Spazio concesso alla rampa di Decelerazione in_PosizionatoreSW.i64_diToleranceWindow = 16; // Finestra di Tolleranza // in_PosizionatoreSW.f_MaximumSpeed_x100_FW = 100.0f; // % of "i32_Max_Speed" in_PosizionatoreSW.f_MaximumSpeed_x100_BW = 100.0f; // // in_PosizionatoreSW.f_ServoLockSpeed_x100_FW = 16.0f; // Riferimento di velocità minima a cui (appena) si muove l'asse [verso FW] // in_PosizionatoreSW.f_ServoLockSpeed_x100_BW = 24.0f; // Riferimento di velocità minima a cui (appena) si muove l'asse [verso BW] in_PosizionatoreSW.f_ServoLockSpeed_x100_FW = 24.0f; // Riferimento di velocità minima a cui (appena) si muove l'asse [verso FW] in_PosizionatoreSW.f_ServoLockSpeed_x100_BW = 24.0f; // Riferimento di velocità minima a cui (appena) si muove l'asse [verso BW] // LA: Ok // JOG Mode // ======== // in_PosizionatoreSW.b_JogMode = false; in_PosizionatoreSW.b_JogFW = false; in_PosizionatoreSW.b_JogBW = false; in_PosizionatoreSW.i32_JogAccel_ms = 500; // [ms] in_PosizionatoreSW.i32_JogDecel_ms = 250; // // 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: Ok // LA: Color RGB Component(s) // ====================== // // RED 0000 1000 0000 0000 min 0x0800 02048 // 1111 1000 0000 0000 max 0xf800 63488 // // GREEN 0000 0000 0010 0000 min 0x0020 00032 // 0000 0111 1110 0000 max 0x07e0 02016 // // BLUE 0000 0000 0000 0001 min 0x0001 00001 // 0000 0000 0001 1111 max 0x001f 00031 // // La componente ROSSA ha 5 bit di escursione (0.. 31), // La componente VERDE ha 6 bit di escursione (0.. 63), // La componente BLU ha 5 bit di escursione (0.. 31), // // Le componenti RGB di "Color" sono quindi scritte negli appropriati registri come segue: // // writeReg(RED, (Color & 0xf800) >> 11); // writeReg(GREEN, (Color & 0x07e0) >> 5); // writeReg(BLUE, (Color & 0x001f)); // LCM_SetTextColor(Scale2RGBColor (0, 0, 0), Scale2RGBColor (31, 0, 0)); // LA: Red on Black LCM_ClearScreen (Scale2RGBColor (0, 0, 0)); // Black Background LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 0), "You Start Me Up ..."); // Intro Text // rDIR_FWD = false; // Collapse // rENA_Off = false; // Power On in_PosizionatoreSW.b_ServoLock = true; in_PosizionatoreSW.rtServoLock_Q = true; // LA: Ok while (1) { // LA: Ok // LA: Scope, Theory of operation. // =========================== // // 1) Sample a Value @ any Step // 2) Store @ the correct ms // 3) Plot the current Section of the Sampling Vector // LCM_ShowTactics ( Stabilus322699.getPulses(), // Row 1 out_PosizionatoreSW.f_ATVSpeed, // 3 adc_temp.read(), // 4 adc_vbat.read(), // 5 adc_vref.read(), // 6 ADC12_IN9.read(), // 8 ADC12_IN15.read(), // 9 (f_PWMPercent* 100), // 10 i32_Velocity, // 11 i32_Acceleration, // 12 i32_Jerk // 13 ); 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 = ci64_TargetPOS; } }