Luca Antolini
All the previous but the PID
Dependencies: mbed QEI PID DmTftLibraryEx
Diff: main.cpp
- Revision:
- 34:0522cebfe489
- Parent:
- 33:f77aa3ecf87d
--- 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