Luca Antolini / Mbed 2 deprecated AAA_Stabilus322699_LA0022

Stabilus 322699 wDoublePID, ErrorGetter

Dependencies:   mbed QEI PID DmTftLibraryEx

main.cpp@36:cab8aa44ef91, 2022-04-11 (annotated)

Committer:
lex9296
Date:
Mon Apr 11 13:48:34 2022 +0000
Revision:
36:cab8aa44ef91
Parent:
35:0f6adc0b95b9
Child:
37:5fc7f2f435e8
Double PID

Who changed what in which revision?

UserRevisionLine numberNew contents of line
GregCr 0:9c0917bfde97 1
lex9296 23:b9d23a2f390e 2 //Warning: Incompatible redefinition of macro "MBED_RAM_SIZE" in "tmp/HU5Hqj", Line: 39, Col: 10
lex9296 23:b9d23a2f390e 3 #ifndef MBED_RAM_SIZE
lex9296 23:b9d23a2f390e 4 #define MBED_RAM_SIZE 0x00018000
lex9296 23:b9d23a2f390e 5 #endif
lex9296 23:b9d23a2f390e 6
lex9296 34:0522cebfe489 7 // ===========================================================
lex9296 34:0522cebfe489 8 // ===========================================================
lex9296 34:0522cebfe489 9 // LA: Stage 01 cleared, Il posizionatore funziona CORRETTAMENTE
lex9296 34:0522cebfe489 10 // ===========================================================
lex9296 34:0522cebfe489 11 // ===========================================================
lex9296 34:0522cebfe489 12 //
lex9296 34:0522cebfe489 13 // Commit & Publish del 11 Aprile 2022, il Pozizionamento è corretto e testato.
lex9296 34:0522cebfe489 14 // La velocità imposta all'asse, tuttavia, NON E' retroazionata.
lex9296 34:0522cebfe489 15 //
lex9296 34:0522cebfe489 16 // Occorrerebbe un anello chiuso sulla velocità, che si muova "almeno" alla velocità di scansione del PWM.
lex9296 34:0522cebfe489 17 // Se la velocità non è ancora raggiunta, la tensione deve venire adeguata conseguentemente agendo sul PWM.
lex9296 34:0522cebfe489 18 //
lex9296 35:0f6adc0b95b9 19 // LA: 11 Aprile 2022, Aggiungo il PID
lex9296 35:0f6adc0b95b9 20 // ===============================
lex9296 35:0f6adc0b95b9 21 //
lex9296 35:0f6adc0b95b9 22 // La retroazione sulla Velocità (Calcolata ogni "cf_MOTPeriod_s") è eseguita, nello stesso intervallo, da "PID_VelocityClosedLoop"
lex9296 35:0f6adc0b95b9 23 // Il parametro di Riferimento è la Velocity Calcolata
lex9296 35:0f6adc0b95b9 24 // Il Fattore di correzione è il PWM in uscita
lex9296 35:0f6adc0b95b9 25 //
lex9296 34:0522cebfe489 26
lex9296 23:b9d23a2f390e 27 #include "QEI.h"
lex9296 23:b9d23a2f390e 28 #include "SWPos.h"
lex9296 23:b9d23a2f390e 29
GregCr 0:9c0917bfde97 30 #include "Timers.h"
GregCr 0:9c0917bfde97 31 #include "Eeprom.h"
GregCr 0:9c0917bfde97 32
lex9296 27:654100855f5c 33 #define MAX_CHAR_PER_LINE 28
lex9296 27:654100855f5c 34 #define TEXT_ROW_SPACING 16
lex9296 27:654100855f5c 35 #define FONT_CHAR_WIDTH 8
lex9296 27:654100855f5c 36 #define FONT_CHAR_HEIGHT 16
lex9296 27:654100855f5c 37
lex9296 27:654100855f5c 38 #include "DisplayDriver.h"
lex9296 27:654100855f5c 39
lex9296 33:f77aa3ecf87d 40 const float cf_SCOPeriod_s = 0.00025000; // 250us
lex9296 33:f77aa3ecf87d 41 const float cf_PWMPeriod_s = 0.01000000; // 10ms
lex9296 33:f77aa3ecf87d 42 const float cf_MOTPeriod_s = 0.01000000; // 10ms
lex9296 33:f77aa3ecf87d 43
lex9296 34:0522cebfe489 44 #include "PID.h"
lex9296 34:0522cebfe489 45 //
lex9296 34:0522cebfe489 46 #define Lx 0.0001
lex9296 34:0522cebfe489 47 #define Ku 0.32200//0.922000
lex9296 34:0522cebfe489 48 #define Pu 0.0125
lex9296 34:0522cebfe489 49 #define Kp Ku*0.6
lex9296 34:0522cebfe489 50 #define Ti Pu*0.5
lex9296 34:0522cebfe489 51 #define Td Pu*0.125
lex9296 34:0522cebfe489 52 //
lex9296 34:0522cebfe489 53 #define P Kp
lex9296 34:0522cebfe489 54 #define I Kp/Ti
lex9296 34:0522cebfe489 55 #define D Kp*Td
lex9296 34:0522cebfe489 56 #define Mstate 6
lex9296 34:0522cebfe489 57 #define SorT 5
lex9296 34:0522cebfe489 58 #define Databit 0
lex9296 34:0522cebfe489 59 #define TIME 0.0125
lex9296 34:0522cebfe489 60
lex9296 34:0522cebfe489 61 //#define RATE (cf_MOTPeriod_s * 10.0f)
lex9296 34:0522cebfe489 62 #define RATE (cf_MOTPeriod_s)
lex9296 34:0522cebfe489 63
lex9296 34:0522cebfe489 64 //Timer Time;
lex9296 34:0522cebfe489 65 //PID SpeedClosedLoop(P,I,D,&Time);
lex9296 34:0522cebfe489 66
lex9296 36:cab8aa44ef91 67 // Kc, Ti, Td, interval
lex9296 36:cab8aa44ef91 68 //PID PID_VelocityClosedLoop_FW (0.1, 0.001, 0.01, RATE);
lex9296 36:cab8aa44ef91 69
lex9296 36:cab8aa44ef91 70 PID PID_VelocityClosedLoop_FW (0.4, 0.0, 0.0001, RATE);
lex9296 36:cab8aa44ef91 71 PID PID_VelocityClosedLoop_BW (0.41, 0.0, 0.01, RATE);
lex9296 34:0522cebfe489 72 //PID PID_VelocityClosedLoop (P, I, D, RATE);
lex9296 34:0522cebfe489 73
lex9296 33:f77aa3ecf87d 74 //const int64_t ci64_TargetPOS = 240; //
lex9296 34:0522cebfe489 75 //const int64_t ci64_TargetPOS = 3096; //
lex9296 34:0522cebfe489 76
lex9296 34:0522cebfe489 77 int64_t ci64_TargetPOS = 3096; // Used as CONST ...
lex9296 33:f77aa3ecf87d 78
lex9296 33:f77aa3ecf87d 79 // LA: LCM_ShowTactics
lex9296 33:f77aa3ecf87d 80 // ===============
lex9296 33:f77aa3ecf87d 81 //
lex9296 32:1be3d79ff4db 82 void LCM_ShowTactics(
lex9296 33:f77aa3ecf87d 83 int64_t i64_Pulses,
lex9296 33:f77aa3ecf87d 84 int32_t i32_ATVSpeed,
lex9296 33:f77aa3ecf87d 85 //
lex9296 33:f77aa3ecf87d 86 float f_ai0000_Aux,
lex9296 33:f77aa3ecf87d 87 float f_ai0001_Aux,
lex9296 33:f77aa3ecf87d 88 float f_ai0002_Aux,
lex9296 33:f77aa3ecf87d 89 float f_ai0003_Aux,
lex9296 33:f77aa3ecf87d 90 float f_ai0004_Aux,
lex9296 33:f77aa3ecf87d 91 float f_ai0005_Aux,
lex9296 33:f77aa3ecf87d 92 //
lex9296 33:f77aa3ecf87d 93 int32_t i32_Velocity,
lex9296 33:f77aa3ecf87d 94 int32_t i32_Acceleration,
lex9296 33:f77aa3ecf87d 95 int32_t i32_Jerk
lex9296 33:f77aa3ecf87d 96
lex9296 32:1be3d79ff4db 97 );
lex9296 32:1be3d79ff4db 98
lex9296 33:f77aa3ecf87d 99 // LA: SampleAndStore
lex9296 33:f77aa3ecf87d 100 // LA: SampleTimer ==
lex9296 33:f77aa3ecf87d 101 // ==============
lex9296 33:f77aa3ecf87d 102 //
lex9296 33:f77aa3ecf87d 103 static void SampleAndStore (void);
lex9296 33:f77aa3ecf87d 104 Ticker SampleTimer; // LA: To Sample 1AI any 'x'ms
lex9296 33:f77aa3ecf87d 105
lex9296 29:7ce8c5779f5e 106 float af_PlotSamples[240]; // LA: "Horiz" Plot Array
lex9296 29:7ce8c5779f5e 107
lex9296 30:e45282a70a4d 108 uint16_t aui16_PlotSamples[240];
lex9296 30:e45282a70a4d 109 uint16_t aui16_PlotClears_Lo[240];
lex9296 30:e45282a70a4d 110 uint16_t aui16_PlotClears_Hi[240];
lex9296 30:e45282a70a4d 111
lex9296 33:f77aa3ecf87d 112 int32_t ai32_POS2VelGraph[4000];
lex9296 33:f77aa3ecf87d 113 uint16_t aui16_PlotPOS2VelSamples[240];
lex9296 33:f77aa3ecf87d 114 uint16_t aui16_PlotPOS2VelClears_Lo[240];
lex9296 33:f77aa3ecf87d 115 uint16_t aui16_PlotPOS2VelClears_Hi[240];
lex9296 33:f77aa3ecf87d 116
lex9296 33:f77aa3ecf87d 117 int32_t ai32_POS2AccGraph[4000];
lex9296 33:f77aa3ecf87d 118 int32_t ai32_POS2JrkGraph[4000];
lex9296 33:f77aa3ecf87d 119
lex9296 33:f77aa3ecf87d 120 // LA: MotionHandler
lex9296 33:f77aa3ecf87d 121 // LA: MotionTimer =
lex9296 33:f77aa3ecf87d 122 // =============
lex9296 33:f77aa3ecf87d 123 //
lex9296 33:f77aa3ecf87d 124 static void MotionHandler (void);
lex9296 33:f77aa3ecf87d 125 Ticker MotionTimer; // LA: Gestisce il rilevamento (RT) di Velocità, Accelerazione e Jerk
lex9296 33:f77aa3ecf87d 126 // Esegue il Movimento Programmato
lex9296 33:f77aa3ecf87d 127
lex9296 33:f77aa3ecf87d 128 float f_PWMPercent; // Deprecated
lex9296 33:f77aa3ecf87d 129
lex9296 33:f77aa3ecf87d 130 float fVelocity;
lex9296 33:f77aa3ecf87d 131 float fAcceleration;
lex9296 33:f77aa3ecf87d 132 float fJerk;
lex9296 33:f77aa3ecf87d 133 float fTorque;
lex9296 33:f77aa3ecf87d 134
lex9296 33:f77aa3ecf87d 135 int64_t i64_Position_Prec;
lex9296 33:f77aa3ecf87d 136 int32_t i32_Velocity;
lex9296 33:f77aa3ecf87d 137 int32_t i32_Velocity_Prec;
lex9296 33:f77aa3ecf87d 138 int32_t i32_Acceleration;
lex9296 33:f77aa3ecf87d 139 int32_t i32_Acceleration_Prec;
lex9296 33:f77aa3ecf87d 140 int32_t i32_Jerk;
lex9296 33:f77aa3ecf87d 141
lex9296 33:f77aa3ecf87d 142 // LA: Motion
lex9296 33:f77aa3ecf87d 143 // ======
lex9296 26:bfca0c471a87 144 // LA: Theory of Operation
lex9296 26:bfca0c471a87 145 // ===================
lex9296 26:bfca0c471a87 146 //
lex9296 26:bfca0c471a87 147 // Il PWM funziona da sè in Interrupt
lex9296 26:bfca0c471a87 148 // Il QEI funziona da sè in Interrupt
lex9296 26:bfca0c471a87 149 // Se si creano dei Ticker (Che sono a loro volta interrupt(s)) è possibile che PWM e QEI perdano correlazione con l'HW.
lex9296 26:bfca0c471a87 150 //
lex9296 26:bfca0c471a87 151 // PQM
lex9296 26:bfca0c471a87 152 //
lex9296 26:bfca0c471a87 153 // Il rinfresco del Display e la gestione del motion vanno fatte il più frequentemente possibile ma fuori dal loop dei Ticker.
lex9296 26:bfca0c471a87 154 // Con qst versione (LA0005, che termina un FORK (il successivo è LA0010) quanto detto sopra è FUNZIONANTE.
lex9296 26:bfca0c471a87 155 // Questo messaggio è incluso nel "commitment"
lex9296 26:bfca0c471a87 156
GregCr 0:9c0917bfde97 157 /*!
GregCr 0:9c0917bfde97 158 * \brief Define IO for Unused Pin
GregCr 0:9c0917bfde97 159 */
lex9296 23:b9d23a2f390e 160 //DigitalOut F_CS (D6); // MBED description of pin
lex9296 23:b9d23a2f390e 161 //DigitalOut SD_CS (D8); // MBED description of pin
lex9296 23:b9d23a2f390e 162
lex9296 23:b9d23a2f390e 163 DigitalIn userButton (USER_BUTTON);
lex9296 32:1be3d79ff4db 164 //
lex9296 32:1be3d79ff4db 165 DigitalOut rENA_Off (PC_0); // CN7.38 - Power Enable Relay, Power Disabled when true
lex9296 32:1be3d79ff4db 166 DigitalOut rDIR_FWD (PC_1); // CN7.36 - Move Direction Relay Bridge, Move FW(Extends) when true
mverdy 15:ed7ed20b7114 167
lex9296 23:b9d23a2f390e 168 AnalogIn adc_temp (ADC_TEMP);
lex9296 23:b9d23a2f390e 169 AnalogIn adc_vref (ADC_VREF);
lex9296 23:b9d23a2f390e 170 AnalogIn adc_vbat (ADC_VBAT);
GregCr 0:9c0917bfde97 171
lex9296 29:7ce8c5779f5e 172 AnalogIn ADC12_IN9 (PA_4); // STM32 PA4
lex9296 29:7ce8c5779f5e 173 AnalogIn ADC12_IN15 (PB_0); // STM32 PB0
lex9296 29:7ce8c5779f5e 174
lex9296 23:b9d23a2f390e 175 // PWM
lex9296 23:b9d23a2f390e 176 // ===
lex9296 23:b9d23a2f390e 177 //
lex9296 25:5fd2d69e2927 178 PwmOut PWM_PB3(PWM_OUT); // LA: PWM_OUT = D3 = PB_3
mverdy 14:ebd89dacc807 179
lex9296 28:443c82750dd9 180 // QEI
lex9296 28:443c82750dd9 181 // ===
lex9296 28:443c82750dd9 182 //
lex9296 28:443c82750dd9 183 QEI Stabilus322699 (PA_1, PA_0, NC, 100, QEI::X4_ENCODING);
lex9296 32:1be3d79ff4db 184 //DigitalIn Hall_A (PA_1);
lex9296 32:1be3d79ff4db 185 //DigitalIn Hall_B (PA_0);
lex9296 28:443c82750dd9 186
lex9296 23:b9d23a2f390e 187 // Motion
lex9296 23:b9d23a2f390e 188 // ======
lex9296 23:b9d23a2f390e 189 //
lex9296 28:443c82750dd9 190 //Ticker POS_MotionScan; // LA: Non uso un Ticker. Agisce sotto Interrupt e falsa la lettura QEI e la sincronicità del PWM
lex9296 23:b9d23a2f390e 191 //
lex9296 23:b9d23a2f390e 192 in_sPosizionatoreSW in_PosizionatoreSW;
lex9296 23:b9d23a2f390e 193 out_sPosizionatoreSW out_PosizionatoreSW;
GregCr 0:9c0917bfde97 194
lex9296 23:b9d23a2f390e 195 // LCD Display
lex9296 23:b9d23a2f390e 196 // ===========
lex9296 23:b9d23a2f390e 197 //
lex9296 28:443c82750dd9 198 //Ticker LCD_RefreshViews; // LA: Non uso un Ticker. Agisce sotto Interrupt e falsa la lettura QEI e la sincronicità del PWM
mverdy 15:ed7ed20b7114 199
lex9296 22:12c555cade79 200 void FactoryReset (void) {
mverdy 15:ed7ed20b7114 201 EepromFactoryReset( );
mverdy 15:ed7ed20b7114 202 HAL_NVIC_SystemReset( );
mverdy 15:ed7ed20b7114 203 }
lex9296 23:b9d23a2f390e 204
lex9296 23:b9d23a2f390e 205 // =======
lex9296 23:b9d23a2f390e 206 // =======
lex9296 23:b9d23a2f390e 207 // Main(s)
lex9296 23:b9d23a2f390e 208 // =======
lex9296 23:b9d23a2f390e 209 // =======
lex9296 23:b9d23a2f390e 210 //
lex9296 23:b9d23a2f390e 211 int main (void){
lex9296 28:443c82750dd9 212
lex9296 32:1be3d79ff4db 213 rDIR_FWD = true; // LA: Actuator Extends
lex9296 32:1be3d79ff4db 214 rENA_Off = true; // LA: Drive Power is Off
lex9296 33:f77aa3ecf87d 215 //
lex9296 33:f77aa3ecf87d 216 in_PosizionatoreSW.b_ServoLock = false;
lex9296 33:f77aa3ecf87d 217 in_PosizionatoreSW.rtServoLock_Q = false;
lex9296 28:443c82750dd9 218
lex9296 23:b9d23a2f390e 219 EepromInit(); // LA: Inizializza la EEProm
lex9296 23:b9d23a2f390e 220 TimersInit(); // LA: Parte il Timer a 1ms
lex9296 23:b9d23a2f390e 221
lex9296 23:b9d23a2f390e 222 // LA: FactoryReset se "userButton" premuto all'avvio
lex9296 23:b9d23a2f390e 223 //
lex9296 23:b9d23a2f390e 224 if (userButton == 0) {
lex9296 23:b9d23a2f390e 225 FactoryReset();
lex9296 23:b9d23a2f390e 226 }
lex9296 28:443c82750dd9 227 DisplayDriverInit();
lex9296 23:b9d23a2f390e 228
lex9296 33:f77aa3ecf87d 229 // SampleTimer.attach_us(&SampleAndStore, 250); // LA: Scope has its own times
lex9296 33:f77aa3ecf87d 230 SampleTimer.attach(&SampleAndStore, cf_SCOPeriod_s); // LA: Avvia l'OscilloScopio con TimeBase x [s]
lex9296 33:f77aa3ecf87d 231 MotionTimer.attach(&MotionHandler, cf_MOTPeriod_s); // LA: Avvia il Motion con TimeBase x [s]
lex9296 33:f77aa3ecf87d 232 PWM_PB3.period(cf_PWMPeriod_s); // LA: Avvia il PWM con TimeBase x [s]
lex9296 33:f77aa3ecf87d 233 PWM_PB3.write((float) 0.0); // Set to 0%
lex9296 23:b9d23a2f390e 234
lex9296 23:b9d23a2f390e 235 // LA: Motion (1st) Setup
lex9296 23:b9d23a2f390e 236 //
lex9296 28:443c82750dd9 237 in_PosizionatoreSW.b_AxisPowered = true;
lex9296 28:443c82750dd9 238 in_PosizionatoreSW.b_ACPos_Homed = true;
lex9296 35:0f6adc0b95b9 239 in_PosizionatoreSW.i32_Max_Speed = 20; // [ui/ms]
lex9296 23:b9d23a2f390e 240 in_PosizionatoreSW.i32_ZeroSpeed = 0; //
lex9296 23:b9d23a2f390e 241
lex9296 32:1be3d79ff4db 242 // POS Mode
lex9296 32:1be3d79ff4db 243 // ========
lex9296 32:1be3d79ff4db 244 //
lex9296 33:f77aa3ecf87d 245 // in_PosizionatoreSW.b_ServoLock = true;
lex9296 33:f77aa3ecf87d 246 // in_PosizionatoreSW.rtServoLock_Q = false;
lex9296 32:1be3d79ff4db 247 //
lex9296 33:f77aa3ecf87d 248 in_PosizionatoreSW.i64_TargetPosition = ci64_TargetPOS; // [ui]
lex9296 32:1be3d79ff4db 249 in_PosizionatoreSW.i64_ActualPosition = Stabilus322699.getPulses(); //
lex9296 35:0f6adc0b95b9 250 in_PosizionatoreSW.i64_AccelerationWindow = 512; // LA: Spazio concesso all'accelerazione.
lex9296 35:0f6adc0b95b9 251 in_PosizionatoreSW.i64_DecelerationWindow = 1024; // Spazio concesso alla decelerazione, è prioritario rispetto all'accelerazione.
lex9296 35:0f6adc0b95b9 252 in_PosizionatoreSW.i64_diToleranceWindow = 16; // Finestra di Tolleranza
lex9296 32:1be3d79ff4db 253 //
lex9296 35:0f6adc0b95b9 254 in_PosizionatoreSW.f_MaximumSpeed_x100_FW = 100.0f; // % of "i32_Max_Speed"
lex9296 35:0f6adc0b95b9 255 in_PosizionatoreSW.f_MaximumSpeed_x100_BW = 100.0f; //
lex9296 35:0f6adc0b95b9 256 in_PosizionatoreSW.f_ServoLockSpeed_x100_FW = 25.0f; // Riferimento di velocità minima a cui (appena) si muove l'asse [verso FW]
lex9296 36:cab8aa44ef91 257 in_PosizionatoreSW.f_ServoLockSpeed_x100_BW = 50.0f; // Riferimento di velocità minima a cui (appena) si muove l'asse [verso BW]
lex9296 32:1be3d79ff4db 258
lex9296 32:1be3d79ff4db 259 // JOG Mode
lex9296 32:1be3d79ff4db 260 // ========
lex9296 32:1be3d79ff4db 261 //
lex9296 23:b9d23a2f390e 262 in_PosizionatoreSW.b_JogMode = false;
lex9296 23:b9d23a2f390e 263 in_PosizionatoreSW.b_JogFW = false;
lex9296 23:b9d23a2f390e 264 in_PosizionatoreSW.b_JogBW = false;
lex9296 23:b9d23a2f390e 265 in_PosizionatoreSW.i32_JogAccel_ms = 500; // [ms]
lex9296 23:b9d23a2f390e 266 in_PosizionatoreSW.i32_JogDecel_ms = 250; //
lex9296 23:b9d23a2f390e 267 //
lex9296 32:1be3d79ff4db 268 in_PosizionatoreSW.f_JogSpeed_x100_FW = (in_PosizionatoreSW.f_MaximumSpeed_x100_FW/ 2); // LA: JOG's the Half of Max POS's Speed
lex9296 32:1be3d79ff4db 269 in_PosizionatoreSW.f_JogSpeed_x100_BW = (in_PosizionatoreSW.f_MaximumSpeed_x100_BW/ 2); //
lex9296 23:b9d23a2f390e 270
lex9296 34:0522cebfe489 271 // Velocity Loop PID
lex9296 34:0522cebfe489 272 // =================
lex9296 34:0522cebfe489 273 //
lex9296 34:0522cebfe489 274 // Input Speed (ref)= 0.. 512[ui/s]
lex9296 36:cab8aa44ef91 275 PID_VelocityClosedLoop_FW.setInputLimits(0.0f, (float)in_PosizionatoreSW.i32_Max_Speed);
lex9296 36:cab8aa44ef91 276 PID_VelocityClosedLoop_BW.setInputLimits(0.0f, (float)in_PosizionatoreSW.i32_Max_Speed);
lex9296 34:0522cebfe489 277 // Output PWM (ref)= 0.. 1
lex9296 36:cab8aa44ef91 278 PID_VelocityClosedLoop_FW.setOutputLimits(0.0f, 1.0f);
lex9296 36:cab8aa44ef91 279 PID_VelocityClosedLoop_BW.setOutputLimits(0.0f, 1.0f);
lex9296 34:0522cebfe489 280
lex9296 34:0522cebfe489 281 // If there's a bias.
lex9296 34:0522cebfe489 282 // PID_VelocityClosedLoop.setBias(0.3);
lex9296 36:cab8aa44ef91 283 // PID_VelocityClosedLoop.setMode(AUTO_MODE);
lex9296 36:cab8aa44ef91 284 PID_VelocityClosedLoop_FW.setMode(MANUAL_MODE);
lex9296 36:cab8aa44ef91 285 PID_VelocityClosedLoop_BW.setMode(MANUAL_MODE);
lex9296 34:0522cebfe489 286
lex9296 28:443c82750dd9 287 // LA: Color RGB Component(s)
lex9296 28:443c82750dd9 288 // ======================
lex9296 28:443c82750dd9 289 //
lex9296 28:443c82750dd9 290 // RED 0000 1000 0000 0000 min 0x0800 02048
lex9296 28:443c82750dd9 291 // 1111 1000 0000 0000 max 0xf800 63488
lex9296 28:443c82750dd9 292 //
lex9296 28:443c82750dd9 293 // GREEN 0000 0000 0010 0000 min 0x0020 00032
lex9296 28:443c82750dd9 294 // 0000 0111 1110 0000 max 0x07e0 02016
lex9296 28:443c82750dd9 295 //
lex9296 28:443c82750dd9 296 // BLUE 0000 0000 0000 0001 min 0x0001 00001
lex9296 28:443c82750dd9 297 // 0000 0000 0001 1111 max 0x001f 00031
lex9296 28:443c82750dd9 298 //
lex9296 28:443c82750dd9 299 // La componente ROSSA ha 5 bit di escursione (0.. 31),
lex9296 28:443c82750dd9 300 // La componente VERDE ha 6 bit di escursione (0.. 63),
lex9296 28:443c82750dd9 301 // La componente BLU ha 5 bit di escursione (0.. 31),
lex9296 28:443c82750dd9 302 //
lex9296 28:443c82750dd9 303 // Le componenti RGB di "Color" sono quindi scritte negli appropriati registri come segue:
lex9296 28:443c82750dd9 304 //
lex9296 28:443c82750dd9 305 // writeReg(RED, (Color & 0xf800) >> 11);
lex9296 28:443c82750dd9 306 // writeReg(GREEN, (Color & 0x07e0) >> 5);
lex9296 28:443c82750dd9 307 // writeReg(BLUE, (Color & 0x001f));
lex9296 28:443c82750dd9 308 //
lex9296 32:1be3d79ff4db 309 LCM_SetTextColor(Scale2RGBColor (0, 0, 0), Scale2RGBColor (31, 0, 0)); // LA: Red on Black
lex9296 32:1be3d79ff4db 310 LCM_ClearScreen (Scale2RGBColor (0, 0, 0)); // Black Background
lex9296 32:1be3d79ff4db 311 LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 0), "You Start Me Up ..."); // Intro Text
lex9296 23:b9d23a2f390e 312
lex9296 32:1be3d79ff4db 313 // rDIR_FWD = false; // Collapse
lex9296 32:1be3d79ff4db 314 // rENA_Off = false; // Power On
lex9296 32:1be3d79ff4db 315
lex9296 32:1be3d79ff4db 316 in_PosizionatoreSW.b_ServoLock = true;
lex9296 32:1be3d79ff4db 317 in_PosizionatoreSW.rtServoLock_Q = true;
lex9296 28:443c82750dd9 318
lex9296 28:443c82750dd9 319 while (1) {
lex9296 29:7ce8c5779f5e 320
lex9296 29:7ce8c5779f5e 321 // LA: Scope, Theory of operation.
lex9296 29:7ce8c5779f5e 322 // ===========================
lex9296 29:7ce8c5779f5e 323 //
lex9296 29:7ce8c5779f5e 324 // 1) Sample a Value @ any Step
lex9296 29:7ce8c5779f5e 325 // 2) Store @ the correct ms
lex9296 29:7ce8c5779f5e 326 // 3) Plot the current Section of the Sampling Vector
lex9296 29:7ce8c5779f5e 327 //
lex9296 32:1be3d79ff4db 328 LCM_ShowTactics (
lex9296 32:1be3d79ff4db 329 Stabilus322699.getPulses(), // Row 1
lex9296 29:7ce8c5779f5e 330
lex9296 32:1be3d79ff4db 331 out_PosizionatoreSW.i32_ATVSpeed, // 3
lex9296 32:1be3d79ff4db 332 adc_temp.read(), // 4
lex9296 32:1be3d79ff4db 333 adc_vbat.read(), // 5
lex9296 32:1be3d79ff4db 334 adc_vref.read(), // 6
lex9296 32:1be3d79ff4db 335
lex9296 32:1be3d79ff4db 336 ADC12_IN9.read(), // 8
lex9296 32:1be3d79ff4db 337 ADC12_IN15.read(), // 9
lex9296 33:f77aa3ecf87d 338 (f_PWMPercent* 100), // 10
lex9296 33:f77aa3ecf87d 339
lex9296 33:f77aa3ecf87d 340 i32_Velocity, // 11
lex9296 33:f77aa3ecf87d 341 i32_Acceleration, // 12
lex9296 33:f77aa3ecf87d 342 i32_Jerk // 13
lex9296 33:f77aa3ecf87d 343
lex9296 32:1be3d79ff4db 344 );
lex9296 32:1be3d79ff4db 345
lex9296 33:f77aa3ecf87d 346 LCM_PlotScope (
lex9296 32:1be3d79ff4db 347 Scale2RGBColor (0, 0, 0), // Back: Black
lex9296 32:1be3d79ff4db 348 Scale2RGBColor (31, 0, 0) // Fore: Red
lex9296 32:1be3d79ff4db 349 );
lex9296 32:1be3d79ff4db 350
lex9296 33:f77aa3ecf87d 351 LCM_PlotSpeed (
lex9296 33:f77aa3ecf87d 352 Scale2RGBColor (0, 0, 0), // Back: Black
lex9296 33:f77aa3ecf87d 353 Scale2RGBColor (31, 0, 0) // Fore: Red
lex9296 33:f77aa3ecf87d 354 );
lex9296 32:1be3d79ff4db 355
lex9296 32:1be3d79ff4db 356 if (out_PosizionatoreSW.b_InPosition)
lex9296 32:1be3d79ff4db 357 if (in_PosizionatoreSW.i64_TargetPosition > 0)
lex9296 32:1be3d79ff4db 358 in_PosizionatoreSW.i64_TargetPosition = 0;
lex9296 32:1be3d79ff4db 359 else
lex9296 33:f77aa3ecf87d 360 in_PosizionatoreSW.i64_TargetPosition = ci64_TargetPOS;
lex9296 28:443c82750dd9 361 }
lex9296 28:443c82750dd9 362 }
lex9296 28:443c82750dd9 363
lex9296 32:1be3d79ff4db 364 void LCM_ShowTactics(
lex9296 33:f77aa3ecf87d 365 int64_t i64_Pulses,
lex9296 32:1be3d79ff4db 366 int32_t i32_ATVSpeed,
lex9296 33:f77aa3ecf87d 367 //
lex9296 33:f77aa3ecf87d 368 float f_ai0000_Aux,
lex9296 33:f77aa3ecf87d 369 float f_ai0001_Aux,
lex9296 33:f77aa3ecf87d 370 float f_ai0002_Aux,
lex9296 33:f77aa3ecf87d 371 float f_ai0003_Aux,
lex9296 33:f77aa3ecf87d 372 float f_ai0004_Aux,
lex9296 33:f77aa3ecf87d 373 float f_ai0005_Aux,
lex9296 33:f77aa3ecf87d 374 //
lex9296 33:f77aa3ecf87d 375 int32_t i32_Velocity,
lex9296 33:f77aa3ecf87d 376 int32_t i32_Acceleration,
lex9296 33:f77aa3ecf87d 377 int32_t i32_Jerk
lex9296 32:1be3d79ff4db 378 ) {
lex9296 32:1be3d79ff4db 379
lex9296 32:1be3d79ff4db 380 char StringText[MAX_CHAR_PER_LINE+ 1]; // don't forget the /0 (end of string)
lex9296 32:1be3d79ff4db 381
lex9296 33:f77aa3ecf87d 382 static int64_t i64_Pulses_Prec;
lex9296 32:1be3d79ff4db 383 static uint32_t ms_0002_prec;
lex9296 32:1be3d79ff4db 384
lex9296 32:1be3d79ff4db 385 static float f_ai0000_prec;
lex9296 32:1be3d79ff4db 386 static float f_ai0001_prec;
lex9296 32:1be3d79ff4db 387 static float f_ai0002_prec;
lex9296 32:1be3d79ff4db 388 static float f_ai0003_prec;
lex9296 32:1be3d79ff4db 389 static float f_ai0004_prec;
lex9296 32:1be3d79ff4db 390 static float f_ai0005_prec;
lex9296 32:1be3d79ff4db 391
lex9296 33:f77aa3ecf87d 392 static uint32_t i32_Velocity_prec;
lex9296 33:f77aa3ecf87d 393 static uint32_t i32_Acceleration_prec;
lex9296 33:f77aa3ecf87d 394 static uint32_t i32_Jerk_prec;
lex9296 33:f77aa3ecf87d 395
lex9296 33:f77aa3ecf87d 396 if (i64_Pulses != i64_Pulses_Prec) {
lex9296 32:1be3d79ff4db 397 sprintf (StringText,
lex9296 33:f77aa3ecf87d 398 "Pulses: %d ", (int32_t)i64_Pulses);
lex9296 32:1be3d79ff4db 399 LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 1), StringText);
lex9296 33:f77aa3ecf87d 400 i64_Pulses_Prec = i64_Pulses;
lex9296 32:1be3d79ff4db 401 }
lex9296 32:1be3d79ff4db 402
lex9296 33:f77aa3ecf87d 403 if (i32_Velocity != i32_Velocity_prec) {
lex9296 33:f77aa3ecf87d 404 sprintf (StringText,
lex9296 35:0f6adc0b95b9 405 "Velocity[ui/ms]: %d ", i32_Velocity); //, fVelocity);
lex9296 34:0522cebfe489 406 LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 3), StringText);
lex9296 33:f77aa3ecf87d 407 i32_Velocity_prec = i32_Velocity;
lex9296 33:f77aa3ecf87d 408 }
lex9296 35:0f6adc0b95b9 409
lex9296 35:0f6adc0b95b9 410 if (out_PosizionatoreSW.i32_ATVSpeed != i32_Acceleration_prec) {
lex9296 35:0f6adc0b95b9 411 sprintf (StringText,
lex9296 35:0f6adc0b95b9 412 "ATVSpeed[ui/ms]: %d ", out_PosizionatoreSW.i32_ATVSpeed); //, fAcceleration);
lex9296 35:0f6adc0b95b9 413 LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 4), StringText);
lex9296 35:0f6adc0b95b9 414 i32_Acceleration_prec = out_PosizionatoreSW.i32_ATVSpeed;
lex9296 35:0f6adc0b95b9 415 }
lex9296 35:0f6adc0b95b9 416 if (f_PWMPercent != f_ai0005_prec) {
lex9296 35:0f6adc0b95b9 417 sprintf (StringText,
lex9296 35:0f6adc0b95b9 418 "PID_FB Compute: %f ", f_PWMPercent); //, fJerk);
lex9296 35:0f6adc0b95b9 419 LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 5), StringText);
lex9296 35:0f6adc0b95b9 420 f_ai0005_prec = f_PWMPercent;
lex9296 35:0f6adc0b95b9 421 }
lex9296 35:0f6adc0b95b9 422
lex9296 35:0f6adc0b95b9 423 /*
lex9296 33:f77aa3ecf87d 424 if (i32_Acceleration != i32_Acceleration_prec) {
lex9296 33:f77aa3ecf87d 425 sprintf (StringText,
lex9296 33:f77aa3ecf87d 426 "Acc[ui/10ms^2]: %d ", i32_Acceleration); //, fAcceleration);
lex9296 34:0522cebfe489 427 // LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 12), StringText);
lex9296 34:0522cebfe489 428 LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 4), StringText);
lex9296 33:f77aa3ecf87d 429 i32_Acceleration_prec = i32_Acceleration;
lex9296 33:f77aa3ecf87d 430 }
lex9296 33:f77aa3ecf87d 431 if (i32_Jerk != i32_Jerk_prec) {
lex9296 33:f77aa3ecf87d 432 sprintf (StringText,
lex9296 33:f77aa3ecf87d 433 "Jerk: %d ", i32_Jerk); //, fJerk);
lex9296 34:0522cebfe489 434 // LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 13), StringText);
lex9296 34:0522cebfe489 435 LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 5), StringText);
lex9296 33:f77aa3ecf87d 436 i32_Jerk_prec = i32_Jerk;
lex9296 33:f77aa3ecf87d 437 }
lex9296 35:0f6adc0b95b9 438 */
lex9296 32:1be3d79ff4db 439 }
lex9296 32:1be3d79ff4db 440
lex9296 33:f77aa3ecf87d 441 static void SampleAndStore (void) {
lex9296 29:7ce8c5779f5e 442 int16_t i16_SampleIndex;
lex9296 33:f77aa3ecf87d 443 int64_t i64_SampleIndex;
lex9296 29:7ce8c5779f5e 444
lex9296 32:1be3d79ff4db 445 // af_PlotSamples[240- 1] = ADC12_IN9.read();
lex9296 32:1be3d79ff4db 446 af_PlotSamples[240- 1] = (float) Stabilus322699.getChannelA() * 0.33f;
lex9296 32:1be3d79ff4db 447
lex9296 32:1be3d79ff4db 448 for (i16_SampleIndex = 0; i16_SampleIndex < (240- 1); i16_SampleIndex++)
lex9296 29:7ce8c5779f5e 449 af_PlotSamples[i16_SampleIndex] = af_PlotSamples[i16_SampleIndex+ 1];
lex9296 29:7ce8c5779f5e 450
lex9296 33:f77aa3ecf87d 451 // LA: Position's Graph Section
lex9296 33:f77aa3ecf87d 452 // ========================
lex9296 33:f77aa3ecf87d 453 //
lex9296 33:f77aa3ecf87d 454 i64_SampleIndex = Stabilus322699.getPulses();
lex9296 33:f77aa3ecf87d 455 ai32_POS2VelGraph[i64_SampleIndex] = i32_Velocity;
lex9296 33:f77aa3ecf87d 456 ai32_POS2AccGraph[i64_SampleIndex] = i32_Acceleration;
lex9296 33:f77aa3ecf87d 457 ai32_POS2JrkGraph[i64_SampleIndex] = i32_Jerk;
lex9296 29:7ce8c5779f5e 458 }
lex9296 29:7ce8c5779f5e 459
lex9296 33:f77aa3ecf87d 460 static void MotionHandler (void) {
lex9296 35:0f6adc0b95b9 461 //static int16_t i16_Index = 0;
lex9296 35:0f6adc0b95b9 462 static uint32_t ui32_PreviousStep_ms;
lex9296 35:0f6adc0b95b9 463 uint32_t ui32_ActualStepSampled_ms;
lex9296 35:0f6adc0b95b9 464 uint32_t ui32_PassedActual_ms;
lex9296 35:0f6adc0b95b9 465 //
lex9296 35:0f6adc0b95b9 466 float fPassedActual_sxs;
lex9296 33:f77aa3ecf87d 467
lex9296 33:f77aa3ecf87d 468 // LA: Retrieve Actual Position
lex9296 33:f77aa3ecf87d 469 //
lex9296 33:f77aa3ecf87d 470 in_PosizionatoreSW.i64_ActualPosition = Stabilus322699.getPulses();
lex9296 33:f77aa3ecf87d 471
lex9296 33:f77aa3ecf87d 472 // LA: Execute Motion
lex9296 33:f77aa3ecf87d 473 //
lex9296 33:f77aa3ecf87d 474 PosizionatoreSW (in_PosizionatoreSW, out_PosizionatoreSW);
lex9296 33:f77aa3ecf87d 475 in_PosizionatoreSW.rtServoLock_Q = false;
lex9296 33:f77aa3ecf87d 476
lex9296 33:f77aa3ecf87d 477 // LA: Handle PostServo
lex9296 33:f77aa3ecf87d 478 //
lex9296 33:f77aa3ecf87d 479
lex9296 33:f77aa3ecf87d 480 // int64_t i64_StartPosition;
lex9296 33:f77aa3ecf87d 481 // int64_t i64_Distance;
lex9296 33:f77aa3ecf87d 482 // bool b_Accelerating; // LA: bACPos_Accelerating
lex9296 33:f77aa3ecf87d 483 // bool b_MaxSpeedReached;
lex9296 33:f77aa3ecf87d 484 // bool b_Decelerating; // bACPos_Decelerating
lex9296 33:f77aa3ecf87d 485 // bool b_InPosition;
lex9296 33:f77aa3ecf87d 486 // bool b_InToleranceFW;
lex9296 33:f77aa3ecf87d 487 // bool b_InToleranceBW;
lex9296 33:f77aa3ecf87d 488
lex9296 33:f77aa3ecf87d 489 // int32_t i32_ATVSpeed;
lex9296 34:0522cebfe489 490 // f_PWMPercent = ((float)out_PosizionatoreSW.i32_ATVSpeed)/ (float)in_PosizionatoreSW.i32_Max_Speed; // LA: In Range (float) 0.. 1
lex9296 34:0522cebfe489 491 // PWM_PB3.write((float) 1.0- f_PWMPercent); // Set to x%
lex9296 33:f77aa3ecf87d 492
lex9296 33:f77aa3ecf87d 493 // bool b_ATVDirectionFW;
lex9296 33:f77aa3ecf87d 494 rDIR_FWD = out_PosizionatoreSW.b_ATVDirectionFW;
lex9296 33:f77aa3ecf87d 495
lex9296 33:f77aa3ecf87d 496 // bool b_ATVDirectionBW;
lex9296 33:f77aa3ecf87d 497
lex9296 33:f77aa3ecf87d 498 // bool b_STW1_On;
lex9296 33:f77aa3ecf87d 499 // bool b_STW1_NoCStop;
lex9296 33:f77aa3ecf87d 500 // bool b_STW1_NoQStop;
lex9296 33:f77aa3ecf87d 501 // bool b_STW1_Enable;
lex9296 33:f77aa3ecf87d 502 rENA_Off = !out_PosizionatoreSW.b_STW1_Enable;
lex9296 33:f77aa3ecf87d 503
lex9296 33:f77aa3ecf87d 504 // LA: Update Motion Dynamic References
lex9296 33:f77aa3ecf87d 505 // ================================
lex9296 35:0f6adc0b95b9 506
lex9296 35:0f6adc0b95b9 507 // LA: Generazione del millisecondo Attuale
lex9296 35:0f6adc0b95b9 508 // ====================================
lex9296 33:f77aa3ecf87d 509 //
lex9296 35:0f6adc0b95b9 510 // Invoca il timer di sistema (TimersTimerValue) e lo confronta col suo precedente.
lex9296 35:0f6adc0b95b9 511 // Una volta elaborato e "scevrato" l'eventuale "Rollover" la sezione ritorna "ui32_PassedActual_ms_Local".
lex9296 35:0f6adc0b95b9 512 // "ui32_PassedActual_ms_Local" rappresenta i [ms] passati tra una istanza e l'altra
lex9296 35:0f6adc0b95b9 513 //
lex9296 35:0f6adc0b95b9 514 ui32_ActualStepSampled_ms = TimersTimerValue(); // Freezes the Actual Sample.
lex9296 35:0f6adc0b95b9 515 if (ui32_ActualStepSampled_ms >= ui32_PreviousStep_ms)
lex9296 35:0f6adc0b95b9 516 ui32_PassedActual_ms = (ui32_ActualStepSampled_ms- ui32_PreviousStep_ms); // Result => Actual- Previous
lex9296 35:0f6adc0b95b9 517 else
lex9296 35:0f6adc0b95b9 518 ui32_PassedActual_ms = ui32_ActualStepSampled_ms+ (0x7fffffff- ui32_PreviousStep_ms); // Result => Actual+ (Rollover- Previous)
lex9296 35:0f6adc0b95b9 519 ui32_PreviousStep_ms = ui32_ActualStepSampled_ms; // Store(s)&Hold(s) actual msSample
lex9296 35:0f6adc0b95b9 520 fPassedActual_sxs = ((float) 1000.0/ (float) ui32_PassedActual_ms); // Steps Any [s]
lex9296 34:0522cebfe489 521
lex9296 35:0f6adc0b95b9 522 i32_Velocity = (int32_t) (in_PosizionatoreSW.i64_ActualPosition- i64_Position_Prec); // LA: Velocity in [ui/ms]
lex9296 35:0f6adc0b95b9 523 i64_Position_Prec = in_PosizionatoreSW.i64_ActualPosition;
lex9296 35:0f6adc0b95b9 524 i32_Acceleration = (i32_Velocity- i32_Velocity_Prec); // LA: Acceleration in [ui/ms^2]
lex9296 35:0f6adc0b95b9 525 i32_Velocity_Prec = i32_Velocity;
lex9296 35:0f6adc0b95b9 526 i32_Jerk = (i32_Acceleration- i32_Acceleration_Prec); // LA: Jerk
lex9296 35:0f6adc0b95b9 527 i32_Acceleration_Prec = i32_Acceleration;
lex9296 33:f77aa3ecf87d 528
lex9296 35:0f6adc0b95b9 529 fVelocity = (float) i32_Velocity * fPassedActual_sxs; // Velocity in [ui/s]
lex9296 35:0f6adc0b95b9 530 fAcceleration = (float) i32_Acceleration * fPassedActual_sxs; // Acceleration in [ui/s^2]
lex9296 35:0f6adc0b95b9 531 fJerk = (float) i32_Jerk * fPassedActual_sxs; // Jerk
lex9296 34:0522cebfe489 532
lex9296 35:0f6adc0b95b9 533 // LA: PID Compute Section
lex9296 35:0f6adc0b95b9 534 // ===================
lex9296 35:0f6adc0b95b9 535 //
lex9296 36:cab8aa44ef91 536 if (out_PosizionatoreSW.b_ATVDirectionFW) {
lex9296 36:cab8aa44ef91 537 PID_VelocityClosedLoop_BW.reset();
lex9296 34:0522cebfe489 538
lex9296 36:cab8aa44ef91 539 // Update the process variable.
lex9296 36:cab8aa44ef91 540 PID_VelocityClosedLoop_FW.setProcessValue((float)i32_Velocity);
lex9296 36:cab8aa44ef91 541 // Set Desired Value
lex9296 36:cab8aa44ef91 542 PID_VelocityClosedLoop_FW.setSetPoint((float)out_PosizionatoreSW.i32_ATVSpeed);
lex9296 36:cab8aa44ef91 543 // Release a new output.
lex9296 36:cab8aa44ef91 544 f_PWMPercent = PID_VelocityClosedLoop_FW.compute();
lex9296 36:cab8aa44ef91 545 }
lex9296 36:cab8aa44ef91 546 else {
lex9296 36:cab8aa44ef91 547 PID_VelocityClosedLoop_FW.reset();
lex9296 34:0522cebfe489 548
lex9296 36:cab8aa44ef91 549 // Update the process variable.
lex9296 36:cab8aa44ef91 550 PID_VelocityClosedLoop_BW.setProcessValue((float)i32_Velocity);
lex9296 36:cab8aa44ef91 551 // Set Desired Value
lex9296 36:cab8aa44ef91 552 PID_VelocityClosedLoop_BW.setSetPoint((float)out_PosizionatoreSW.i32_ATVSpeed);
lex9296 36:cab8aa44ef91 553 // Release a new output.
lex9296 36:cab8aa44ef91 554 f_PWMPercent = PID_VelocityClosedLoop_BW.compute();
lex9296 36:cab8aa44ef91 555 }
lex9296 35:0f6adc0b95b9 556 PWM_PB3.write((float) 1.0- f_PWMPercent); // Set to x%
lex9296 34:0522cebfe489 557 }