Luca Antolini
Velocity Closed Loop Dynamic error correction
Dependencies: mbed QEI PID DmTftLibraryEx
Diff: main.cpp
- Revision:
- 39:be7055a0e9a4
- Parent:
- 37:5fc7f2f435e8
--- a/main.cpp Tue Apr 12 07:55:59 2022 +0000
+++ b/main.cpp Fri Apr 15 07:01:02 2022 +0000
@@ -22,6 +22,26 @@
// 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
+// Con la versione Pubblicata 0022 Il Doppio PID è correttamente funzionante.
+//
+// LA: 12 Aprile 2022, Introduco la retroazione ad anello Avanzata
+// ===========================================================
+//
+// Provo ad escludere il PID (I PID, attualmente son due) a favore di un controllo di retroazione più incisivo.
+// L'attuale PID non tiene conto del verso di correzione, mentre nel moto attuale mi aspetto maggiori difficoltà a frenare che ad
+// accelerare.
+// PQM introduco un controllo (in retroazione di proporzionalità all'errore) che possa diversificarsi tra Accelerazione e FRENO.
+// i.e. deve essere più incisivo quando frena, meno quando accelera.
+//
+// 15/4: Retroazione Avanzata e correzione dinamica dell'errore.
+//
+// In base all'errore calcolato sulla velocità, opero una retroazione semplice per far tornare il sistema in stabilità.
+// Ad ogni "chiamata" del motion correggo l'uscita nel verso suggerito dall'errore di un fattore ad esso proporzionale.
+// Ad oggi è il risultato migliore.
+//
+// Committo
+// Forco
+// Pubblico la 0030
//
#include "QEI.h"
@@ -29,6 +49,7 @@
#include "Timers.h"
#include "Eeprom.h"
+#include "math.h"
#define MAX_CHAR_PER_LINE 28
#define TEXT_ROW_SPACING 16
@@ -43,58 +64,30 @@
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
-#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
-*/
-
-// 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
// ===============
//
+/*
void LCM_ShowTactics(
- int64_t i64_Pulses,
- int32_t i32_ATVSpeed,
- //
- float f_ai0000_Aux,
- float f_ai0001_Aux,
- float f_ai0002_Aux,
- float f_ai0003_Aux,
- float f_ai0004_Aux,
- float f_ai0005_Aux,
- //
- int32_t i32_Velocity,
- int32_t i32_Acceleration,
- int32_t i32_Jerk
-
+ int64_t i64_Pulses,
+ float f_ATVSpeed,
+ float f_ai0000_Aux,
+ float f_ai0001_Aux,
+ float f_ai0002_Aux,
+ float f_ai0003_Aux,
+ float f_ai0004_Aux,
+ float f_ai0005_Aux,
+ int32_t i32_Velocity,
+ int32_t i32_Acceleration,
+ int32_t i32_Jerk
);
+*/
// LA: SampleAndStore
// LA: SampleTimer ==
// ==============
//
-static void SampleAndStore (void);
+//static void SampleAndStore (void);
Ticker SampleTimer; // LA: To Sample 1AI any 'x'ms
float af_PlotSamples[240]; // LA: "Horiz" Plot Array
@@ -115,12 +108,18 @@
// LA: MotionTimer =
// =============
//
-static void MotionHandler (void);
+//static void MotionHandler (void);
Ticker MotionTimer; // LA: Gestisce il rilevamento (RT) di Velocità, Accelerazione e Jerk
// Esegue il Movimento Programmato
-float f_PWMPercent; // Deprecated
-float fPID_Error; //
+float f_PWMPercent; //
+float F_VelocityLoopFB_FW; // LA: Correzione Dinamica FW
+float F_VelocityLoopFB_BW; // LA: Correzione Dinamica BW
+//float fPID_Error; //
+
+float fMOT_VelocityAct; // 0.0= 0%, 1.0= 100%
+float fMOT_VelocityReq; // 0.0= 0%, 1.0= 100%
+float fMOT_VelocityError; //
float fVelocity;
float fAcceleration;
@@ -197,197 +196,28 @@
HAL_NVIC_SystemReset( );
}
-// =======
-// =======
-// Main(s)
-// =======
-// =======
-//
-int main (void){
-
- rDIR_FWD = true; // LA: Actuator Extends
- rENA_Off = true; // LA: Drive Power is Off
- //
- in_PosizionatoreSW.b_ServoLock = false;
- in_PosizionatoreSW.rtServoLock_Q = false;
-
- EepromInit(); // LA: Inizializza la EEProm
- TimersInit(); // LA: Parte il Timer a 1ms
-
- // LA: FactoryReset se "userButton" premuto all'avvio
- //
- if (userButton == 0) {
- FactoryReset();
- }
- DisplayDriverInit();
-
- 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]
- PWM_PB3.write((float) 0.0); // Set to 0%
-
- // LA: Motion (1st) Setup
- //
- in_PosizionatoreSW.b_AxisPowered = true;
- in_PosizionatoreSW.b_ACPos_Homed = true;
- in_PosizionatoreSW.i32_Max_Speed = 20; // [ui/ms]
- in_PosizionatoreSW.i32_ZeroSpeed = 0; //
-
- // POS Mode
- // ========
- //
-// in_PosizionatoreSW.b_ServoLock = true;
-// in_PosizionatoreSW.rtServoLock_Q = false;
- //
- in_PosizionatoreSW.i64_TargetPosition = ci64_TargetPOS; // [ui]
- in_PosizionatoreSW.i64_ActualPosition = Stabilus322699.getPulses(); //
- 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 = 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 = 48.0f; // Riferimento di velocità minima a cui (appena) si muove l'asse [verso BW]
-
- // JOG Mode
- // ========
- //
- in_PosizionatoreSW.b_JogMode = false;
- in_PosizionatoreSW.b_JogFW = false;
- in_PosizionatoreSW.b_JogBW = false;
- in_PosizionatoreSW.i32_JogAccel_ms = 500; // [ms]
- in_PosizionatoreSW.i32_JogDecel_ms = 250; //
- //
- 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_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_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);
- PID_VelocityClosedLoop_FW.setMode(MANUAL_MODE);
- PID_VelocityClosedLoop_BW.setMode(MANUAL_MODE);
-
- // LA: Color RGB Component(s)
- // ======================
- //
- // RED 0000 1000 0000 0000 min 0x0800 02048
- // 1111 1000 0000 0000 max 0xf800 63488
- //
- // GREEN 0000 0000 0010 0000 min 0x0020 00032
- // 0000 0111 1110 0000 max 0x07e0 02016
- //
- // BLUE 0000 0000 0000 0001 min 0x0001 00001
- // 0000 0000 0001 1111 max 0x001f 00031
- //
- // La componente ROSSA ha 5 bit di escursione (0.. 31),
- // La componente VERDE ha 6 bit di escursione (0.. 63),
- // La componente BLU ha 5 bit di escursione (0.. 31),
- //
- // Le componenti RGB di "Color" sono quindi scritte negli appropriati registri come segue:
- //
- // writeReg(RED, (Color & 0xf800) >> 11);
- // writeReg(GREEN, (Color & 0x07e0) >> 5);
- // writeReg(BLUE, (Color & 0x001f));
- //
- LCM_SetTextColor(Scale2RGBColor (0, 0, 0), Scale2RGBColor (31, 0, 0)); // LA: Red on Black
- LCM_ClearScreen (Scale2RGBColor (0, 0, 0)); // Black Background
- LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 0), "You Start Me Up ..."); // Intro Text
-
-// rDIR_FWD = false; // Collapse
-// rENA_Off = false; // Power On
-
- in_PosizionatoreSW.b_ServoLock = true;
- in_PosizionatoreSW.rtServoLock_Q = true;
-
- while (1) {
-
- // LA: Scope, Theory of operation.
- // ===========================
- //
- // 1) Sample a Value @ any Step
- // 2) Store @ the correct ms
- // 3) Plot the current Section of the Sampling Vector
- //
- LCM_ShowTactics (
- Stabilus322699.getPulses(), // Row 1
-
- out_PosizionatoreSW.i32_ATVSpeed, // 3
- adc_temp.read(), // 4
- adc_vbat.read(), // 5
- adc_vref.read(), // 6
-
- ADC12_IN9.read(), // 8
- ADC12_IN15.read(), // 9
- (f_PWMPercent* 100), // 10
-
- i32_Velocity, // 11
- i32_Acceleration, // 12
- i32_Jerk // 13
-
- );
-
- LCM_PlotScope (
- Scale2RGBColor (0, 0, 0), // Back: Black
- Scale2RGBColor (31, 0, 0) // Fore: Red
- );
-
- LCM_PlotSpeed (
- Scale2RGBColor (0, 0, 0), // Back: Black
- Scale2RGBColor (31, 0, 0) // Fore: Red
- );
-
- if (out_PosizionatoreSW.b_InPosition)
- if (in_PosizionatoreSW.i64_TargetPosition > 0)
- in_PosizionatoreSW.i64_TargetPosition = 0;
- else
- in_PosizionatoreSW.i64_TargetPosition = ci64_TargetPOS;
- }
-}
-
void LCM_ShowTactics(
int64_t i64_Pulses,
- int32_t i32_ATVSpeed,
- //
+ float f_ATVSpeed,
float f_ai0000_Aux,
float f_ai0001_Aux,
float f_ai0002_Aux,
float f_ai0003_Aux,
float f_ai0004_Aux,
float f_ai0005_Aux,
- //
int32_t i32_Velocity,
int32_t i32_Acceleration,
int32_t i32_Jerk
) {
char StringText[MAX_CHAR_PER_LINE+ 1]; // don't forget the /0 (end of string)
-
static int64_t i64_Pulses_Prec;
-static uint32_t ms_0002_prec;
-
-static float f_ai0000_prec;
-static float f_ai0001_prec;
-static float f_ai0002_prec;
-static float f_ai0003_prec;
-static float f_ai0004_prec;
static float f_ai0005_prec;
static float f_ai0006_prec;
-
+//static float f_ai0007_prec;
+static float f_ai0008_prec;
static uint32_t i32_Velocity_prec;
-static uint32_t i32_Acceleration_prec;
-static uint32_t i32_Jerk_prec;
+static float f_ATVSpeed_prec;
if (i64_Pulses != i64_Pulses_Prec) {
sprintf (StringText,
@@ -398,44 +228,41 @@
if (i32_Velocity != i32_Velocity_prec) {
sprintf (StringText,
- "Velocity[ui/ms]: %d ", i32_Velocity); //, fVelocity);
- LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 3), StringText);
+ "Velocity[ui/ms]: %d ", i32_Velocity);
+ LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 2), StringText);
i32_Velocity_prec = i32_Velocity;
}
- if (out_PosizionatoreSW.i32_ATVSpeed != i32_Acceleration_prec) {
+ if (fMOT_VelocityAct != f_ATVSpeed_prec) {
sprintf (StringText,
- "ATVSpeed[ui/ms]: %d ", out_PosizionatoreSW.i32_ATVSpeed); //, fAcceleration);
+ "VAct[0..1]: %f ", fMOT_VelocityAct);
+ LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 3), StringText);
+ f_ATVSpeed_prec = fMOT_VelocityAct;
+ }
+ if (fMOT_VelocityReq != f_ai0008_prec) {
+ sprintf (StringText,
+ "VReq[0..1]: %f ", fMOT_VelocityReq);
LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 4), StringText);
- i32_Acceleration_prec = out_PosizionatoreSW.i32_ATVSpeed;
+ f_ai0008_prec = fMOT_VelocityReq;
}
if (f_PWMPercent != f_ai0005_prec) {
sprintf (StringText,
- "PID_FB Compute: %f ", f_PWMPercent); //, fJerk);
+ "PWM [0..1]: %f ", f_PWMPercent);
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) {
+ if (fMOT_VelocityError != f_ai0006_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* 4), StringText);
- i32_Acceleration_prec = i32_Acceleration;
+ "Err [0..1): %f ", fMOT_VelocityError);
+ LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 6), StringText);
+ f_ai0006_prec = fMOT_VelocityError;
}
- if (i32_Jerk != i32_Jerk_prec) {
+/*
+ if (F_VelocityLoopFB != f_ai0007_prec) {
sprintf (StringText,
- "Jerk: %d ", i32_Jerk); //, fJerk);
-// LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 13), StringText);
- LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 5), StringText);
- i32_Jerk_prec = i32_Jerk;
+ "Corr[0..1]: %f ", F_VelocityLoopFB); //, fJerk);
+ LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 7), StringText);
+ f_ai0007_prec = F_VelocityLoopFB;
}
*/
}
@@ -460,7 +287,6 @@
}
static void MotionHandler (void) {
-//static int16_t i16_Index = 0;
static uint32_t ui32_PreviousStep_ms;
uint32_t ui32_ActualStepSampled_ms;
uint32_t ui32_PassedActual_ms;
@@ -528,36 +354,218 @@
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
+ 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: Elaborazione dell'Uscita di Controllo della velocità
+ // ====================================================
+ //
+ // Verifica della velocità raggiunta.
+ // Calcolo dell'ERRORE.
+ // Elaborazione e applicazione della correzione.
+ //
- // LA: PID Compute Section
- // ===================
+ // LA: i32_Velocity, Velocità Attuale (Reale, rilevata)
+ // LA: out_PosizionatoreSW.i32_ATVSpeed, Velocità Richiesta (Elaborata in base al profilo)
+ //
+ // Si suppone che la velocità massima sia raggiungibile al massimo valore di f_PWMPercent (-> 1.0)
+ // Si suppone che la velocità minima sia raggiungibile al minimo valore di f_PWMPercent (-> 0.0)
//
+
+ // LA: Calcolo dell'Errore scalato 0..1
+ //
+ fMOT_VelocityAct = ((float)abs(i32_Velocity)/ in_PosizionatoreSW.f_Max_Speed); // 0.0= 0%, 1.0= 100%
+ fMOT_VelocityReq = (out_PosizionatoreSW.f_ATVSpeed/ in_PosizionatoreSW.f_Max_Speed); // 0.0= 0%, 1.0= 100%
+ fMOT_VelocityError = fMOT_VelocityReq- fMOT_VelocityAct; //
+
+// f_PWMPercent = out_PosizionatoreSW.f_ATVSpeed/ in_PosizionatoreSW.f_Max_Speed; // LA: In Range (float) 0.. 1
if (out_PosizionatoreSW.b_ATVDirectionFW) {
- PID_VelocityClosedLoop_BW.reset();
+ F_VelocityLoopFB_BW = 0.0f;
+
+ f_PWMPercent = (out_PosizionatoreSW.f_ATVSpeed/ in_PosizionatoreSW.f_Max_Speed)/ 4.0f; // FW is about 4:1 BW
+ f_PWMPercent += (fMOT_VelocityError* 0.1f);
- // 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.
-
- fPID_Error = PID_VelocityClosedLoop_FW.getError();
- f_PWMPercent = PID_VelocityClosedLoop_FW.compute();
+ /*
+ if (fMOT_VelocityError > 0.001f)
+ F_VelocityLoopFB_FW += 0.0002f;
+ if (fMOT_VelocityError < -0.001f)
+ F_VelocityLoopFB_FW -= 0.0002f;
+ */
+ F_VelocityLoopFB_FW += (0.004f* fMOT_VelocityError);
+ f_PWMPercent += F_VelocityLoopFB_FW;
}
else {
- PID_VelocityClosedLoop_FW.reset();
+ F_VelocityLoopFB_FW = 0.0f;
+
+ f_PWMPercent = (out_PosizionatoreSW.f_ATVSpeed/ in_PosizionatoreSW.f_Max_Speed)/ 1.4f;
+ f_PWMPercent += (fMOT_VelocityError* 0.1f);
+
+ /*
+ if (fMOT_VelocityError > 0.001f)
+ F_VelocityLoopFB_BW += 0.0005f;
+ if (fMOT_VelocityError < -0.001f)
+ F_VelocityLoopFB_BW -= 0.0005f;
+ */
+ F_VelocityLoopFB_BW += (0.01f* fMOT_VelocityError);
+ f_PWMPercent += F_VelocityLoopFB_BW;
+ }
+ if (f_PWMPercent > 1.0f)
+ f_PWMPercent = 1.0f;
+ if (f_PWMPercent < 0.0f)
+ f_PWMPercent = 0.0f;
+ PWM_PB3.write((float) 1.0- f_PWMPercent); // Set to x%
+}
+
+// =======
+// =======
+// Main(s)
+// =======
+// =======
+//
+int main (void){
+ rDIR_FWD = true; // LA: Actuator Extends
+ rENA_Off = true; // LA: Drive Power is Off
+ //
+ in_PosizionatoreSW.b_ServoLock = false;
+ in_PosizionatoreSW.rtServoLock_Q = false;
+
+ EepromInit(); // LA: Inizializza la EEProm
+ TimersInit(); // LA: Parte il Timer a 1ms
+
+ // LA: FactoryReset se "userButton" premuto all'avvio
+ //
+ if (userButton == 0) {
+ FactoryReset();
+ }
+ DisplayDriverInit();
+
+ 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]
+ PWM_PB3.write(0.0f); // Set to 0%
+
+// LA: Ok
+
+ // LA: Motion (1st) Setup
+ //
+ in_PosizionatoreSW.b_AxisPowered = true;
+ in_PosizionatoreSW.b_ACPos_Homed = true;
+ in_PosizionatoreSW.f_Max_Speed = 20.0f; // [ui/ms]
+ in_PosizionatoreSW.f_ZeroSpeed = 0.0f; //
+
+ // POS Mode
+ // ========
+ //
+// in_PosizionatoreSW.b_ServoLock = true;
+// in_PosizionatoreSW.rtServoLock_Q = false;
+ //
+ in_PosizionatoreSW.i64_TargetPosition = ci64_TargetPOS; // [ui]
+ in_PosizionatoreSW.i64_ActualPosition = Stabilus322699.getPulses(); //
+ in_PosizionatoreSW.i64_AccelerationWindow = 64; // LA: Spazio concesso alla rampa di Accelerazione
+ in_PosizionatoreSW.i64_DecelerationWindow = 1024; // Spazio concesso alla rampa di Decelerazione
+ in_PosizionatoreSW.i64_diToleranceWindow = 16; // Finestra di Tolleranza
+ //
+ 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 = 16.0f; // Riferimento di velocità minima a cui (appena) si muove l'asse [verso FW]
+// in_PosizionatoreSW.f_ServoLockSpeed_x100_BW = 24.0f; // Riferimento di velocità minima a cui (appena) si muove l'asse [verso BW]
+ in_PosizionatoreSW.f_ServoLockSpeed_x100_FW = 24.0f; // Riferimento di velocità minima a cui (appena) si muove l'asse [verso FW]
+ in_PosizionatoreSW.f_ServoLockSpeed_x100_BW = 24.0f; // Riferimento di velocità minima a cui (appena) si muove l'asse [verso BW]
+
+// LA: Ok
- // 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.
+ // JOG Mode
+ // ========
+ //
+ in_PosizionatoreSW.b_JogMode = false;
+ in_PosizionatoreSW.b_JogFW = false;
+ in_PosizionatoreSW.b_JogBW = false;
+ in_PosizionatoreSW.i32_JogAccel_ms = 500; // [ms]
+ in_PosizionatoreSW.i32_JogDecel_ms = 250; //
+ //
+ 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); //
+
+// LA: Ok
+
+ // LA: Color RGB Component(s)
+ // ======================
+ //
+ // RED 0000 1000 0000 0000 min 0x0800 02048
+ // 1111 1000 0000 0000 max 0xf800 63488
+ //
+ // GREEN 0000 0000 0010 0000 min 0x0020 00032
+ // 0000 0111 1110 0000 max 0x07e0 02016
+ //
+ // BLUE 0000 0000 0000 0001 min 0x0001 00001
+ // 0000 0000 0001 1111 max 0x001f 00031
+ //
+ // La componente ROSSA ha 5 bit di escursione (0.. 31),
+ // La componente VERDE ha 6 bit di escursione (0.. 63),
+ // La componente BLU ha 5 bit di escursione (0.. 31),
+ //
+ // Le componenti RGB di "Color" sono quindi scritte negli appropriati registri come segue:
+ //
+ // writeReg(RED, (Color & 0xf800) >> 11);
+ // writeReg(GREEN, (Color & 0x07e0) >> 5);
+ // writeReg(BLUE, (Color & 0x001f));
+ //
+ LCM_SetTextColor(Scale2RGBColor (0, 0, 0), Scale2RGBColor (31, 0, 0)); // LA: Red on Black
+ LCM_ClearScreen (Scale2RGBColor (0, 0, 0)); // Black Background
+ LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 0), "You Start Me Up ..."); // Intro Text
+
+// rDIR_FWD = false; // Collapse
+// rENA_Off = false; // Power On
+
+ in_PosizionatoreSW.b_ServoLock = true;
+ in_PosizionatoreSW.rtServoLock_Q = true;
- fPID_Error = PID_VelocityClosedLoop_BW.getError();
- f_PWMPercent = PID_VelocityClosedLoop_BW.compute();
+// LA: Ok
+
+ while (1) {
+
+// LA: Ok
+
+ // LA: Scope, Theory of operation.
+ // ===========================
+ //
+ // 1) Sample a Value @ any Step
+ // 2) Store @ the correct ms
+ // 3) Plot the current Section of the Sampling Vector
+ //
+ LCM_ShowTactics (
+ Stabilus322699.getPulses(), // Row 1
+
+ out_PosizionatoreSW.f_ATVSpeed, // 3
+ adc_temp.read(), // 4
+ adc_vbat.read(), // 5
+ adc_vref.read(), // 6
+
+ ADC12_IN9.read(), // 8
+ ADC12_IN15.read(), // 9
+ (f_PWMPercent* 100), // 10
+
+ i32_Velocity, // 11
+ i32_Acceleration, // 12
+ i32_Jerk // 13
+
+ );
+
+ LCM_PlotScope (
+ Scale2RGBColor (0, 0, 0), // Back: Black
+ Scale2RGBColor (31, 0, 0) // Fore: Red
+ );
+
+ LCM_PlotSpeed (
+ Scale2RGBColor (0, 0, 0), // Back: Black
+ Scale2RGBColor (31, 0, 0) // Fore: Red
+ );
+
+ if (out_PosizionatoreSW.b_InPosition)
+ if (in_PosizionatoreSW.i64_TargetPosition > 0)
+ in_PosizionatoreSW.i64_TargetPosition = 0;
+ else
+ in_PosizionatoreSW.i64_TargetPosition = ci64_TargetPOS;
}
- PWM_PB3.write((float) 1.0- f_PWMPercent); // Set to x%
-}
+}
\ No newline at end of file