Stabilus 322699 wDoublePID, ErrorGetter

Dependencies:   mbed QEI PID DmTftLibraryEx

Committer:
lex9296
Date:
Mon Apr 11 11:52:49 2022 +0000
Revision:
35:0f6adc0b95b9
Parent:
34:0522cebfe489
Child:
36:cab8aa44ef91
Backup;

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