Diff: main.cpp

Revision:

36:cab8aa44ef91

Parent:

35:0f6adc0b95b9

Child:

37:5fc7f2f435e8

--- a/main.cpp	Mon Apr 11 11:52:49 2022 +0000
+++ b/main.cpp	Mon Apr 11 13:48:34 2022 +0000
@@ -64,8 +64,11 @@
 //Timer Time;
 //PID SpeedClosedLoop(P,I,D,&Time);
 
-//                          Kc,  Ti,  Td,  interval
-PID PID_VelocityClosedLoop (1.2, 0.0, 0.0, RATE);
+//                             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;   //
@@ -251,7 +254,7 @@
     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 =   25.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]
 
     //  JOG Mode
     //  ========
@@ -269,15 +272,17 @@
     //  =================
     //
     //  Input Speed (ref)=  0.. 512[ui/s]
-    PID_VelocityClosedLoop.setInputLimits(0.0f, (float)in_PosizionatoreSW.i32_Max_Speed);
+    PID_VelocityClosedLoop_FW.setInputLimits(0.0f, (float)in_PosizionatoreSW.i32_Max_Speed);
+    PID_VelocityClosedLoop_BW.setInputLimits(0.0f, (float)in_PosizionatoreSW.i32_Max_Speed);
     //  Output PWM  (ref)=  0.. 1
-    PID_VelocityClosedLoop.setOutputLimits(0.0f, 1.0f);
+    PID_VelocityClosedLoop_FW.setOutputLimits(0.0f, 1.0f);
+    PID_VelocityClosedLoop_BW.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);
+//    PID_VelocityClosedLoop.setMode(AUTO_MODE);
+    PID_VelocityClosedLoop_FW.setMode(MANUAL_MODE);
+    PID_VelocityClosedLoop_BW.setMode(MANUAL_MODE);
 
     // LA:  Color RGB Component(s)
     //      ======================
@@ -528,15 +533,25 @@
     // LA:  PID Compute Section
     //      ===================
     //
-
-    //  Update the process variable.
-    PID_VelocityClosedLoop.setProcessValue((float)i32_Velocity);
+    if  (out_PosizionatoreSW.b_ATVDirectionFW) {
+        PID_VelocityClosedLoop_BW.reset();
 
-    //  Set Desired Value
-    PID_VelocityClosedLoop.setSetPoint((float)out_PosizionatoreSW.i32_ATVSpeed);
+        //  Update the process variable.
+        PID_VelocityClosedLoop_FW.setProcessValue((float)i32_Velocity);
+        //  Set Desired Value
+        PID_VelocityClosedLoop_FW.setSetPoint((float)out_PosizionatoreSW.i32_ATVSpeed);
+        //  Release a new output.
+        f_PWMPercent =  PID_VelocityClosedLoop_FW.compute();
+    }
+    else {
+        PID_VelocityClosedLoop_FW.reset();
 
-    //  Release a new output.
-    f_PWMPercent =  PID_VelocityClosedLoop.compute();
+        //  Update the process variable.
+        PID_VelocityClosedLoop_BW.setProcessValue((float)i32_Velocity);
+        //  Set Desired Value
+        PID_VelocityClosedLoop_BW.setSetPoint((float)out_PosizionatoreSW.i32_ATVSpeed);
+        //  Release a new output.
+        f_PWMPercent =  PID_VelocityClosedLoop_BW.compute();
+    }
     PWM_PB3.write((float) 1.0- f_PWMPercent);                                                               //      Set to x%
-
 }