Luca Antolini / Mbed 2 deprecated AAA_Stabilus322699_LA0022

Stabilus 322699 wDoublePID, ErrorGetter

Dependencies:   mbed QEI PID DmTftLibraryEx

main.cpp@34:0522cebfe489, 2022-04-11 (annotated)

Committer:
lex9296
Date:
Mon Apr 11 09:20:40 2022 +0000
Revision:
34:0522cebfe489
Parent:
33:f77aa3ecf87d
Child:
35:0f6adc0b95b9
Added some PID (Velocity Loop is Closed)

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 34:0522cebfe489 19
lex9296 23:b9d23a2f390e 20 #include "QEI.h"
lex9296 23:b9d23a2f390e 21 #include "SWPos.h"
lex9296 23:b9d23a2f390e 22
GregCr 0:9c0917bfde97 23 #include "Timers.h"
GregCr 0:9c0917bfde97 24 #include "Eeprom.h"
GregCr 0:9c0917bfde97 25
lex9296 27:654100855f5c 26 #define MAX_CHAR_PER_LINE 28
lex9296 27:654100855f5c 27 #define TEXT_ROW_SPACING 16
lex9296 27:654100855f5c 28 #define FONT_CHAR_WIDTH 8
lex9296 27:654100855f5c 29 #define FONT_CHAR_HEIGHT 16
lex9296 27:654100855f5c 30
lex9296 27:654100855f5c 31 #include "DisplayDriver.h"
lex9296 27:654100855f5c 32
lex9296 33:f77aa3ecf87d 33 const float cf_SCOPeriod_s = 0.00025000; // 250us
lex9296 33:f77aa3ecf87d 34 const float cf_PWMPeriod_s = 0.01000000; // 10ms
lex9296 33:f77aa3ecf87d 35 const float cf_MOTPeriod_s = 0.01000000; // 10ms
lex9296 33:f77aa3ecf87d 36
lex9296 34:0522cebfe489 37 #include "PID.h"
lex9296 34:0522cebfe489 38 //
lex9296 34:0522cebfe489 39 #define Lx 0.0001
lex9296 34:0522cebfe489 40 #define Ku 0.32200//0.922000
lex9296 34:0522cebfe489 41 #define Pu 0.0125
lex9296 34:0522cebfe489 42 #define Kp Ku*0.6
lex9296 34:0522cebfe489 43 #define Ti Pu*0.5
lex9296 34:0522cebfe489 44 #define Td Pu*0.125
lex9296 34:0522cebfe489 45 //
lex9296 34:0522cebfe489 46 #define P Kp
lex9296 34:0522cebfe489 47 #define I Kp/Ti
lex9296 34:0522cebfe489 48 #define D Kp*Td
lex9296 34:0522cebfe489 49 #define Mstate 6
lex9296 34:0522cebfe489 50 #define SorT 5
lex9296 34:0522cebfe489 51 #define Databit 0
lex9296 34:0522cebfe489 52 #define TIME 0.0125
lex9296 34:0522cebfe489 53
lex9296 34:0522cebfe489 54 //#define RATE (cf_MOTPeriod_s * 10.0f)
lex9296 34:0522cebfe489 55 #define RATE (cf_MOTPeriod_s)
lex9296 34:0522cebfe489 56
lex9296 34:0522cebfe489 57 //Timer Time;
lex9296 34:0522cebfe489 58 //PID SpeedClosedLoop(P,I,D,&Time);
lex9296 34:0522cebfe489 59
lex9296 34:0522cebfe489 60 // Kc, Ti, Td, interval
lex9296 34:0522cebfe489 61 PID PID_VelocityClosedLoop (1.0, 0.0, 0.0, RATE);
lex9296 34:0522cebfe489 62 //PID PID_VelocityClosedLoop (P, I, D, RATE);
lex9296 34:0522cebfe489 63
lex9296 33:f77aa3ecf87d 64 //const int64_t ci64_TargetPOS = 240; //
lex9296 34:0522cebfe489 65 //const int64_t ci64_TargetPOS = 3096; //
lex9296 34:0522cebfe489 66
lex9296 34:0522cebfe489 67 int64_t ci64_TargetPOS = 3096; // Used as CONST ...
lex9296 33:f77aa3ecf87d 68
lex9296 33:f77aa3ecf87d 69 // LA: LCM_ShowTactics
lex9296 33:f77aa3ecf87d 70 // ===============
lex9296 33:f77aa3ecf87d 71 //
lex9296 32:1be3d79ff4db 72 void LCM_ShowTactics(
lex9296 33:f77aa3ecf87d 73 int64_t i64_Pulses,
lex9296 33:f77aa3ecf87d 74 int32_t i32_ATVSpeed,
lex9296 33:f77aa3ecf87d 75 //
lex9296 33:f77aa3ecf87d 76 float f_ai0000_Aux,
lex9296 33:f77aa3ecf87d 77 float f_ai0001_Aux,
lex9296 33:f77aa3ecf87d 78 float f_ai0002_Aux,
lex9296 33:f77aa3ecf87d 79 float f_ai0003_Aux,
lex9296 33:f77aa3ecf87d 80 float f_ai0004_Aux,
lex9296 33:f77aa3ecf87d 81 float f_ai0005_Aux,
lex9296 33:f77aa3ecf87d 82 //
lex9296 33:f77aa3ecf87d 83 int32_t i32_Velocity,
lex9296 33:f77aa3ecf87d 84 int32_t i32_Acceleration,
lex9296 33:f77aa3ecf87d 85 int32_t i32_Jerk
lex9296 33:f77aa3ecf87d 86
lex9296 32:1be3d79ff4db 87 );
lex9296 32:1be3d79ff4db 88
lex9296 33:f77aa3ecf87d 89 // LA: SampleAndStore
lex9296 33:f77aa3ecf87d 90 // LA: SampleTimer ==
lex9296 33:f77aa3ecf87d 91 // ==============
lex9296 33:f77aa3ecf87d 92 //
lex9296 33:f77aa3ecf87d 93 static void SampleAndStore (void);
lex9296 33:f77aa3ecf87d 94 Ticker SampleTimer; // LA: To Sample 1AI any 'x'ms
lex9296 33:f77aa3ecf87d 95
lex9296 29:7ce8c5779f5e 96 float af_PlotSamples[240]; // LA: "Horiz" Plot Array
lex9296 29:7ce8c5779f5e 97
lex9296 30:e45282a70a4d 98 uint16_t aui16_PlotSamples[240];
lex9296 30:e45282a70a4d 99 uint16_t aui16_PlotClears_Lo[240];
lex9296 30:e45282a70a4d 100 uint16_t aui16_PlotClears_Hi[240];
lex9296 30:e45282a70a4d 101
lex9296 33:f77aa3ecf87d 102 int32_t ai32_POS2VelGraph[4000];
lex9296 33:f77aa3ecf87d 103 uint16_t aui16_PlotPOS2VelSamples[240];
lex9296 33:f77aa3ecf87d 104 uint16_t aui16_PlotPOS2VelClears_Lo[240];
lex9296 33:f77aa3ecf87d 105 uint16_t aui16_PlotPOS2VelClears_Hi[240];
lex9296 33:f77aa3ecf87d 106
lex9296 33:f77aa3ecf87d 107 int32_t ai32_POS2AccGraph[4000];
lex9296 33:f77aa3ecf87d 108 int32_t ai32_POS2JrkGraph[4000];
lex9296 33:f77aa3ecf87d 109
lex9296 33:f77aa3ecf87d 110 // LA: MotionHandler
lex9296 33:f77aa3ecf87d 111 // LA: MotionTimer =
lex9296 33:f77aa3ecf87d 112 // =============
lex9296 33:f77aa3ecf87d 113 //
lex9296 33:f77aa3ecf87d 114 static void MotionHandler (void);
lex9296 33:f77aa3ecf87d 115 Ticker MotionTimer; // LA: Gestisce il rilevamento (RT) di Velocità, Accelerazione e Jerk
lex9296 33:f77aa3ecf87d 116 // Esegue il Movimento Programmato
lex9296 33:f77aa3ecf87d 117
lex9296 33:f77aa3ecf87d 118 float f_PWMPercent; // Deprecated
lex9296 33:f77aa3ecf87d 119
lex9296 33:f77aa3ecf87d 120 float fVelocity;
lex9296 33:f77aa3ecf87d 121 float fAcceleration;
lex9296 33:f77aa3ecf87d 122 float fJerk;
lex9296 33:f77aa3ecf87d 123 float fTorque;
lex9296 33:f77aa3ecf87d 124
lex9296 33:f77aa3ecf87d 125 int64_t i64_Position_Prec;
lex9296 33:f77aa3ecf87d 126 int32_t i32_Velocity;
lex9296 33:f77aa3ecf87d 127 int32_t i32_Velocity_Prec;
lex9296 33:f77aa3ecf87d 128 int32_t i32_Acceleration;
lex9296 33:f77aa3ecf87d 129 int32_t i32_Acceleration_Prec;
lex9296 33:f77aa3ecf87d 130 int32_t i32_Jerk;
lex9296 33:f77aa3ecf87d 131
lex9296 33:f77aa3ecf87d 132 // LA: Motion
lex9296 33:f77aa3ecf87d 133 // ======
lex9296 26:bfca0c471a87 134 // LA: Theory of Operation
lex9296 26:bfca0c471a87 135 // ===================
lex9296 26:bfca0c471a87 136 //
lex9296 26:bfca0c471a87 137 // Il PWM funziona da sè in Interrupt
lex9296 26:bfca0c471a87 138 // Il QEI funziona da sè in Interrupt
lex9296 26:bfca0c471a87 139 // 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 140 //
lex9296 26:bfca0c471a87 141 // PQM
lex9296 26:bfca0c471a87 142 //
lex9296 26:bfca0c471a87 143 // Il rinfresco del Display e la gestione del motion vanno fatte il più frequentemente possibile ma fuori dal loop dei Ticker.
lex9296 26:bfca0c471a87 144 // Con qst versione (LA0005, che termina un FORK (il successivo è LA0010) quanto detto sopra è FUNZIONANTE.
lex9296 26:bfca0c471a87 145 // Questo messaggio è incluso nel "commitment"
lex9296 26:bfca0c471a87 146
GregCr 0:9c0917bfde97 147 /*!
GregCr 0:9c0917bfde97 148 * \brief Define IO for Unused Pin
GregCr 0:9c0917bfde97 149 */
lex9296 23:b9d23a2f390e 150 //DigitalOut F_CS (D6); // MBED description of pin
lex9296 23:b9d23a2f390e 151 //DigitalOut SD_CS (D8); // MBED description of pin
lex9296 23:b9d23a2f390e 152
lex9296 23:b9d23a2f390e 153 DigitalIn userButton (USER_BUTTON);
lex9296 32:1be3d79ff4db 154 //
lex9296 32:1be3d79ff4db 155 DigitalOut rENA_Off (PC_0); // CN7.38 - Power Enable Relay, Power Disabled when true
lex9296 32:1be3d79ff4db 156 DigitalOut rDIR_FWD (PC_1); // CN7.36 - Move Direction Relay Bridge, Move FW(Extends) when true
mverdy 15:ed7ed20b7114 157
lex9296 23:b9d23a2f390e 158 AnalogIn adc_temp (ADC_TEMP);
lex9296 23:b9d23a2f390e 159 AnalogIn adc_vref (ADC_VREF);
lex9296 23:b9d23a2f390e 160 AnalogIn adc_vbat (ADC_VBAT);
GregCr 0:9c0917bfde97 161
lex9296 29:7ce8c5779f5e 162 AnalogIn ADC12_IN9 (PA_4); // STM32 PA4
lex9296 29:7ce8c5779f5e 163 AnalogIn ADC12_IN15 (PB_0); // STM32 PB0
lex9296 29:7ce8c5779f5e 164
lex9296 23:b9d23a2f390e 165 // PWM
lex9296 23:b9d23a2f390e 166 // ===
lex9296 23:b9d23a2f390e 167 //
lex9296 25:5fd2d69e2927 168 PwmOut PWM_PB3(PWM_OUT); // LA: PWM_OUT = D3 = PB_3
mverdy 14:ebd89dacc807 169
lex9296 28:443c82750dd9 170 // QEI
lex9296 28:443c82750dd9 171 // ===
lex9296 28:443c82750dd9 172 //
lex9296 28:443c82750dd9 173 QEI Stabilus322699 (PA_1, PA_0, NC, 100, QEI::X4_ENCODING);
lex9296 32:1be3d79ff4db 174 //DigitalIn Hall_A (PA_1);
lex9296 32:1be3d79ff4db 175 //DigitalIn Hall_B (PA_0);
lex9296 28:443c82750dd9 176
lex9296 23:b9d23a2f390e 177 // Motion
lex9296 23:b9d23a2f390e 178 // ======
lex9296 23:b9d23a2f390e 179 //
lex9296 28:443c82750dd9 180 //Ticker POS_MotionScan; // LA: Non uso un Ticker. Agisce sotto Interrupt e falsa la lettura QEI e la sincronicità del PWM
lex9296 23:b9d23a2f390e 181 //
lex9296 23:b9d23a2f390e 182 in_sPosizionatoreSW in_PosizionatoreSW;
lex9296 23:b9d23a2f390e 183 out_sPosizionatoreSW out_PosizionatoreSW;
GregCr 0:9c0917bfde97 184
lex9296 23:b9d23a2f390e 185 // LCD Display
lex9296 23:b9d23a2f390e 186 // ===========
lex9296 23:b9d23a2f390e 187 //
lex9296 28:443c82750dd9 188 //Ticker LCD_RefreshViews; // LA: Non uso un Ticker. Agisce sotto Interrupt e falsa la lettura QEI e la sincronicità del PWM
mverdy 15:ed7ed20b7114 189
lex9296 22:12c555cade79 190 void FactoryReset (void) {
mverdy 15:ed7ed20b7114 191 EepromFactoryReset( );
mverdy 15:ed7ed20b7114 192 HAL_NVIC_SystemReset( );
mverdy 15:ed7ed20b7114 193 }
lex9296 23:b9d23a2f390e 194
lex9296 23:b9d23a2f390e 195 // =======
lex9296 23:b9d23a2f390e 196 // =======
lex9296 23:b9d23a2f390e 197 // Main(s)
lex9296 23:b9d23a2f390e 198 // =======
lex9296 23:b9d23a2f390e 199 // =======
lex9296 23:b9d23a2f390e 200 //
lex9296 23:b9d23a2f390e 201 int main (void){
lex9296 28:443c82750dd9 202
lex9296 32:1be3d79ff4db 203 rDIR_FWD = true; // LA: Actuator Extends
lex9296 32:1be3d79ff4db 204 rENA_Off = true; // LA: Drive Power is Off
lex9296 33:f77aa3ecf87d 205 //
lex9296 33:f77aa3ecf87d 206 in_PosizionatoreSW.b_ServoLock = false;
lex9296 33:f77aa3ecf87d 207 in_PosizionatoreSW.rtServoLock_Q = false;
lex9296 28:443c82750dd9 208
lex9296 23:b9d23a2f390e 209 EepromInit(); // LA: Inizializza la EEProm
lex9296 23:b9d23a2f390e 210 TimersInit(); // LA: Parte il Timer a 1ms
lex9296 23:b9d23a2f390e 211
lex9296 23:b9d23a2f390e 212 // LA: FactoryReset se "userButton" premuto all'avvio
lex9296 23:b9d23a2f390e 213 //
lex9296 23:b9d23a2f390e 214 if (userButton == 0) {
lex9296 23:b9d23a2f390e 215 FactoryReset();
lex9296 23:b9d23a2f390e 216 }
lex9296 28:443c82750dd9 217 DisplayDriverInit();
lex9296 23:b9d23a2f390e 218
lex9296 33:f77aa3ecf87d 219 // SampleTimer.attach_us(&SampleAndStore, 250); // LA: Scope has its own times
lex9296 33:f77aa3ecf87d 220 SampleTimer.attach(&SampleAndStore, cf_SCOPeriod_s); // LA: Avvia l'OscilloScopio con TimeBase x [s]
lex9296 33:f77aa3ecf87d 221 MotionTimer.attach(&MotionHandler, cf_MOTPeriod_s); // LA: Avvia il Motion con TimeBase x [s]
lex9296 33:f77aa3ecf87d 222 PWM_PB3.period(cf_PWMPeriod_s); // LA: Avvia il PWM con TimeBase x [s]
lex9296 33:f77aa3ecf87d 223 PWM_PB3.write((float) 0.0); // Set to 0%
lex9296 23:b9d23a2f390e 224
lex9296 23:b9d23a2f390e 225 // LA: Motion (1st) Setup
lex9296 23:b9d23a2f390e 226 //
lex9296 28:443c82750dd9 227 in_PosizionatoreSW.b_AxisPowered = true;
lex9296 28:443c82750dd9 228 in_PosizionatoreSW.b_ACPos_Homed = true;
lex9296 34:0522cebfe489 229 in_PosizionatoreSW.i32_Max_Speed = 512; // [ui/s]
lex9296 23:b9d23a2f390e 230 in_PosizionatoreSW.i32_ZeroSpeed = 0; //
lex9296 23:b9d23a2f390e 231
lex9296 32:1be3d79ff4db 232 // POS Mode
lex9296 32:1be3d79ff4db 233 // ========
lex9296 32:1be3d79ff4db 234 //
lex9296 33:f77aa3ecf87d 235 // in_PosizionatoreSW.b_ServoLock = true;
lex9296 33:f77aa3ecf87d 236 // in_PosizionatoreSW.rtServoLock_Q = false;
lex9296 32:1be3d79ff4db 237 //
lex9296 33:f77aa3ecf87d 238 in_PosizionatoreSW.i64_TargetPosition = ci64_TargetPOS; // [ui]
lex9296 32:1be3d79ff4db 239 in_PosizionatoreSW.i64_ActualPosition = Stabilus322699.getPulses(); //
lex9296 33:f77aa3ecf87d 240 // in_PosizionatoreSW.i64_AccelerationWindow = 32; // LA: Spazio concesso all'accelerazione.
lex9296 33:f77aa3ecf87d 241 // in_PosizionatoreSW.i64_DecelerationWindow = 256; // Spazio concesso alla decelerazione, è prioritario rispetto all'accelerazione.
lex9296 34:0522cebfe489 242 in_PosizionatoreSW.i64_AccelerationWindow = 1024; // LA: Spazio concesso all'accelerazione.
lex9296 33:f77aa3ecf87d 243 in_PosizionatoreSW.i64_DecelerationWindow = 2048; // Spazio concesso alla decelerazione, è prioritario rispetto all'accelerazione.
lex9296 33:f77aa3ecf87d 244 in_PosizionatoreSW.i64_diToleranceWindow = 16; // Finestra di Tolleranza
lex9296 32:1be3d79ff4db 245 //
lex9296 34:0522cebfe489 246 // in_PosizionatoreSW.f_MaximumSpeed_x100_FW = 16.0; // % of "i32_Max_Speed"
lex9296 34:0522cebfe489 247 // in_PosizionatoreSW.f_MaximumSpeed_x100_BW = 62.0; //
lex9296 34:0522cebfe489 248 in_PosizionatoreSW.f_MaximumSpeed_x100_FW = 26.0; // % of "i32_Max_Speed"
lex9296 34:0522cebfe489 249 in_PosizionatoreSW.f_MaximumSpeed_x100_BW = 100.0; //
lex9296 33:f77aa3ecf87d 250 // in_PosizionatoreSW.f_ServoLockSpeed_x100_FW = 4.8; // Riferimento di velocità minima a cui (appena) si muove l'asse [verso FW]
lex9296 33:f77aa3ecf87d 251 // in_PosizionatoreSW.f_ServoLockSpeed_x100_BW = 18.0; // Riferimento di velocità minima a cui (appena) si muove l'asse [verso BW]
lex9296 34:0522cebfe489 252 in_PosizionatoreSW.f_ServoLockSpeed_x100_FW = 6.2; // Riferimento di velocità minima a cui (appena) si muove l'asse [verso FW]
lex9296 33:f77aa3ecf87d 253 // in_PosizionatoreSW.f_ServoLockSpeed_x100_FW = 4.8; // Riferimento di velocità minima a cui (appena) si muove l'asse [verso FW]
lex9296 34:0522cebfe489 254 in_PosizionatoreSW.f_ServoLockSpeed_x100_BW = 24.0; // 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 /*
lex9296 32:1be3d79ff4db 399 if (i32_ATVSpeed != ms_0002_prec) {
lex9296 32:1be3d79ff4db 400 sprintf (StringText,
lex9296 32:1be3d79ff4db 401 "Speed[ui]: %d ", i32_ATVSpeed);
lex9296 32:1be3d79ff4db 402 LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 3), StringText);
lex9296 32:1be3d79ff4db 403 ms_0002_prec = i32_ATVSpeed;
lex9296 32:1be3d79ff4db 404 }
lex9296 32:1be3d79ff4db 405
lex9296 32:1be3d79ff4db 406 if (f_ai0000_Aux != f_ai0000_prec) {
lex9296 32:1be3d79ff4db 407 sprintf (StringText,
lex9296 32:1be3d79ff4db 408 "ADC Temp = %f ", (f_ai0000_Aux* 100));
lex9296 32:1be3d79ff4db 409 LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 4), StringText);
lex9296 32:1be3d79ff4db 410 f_ai0000_prec = f_ai0000_Aux;
lex9296 32:1be3d79ff4db 411 }
lex9296 32:1be3d79ff4db 412
lex9296 32:1be3d79ff4db 413 if (f_ai0001_Aux != f_ai0001_prec) {
lex9296 32:1be3d79ff4db 414 sprintf (StringText,
lex9296 32:1be3d79ff4db 415 "ADC VBat = %f ", (f_ai0001_Aux* 10));
lex9296 32:1be3d79ff4db 416 LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 5), StringText);
lex9296 32:1be3d79ff4db 417 f_ai0001_prec = f_ai0001_Aux;
lex9296 32:1be3d79ff4db 418 }
lex9296 32:1be3d79ff4db 419
lex9296 32:1be3d79ff4db 420 if (f_ai0002_Aux != f_ai0002_prec) {
lex9296 32:1be3d79ff4db 421 sprintf (StringText,
lex9296 32:1be3d79ff4db 422 "ADC VRef = %f ", (f_ai0002_Aux* 10));
lex9296 32:1be3d79ff4db 423 LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 6), StringText);
lex9296 32:1be3d79ff4db 424 f_ai0002_prec = f_ai0002_Aux;
lex9296 32:1be3d79ff4db 425 }
lex9296 32:1be3d79ff4db 426
lex9296 32:1be3d79ff4db 427 if (f_ai0003_Aux != f_ai0003_prec) {
lex9296 32:1be3d79ff4db 428 sprintf (StringText,
lex9296 32:1be3d79ff4db 429 "ADC12.09 = %f ", (f_ai0003_Aux* 10)/ 3);
lex9296 32:1be3d79ff4db 430 LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 8), StringText);
lex9296 32:1be3d79ff4db 431 f_ai0003_prec = f_ai0003_Aux;
lex9296 32:1be3d79ff4db 432 }
lex9296 32:1be3d79ff4db 433
lex9296 32:1be3d79ff4db 434 if (f_ai0004_Aux != f_ai0004_prec) {
lex9296 32:1be3d79ff4db 435 sprintf (StringText,
lex9296 32:1be3d79ff4db 436 "ADC12.15 = %f ", (f_ai0004_Aux* 10)/ 3);
lex9296 32:1be3d79ff4db 437 LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 9), StringText);
lex9296 32:1be3d79ff4db 438 f_ai0004_prec = f_ai0004_Aux;
lex9296 32:1be3d79ff4db 439 }
lex9296 32:1be3d79ff4db 440
lex9296 32:1be3d79ff4db 441 if (f_ai0005_Aux != f_ai0005_prec) {
lex9296 32:1be3d79ff4db 442 sprintf (StringText,
lex9296 32:1be3d79ff4db 443 "PB3 PWM%% = %f ", (f_ai0005_Aux));
lex9296 32:1be3d79ff4db 444 LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 10), StringText);
lex9296 32:1be3d79ff4db 445 f_ai0005_prec = f_ai0005_Aux;
lex9296 32:1be3d79ff4db 446 }
lex9296 33:f77aa3ecf87d 447 */
lex9296 33:f77aa3ecf87d 448 if (i32_Velocity != i32_Velocity_prec) {
lex9296 33:f77aa3ecf87d 449 sprintf (StringText,
lex9296 33:f77aa3ecf87d 450 "Vel[ui/10ms]: %d ", i32_Velocity); //, fVelocity);
lex9296 34:0522cebfe489 451 // LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 11), StringText);
lex9296 34:0522cebfe489 452 LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 3), StringText);
lex9296 33:f77aa3ecf87d 453 i32_Velocity_prec = i32_Velocity;
lex9296 33:f77aa3ecf87d 454 }
lex9296 33:f77aa3ecf87d 455 if (i32_Acceleration != i32_Acceleration_prec) {
lex9296 33:f77aa3ecf87d 456 sprintf (StringText,
lex9296 33:f77aa3ecf87d 457 "Acc[ui/10ms^2]: %d ", i32_Acceleration); //, fAcceleration);
lex9296 34:0522cebfe489 458 // LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 12), StringText);
lex9296 34:0522cebfe489 459 LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 4), StringText);
lex9296 33:f77aa3ecf87d 460 i32_Acceleration_prec = i32_Acceleration;
lex9296 33:f77aa3ecf87d 461 }
lex9296 33:f77aa3ecf87d 462 if (i32_Jerk != i32_Jerk_prec) {
lex9296 33:f77aa3ecf87d 463 sprintf (StringText,
lex9296 33:f77aa3ecf87d 464 "Jerk: %d ", i32_Jerk); //, fJerk);
lex9296 34:0522cebfe489 465 // LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 13), StringText);
lex9296 34:0522cebfe489 466 LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 5), StringText);
lex9296 33:f77aa3ecf87d 467 i32_Jerk_prec = i32_Jerk;
lex9296 33:f77aa3ecf87d 468 }
lex9296 32:1be3d79ff4db 469 }
lex9296 32:1be3d79ff4db 470
lex9296 33:f77aa3ecf87d 471 static void SampleAndStore (void) {
lex9296 29:7ce8c5779f5e 472 int16_t i16_SampleIndex;
lex9296 33:f77aa3ecf87d 473 int64_t i64_SampleIndex;
lex9296 29:7ce8c5779f5e 474
lex9296 32:1be3d79ff4db 475 // af_PlotSamples[240- 1] = ADC12_IN9.read();
lex9296 32:1be3d79ff4db 476 af_PlotSamples[240- 1] = (float) Stabilus322699.getChannelA() * 0.33f;
lex9296 32:1be3d79ff4db 477
lex9296 32:1be3d79ff4db 478 for (i16_SampleIndex = 0; i16_SampleIndex < (240- 1); i16_SampleIndex++)
lex9296 29:7ce8c5779f5e 479 af_PlotSamples[i16_SampleIndex] = af_PlotSamples[i16_SampleIndex+ 1];
lex9296 29:7ce8c5779f5e 480
lex9296 33:f77aa3ecf87d 481 // LA: Position's Graph Section
lex9296 33:f77aa3ecf87d 482 // ========================
lex9296 33:f77aa3ecf87d 483 //
lex9296 33:f77aa3ecf87d 484 i64_SampleIndex = Stabilus322699.getPulses();
lex9296 33:f77aa3ecf87d 485 ai32_POS2VelGraph[i64_SampleIndex] = i32_Velocity;
lex9296 33:f77aa3ecf87d 486 ai32_POS2AccGraph[i64_SampleIndex] = i32_Acceleration;
lex9296 33:f77aa3ecf87d 487 ai32_POS2JrkGraph[i64_SampleIndex] = i32_Jerk;
lex9296 29:7ce8c5779f5e 488 }
lex9296 29:7ce8c5779f5e 489
lex9296 33:f77aa3ecf87d 490 static void MotionHandler (void) {
lex9296 33:f77aa3ecf87d 491 static int16_t i16_Index = 0;
lex9296 33:f77aa3ecf87d 492
lex9296 33:f77aa3ecf87d 493 // LA: Retrieve Actual Position
lex9296 33:f77aa3ecf87d 494 //
lex9296 33:f77aa3ecf87d 495 in_PosizionatoreSW.i64_ActualPosition = Stabilus322699.getPulses();
lex9296 33:f77aa3ecf87d 496
lex9296 33:f77aa3ecf87d 497 // LA: Execute Motion
lex9296 33:f77aa3ecf87d 498 //
lex9296 33:f77aa3ecf87d 499 PosizionatoreSW (in_PosizionatoreSW, out_PosizionatoreSW);
lex9296 33:f77aa3ecf87d 500 in_PosizionatoreSW.rtServoLock_Q = false;
lex9296 33:f77aa3ecf87d 501
lex9296 33:f77aa3ecf87d 502 // LA: Handle PostServo
lex9296 33:f77aa3ecf87d 503 //
lex9296 33:f77aa3ecf87d 504
lex9296 33:f77aa3ecf87d 505 // int64_t i64_StartPosition;
lex9296 33:f77aa3ecf87d 506 // int64_t i64_Distance;
lex9296 33:f77aa3ecf87d 507 // bool b_Accelerating; // LA: bACPos_Accelerating
lex9296 33:f77aa3ecf87d 508 // bool b_MaxSpeedReached;
lex9296 33:f77aa3ecf87d 509 // bool b_Decelerating; // bACPos_Decelerating
lex9296 33:f77aa3ecf87d 510 // bool b_InPosition;
lex9296 33:f77aa3ecf87d 511 // bool b_InToleranceFW;
lex9296 33:f77aa3ecf87d 512 // bool b_InToleranceBW;
lex9296 33:f77aa3ecf87d 513
lex9296 33:f77aa3ecf87d 514 // int32_t i32_ATVSpeed;
lex9296 34:0522cebfe489 515 // f_PWMPercent = ((float)out_PosizionatoreSW.i32_ATVSpeed)/ (float)in_PosizionatoreSW.i32_Max_Speed; // LA: In Range (float) 0.. 1
lex9296 34:0522cebfe489 516 // PWM_PB3.write((float) 1.0- f_PWMPercent); // Set to x%
lex9296 33:f77aa3ecf87d 517
lex9296 33:f77aa3ecf87d 518 // bool b_ATVDirectionFW;
lex9296 33:f77aa3ecf87d 519 rDIR_FWD = out_PosizionatoreSW.b_ATVDirectionFW;
lex9296 33:f77aa3ecf87d 520
lex9296 33:f77aa3ecf87d 521 // bool b_ATVDirectionBW;
lex9296 33:f77aa3ecf87d 522
lex9296 33:f77aa3ecf87d 523 // bool b_STW1_On;
lex9296 33:f77aa3ecf87d 524 // bool b_STW1_NoCStop;
lex9296 33:f77aa3ecf87d 525 // bool b_STW1_NoQStop;
lex9296 33:f77aa3ecf87d 526 // bool b_STW1_Enable;
lex9296 33:f77aa3ecf87d 527 rENA_Off = !out_PosizionatoreSW.b_STW1_Enable;
lex9296 33:f77aa3ecf87d 528
lex9296 33:f77aa3ecf87d 529 // LA: Update Motion Dynamic References
lex9296 33:f77aa3ecf87d 530 // ================================
lex9296 33:f77aa3ecf87d 531 //
lex9296 33:f77aa3ecf87d 532 // Per avere maggiore granularità delle misure, l'acquisizione viene fatta ogni 10 interventi
lex9296 33:f77aa3ecf87d 533 // Se il motion gira a 10ms, i riferimenti dinamici saranno calcolati ogni 100
lex9296 33:f77aa3ecf87d 534 //
lex9296 34:0522cebfe489 535 // if (i16_Index == 0) {
lex9296 33:f77aa3ecf87d 536 static uint32_t ui32_PreviousStep_ms;
lex9296 33:f77aa3ecf87d 537 uint32_t ui32_ActualStepSampled_ms;
lex9296 33:f77aa3ecf87d 538 uint32_t ui32_PassedActual_ms;
lex9296 33:f77aa3ecf87d 539 //
lex9296 33:f77aa3ecf87d 540 float fPassedActual_sxs;
lex9296 33:f77aa3ecf87d 541
lex9296 33:f77aa3ecf87d 542 // LA: Generazione del millisecondo Attuale
lex9296 33:f77aa3ecf87d 543 // ====================================
lex9296 33:f77aa3ecf87d 544 //
lex9296 33:f77aa3ecf87d 545 // Invoca il timer di sistema (TimersTimerValue) e lo confronta col suo precedente.
lex9296 33:f77aa3ecf87d 546 // Una volta elaborato e "scevrato" l'eventuale "Rollover" la sezione ritorna "ui32_PassedActual_ms_Local".
lex9296 33:f77aa3ecf87d 547 // "ui32_PassedActual_ms_Local" rappresenta i [ms] passati tra una istanza e l'altra
lex9296 33:f77aa3ecf87d 548 //
lex9296 33:f77aa3ecf87d 549 ui32_ActualStepSampled_ms = TimersTimerValue(); // Freezes the Actual Sample.
lex9296 33:f77aa3ecf87d 550 if (ui32_ActualStepSampled_ms >= ui32_PreviousStep_ms)
lex9296 33:f77aa3ecf87d 551 ui32_PassedActual_ms = (ui32_ActualStepSampled_ms- ui32_PreviousStep_ms); // Result => Actual- Previous
lex9296 33:f77aa3ecf87d 552 else
lex9296 33:f77aa3ecf87d 553 ui32_PassedActual_ms = ui32_ActualStepSampled_ms+ (0x7fffffff- ui32_PreviousStep_ms); // Result => Actual+ (Rollover- Previous)
lex9296 33:f77aa3ecf87d 554 ui32_PreviousStep_ms = ui32_ActualStepSampled_ms; // Store(s)&Hold(s) actual msSample
lex9296 33:f77aa3ecf87d 555 fPassedActual_sxs = ((float) 1000.0/ (float) ui32_PassedActual_ms); // Steps Any [s]
lex9296 33:f77aa3ecf87d 556
lex9296 33:f77aa3ecf87d 557 i32_Velocity = (int32_t) (in_PosizionatoreSW.i64_ActualPosition- i64_Position_Prec); // LA: Velocity in [ui/10ms]
lex9296 33:f77aa3ecf87d 558 i64_Position_Prec = in_PosizionatoreSW.i64_ActualPosition;
lex9296 33:f77aa3ecf87d 559 i32_Acceleration = (i32_Velocity- i32_Velocity_Prec); // LA: Acceleration in [ui/10ms^2]
lex9296 33:f77aa3ecf87d 560 i32_Velocity_Prec = i32_Velocity;
lex9296 33:f77aa3ecf87d 561 i32_Jerk = (i32_Acceleration- i32_Acceleration_Prec); // LA: Jerk
lex9296 33:f77aa3ecf87d 562 i32_Acceleration_Prec = i32_Acceleration;
lex9296 33:f77aa3ecf87d 563
lex9296 33:f77aa3ecf87d 564 fVelocity = (float) i32_Velocity * fPassedActual_sxs; // Velocity in [ui/s]
lex9296 33:f77aa3ecf87d 565 fAcceleration = (float) i32_Acceleration * fPassedActual_sxs; // Acceleration in [ui/s^2]
lex9296 33:f77aa3ecf87d 566 fJerk = (float) i32_Jerk * fPassedActual_sxs; // Jerk
lex9296 34:0522cebfe489 567
lex9296 34:0522cebfe489 568 // LA: PID Compute Section
lex9296 34:0522cebfe489 569 // ===================
lex9296 34:0522cebfe489 570 //
lex9296 34:0522cebfe489 571
lex9296 34:0522cebfe489 572 // Update the process variable.
lex9296 34:0522cebfe489 573 PID_VelocityClosedLoop.setProcessValue((float)i32_Velocity);
lex9296 34:0522cebfe489 574
lex9296 34:0522cebfe489 575 // Set Desired Value
lex9296 34:0522cebfe489 576 PID_VelocityClosedLoop.setSetPoint((float)out_PosizionatoreSW.i32_ATVSpeed);
lex9296 34:0522cebfe489 577
lex9296 34:0522cebfe489 578 // Release a new output.
lex9296 34:0522cebfe489 579 // f_PWMPercent = ((float)out_PosizionatoreSW.i32_ATVSpeed)/ (float)in_PosizionatoreSW.i32_Max_Speed; // LA: In Range (float) 0.. 1
lex9296 34:0522cebfe489 580 f_PWMPercent = PID_VelocityClosedLoop.compute();
lex9296 34:0522cebfe489 581 PWM_PB3.write((float) 1.0- f_PWMPercent); // Set to x%
lex9296 34:0522cebfe489 582 // }
lex9296 34:0522cebfe489 583 // i16_Index++;
lex9296 34:0522cebfe489 584 // if (i16_Index >= 10)
lex9296 34:0522cebfe489 585 // i16_Index = 0;
lex9296 33:f77aa3ecf87d 586
lex9296 33:f77aa3ecf87d 587 /*
lex9296 33:f77aa3ecf87d 588 // LA: Position's Graph Section
lex9296 33:f77aa3ecf87d 589 // ========================
lex9296 33:f77aa3ecf87d 590 //
lex9296 33:f77aa3ecf87d 591 ai32_POS2VelGraph[in_PosizionatoreSW.i64_ActualPosition] = i32_Velocity;
lex9296 33:f77aa3ecf87d 592 ai32_POS2AccGraph[in_PosizionatoreSW.i64_ActualPosition] = i32_Acceleration;
lex9296 33:f77aa3ecf87d 593 ai32_POS2JrkGraph[in_PosizionatoreSW.i64_ActualPosition] = i32_Jerk;
lex9296 33:f77aa3ecf87d 594 */
lex9296 33:f77aa3ecf87d 595 }
lex9296 34:0522cebfe489 596
lex9296 34:0522cebfe489 597 /*
lex9296 34:0522cebfe489 598 #include "PID.h"
lex9296 34:0522cebfe489 599
lex9296 34:0522cebfe489 600 #define RATE 0.1
lex9296 34:0522cebfe489 601
lex9296 34:0522cebfe489 602 //Kc, Ti, Td, interval
lex9296 34:0522cebfe489 603 PID controller(1.0, 0.0, 0.0, RATE);
lex9296 34:0522cebfe489 604 AnalogIn pv(p15);
lex9296 34:0522cebfe489 605 PwmOut co(p26);
lex9296 34:0522cebfe489 606
lex9296 34:0522cebfe489 607 int main(){
lex9296 34:0522cebfe489 608
lex9296 34:0522cebfe489 609 //Analog input from 0.0 to 3.3V
lex9296 34:0522cebfe489 610 controller.setInputLimits(0.0, 3.3);
lex9296 34:0522cebfe489 611 //Pwm output from 0.0 to 1.0
lex9296 34:0522cebfe489 612 controller.setOutputLimits(0.0, 1.0);
lex9296 34:0522cebfe489 613 //If there's a bias.
lex9296 34:0522cebfe489 614 controller.setBias(0.3);
lex9296 34:0522cebfe489 615 controller.setMode(AUTO_MODE);
lex9296 34:0522cebfe489 616 //We want the process variable to be 1.7V
lex9296 34:0522cebfe489 617 controller.setSetPoint(1.7);
lex9296 34:0522cebfe489 618
lex9296 34:0522cebfe489 619 while(1){
lex9296 34:0522cebfe489 620 //Update the process variable.
lex9296 34:0522cebfe489 621 controller.setProcessValue(pv.read());
lex9296 34:0522cebfe489 622 //Set the new output.
lex9296 34:0522cebfe489 623 co = controller.compute();
lex9296 34:0522cebfe489 624 //Wait for another loop calculation.
lex9296 34:0522cebfe489 625 wait(RATE);
lex9296 34:0522cebfe489 626 }
lex9296 34:0522cebfe489 627
lex9296 34:0522cebfe489 628 }
lex9296 34:0522cebfe489 629 */