Diff: main.cpp

Revision:

37:5fc7f2f435e8

Parent:

36:cab8aa44ef91

--- 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%