All the previous but the PID

Dependencies:   mbed QEI PID DmTftLibraryEx

Files at this revision

API Documentation at this revision

Comitter:
lex9296
Date:
Mon Apr 11 09:20:40 2022 +0000
Parent:
33:f77aa3ecf87d
Commit message:
Added some PID (Velocity Loop is Closed)

Changed in this revision

Display/DisplayDriver.cpp Show annotated file Show diff for this revision Revisions of this file
Display/DisplayDriver.h Show annotated file Show diff for this revision Revisions of this file
PID.lib Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
diff -r f77aa3ecf87d -r 0522cebfe489 Display/DisplayDriver.cpp
--- a/Display/DisplayDriver.cpp	Mon Apr 11 06:01:01 2022 +0000
+++ b/Display/DisplayDriver.cpp	Mon Apr 11 09:20:40 2022 +0000
@@ -570,7 +570,8 @@
     }
 }
 
-const int64_t   ci64_TargetPOS =    3096;   //
+//extern  const int64_t   ci64_TargetPOS =    3096;   //
+extern  int64_t ci64_TargetPOS;
 
 extern  int32_t     ai32_POS2VelGraph[4000];
 extern  uint16_t    aui16_PlotPOS2VelSamples[240];
@@ -587,14 +588,13 @@
 
         // LA:  Clear Previous Plot by means of the Hi/Lo Array(s)
         //
-        Tft.drawVerticalLineEx  (ui16_Index000, 150 - aui16_PlotPOS2VelClears_Hi[ui16_Index000], (int16_t) aui16_PlotPOS2VelClears_Hi[ui16_Index000]- aui16_PlotPOS2VelClears_Lo[ui16_Index000], ui16_Background);
+        Tft.drawVerticalLineEx  (ui16_Index000, 175 - aui16_PlotPOS2VelClears_Hi[ui16_Index000], (int16_t) aui16_PlotPOS2VelClears_Hi[ui16_Index000]- aui16_PlotPOS2VelClears_Lo[ui16_Index000], ui16_Background);
 
         // LA:  Then PLOT the New
         //
-//        aui16_PlotPOS2VelClears_Hi[ui16_Index000] = (uint16_t) (ai32_POS2VelGraph[(uint16_t)((float)ui16_Index000* ((float)ci64_TargetPOS/ (float)240.0))]/ 2);
-        aui16_PlotPOS2VelClears_Hi[ui16_Index000] = (uint16_t) (ai32_POS2VelGraph[(ui16_Index000* ci64_TargetPOS)/ 240]/ 2);
-        aui16_PlotPOS2VelClears_Lo[ui16_Index000] = (uint16_t) (ai32_POS2VelGraph[((ui16_Index000* ci64_TargetPOS)/ 240)- 1]/ 2);
+        aui16_PlotPOS2VelClears_Hi[ui16_Index000] = (uint16_t) (ai32_POS2VelGraph[(ui16_Index000* ci64_TargetPOS)/ 240]/ 3);
+        aui16_PlotPOS2VelClears_Lo[ui16_Index000] = (uint16_t) (ai32_POS2VelGraph[((ui16_Index000* ci64_TargetPOS)/ 240)- 1]/ 3);
         //
-        Tft.drawVerticalLineEx  (ui16_Index000, 150 - aui16_PlotPOS2VelClears_Hi[ui16_Index000], (int16_t) aui16_PlotPOS2VelClears_Hi[ui16_Index000]- aui16_PlotPOS2VelClears_Lo[ui16_Index000], ui16_Foreground);
+        Tft.drawVerticalLineEx  (ui16_Index000, 175 - aui16_PlotPOS2VelClears_Hi[ui16_Index000], (int16_t) aui16_PlotPOS2VelClears_Hi[ui16_Index000]- aui16_PlotPOS2VelClears_Lo[ui16_Index000], ui16_Foreground);
     }
 }
\ No newline at end of file
diff -r f77aa3ecf87d -r 0522cebfe489 Display/DisplayDriver.h
--- a/Display/DisplayDriver.h	Mon Apr 11 06:01:01 2022 +0000
+++ b/Display/DisplayDriver.h	Mon Apr 11 09:20:40 2022 +0000
@@ -29,7 +29,6 @@
 #define TEXT_VALUE                  YELLOW
 #define CIRCLE_FCOLOR               GRAY1
 
-
 typedef enum
 {
     GO_STATUS_NOERR,
diff -r f77aa3ecf87d -r 0522cebfe489 PID.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID.lib	Mon Apr 11 09:20:40 2022 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/aberk/code/PID/#6e12a3e5af19
diff -r f77aa3ecf87d -r 0522cebfe489 main.cpp
--- a/main.cpp	Mon Apr 11 06:01:01 2022 +0000
+++ b/main.cpp	Mon Apr 11 09:20:40 2022 +0000
@@ -4,6 +4,19 @@
 #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.
+//
+
 #include "QEI.h"
 #include "SWPos.h"
 
@@ -21,8 +34,37 @@
 const float cf_PWMPeriod_s = 0.01000000;    //  10ms
 const float cf_MOTPeriod_s = 0.01000000;    //  10ms
 
+#include "PID.h"
+//
+#define Lx 0.0001
+#define Ku 0.32200//0.922000
+#define Pu 0.0125
+#define Kp Ku*0.6
+#define Ti Pu*0.5
+#define Td Pu*0.125
+//
+#define P Kp
+#define I Kp/Ti
+#define D Kp*Td
+#define Mstate 6
+#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 (1.0, 0.0, 0.0, RATE);
+//PID PID_VelocityClosedLoop (P, I, D, RATE);
+
 //const int64_t   ci64_TargetPOS =    240;   //
-const int64_t   ci64_TargetPOS =    3200;   //
+//const int64_t   ci64_TargetPOS =    3096;   //
+
+int64_t ci64_TargetPOS =    3096;   //  Used as CONST ...
 
 // LA:  LCM_ShowTactics
 //      ===============
@@ -184,7 +226,7 @@
     //
     in_PosizionatoreSW.b_AxisPowered =  true;
     in_PosizionatoreSW.b_ACPos_Homed =  true;
-    in_PosizionatoreSW.i32_Max_Speed =  1024;       // [ui]
+    in_PosizionatoreSW.i32_Max_Speed =  512;        // [ui/s]
     in_PosizionatoreSW.i32_ZeroSpeed =  0;          //
 
     //  POS Mode
@@ -197,17 +239,19 @@
     in_PosizionatoreSW.i64_ActualPosition = Stabilus322699.getPulses();     //
 //    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_AccelerationWindow = 1024;                        // 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_MaximumSpeed_x100_FW = 16.0;                       // % of "i32_Max_Speed"
+//    in_PosizionatoreSW.f_MaximumSpeed_x100_BW = 62.0;                       //
+    in_PosizionatoreSW.f_MaximumSpeed_x100_FW = 26.0;                       // % of "i32_Max_Speed"
+    in_PosizionatoreSW.f_MaximumSpeed_x100_BW = 100.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 =   5.2;                    //      Riferimento di velocità minima a cui (appena) si muove l'asse [verso FW]
+    in_PosizionatoreSW.f_ServoLockSpeed_x100_FW =   6.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]
+    in_PosizionatoreSW.f_ServoLockSpeed_x100_BW =   24.0;                   //      Riferimento di velocità minima a cui (appena) si muove l'asse [verso BW]
 
     //  JOG Mode
     //  ========
@@ -221,6 +265,20 @@
     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);     //
 
+    //  Velocity Loop PID
+    //  =================
+    //
+    //  Input Speed (ref)=  0.. 512[ui/s]
+    PID_VelocityClosedLoop.setInputLimits(0.0f, (float)in_PosizionatoreSW.i32_Max_Speed);
+    //  Output PWM  (ref)=  0.. 1
+    PID_VelocityClosedLoop.setOutputLimits(0.0f, 1.0f);
+
+    //  If there's a bias.
+//    PID_VelocityClosedLoop.setBias(0.3);
+    PID_VelocityClosedLoop.setMode(AUTO_MODE);
+    //  Starts @Zero
+    PID_VelocityClosedLoop.setSetPoint(0.0f);
+
     // LA:  Color RGB Component(s)
     //      ======================
     //
@@ -390,19 +448,22 @@
     if  (i32_Velocity != i32_Velocity_prec) {
         sprintf (StringText,
                 "Vel[ui/10ms]:   %d ", i32_Velocity);                   //, fVelocity);
-        LCM_DrawString  (0, 0+ (TEXT_ROW_SPACING* 11), StringText);
+//        LCM_DrawString  (0, 0+ (TEXT_ROW_SPACING* 11), StringText);
+        LCM_DrawString  (0, 0+ (TEXT_ROW_SPACING* 3), 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);
+//        LCM_DrawString  (0, 0+ (TEXT_ROW_SPACING* 12), StringText);
+        LCM_DrawString  (0, 0+ (TEXT_ROW_SPACING* 4), 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);
+//        LCM_DrawString  (0, 0+ (TEXT_ROW_SPACING* 13), StringText);
+        LCM_DrawString  (0, 0+ (TEXT_ROW_SPACING* 5), StringText);
         i32_Jerk_prec = i32_Jerk;
     }
 }
@@ -451,8 +512,8 @@
     //    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%
+//    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;
@@ -471,7 +532,7 @@
     //  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) {
+//    if  (i16_Index == 0) {
     static uint32_t ui32_PreviousStep_ms;
     uint32_t    ui32_ActualStepSampled_ms;
     uint32_t    ui32_PassedActual_ms;
@@ -503,10 +564,25 @@
         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:  PID Compute Section
+        //      ===================
+        //
+
+        //  Update the process variable.
+        PID_VelocityClosedLoop.setProcessValue((float)i32_Velocity);
+
+        //  Set Desired Value
+        PID_VelocityClosedLoop.setSetPoint((float)out_PosizionatoreSW.i32_ATVSpeed);
+
+        //  Release a new output.
+//        f_PWMPercent =  ((float)out_PosizionatoreSW.i32_ATVSpeed)/ (float)in_PosizionatoreSW.i32_Max_Speed;     // LA:  In Range (float) 0.. 1
+        f_PWMPercent =  PID_VelocityClosedLoop.compute();
+        PWM_PB3.write((float) 1.0- f_PWMPercent);                                                               //      Set to x%
+//    }
+//    i16_Index++;
+//    if  (i16_Index >= 10)
+//        i16_Index = 0;
 
 /*
     // LA:  Position's Graph Section
@@ -517,3 +593,37 @@
     ai32_POS2JrkGraph[in_PosizionatoreSW.i64_ActualPosition] =  i32_Jerk;
 */
 }
+
+/*
+#include "PID.h"
+
+#define RATE 0.1
+
+//Kc, Ti, Td, interval
+PID controller(1.0, 0.0, 0.0, RATE);
+AnalogIn pv(p15);
+PwmOut   co(p26);
+
+int main(){
+
+  //Analog input from 0.0 to 3.3V
+  controller.setInputLimits(0.0, 3.3);
+  //Pwm output from 0.0 to 1.0
+  controller.setOutputLimits(0.0, 1.0);
+  //If there's a bias.
+  controller.setBias(0.3);
+  controller.setMode(AUTO_MODE);
+  //We want the process variable to be 1.7V
+  controller.setSetPoint(1.7);
+
+  while(1){
+    //Update the process variable.
+    controller.setProcessValue(pv.read());
+    //Set the new output.
+    co = controller.compute();
+    //Wait for another loop calculation.
+    wait(RATE);
+  }
+
+}
+*/
\ No newline at end of file