Luca Antolini / Mbed 2 deprecated AAA_Stabilus322699_LA0020

All the previous but the PID

Dependencies:   mbed QEI PID DmTftLibraryEx

main.cpp@33:f77aa3ecf87d, 2022-04-11 (annotated)

Committer:
lex9296
Date:
Mon Apr 11 06:01:01 2022 +0000
Revision:
33:f77aa3ecf87d
Parent:
32:1be3d79ff4db
Child:
34:0522cebfe489
Added some "Position to Speed" graph

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