Stabilus 322699 wDoublePID, ErrorGetter
Dependencies: mbed QEI PID DmTftLibraryEx
Diff: main.cpp
- Revision:
- 37:5fc7f2f435e8
- Parent:
- 36:cab8aa44ef91
diff -r cab8aa44ef91 -r 5fc7f2f435e8 main.cpp --- a/main.cpp Mon Apr 11 13:48:34 2022 +0000 +++ b/main.cpp Tue Apr 12 07:54:19 2022 +0000 @@ -41,8 +41,11 @@ const float cf_PWMPeriod_s = 0.01000000; // 10ms const float cf_MOTPeriod_s = 0.01000000; // 10ms +int64_t ci64_TargetPOS = 3096; // Used also Outside + #include "PID.h" -// + +/* #define Lx 0.0001 #define Ku 0.32200//0.922000 #define Pu 0.0125 @@ -57,24 +60,15 @@ #define SorT 5 #define Databit 0 #define TIME 0.0125 - -//#define RATE (cf_MOTPeriod_s * 10.0f) -#define RATE (cf_MOTPeriod_s) - -//Timer Time; -//PID SpeedClosedLoop(P,I,D,&Time); +*/ -// Kc, Ti, Td, interval -//PID PID_VelocityClosedLoop_FW (0.1, 0.001, 0.01, RATE); - -PID PID_VelocityClosedLoop_FW (0.4, 0.0, 0.0001, RATE); -PID PID_VelocityClosedLoop_BW (0.41, 0.0, 0.01, RATE); -//PID PID_VelocityClosedLoop (P, I, D, RATE); - -//const int64_t ci64_TargetPOS = 240; // -//const int64_t ci64_TargetPOS = 3096; // - -int64_t ci64_TargetPOS = 3096; // Used as CONST ... +// Kc, Ti, Td, interval +//PID PID_VelocityClosedLoop_FW (0.4, 0.0, /*1.0E-32,*/ 1.0E-9, cf_MOTPeriod_s); +//PID PID_VelocityClosedLoop_BW (0.41, 0.0, /*1.0E-32,*/ 1.0E-9, cf_MOTPeriod_s); +//PID PID_VelocityClosedLoop_FW (0.55, 0.0, /*1.0E-32,*/ 1.0E-9, cf_MOTPeriod_s); +//PID PID_VelocityClosedLoop_BW (0.55, 0.0, /*1.0E-32,*/ 1.0E-2, cf_MOTPeriod_s); +PID PID_VelocityClosedLoop_FW (0.55, 0.0, 0.0, cf_MOTPeriod_s); +PID PID_VelocityClosedLoop_BW (0.55, 0.0, 0.0, cf_MOTPeriod_s); // LA: LCM_ShowTactics // =============== @@ -126,6 +120,7 @@ // Esegue il Movimento Programmato float f_PWMPercent; // Deprecated +float fPID_Error; // float fVelocity; float fAcceleration; @@ -226,7 +221,6 @@ } DisplayDriverInit(); -// 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] @@ -254,7 +248,8 @@ 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 = 25.0f; // Riferimento di velocità minima a cui (appena) si muove l'asse [verso FW] - in_PosizionatoreSW.f_ServoLockSpeed_x100_BW = 50.0f; // Riferimento di velocità minima a cui (appena) si muove l'asse [verso BW] +// in_PosizionatoreSW.f_ServoLockSpeed_x100_BW = 50.0f; // Riferimento di velocità minima a cui (appena) si muove l'asse [verso BW] + in_PosizionatoreSW.f_ServoLockSpeed_x100_BW = 48.0f; // Riferimento di velocità minima a cui (appena) si muove l'asse [verso BW] // JOG Mode // ======== @@ -388,6 +383,7 @@ static float f_ai0003_prec; static float f_ai0004_prec; static float f_ai0005_prec; +static float f_ai0006_prec; static uint32_t i32_Velocity_prec; static uint32_t i32_Acceleration_prec; @@ -419,6 +415,12 @@ LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 5), StringText); f_ai0005_prec = f_PWMPercent; } + if (fPID_Error != f_ai0006_prec) { + sprintf (StringText, + "PID_FB Error: %f ", fPID_Error); //, fJerk); + LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 6), StringText); + f_ai0006_prec = fPID_Error; + } /* if (i32_Acceleration != i32_Acceleration_prec) { @@ -541,6 +543,8 @@ // Set Desired Value PID_VelocityClosedLoop_FW.setSetPoint((float)out_PosizionatoreSW.i32_ATVSpeed); // Release a new output. + + fPID_Error = PID_VelocityClosedLoop_FW.getError(); f_PWMPercent = PID_VelocityClosedLoop_FW.compute(); } else { @@ -551,6 +555,8 @@ // Set Desired Value PID_VelocityClosedLoop_BW.setSetPoint((float)out_PosizionatoreSW.i32_ATVSpeed); // Release a new output. + + fPID_Error = PID_VelocityClosedLoop_BW.getError(); f_PWMPercent = PID_VelocityClosedLoop_BW.compute(); } PWM_PB3.write((float) 1.0- f_PWMPercent); // Set to x%