Luca Antolini / Mbed 2 deprecated AAA_Stabilus322699_LA0022

Stabilus 322699 wDoublePID, ErrorGetter

Dependencies:   mbed QEI PID DmTftLibraryEx

main.cpp@38:72394e4c35f8, 2022-04-12 (annotated)

Committer:
lex9296
Date:
Tue Apr 12 07:55:59 2022 +0000
Revision:
38:72394e4c35f8
Parent:
37:5fc7f2f435e8 
Update

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 37:5fc7f2f435e8 44 int64_t ci64_TargetPOS = 3096; // Used also Outside
lex9296 37:5fc7f2f435e8 45
lex9296 34:0522cebfe489 46 #include "PID.h"
lex9296 37:5fc7f2f435e8 47
lex9296 37:5fc7f2f435e8 48 /*
lex9296 34:0522cebfe489 49 #define Lx 0.0001
lex9296 34:0522cebfe489 50 #define Ku 0.32200//0.922000
lex9296 34:0522cebfe489 51 #define Pu 0.0125
lex9296 34:0522cebfe489 52 #define Kp Ku*0.6
lex9296 34:0522cebfe489 53 #define Ti Pu*0.5
lex9296 34:0522cebfe489 54 #define Td Pu*0.125
lex9296 34:0522cebfe489 55 //
lex9296 34:0522cebfe489 56 #define P Kp
lex9296 34:0522cebfe489 57 #define I Kp/Ti
lex9296 34:0522cebfe489 58 #define D Kp*Td
lex9296 34:0522cebfe489 59 #define Mstate 6
lex9296 34:0522cebfe489 60 #define SorT 5
lex9296 34:0522cebfe489 61 #define Databit 0
lex9296 34:0522cebfe489 62 #define TIME 0.0125
lex9296 37:5fc7f2f435e8 63 */
lex9296 34:0522cebfe489 64
lex9296 37:5fc7f2f435e8 65 // Kc, Ti, Td, interval
lex9296 37:5fc7f2f435e8 66 //PID PID_VelocityClosedLoop_FW (0.4, 0.0, /*1.0E-32,*/ 1.0E-9, cf_MOTPeriod_s);
lex9296 37:5fc7f2f435e8 67 //PID PID_VelocityClosedLoop_BW (0.41, 0.0, /*1.0E-32,*/ 1.0E-9, cf_MOTPeriod_s);
lex9296 37:5fc7f2f435e8 68 //PID PID_VelocityClosedLoop_FW (0.55, 0.0, /*1.0E-32,*/ 1.0E-9, cf_MOTPeriod_s);
lex9296 37:5fc7f2f435e8 69 //PID PID_VelocityClosedLoop_BW (0.55, 0.0, /*1.0E-32,*/ 1.0E-2, cf_MOTPeriod_s);
lex9296 37:5fc7f2f435e8 70 PID PID_VelocityClosedLoop_FW (0.55, 0.0, 0.0, cf_MOTPeriod_s);
lex9296 37:5fc7f2f435e8 71 PID PID_VelocityClosedLoop_BW (0.55, 0.0, 0.0, cf_MOTPeriod_s);
lex9296 33:f77aa3ecf87d 72
lex9296 33:f77aa3ecf87d 73 // LA: LCM_ShowTactics
lex9296 33:f77aa3ecf87d 74 // ===============
lex9296 33:f77aa3ecf87d 75 //
lex9296 32:1be3d79ff4db 76 void LCM_ShowTactics(
lex9296 33:f77aa3ecf87d 77 int64_t i64_Pulses,
lex9296 33:f77aa3ecf87d 78 int32_t i32_ATVSpeed,
lex9296 33:f77aa3ecf87d 79 //
lex9296 33:f77aa3ecf87d 80 float f_ai0000_Aux,
lex9296 33:f77aa3ecf87d 81 float f_ai0001_Aux,
lex9296 33:f77aa3ecf87d 82 float f_ai0002_Aux,
lex9296 33:f77aa3ecf87d 83 float f_ai0003_Aux,
lex9296 33:f77aa3ecf87d 84 float f_ai0004_Aux,
lex9296 33:f77aa3ecf87d 85 float f_ai0005_Aux,
lex9296 33:f77aa3ecf87d 86 //
lex9296 33:f77aa3ecf87d 87 int32_t i32_Velocity,
lex9296 33:f77aa3ecf87d 88 int32_t i32_Acceleration,
lex9296 33:f77aa3ecf87d 89 int32_t i32_Jerk
lex9296 33:f77aa3ecf87d 90
lex9296 32:1be3d79ff4db 91 );
lex9296 32:1be3d79ff4db 92
lex9296 33:f77aa3ecf87d 93 // LA: SampleAndStore
lex9296 33:f77aa3ecf87d 94 // LA: SampleTimer ==
lex9296 33:f77aa3ecf87d 95 // ==============
lex9296 33:f77aa3ecf87d 96 //
lex9296 33:f77aa3ecf87d 97 static void SampleAndStore (void);
lex9296 33:f77aa3ecf87d 98 Ticker SampleTimer; // LA: To Sample 1AI any 'x'ms
lex9296 33:f77aa3ecf87d 99
lex9296 29:7ce8c5779f5e 100 float af_PlotSamples[240]; // LA: "Horiz" Plot Array
lex9296 29:7ce8c5779f5e 101
lex9296 30:e45282a70a4d 102 uint16_t aui16_PlotSamples[240];
lex9296 30:e45282a70a4d 103 uint16_t aui16_PlotClears_Lo[240];
lex9296 30:e45282a70a4d 104 uint16_t aui16_PlotClears_Hi[240];
lex9296 30:e45282a70a4d 105
lex9296 33:f77aa3ecf87d 106 int32_t ai32_POS2VelGraph[4000];
lex9296 33:f77aa3ecf87d 107 uint16_t aui16_PlotPOS2VelSamples[240];
lex9296 33:f77aa3ecf87d 108 uint16_t aui16_PlotPOS2VelClears_Lo[240];
lex9296 33:f77aa3ecf87d 109 uint16_t aui16_PlotPOS2VelClears_Hi[240];
lex9296 33:f77aa3ecf87d 110
lex9296 33:f77aa3ecf87d 111 int32_t ai32_POS2AccGraph[4000];
lex9296 33:f77aa3ecf87d 112 int32_t ai32_POS2JrkGraph[4000];
lex9296 33:f77aa3ecf87d 113
lex9296 33:f77aa3ecf87d 114 // LA: MotionHandler
lex9296 33:f77aa3ecf87d 115 // LA: MotionTimer =
lex9296 33:f77aa3ecf87d 116 // =============
lex9296 33:f77aa3ecf87d 117 //
lex9296 33:f77aa3ecf87d 118 static void MotionHandler (void);
lex9296 33:f77aa3ecf87d 119 Ticker MotionTimer; // LA: Gestisce il rilevamento (RT) di Velocità, Accelerazione e Jerk
lex9296 33:f77aa3ecf87d 120 // Esegue il Movimento Programmato
lex9296 33:f77aa3ecf87d 121
lex9296 33:f77aa3ecf87d 122 float f_PWMPercent; // Deprecated
lex9296 37:5fc7f2f435e8 123 float fPID_Error; //
lex9296 33:f77aa3ecf87d 124
lex9296 33:f77aa3ecf87d 125 float fVelocity;
lex9296 33:f77aa3ecf87d 126 float fAcceleration;
lex9296 33:f77aa3ecf87d 127 float fJerk;
lex9296 33:f77aa3ecf87d 128 float fTorque;
lex9296 33:f77aa3ecf87d 129
lex9296 33:f77aa3ecf87d 130 int64_t i64_Position_Prec;
lex9296 33:f77aa3ecf87d 131 int32_t i32_Velocity;
lex9296 33:f77aa3ecf87d 132 int32_t i32_Velocity_Prec;
lex9296 33:f77aa3ecf87d 133 int32_t i32_Acceleration;
lex9296 33:f77aa3ecf87d 134 int32_t i32_Acceleration_Prec;
lex9296 33:f77aa3ecf87d 135 int32_t i32_Jerk;
lex9296 33:f77aa3ecf87d 136
lex9296 33:f77aa3ecf87d 137 // LA: Motion
lex9296 33:f77aa3ecf87d 138 // ======
lex9296 26:bfca0c471a87 139 // LA: Theory of Operation
lex9296 26:bfca0c471a87 140 // ===================
lex9296 26:bfca0c471a87 141 //
lex9296 26:bfca0c471a87 142 // Il PWM funziona da sè in Interrupt
lex9296 26:bfca0c471a87 143 // Il QEI funziona da sè in Interrupt
lex9296 26:bfca0c471a87 144 // 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 145 //
lex9296 26:bfca0c471a87 146 // PQM
lex9296 26:bfca0c471a87 147 //
lex9296 26:bfca0c471a87 148 // Il rinfresco del Display e la gestione del motion vanno fatte il più frequentemente possibile ma fuori dal loop dei Ticker.
lex9296 26:bfca0c471a87 149 // Con qst versione (LA0005, che termina un FORK (il successivo è LA0010) quanto detto sopra è FUNZIONANTE.
lex9296 26:bfca0c471a87 150 // Questo messaggio è incluso nel "commitment"
lex9296 26:bfca0c471a87 151
GregCr 0:9c0917bfde97 152 /*!
GregCr 0:9c0917bfde97 153 * \brief Define IO for Unused Pin
GregCr 0:9c0917bfde97 154 */
lex9296 23:b9d23a2f390e 155 //DigitalOut F_CS (D6); // MBED description of pin
lex9296 23:b9d23a2f390e 156 //DigitalOut SD_CS (D8); // MBED description of pin
lex9296 23:b9d23a2f390e 157
lex9296 23:b9d23a2f390e 158 DigitalIn userButton (USER_BUTTON);
lex9296 32:1be3d79ff4db 159 //
lex9296 32:1be3d79ff4db 160 DigitalOut rENA_Off (PC_0); // CN7.38 - Power Enable Relay, Power Disabled when true
lex9296 32:1be3d79ff4db 161 DigitalOut rDIR_FWD (PC_1); // CN7.36 - Move Direction Relay Bridge, Move FW(Extends) when true
mverdy 15:ed7ed20b7114 162
lex9296 23:b9d23a2f390e 163 AnalogIn adc_temp (ADC_TEMP);
lex9296 23:b9d23a2f390e 164 AnalogIn adc_vref (ADC_VREF);
lex9296 23:b9d23a2f390e 165 AnalogIn adc_vbat (ADC_VBAT);
GregCr 0:9c0917bfde97 166
lex9296 29:7ce8c5779f5e 167 AnalogIn ADC12_IN9 (PA_4); // STM32 PA4
lex9296 29:7ce8c5779f5e 168 AnalogIn ADC12_IN15 (PB_0); // STM32 PB0
lex9296 29:7ce8c5779f5e 169
lex9296 23:b9d23a2f390e 170 // PWM
lex9296 23:b9d23a2f390e 171 // ===
lex9296 23:b9d23a2f390e 172 //
lex9296 25:5fd2d69e2927 173 PwmOut PWM_PB3(PWM_OUT); // LA: PWM_OUT = D3 = PB_3
mverdy 14:ebd89dacc807 174
lex9296 28:443c82750dd9 175 // QEI
lex9296 28:443c82750dd9 176 // ===
lex9296 28:443c82750dd9 177 //
lex9296 28:443c82750dd9 178 QEI Stabilus322699 (PA_1, PA_0, NC, 100, QEI::X4_ENCODING);
lex9296 32:1be3d79ff4db 179 //DigitalIn Hall_A (PA_1);
lex9296 32:1be3d79ff4db 180 //DigitalIn Hall_B (PA_0);
lex9296 28:443c82750dd9 181
lex9296 23:b9d23a2f390e 182 // Motion
lex9296 23:b9d23a2f390e 183 // ======
lex9296 23:b9d23a2f390e 184 //
lex9296 28:443c82750dd9 185 //Ticker POS_MotionScan; // LA: Non uso un Ticker. Agisce sotto Interrupt e falsa la lettura QEI e la sincronicità del PWM
lex9296 23:b9d23a2f390e 186 //
lex9296 23:b9d23a2f390e 187 in_sPosizionatoreSW in_PosizionatoreSW;
lex9296 23:b9d23a2f390e 188 out_sPosizionatoreSW out_PosizionatoreSW;
GregCr 0:9c0917bfde97 189
lex9296 23:b9d23a2f390e 190 // LCD Display
lex9296 23:b9d23a2f390e 191 // ===========
lex9296 23:b9d23a2f390e 192 //
lex9296 28:443c82750dd9 193 //Ticker LCD_RefreshViews; // LA: Non uso un Ticker. Agisce sotto Interrupt e falsa la lettura QEI e la sincronicità del PWM
mverdy 15:ed7ed20b7114 194
lex9296 22:12c555cade79 195 void FactoryReset (void) {
mverdy 15:ed7ed20b7114 196 EepromFactoryReset( );
mverdy 15:ed7ed20b7114 197 HAL_NVIC_SystemReset( );
mverdy 15:ed7ed20b7114 198 }
lex9296 23:b9d23a2f390e 199
lex9296 23:b9d23a2f390e 200 // =======
lex9296 23:b9d23a2f390e 201 // =======
lex9296 23:b9d23a2f390e 202 // Main(s)
lex9296 23:b9d23a2f390e 203 // =======
lex9296 23:b9d23a2f390e 204 // =======
lex9296 23:b9d23a2f390e 205 //
lex9296 23:b9d23a2f390e 206 int main (void){
lex9296 28:443c82750dd9 207
lex9296 32:1be3d79ff4db 208 rDIR_FWD = true; // LA: Actuator Extends
lex9296 32:1be3d79ff4db 209 rENA_Off = true; // LA: Drive Power is Off
lex9296 33:f77aa3ecf87d 210 //
lex9296 33:f77aa3ecf87d 211 in_PosizionatoreSW.b_ServoLock = false;
lex9296 33:f77aa3ecf87d 212 in_PosizionatoreSW.rtServoLock_Q = false;
lex9296 28:443c82750dd9 213
lex9296 23:b9d23a2f390e 214 EepromInit(); // LA: Inizializza la EEProm
lex9296 23:b9d23a2f390e 215 TimersInit(); // LA: Parte il Timer a 1ms
lex9296 23:b9d23a2f390e 216
lex9296 23:b9d23a2f390e 217 // LA: FactoryReset se "userButton" premuto all'avvio
lex9296 23:b9d23a2f390e 218 //
lex9296 23:b9d23a2f390e 219 if (userButton == 0) {
lex9296 23:b9d23a2f390e 220 FactoryReset();
lex9296 23:b9d23a2f390e 221 }
lex9296 28:443c82750dd9 222 DisplayDriverInit();
lex9296 23:b9d23a2f390e 223
lex9296 33:f77aa3ecf87d 224 SampleTimer.attach(&SampleAndStore, cf_SCOPeriod_s); // LA: Avvia l'OscilloScopio con TimeBase x [s]
lex9296 33:f77aa3ecf87d 225 MotionTimer.attach(&MotionHandler, cf_MOTPeriod_s); // LA: Avvia il Motion con TimeBase x [s]
lex9296 33:f77aa3ecf87d 226 PWM_PB3.period(cf_PWMPeriod_s); // LA: Avvia il PWM con TimeBase x [s]
lex9296 33:f77aa3ecf87d 227 PWM_PB3.write((float) 0.0); // Set to 0%
lex9296 23:b9d23a2f390e 228
lex9296 23:b9d23a2f390e 229 // LA: Motion (1st) Setup
lex9296 23:b9d23a2f390e 230 //
lex9296 28:443c82750dd9 231 in_PosizionatoreSW.b_AxisPowered = true;
lex9296 28:443c82750dd9 232 in_PosizionatoreSW.b_ACPos_Homed = true;
lex9296 35:0f6adc0b95b9 233 in_PosizionatoreSW.i32_Max_Speed = 20; // [ui/ms]
lex9296 23:b9d23a2f390e 234 in_PosizionatoreSW.i32_ZeroSpeed = 0; //
lex9296 23:b9d23a2f390e 235
lex9296 32:1be3d79ff4db 236 // POS Mode
lex9296 32:1be3d79ff4db 237 // ========
lex9296 32:1be3d79ff4db 238 //
lex9296 33:f77aa3ecf87d 239 // in_PosizionatoreSW.b_ServoLock = true;
lex9296 33:f77aa3ecf87d 240 // in_PosizionatoreSW.rtServoLock_Q = false;
lex9296 32:1be3d79ff4db 241 //
lex9296 33:f77aa3ecf87d 242 in_PosizionatoreSW.i64_TargetPosition = ci64_TargetPOS; // [ui]
lex9296 32:1be3d79ff4db 243 in_PosizionatoreSW.i64_ActualPosition = Stabilus322699.getPulses(); //
lex9296 35:0f6adc0b95b9 244 in_PosizionatoreSW.i64_AccelerationWindow = 512; // LA: Spazio concesso all'accelerazione.
lex9296 35:0f6adc0b95b9 245 in_PosizionatoreSW.i64_DecelerationWindow = 1024; // Spazio concesso alla decelerazione, è prioritario rispetto all'accelerazione.
lex9296 35:0f6adc0b95b9 246 in_PosizionatoreSW.i64_diToleranceWindow = 16; // Finestra di Tolleranza
lex9296 32:1be3d79ff4db 247 //
lex9296 35:0f6adc0b95b9 248 in_PosizionatoreSW.f_MaximumSpeed_x100_FW = 100.0f; // % of "i32_Max_Speed"
lex9296 35:0f6adc0b95b9 249 in_PosizionatoreSW.f_MaximumSpeed_x100_BW = 100.0f; //
lex9296 35:0f6adc0b95b9 250 in_PosizionatoreSW.f_ServoLockSpeed_x100_FW = 25.0f; // Riferimento di velocità minima a cui (appena) si muove l'asse [verso FW]
lex9296 37:5fc7f2f435e8 251 // in_PosizionatoreSW.f_ServoLockSpeed_x100_BW = 50.0f; // Riferimento di velocità minima a cui (appena) si muove l'asse [verso BW]
lex9296 37:5fc7f2f435e8 252 in_PosizionatoreSW.f_ServoLockSpeed_x100_BW = 48.0f; // Riferimento di velocità minima a cui (appena) si muove l'asse [verso BW]
lex9296 32:1be3d79ff4db 253
lex9296 32:1be3d79ff4db 254 // JOG Mode
lex9296 32:1be3d79ff4db 255 // ========
lex9296 32:1be3d79ff4db 256 //
lex9296 23:b9d23a2f390e 257 in_PosizionatoreSW.b_JogMode = false;
lex9296 23:b9d23a2f390e 258 in_PosizionatoreSW.b_JogFW = false;
lex9296 23:b9d23a2f390e 259 in_PosizionatoreSW.b_JogBW = false;
lex9296 23:b9d23a2f390e 260 in_PosizionatoreSW.i32_JogAccel_ms = 500; // [ms]
lex9296 23:b9d23a2f390e 261 in_PosizionatoreSW.i32_JogDecel_ms = 250; //
lex9296 23:b9d23a2f390e 262 //
lex9296 32:1be3d79ff4db 263 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 264 in_PosizionatoreSW.f_JogSpeed_x100_BW = (in_PosizionatoreSW.f_MaximumSpeed_x100_BW/ 2); //
lex9296 23:b9d23a2f390e 265
lex9296 34:0522cebfe489 266 // Velocity Loop PID
lex9296 34:0522cebfe489 267 // =================
lex9296 34:0522cebfe489 268 //
lex9296 34:0522cebfe489 269 // Input Speed (ref)= 0.. 512[ui/s]
lex9296 36:cab8aa44ef91 270 PID_VelocityClosedLoop_FW.setInputLimits(0.0f, (float)in_PosizionatoreSW.i32_Max_Speed);
lex9296 36:cab8aa44ef91 271 PID_VelocityClosedLoop_BW.setInputLimits(0.0f, (float)in_PosizionatoreSW.i32_Max_Speed);
lex9296 34:0522cebfe489 272 // Output PWM (ref)= 0.. 1
lex9296 36:cab8aa44ef91 273 PID_VelocityClosedLoop_FW.setOutputLimits(0.0f, 1.0f);
lex9296 36:cab8aa44ef91 274 PID_VelocityClosedLoop_BW.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 36:cab8aa44ef91 278 // PID_VelocityClosedLoop.setMode(AUTO_MODE);
lex9296 36:cab8aa44ef91 279 PID_VelocityClosedLoop_FW.setMode(MANUAL_MODE);
lex9296 36:cab8aa44ef91 280 PID_VelocityClosedLoop_BW.setMode(MANUAL_MODE);
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 37:5fc7f2f435e8 386 static float f_ai0006_prec;
lex9296 32:1be3d79ff4db 387
lex9296 33:f77aa3ecf87d 388 static uint32_t i32_Velocity_prec;
lex9296 33:f77aa3ecf87d 389 static uint32_t i32_Acceleration_prec;
lex9296 33:f77aa3ecf87d 390 static uint32_t i32_Jerk_prec;
lex9296 33:f77aa3ecf87d 391
lex9296 33:f77aa3ecf87d 392 if (i64_Pulses != i64_Pulses_Prec) {
lex9296 32:1be3d79ff4db 393 sprintf (StringText,
lex9296 33:f77aa3ecf87d 394 "Pulses: %d ", (int32_t)i64_Pulses);
lex9296 32:1be3d79ff4db 395 LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 1), StringText);
lex9296 33:f77aa3ecf87d 396 i64_Pulses_Prec = i64_Pulses;
lex9296 32:1be3d79ff4db 397 }
lex9296 32:1be3d79ff4db 398
lex9296 33:f77aa3ecf87d 399 if (i32_Velocity != i32_Velocity_prec) {
lex9296 33:f77aa3ecf87d 400 sprintf (StringText,
lex9296 35:0f6adc0b95b9 401 "Velocity[ui/ms]: %d ", i32_Velocity); //, fVelocity);
lex9296 34:0522cebfe489 402 LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 3), StringText);
lex9296 33:f77aa3ecf87d 403 i32_Velocity_prec = i32_Velocity;
lex9296 33:f77aa3ecf87d 404 }
lex9296 35:0f6adc0b95b9 405
lex9296 35:0f6adc0b95b9 406 if (out_PosizionatoreSW.i32_ATVSpeed != i32_Acceleration_prec) {
lex9296 35:0f6adc0b95b9 407 sprintf (StringText,
lex9296 35:0f6adc0b95b9 408 "ATVSpeed[ui/ms]: %d ", out_PosizionatoreSW.i32_ATVSpeed); //, fAcceleration);
lex9296 35:0f6adc0b95b9 409 LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 4), StringText);
lex9296 35:0f6adc0b95b9 410 i32_Acceleration_prec = out_PosizionatoreSW.i32_ATVSpeed;
lex9296 35:0f6adc0b95b9 411 }
lex9296 35:0f6adc0b95b9 412 if (f_PWMPercent != f_ai0005_prec) {
lex9296 35:0f6adc0b95b9 413 sprintf (StringText,
lex9296 35:0f6adc0b95b9 414 "PID_FB Compute: %f ", f_PWMPercent); //, fJerk);
lex9296 35:0f6adc0b95b9 415 LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 5), StringText);
lex9296 35:0f6adc0b95b9 416 f_ai0005_prec = f_PWMPercent;
lex9296 35:0f6adc0b95b9 417 }
lex9296 37:5fc7f2f435e8 418 if (fPID_Error != f_ai0006_prec) {
lex9296 37:5fc7f2f435e8 419 sprintf (StringText,
lex9296 37:5fc7f2f435e8 420 "PID_FB Error: %f ", fPID_Error); //, fJerk);
lex9296 37:5fc7f2f435e8 421 LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 6), StringText);
lex9296 37:5fc7f2f435e8 422 f_ai0006_prec = fPID_Error;
lex9296 37:5fc7f2f435e8 423 }
lex9296 35:0f6adc0b95b9 424
lex9296 35:0f6adc0b95b9 425 /*
lex9296 33:f77aa3ecf87d 426 if (i32_Acceleration != i32_Acceleration_prec) {
lex9296 33:f77aa3ecf87d 427 sprintf (StringText,
lex9296 33:f77aa3ecf87d 428 "Acc[ui/10ms^2]: %d ", i32_Acceleration); //, fAcceleration);
lex9296 34:0522cebfe489 429 // LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 12), StringText);
lex9296 34:0522cebfe489 430 LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 4), StringText);
lex9296 33:f77aa3ecf87d 431 i32_Acceleration_prec = i32_Acceleration;
lex9296 33:f77aa3ecf87d 432 }
lex9296 33:f77aa3ecf87d 433 if (i32_Jerk != i32_Jerk_prec) {
lex9296 33:f77aa3ecf87d 434 sprintf (StringText,
lex9296 33:f77aa3ecf87d 435 "Jerk: %d ", i32_Jerk); //, fJerk);
lex9296 34:0522cebfe489 436 // LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 13), StringText);
lex9296 34:0522cebfe489 437 LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 5), StringText);
lex9296 33:f77aa3ecf87d 438 i32_Jerk_prec = i32_Jerk;
lex9296 33:f77aa3ecf87d 439 }
lex9296 35:0f6adc0b95b9 440 */
lex9296 32:1be3d79ff4db 441 }
lex9296 32:1be3d79ff4db 442
lex9296 33:f77aa3ecf87d 443 static void SampleAndStore (void) {
lex9296 29:7ce8c5779f5e 444 int16_t i16_SampleIndex;
lex9296 33:f77aa3ecf87d 445 int64_t i64_SampleIndex;
lex9296 29:7ce8c5779f5e 446
lex9296 32:1be3d79ff4db 447 // af_PlotSamples[240- 1] = ADC12_IN9.read();
lex9296 32:1be3d79ff4db 448 af_PlotSamples[240- 1] = (float) Stabilus322699.getChannelA() * 0.33f;
lex9296 32:1be3d79ff4db 449
lex9296 32:1be3d79ff4db 450 for (i16_SampleIndex = 0; i16_SampleIndex < (240- 1); i16_SampleIndex++)
lex9296 29:7ce8c5779f5e 451 af_PlotSamples[i16_SampleIndex] = af_PlotSamples[i16_SampleIndex+ 1];
lex9296 29:7ce8c5779f5e 452
lex9296 33:f77aa3ecf87d 453 // LA: Position's Graph Section
lex9296 33:f77aa3ecf87d 454 // ========================
lex9296 33:f77aa3ecf87d 455 //
lex9296 33:f77aa3ecf87d 456 i64_SampleIndex = Stabilus322699.getPulses();
lex9296 33:f77aa3ecf87d 457 ai32_POS2VelGraph[i64_SampleIndex] = i32_Velocity;
lex9296 33:f77aa3ecf87d 458 ai32_POS2AccGraph[i64_SampleIndex] = i32_Acceleration;
lex9296 33:f77aa3ecf87d 459 ai32_POS2JrkGraph[i64_SampleIndex] = i32_Jerk;
lex9296 29:7ce8c5779f5e 460 }
lex9296 29:7ce8c5779f5e 461
lex9296 33:f77aa3ecf87d 462 static void MotionHandler (void) {
lex9296 35:0f6adc0b95b9 463 //static int16_t i16_Index = 0;
lex9296 35:0f6adc0b95b9 464 static uint32_t ui32_PreviousStep_ms;
lex9296 35:0f6adc0b95b9 465 uint32_t ui32_ActualStepSampled_ms;
lex9296 35:0f6adc0b95b9 466 uint32_t ui32_PassedActual_ms;
lex9296 35:0f6adc0b95b9 467 //
lex9296 35:0f6adc0b95b9 468 float fPassedActual_sxs;
lex9296 33:f77aa3ecf87d 469
lex9296 33:f77aa3ecf87d 470 // LA: Retrieve Actual Position
lex9296 33:f77aa3ecf87d 471 //
lex9296 33:f77aa3ecf87d 472 in_PosizionatoreSW.i64_ActualPosition = Stabilus322699.getPulses();
lex9296 33:f77aa3ecf87d 473
lex9296 33:f77aa3ecf87d 474 // LA: Execute Motion
lex9296 33:f77aa3ecf87d 475 //
lex9296 33:f77aa3ecf87d 476 PosizionatoreSW (in_PosizionatoreSW, out_PosizionatoreSW);
lex9296 33:f77aa3ecf87d 477 in_PosizionatoreSW.rtServoLock_Q = false;
lex9296 33:f77aa3ecf87d 478
lex9296 33:f77aa3ecf87d 479 // LA: Handle PostServo
lex9296 33:f77aa3ecf87d 480 //
lex9296 33:f77aa3ecf87d 481
lex9296 33:f77aa3ecf87d 482 // int64_t i64_StartPosition;
lex9296 33:f77aa3ecf87d 483 // int64_t i64_Distance;
lex9296 33:f77aa3ecf87d 484 // bool b_Accelerating; // LA: bACPos_Accelerating
lex9296 33:f77aa3ecf87d 485 // bool b_MaxSpeedReached;
lex9296 33:f77aa3ecf87d 486 // bool b_Decelerating; // bACPos_Decelerating
lex9296 33:f77aa3ecf87d 487 // bool b_InPosition;
lex9296 33:f77aa3ecf87d 488 // bool b_InToleranceFW;
lex9296 33:f77aa3ecf87d 489 // bool b_InToleranceBW;
lex9296 33:f77aa3ecf87d 490
lex9296 33:f77aa3ecf87d 491 // int32_t i32_ATVSpeed;
lex9296 34:0522cebfe489 492 // f_PWMPercent = ((float)out_PosizionatoreSW.i32_ATVSpeed)/ (float)in_PosizionatoreSW.i32_Max_Speed; // LA: In Range (float) 0.. 1
lex9296 34:0522cebfe489 493 // PWM_PB3.write((float) 1.0- f_PWMPercent); // Set to x%
lex9296 33:f77aa3ecf87d 494
lex9296 33:f77aa3ecf87d 495 // bool b_ATVDirectionFW;
lex9296 33:f77aa3ecf87d 496 rDIR_FWD = out_PosizionatoreSW.b_ATVDirectionFW;
lex9296 33:f77aa3ecf87d 497
lex9296 33:f77aa3ecf87d 498 // bool b_ATVDirectionBW;
lex9296 33:f77aa3ecf87d 499
lex9296 33:f77aa3ecf87d 500 // bool b_STW1_On;
lex9296 33:f77aa3ecf87d 501 // bool b_STW1_NoCStop;
lex9296 33:f77aa3ecf87d 502 // bool b_STW1_NoQStop;
lex9296 33:f77aa3ecf87d 503 // bool b_STW1_Enable;
lex9296 33:f77aa3ecf87d 504 rENA_Off = !out_PosizionatoreSW.b_STW1_Enable;
lex9296 33:f77aa3ecf87d 505
lex9296 33:f77aa3ecf87d 506 // LA: Update Motion Dynamic References
lex9296 33:f77aa3ecf87d 507 // ================================
lex9296 35:0f6adc0b95b9 508
lex9296 35:0f6adc0b95b9 509 // LA: Generazione del millisecondo Attuale
lex9296 35:0f6adc0b95b9 510 // ====================================
lex9296 33:f77aa3ecf87d 511 //
lex9296 35:0f6adc0b95b9 512 // Invoca il timer di sistema (TimersTimerValue) e lo confronta col suo precedente.
lex9296 35:0f6adc0b95b9 513 // Una volta elaborato e "scevrato" l'eventuale "Rollover" la sezione ritorna "ui32_PassedActual_ms_Local".
lex9296 35:0f6adc0b95b9 514 // "ui32_PassedActual_ms_Local" rappresenta i [ms] passati tra una istanza e l'altra
lex9296 35:0f6adc0b95b9 515 //
lex9296 35:0f6adc0b95b9 516 ui32_ActualStepSampled_ms = TimersTimerValue(); // Freezes the Actual Sample.
lex9296 35:0f6adc0b95b9 517 if (ui32_ActualStepSampled_ms >= ui32_PreviousStep_ms)
lex9296 35:0f6adc0b95b9 518 ui32_PassedActual_ms = (ui32_ActualStepSampled_ms- ui32_PreviousStep_ms); // Result => Actual- Previous
lex9296 35:0f6adc0b95b9 519 else
lex9296 35:0f6adc0b95b9 520 ui32_PassedActual_ms = ui32_ActualStepSampled_ms+ (0x7fffffff- ui32_PreviousStep_ms); // Result => Actual+ (Rollover- Previous)
lex9296 35:0f6adc0b95b9 521 ui32_PreviousStep_ms = ui32_ActualStepSampled_ms; // Store(s)&Hold(s) actual msSample
lex9296 35:0f6adc0b95b9 522 fPassedActual_sxs = ((float) 1000.0/ (float) ui32_PassedActual_ms); // Steps Any [s]
lex9296 34:0522cebfe489 523
lex9296 35:0f6adc0b95b9 524 i32_Velocity = (int32_t) (in_PosizionatoreSW.i64_ActualPosition- i64_Position_Prec); // LA: Velocity in [ui/ms]
lex9296 35:0f6adc0b95b9 525 i64_Position_Prec = in_PosizionatoreSW.i64_ActualPosition;
lex9296 35:0f6adc0b95b9 526 i32_Acceleration = (i32_Velocity- i32_Velocity_Prec); // LA: Acceleration in [ui/ms^2]
lex9296 35:0f6adc0b95b9 527 i32_Velocity_Prec = i32_Velocity;
lex9296 35:0f6adc0b95b9 528 i32_Jerk = (i32_Acceleration- i32_Acceleration_Prec); // LA: Jerk
lex9296 35:0f6adc0b95b9 529 i32_Acceleration_Prec = i32_Acceleration;
lex9296 33:f77aa3ecf87d 530
lex9296 35:0f6adc0b95b9 531 fVelocity = (float) i32_Velocity * fPassedActual_sxs; // Velocity in [ui/s]
lex9296 35:0f6adc0b95b9 532 fAcceleration = (float) i32_Acceleration * fPassedActual_sxs; // Acceleration in [ui/s^2]
lex9296 35:0f6adc0b95b9 533 fJerk = (float) i32_Jerk * fPassedActual_sxs; // Jerk
lex9296 34:0522cebfe489 534
lex9296 35:0f6adc0b95b9 535 // LA: PID Compute Section
lex9296 35:0f6adc0b95b9 536 // ===================
lex9296 35:0f6adc0b95b9 537 //
lex9296 36:cab8aa44ef91 538 if (out_PosizionatoreSW.b_ATVDirectionFW) {
lex9296 36:cab8aa44ef91 539 PID_VelocityClosedLoop_BW.reset();
lex9296 34:0522cebfe489 540
lex9296 36:cab8aa44ef91 541 // Update the process variable.
lex9296 36:cab8aa44ef91 542 PID_VelocityClosedLoop_FW.setProcessValue((float)i32_Velocity);
lex9296 36:cab8aa44ef91 543 // Set Desired Value
lex9296 36:cab8aa44ef91 544 PID_VelocityClosedLoop_FW.setSetPoint((float)out_PosizionatoreSW.i32_ATVSpeed);
lex9296 36:cab8aa44ef91 545 // Release a new output.
lex9296 37:5fc7f2f435e8 546
lex9296 37:5fc7f2f435e8 547 fPID_Error = PID_VelocityClosedLoop_FW.getError();
lex9296 36:cab8aa44ef91 548 f_PWMPercent = PID_VelocityClosedLoop_FW.compute();
lex9296 36:cab8aa44ef91 549 }
lex9296 36:cab8aa44ef91 550 else {
lex9296 36:cab8aa44ef91 551 PID_VelocityClosedLoop_FW.reset();
lex9296 34:0522cebfe489 552
lex9296 36:cab8aa44ef91 553 // Update the process variable.
lex9296 36:cab8aa44ef91 554 PID_VelocityClosedLoop_BW.setProcessValue((float)i32_Velocity);
lex9296 36:cab8aa44ef91 555 // Set Desired Value
lex9296 36:cab8aa44ef91 556 PID_VelocityClosedLoop_BW.setSetPoint((float)out_PosizionatoreSW.i32_ATVSpeed);
lex9296 36:cab8aa44ef91 557 // Release a new output.
lex9296 37:5fc7f2f435e8 558
lex9296 37:5fc7f2f435e8 559 fPID_Error = PID_VelocityClosedLoop_BW.getError();
lex9296 36:cab8aa44ef91 560 f_PWMPercent = PID_VelocityClosedLoop_BW.compute();
lex9296 36:cab8aa44ef91 561 }
lex9296 35:0f6adc0b95b9 562 PWM_PB3.write((float) 1.0- f_PWMPercent); // Set to x%
lex9296 34:0522cebfe489 563 }