Luca Antolini
Stabilus 322699 wDoublePID, ErrorGetter
Dependencies: mbed QEI PID DmTftLibraryEx
Diff: main.cpp
- Revision:
- 35:0f6adc0b95b9
- Parent:
- 34:0522cebfe489
- Child:
- 36:cab8aa44ef91
diff -r 0522cebfe489 -r 0f6adc0b95b9 main.cpp
--- a/main.cpp Mon Apr 11 09:20:40 2022 +0000
+++ b/main.cpp Mon Apr 11 11:52:49 2022 +0000
@@ -16,6 +16,13 @@
// 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.
//
+// LA: 11 Aprile 2022, Aggiungo il PID
+// ===============================
+//
+// La retroazione sulla Velocità (Calcolata ogni "cf_MOTPeriod_s") è eseguita, nello stesso intervallo, da "PID_VelocityClosedLoop"
+// Il parametro di Riferimento è la Velocity Calcolata
+// Il Fattore di correzione è il PWM in uscita
+//
#include "QEI.h"
#include "SWPos.h"
@@ -58,7 +65,7 @@
//PID SpeedClosedLoop(P,I,D,&Time);
// Kc, Ti, Td, interval
-PID PID_VelocityClosedLoop (1.0, 0.0, 0.0, RATE);
+PID PID_VelocityClosedLoop (1.2, 0.0, 0.0, RATE);
//PID PID_VelocityClosedLoop (P, I, D, RATE);
//const int64_t ci64_TargetPOS = 240; //
@@ -226,7 +233,7 @@
//
in_PosizionatoreSW.b_AxisPowered = true;
in_PosizionatoreSW.b_ACPos_Homed = true;
- in_PosizionatoreSW.i32_Max_Speed = 512; // [ui/s]
+ in_PosizionatoreSW.i32_Max_Speed = 20; // [ui/ms]
in_PosizionatoreSW.i32_ZeroSpeed = 0; //
// POS Mode
@@ -237,21 +244,14 @@
//
in_PosizionatoreSW.i64_TargetPosition = ci64_TargetPOS; // [ui]
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 = 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.i64_AccelerationWindow = 512; // LA: Spazio concesso all'accelerazione.
+ in_PosizionatoreSW.i64_DecelerationWindow = 1024; // 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 = 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 = 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 = 24.0; // Riferimento di velocità minima a cui (appena) si muove l'asse [verso BW]
+ 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]
// JOG Mode
// ========
@@ -395,63 +395,27 @@
i64_Pulses_Prec = i64_Pulses;
}
-/*
- if (i32_ATVSpeed != ms_0002_prec) {
- sprintf (StringText,
- "Speed[ui]: %d ", i32_ATVSpeed);
- LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 3), StringText);
- ms_0002_prec = i32_ATVSpeed;
- }
-
- if (f_ai0000_Aux != f_ai0000_prec) {
- sprintf (StringText,
- "ADC Temp = %f ", (f_ai0000_Aux* 100));
- LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 4), StringText);
- f_ai0000_prec = f_ai0000_Aux;
- }
-
- if (f_ai0001_Aux != f_ai0001_prec) {
- sprintf (StringText,
- "ADC VBat = %f ", (f_ai0001_Aux* 10));
- LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 5), StringText);
- f_ai0001_prec = f_ai0001_Aux;
- }
-
- if (f_ai0002_Aux != f_ai0002_prec) {
- sprintf (StringText,
- "ADC VRef = %f ", (f_ai0002_Aux* 10));
- LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 6), StringText);
- f_ai0002_prec = f_ai0002_Aux;
- }
-
- if (f_ai0003_Aux != f_ai0003_prec) {
- sprintf (StringText,
- "ADC12.09 = %f ", (f_ai0003_Aux* 10)/ 3);
- LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 8), StringText);
- f_ai0003_prec = f_ai0003_Aux;
- }
-
- if (f_ai0004_Aux != f_ai0004_prec) {
- sprintf (StringText,
- "ADC12.15 = %f ", (f_ai0004_Aux* 10)/ 3);
- LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 9), StringText);
- f_ai0004_prec = f_ai0004_Aux;
- }
-
- if (f_ai0005_Aux != f_ai0005_prec) {
- sprintf (StringText,
- "PB3 PWM%% = %f ", (f_ai0005_Aux));
- LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 10), StringText);
- f_ai0005_prec = f_ai0005_Aux;
- }
-*/
if (i32_Velocity != i32_Velocity_prec) {
sprintf (StringText,
- "Vel[ui/10ms]: %d ", i32_Velocity); //, fVelocity);
-// LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 11), StringText);
+ "Velocity[ui/ms]: %d ", i32_Velocity); //, fVelocity);
LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 3), StringText);
i32_Velocity_prec = i32_Velocity;
}
+
+ if (out_PosizionatoreSW.i32_ATVSpeed != i32_Acceleration_prec) {
+ sprintf (StringText,
+ "ATVSpeed[ui/ms]: %d ", out_PosizionatoreSW.i32_ATVSpeed); //, fAcceleration);
+ LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 4), StringText);
+ i32_Acceleration_prec = out_PosizionatoreSW.i32_ATVSpeed;
+ }
+ if (f_PWMPercent != f_ai0005_prec) {
+ sprintf (StringText,
+ "PID_FB Compute: %f ", f_PWMPercent); //, fJerk);
+ LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 5), StringText);
+ f_ai0005_prec = f_PWMPercent;
+ }
+
+/*
if (i32_Acceleration != i32_Acceleration_prec) {
sprintf (StringText,
"Acc[ui/10ms^2]: %d ", i32_Acceleration); //, fAcceleration);
@@ -466,6 +430,7 @@
LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 5), StringText);
i32_Jerk_prec = i32_Jerk;
}
+*/
}
static void SampleAndStore (void) {
@@ -488,7 +453,12 @@
}
static void MotionHandler (void) {
-static int16_t i16_Index = 0;
+//static int16_t i16_Index = 0;
+static uint32_t ui32_PreviousStep_ms;
+uint32_t ui32_ActualStepSampled_ms;
+uint32_t ui32_PassedActual_ms;
+//
+float fPassedActual_sxs;
// LA: Retrieve Actual Position
//
@@ -528,102 +498,45 @@
// LA: Update Motion Dynamic References
// ================================
- //
- // 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) {
- static uint32_t ui32_PreviousStep_ms;
- uint32_t ui32_ActualStepSampled_ms;
- uint32_t ui32_PassedActual_ms;
+
+ // LA: Generazione del millisecondo Attuale
+ // ====================================
//
- float fPassedActual_sxs;
-
- // LA: Generazione del millisecondo Attuale
- // ====================================
- //
- // Invoca il timer di sistema (TimersTimerValue) e lo confronta col suo precedente.
- // Una volta elaborato e "scevrato" l'eventuale "Rollover" la sezione ritorna "ui32_PassedActual_ms_Local".
- // "ui32_PassedActual_ms_Local" rappresenta i [ms] passati tra una istanza e l'altra
- //
- ui32_ActualStepSampled_ms = TimersTimerValue(); // Freezes the Actual Sample.
- if (ui32_ActualStepSampled_ms >= ui32_PreviousStep_ms)
- ui32_PassedActual_ms = (ui32_ActualStepSampled_ms- ui32_PreviousStep_ms); // Result => Actual- Previous
- else
- ui32_PassedActual_ms = ui32_ActualStepSampled_ms+ (0x7fffffff- ui32_PreviousStep_ms); // Result => Actual+ (Rollover- Previous)
- ui32_PreviousStep_ms = ui32_ActualStepSampled_ms; // Store(s)&Hold(s) actual msSample
- fPassedActual_sxs = ((float) 1000.0/ (float) ui32_PassedActual_ms); // Steps Any [s]
-
- i32_Velocity = (int32_t) (in_PosizionatoreSW.i64_ActualPosition- i64_Position_Prec); // LA: Velocity in [ui/10ms]
- i64_Position_Prec = in_PosizionatoreSW.i64_ActualPosition;
- i32_Acceleration = (i32_Velocity- i32_Velocity_Prec); // LA: Acceleration in [ui/10ms^2]
- i32_Velocity_Prec = i32_Velocity;
- i32_Jerk = (i32_Acceleration- i32_Acceleration_Prec); // LA: Jerk
- i32_Acceleration_Prec = i32_Acceleration;
-
- 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
-
- // 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);
+ // Invoca il timer di sistema (TimersTimerValue) e lo confronta col suo precedente.
+ // Una volta elaborato e "scevrato" l'eventuale "Rollover" la sezione ritorna "ui32_PassedActual_ms_Local".
+ // "ui32_PassedActual_ms_Local" rappresenta i [ms] passati tra una istanza e l'altra
+ //
+ ui32_ActualStepSampled_ms = TimersTimerValue(); // Freezes the Actual Sample.
+ if (ui32_ActualStepSampled_ms >= ui32_PreviousStep_ms)
+ ui32_PassedActual_ms = (ui32_ActualStepSampled_ms- ui32_PreviousStep_ms); // Result => Actual- Previous
+ else
+ ui32_PassedActual_ms = ui32_ActualStepSampled_ms+ (0x7fffffff- ui32_PreviousStep_ms); // Result => Actual+ (Rollover- Previous)
+ ui32_PreviousStep_ms = ui32_ActualStepSampled_ms; // Store(s)&Hold(s) actual msSample
+ fPassedActual_sxs = ((float) 1000.0/ (float) ui32_PassedActual_ms); // Steps Any [s]
- // 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;
+ i32_Velocity = (int32_t) (in_PosizionatoreSW.i64_ActualPosition- i64_Position_Prec); // LA: Velocity in [ui/ms]
+ i64_Position_Prec = in_PosizionatoreSW.i64_ActualPosition;
+ i32_Acceleration = (i32_Velocity- i32_Velocity_Prec); // LA: Acceleration in [ui/ms^2]
+ i32_Velocity_Prec = i32_Velocity;
+ i32_Jerk = (i32_Acceleration- i32_Acceleration_Prec); // LA: Jerk
+ i32_Acceleration_Prec = i32_Acceleration;
-/*
- // LA: Position's Graph Section
- // ========================
- //
- ai32_POS2VelGraph[in_PosizionatoreSW.i64_ActualPosition] = i32_Velocity;
- ai32_POS2AccGraph[in_PosizionatoreSW.i64_ActualPosition] = i32_Acceleration;
- ai32_POS2JrkGraph[in_PosizionatoreSW.i64_ActualPosition] = i32_Jerk;
-*/
-}
-
-/*
-#include "PID.h"
-
-#define RATE 0.1
+ 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
-//Kc, Ti, Td, interval
-PID controller(1.0, 0.0, 0.0, RATE);
-AnalogIn pv(p15);
-PwmOut co(p26);
+ // LA: PID Compute Section
+ // ===================
+ //
-int main(){
+ // Update the process variable.
+ PID_VelocityClosedLoop.setProcessValue((float)i32_Velocity);
- //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);
+ // Set Desired Value
+ PID_VelocityClosedLoop.setSetPoint((float)out_PosizionatoreSW.i32_ATVSpeed);
- while(1){
- //Update the process variable.
- controller.setProcessValue(pv.read());
- //Set the new output.
- co = controller.compute();
- //Wait for another loop calculation.
- wait(RATE);
- }
+ // Release a new output.
+ f_PWMPercent = PID_VelocityClosedLoop.compute();
+ PWM_PB3.write((float) 1.0- f_PWMPercent); // Set to x%
}
-*/
\ No newline at end of file