Luca Antolini
Forked to initiate the Current Loop Gain
Dependencies: mbed QEI DmTftLibraryEx
Revision 33:f77aa3ecf87d, committed 2022-04-11
- Comitter:
- lex9296
- Date:
- Mon Apr 11 06:01:01 2022 +0000
- Parent:
- 32:1be3d79ff4db
- Commit message:
- Added some "Position to Speed" graph
Changed in this revision
diff -r 1be3d79ff4db -r f77aa3ecf87d Display/DisplayDriver.cpp
--- a/Display/DisplayDriver.cpp Fri Apr 08 05:27:20 2022 +0000
+++ b/Display/DisplayDriver.cpp Mon Apr 11 06:01:01 2022 +0000
@@ -542,7 +542,7 @@
extern uint16_t aui16_PlotClears_Lo[240];
extern uint16_t aui16_PlotClears_Hi[240];
-void LCM_PlotVector (uint16_t ui16_Background, uint16_t ui16_Foreground) {
+void LCM_PlotScope (uint16_t ui16_Background, uint16_t ui16_Foreground) {
uint16_t ui16_Index000;
// LA: Scope Bar Plot, Theory of Operation
@@ -569,3 +569,32 @@
Tft.drawVerticalLineEx (ui16_Index000, 300 - aui16_PlotClears_Hi[ui16_Index000], (int16_t) aui16_PlotClears_Hi[ui16_Index000]- aui16_PlotClears_Lo[ui16_Index000], ui16_Foreground);
}
}
+
+const int64_t ci64_TargetPOS = 3096; //
+
+extern int32_t ai32_POS2VelGraph[4000];
+extern uint16_t aui16_PlotPOS2VelSamples[240];
+extern uint16_t aui16_PlotPOS2VelClears_Lo[240];
+extern uint16_t aui16_PlotPOS2VelClears_Hi[240];
+
+extern int32_t ai32_POS2AccGraph[4000];
+extern int32_t ai32_POS2JrkGraph[4000];
+
+void LCM_PlotSpeed (uint16_t ui16_Background, uint16_t ui16_Foreground) {
+uint16_t ui16_Index000;
+
+ for (ui16_Index000 = 1; ui16_Index000 < 240; ui16_Index000++) {
+
+ // LA: Clear Previous Plot by means of the Hi/Lo Array(s)
+ //
+ Tft.drawVerticalLineEx (ui16_Index000, 150 - aui16_PlotPOS2VelClears_Hi[ui16_Index000], (int16_t) aui16_PlotPOS2VelClears_Hi[ui16_Index000]- aui16_PlotPOS2VelClears_Lo[ui16_Index000], ui16_Background);
+
+ // LA: Then PLOT the New
+ //
+// aui16_PlotPOS2VelClears_Hi[ui16_Index000] = (uint16_t) (ai32_POS2VelGraph[(uint16_t)((float)ui16_Index000* ((float)ci64_TargetPOS/ (float)240.0))]/ 2);
+ aui16_PlotPOS2VelClears_Hi[ui16_Index000] = (uint16_t) (ai32_POS2VelGraph[(ui16_Index000* ci64_TargetPOS)/ 240]/ 2);
+ aui16_PlotPOS2VelClears_Lo[ui16_Index000] = (uint16_t) (ai32_POS2VelGraph[((ui16_Index000* ci64_TargetPOS)/ 240)- 1]/ 2);
+ //
+ Tft.drawVerticalLineEx (ui16_Index000, 150 - aui16_PlotPOS2VelClears_Hi[ui16_Index000], (int16_t) aui16_PlotPOS2VelClears_Hi[ui16_Index000]- aui16_PlotPOS2VelClears_Lo[ui16_Index000], ui16_Foreground);
+ }
+}
\ No newline at end of file
diff -r 1be3d79ff4db -r f77aa3ecf87d Display/DisplayDriver.h --- a/Display/DisplayDriver.h Fri Apr 08 05:27:20 2022 +0000 +++ b/Display/DisplayDriver.h Mon Apr 11 06:01:01 2022 +0000 @@ -149,7 +149,8 @@ void LCM_SetPixel (uint16_t ui_X, uint16_t ui_Y, uint16_t ui16_Color); void LCM_DrawLine (uint16_t ui_X0, uint16_t ui_Y0, uint16_t ui_X1, uint16_t ui_Y1, uint16_t ui16_Color); -void LCM_PlotVector (uint16_t ui16_Background, uint16_t ui16_Foreground); +void LCM_PlotScope (uint16_t ui16_Background, uint16_t ui16_Foreground); +void LCM_PlotSpeed (uint16_t ui16_Background, uint16_t ui16_Foreground); #endif //TFT_DISPLAY_DRIVER_H
diff -r 1be3d79ff4db -r f77aa3ecf87d QEI.lib --- a/QEI.lib Fri Apr 08 05:27:20 2022 +0000 +++ b/QEI.lib Mon Apr 11 06:01:01 2022 +0000 @@ -1,1 +1,1 @@ -https://os.mbed.com/users/lex9296/code/QEI/#0db131925e56 +https://os.mbed.com/users/lex9296/code/QEI/#46e47753206d
diff -r 1be3d79ff4db -r f77aa3ecf87d SWPos/SWPos.cpp
--- a/SWPos/SWPos.cpp Fri Apr 08 05:27:20 2022 +0000
+++ b/SWPos/SWPos.cpp Mon Apr 11 06:01:01 2022 +0000
@@ -46,6 +46,16 @@
// Se non ho il "tempo" per calcolarlo uso un trucco:
// Se il profilo è da correggere (Acc+ Dec > Distance) carico in ActualSpeed la Velocità di Servolock e lascio che l'asse si muova alla "Minima".
//
+// xx/xx/2021: JOG Movement.
+// Ho introdotto la modalità di JOG dell'asse, in cui le rampe, anzichè in Spazio son gestite a Tempo.
+// PQM rilevo l'attuale tempo di scansione (tra una chiamata e l'altra del posizionatore) e adeguo gli inteventi sulla pendenza
+// in maniera conseguente.
+// Per usare il JOG non è necessario che l'home sia fatto, solo che l'asse sia abilitato.
+// Il JOG ha prelazione sul Posizionamento: se è attivo il posizionamento è mutualmente escluso (Anche se Servolock è "TRUE").
+//
+// 11/04/2022: L'individuazione del "Triangolo" ed i relativi calcoli sono STATICI, effettuati PRIMA dell'inizio del movimento.
+// PQM le equazioni base vanno create usando i limiti TEORICI (MaxSpeed) non quelli realmente raggiunti (ActualSpeed).
+//
*/
// LA: Includes
@@ -323,7 +333,7 @@
i32_MaximumSpeed_BW = (int32_t)(((in.f_MaximumSpeed_x100_BW)* (float)(in.i32_Max_Speed- in.i32_ZeroSpeed)/ 100));
i32_ServoLockSpeed_BW = (int32_t)(((in.f_ServoLockSpeed_x100_BW)* (float)(in.i32_Max_Speed- in.i32_ZeroSpeed)/ 100));
- // LA: Verifica del Profilo (Trapezio o Triangolo)
+ // LA: Verifica (STATICA) del Profilo (Trapezio o Triangolo)
//
if (i64_Distance_Local < (in.i64_AccelerationWindow+ in.i64_DecelerationWindow)) {
@@ -357,7 +367,8 @@
d_X1 = (double)i64_StartPosition_Local;
d_X2 = (double)(i64_StartPosition_Local+ in.i64_AccelerationWindow);
- d_Y1 = (double)in.i32_ZeroSpeed;
+ //d_Y1 = (double)in.i32_ZeroSpeed;
+ d_Y1 = (double)i32_ServoLockSpeed_FW;
d_Y2 = (double)i32_MaximumSpeed_FW;
d_Y2_meno_Y1 = (d_Y2- d_Y1); // LA: From Zero to Max
@@ -389,8 +400,9 @@
d_X1 = (double)(in.i64_TargetPosition- in.i64_DecelerationWindow);
d_X2 = (double)in.i64_TargetPosition;
- d_Y1 = (double)i32_ActualSpeed; // LA: Maximum Speed Planned MIGHT have NOT been Reached
- d_Y2 = (double)in.i32_ZeroSpeed;
+ d_Y1 = (double)i32_MaximumSpeed_FW;
+ //d_Y2 = (double)in.i32_ZeroSpeed;
+ d_Y2 = (double)i32_ServoLockSpeed_FW;
d_Y2_meno_Y1 = (d_Y2- d_Y1); // LA: From Max to Zero
d_X2_meno_X1 = (d_X2- d_X1); // LA: Deceleration EndPoint
@@ -417,7 +429,7 @@
// MaxSpeed = (m* ((t- q)/ (m- n))) + q
i64_AccelerationWindow_Local = (long)(((d_t- d_q)/ (d_m- d_n))- (double)i64_StartPosition_Local);
- i64_DecelerationWindow_Local = (long)((double)i64_StartPosition_Local- ((d_t- d_q)/ (d_m- d_n)));
+ i64_DecelerationWindow_Local = (long)((double)i64_TargetPosition_Prec- ((d_t- d_q)/ (d_m- d_n)));
i32_MaximumSpeed_Local = (int32_t)((d_m* ((d_t- d_q)/ (d_m- d_n)))+ d_q);
}
@@ -450,7 +462,8 @@
d_X1 = (double)i64_StartPosition_Local;
d_X2 = (double)(i64_StartPosition_Local- in.i64_AccelerationWindow);
- d_Y1 = (double)in.i32_ZeroSpeed;
+ //d_Y1 = (double)in.i32_ZeroSpeed;
+ d_Y1 = (double)i32_ServoLockSpeed_BW;
d_Y2 = (double)i32_MaximumSpeed_BW;
d_Y2_meno_Y1 = (d_Y2- d_Y1); // LA: From Zero to Max
@@ -484,8 +497,9 @@
d_X1 = (double)(in.i64_TargetPosition + in.i64_DecelerationWindow);
d_X2 = (double)(in.i64_TargetPosition);
- d_Y1 = (double)(i32_ActualSpeed); // LA: Maximum Speed Planned MIGHT have NOT been Reached
- d_Y2 = (double)(in.i32_ZeroSpeed);
+ d_Y1 = (double)(i32_MaximumSpeed_BW);
+ //d_Y2 = (double)(in.i32_ZeroSpeed);
+ d_Y2 = (double)(i32_ServoLockSpeed_BW);
d_Y2_meno_Y1 = (d_Y2- d_Y1); // LA: From Max to Zero
d_X2_meno_X1 = (d_X2- d_X1); // LA: Deceleration EndPoint
@@ -512,7 +526,7 @@
// MaxSpeed = (m* ((t- q)/ (m- n))) + q
i64_AccelerationWindow_Local = (long)((double)i64_StartPosition_Local- ((d_t- d_q)/ (d_m- d_n)));
- i64_DecelerationWindow_Local = (long)(((d_t- d_q)/ (d_m- d_n))- (double)i64_StartPosition_Local);
+ i64_DecelerationWindow_Local = (long)(((d_t- d_q)/ (d_m- d_n))- (double)i64_TargetPosition_Prec);
i32_MaximumSpeed_Local = (int32_t)((d_m* ((d_t- d_q)/ (d_m- d_n)))+ d_q);
}
}
@@ -630,7 +644,8 @@
d_X1 = (double)i64_StartPosition_Local;
d_X2 = (double)(i64_StartPosition_Local+ i64_AccelerationWindow_Local);
- d_Y1 = (double)in.i32_ZeroSpeed;
+ //d_Y1 = (double)in.i32_ZeroSpeed;
+ d_Y1 = (double)i32_ServoLockSpeed_FW;
d_Y2 = (double)i32_MaximumSpeed_Local;
d_Y2_meno_Y1 = (d_Y2- d_Y1); // LA: From Zero to Max
@@ -703,7 +718,8 @@
d_X1 = (double)(in.i64_TargetPosition- i64_DecelerationWindow_Local);
d_X2 = (double)in.i64_TargetPosition;
d_Y1 = (double)i32_ActualSpeed; // LA: Maximum Speed Planned MIGHT have NOT been Reached
- d_Y2 = (double)in.i32_ZeroSpeed;
+ //d_Y2 = (double)in.i32_ZeroSpeed;
+ d_Y2 = (double)i32_ServoLockSpeed_FW;
d_Y2_meno_Y1 = (d_Y2- d_Y1); // LA: From Max to Zero
d_X2_meno_X1 = (d_X2- d_X1); // LA: Deceleration EndPoint
@@ -780,9 +796,10 @@
d_X1 = (double)i64_StartPosition_Local;
d_X2 = (double)(i64_StartPosition_Local- i64_AccelerationWindow_Local);
- d_Y1 = (double)in.i32_ZeroSpeed;
+ //d_Y1 = (double)in.i32_ZeroSpeed;
+ d_Y1 = (double)i32_ServoLockSpeed_BW;
d_Y2 = (double)i32_MaximumSpeed_Local;
-
+
d_Y2_meno_Y1 = (d_Y2- d_Y1); // LA: From Zero to Max
d_X2_meno_X1 = (d_X2- d_X1); // LA: Acceleration EndPoint
d_X2_per_Y1 = (d_X2* d_Y1);
@@ -853,7 +870,8 @@
d_X1 = (double)(in.i64_TargetPosition + i64_DecelerationWindow_Local);
d_X2 = (double)in.i64_TargetPosition;
d_Y1 = (double)i32_ActualSpeed; // LA: Maximum Speed Planned MIGHT have NOT been Reached
- d_Y2 = (double)in.i32_ZeroSpeed;
+ //d_Y2 = (double)in.i32_ZeroSpeed;
+ d_Y2 = (double)i32_ServoLockSpeed_BW;
d_Y2_meno_Y1 = (d_Y2- d_Y1); // LA: From Max to Zero
d_X2_meno_X1 = (d_X2- d_X1); // LA: Deceleration EndPoint
@@ -865,7 +883,6 @@
b_AuxCalculateProfile_003 = true;
}
-
i32_ActualSpeed = abs((int)((d_m* (double)(in.i64_ActualPosition))+ d_q));
}
@@ -905,11 +922,6 @@
i32_ActualSpeed = in.i32_ZeroSpeed;
}
- // Arrivato Qui 07/02/2022
- // Arrivato Qui 07/02/2022
- // Arrivato Qui 07/02/2022
- // Arrivato Qui 07/02/2022
-
// ===========================
// ===========================
// LA: Managing the Speed Limit(s)
@@ -942,8 +954,8 @@
else {
// i64_Distance_Local < in.i64_diToleranceWindow
- // LA: Attenzione, questo esegue un OVERRIDE alle assegnazioni precedenti.
- // ===================================================================
+ // LA: Attenzione, questo esegue un <eventuale> OVERRIDE alle assegnazioni precedenti.
+ // ===============================================================================
//
// Se il controllo di distanza vede che l'asse è in Anello di tolleranza forza a Zerospeed la velocità.
// Il comportamento conseguente è che l'asse rimane in quiete, senza correnti applicate, fintanto che resta all'interno della finestra.
diff -r 1be3d79ff4db -r f77aa3ecf87d main.cpp
--- a/main.cpp Fri Apr 08 05:27:20 2022 +0000
+++ b/main.cpp Mon Apr 11 06:01:01 2022 +0000
@@ -17,25 +17,78 @@
#include "DisplayDriver.h"
+const float cf_SCOPeriod_s = 0.00025000; // 250us
+const float cf_PWMPeriod_s = 0.01000000; // 10ms
+const float cf_MOTPeriod_s = 0.01000000; // 10ms
+
+//const int64_t ci64_TargetPOS = 240; //
+const int64_t ci64_TargetPOS = 3200; //
+
+// LA: LCM_ShowTactics
+// ===============
+//
void LCM_ShowTactics(
- int32_t i32_Pulses,
- int32_t i32_ATVSpeed,
- float f_ai0000_Aux,
- float f_ai0001_Aux,
- float f_ai0002_Aux,
- float f_ai0003_Aux,
- float f_ai0004_Aux,
- float f_ai0005_Aux
+ int64_t i64_Pulses,
+ int32_t i32_ATVSpeed,
+ //
+ float f_ai0000_Aux,
+ float f_ai0001_Aux,
+ float f_ai0002_Aux,
+ float f_ai0003_Aux,
+ float f_ai0004_Aux,
+ float f_ai0005_Aux,
+ //
+ int32_t i32_Velocity,
+ int32_t i32_Acceleration,
+ int32_t i32_Jerk
+
);
-static void SampleAndStore (void);
-Ticker SampleTimer; // LA: To Sample 1AI any ms
+// LA: SampleAndStore
+// LA: SampleTimer ==
+// ==============
+//
+static void SampleAndStore (void);
+Ticker SampleTimer; // LA: To Sample 1AI any 'x'ms
+
float af_PlotSamples[240]; // LA: "Horiz" Plot Array
uint16_t aui16_PlotSamples[240];
uint16_t aui16_PlotClears_Lo[240];
uint16_t aui16_PlotClears_Hi[240];
+int32_t ai32_POS2VelGraph[4000];
+uint16_t aui16_PlotPOS2VelSamples[240];
+uint16_t aui16_PlotPOS2VelClears_Lo[240];
+uint16_t aui16_PlotPOS2VelClears_Hi[240];
+
+int32_t ai32_POS2AccGraph[4000];
+int32_t ai32_POS2JrkGraph[4000];
+
+// LA: MotionHandler
+// LA: MotionTimer =
+// =============
+//
+static void MotionHandler (void);
+Ticker MotionTimer; // LA: Gestisce il rilevamento (RT) di Velocità, Accelerazione e Jerk
+ // Esegue il Movimento Programmato
+
+float f_PWMPercent; // Deprecated
+
+float fVelocity;
+float fAcceleration;
+float fJerk;
+float fTorque;
+
+int64_t i64_Position_Prec;
+int32_t i32_Velocity;
+int32_t i32_Velocity_Prec;
+int32_t i32_Acceleration;
+int32_t i32_Acceleration_Prec;
+int32_t i32_Jerk;
+
+// LA: Motion
+// ======
// LA: Theory of Operation
// ===================
//
@@ -104,16 +157,16 @@
// =======
//
int main (void){
-const float cf_PWMPeriod_s = 0.010;
rDIR_FWD = true; // LA: Actuator Extends
rENA_Off = true; // LA: Drive Power is Off
+ //
+ in_PosizionatoreSW.b_ServoLock = false;
+ in_PosizionatoreSW.rtServoLock_Q = false;
EepromInit(); // LA: Inizializza la EEProm
TimersInit(); // LA: Parte il Timer a 1ms
- SampleTimer.attach_us(&SampleAndStore, 250);
-
// LA: FactoryReset se "userButton" premuto all'avvio
//
if (userButton == 0) {
@@ -121,8 +174,11 @@
}
DisplayDriverInit();
- PWM_PB3.period(cf_PWMPeriod_s); // LA: Avvia il PWM con TimeBase x [s]
- PWM_PB3.write((float) 0.0); // Set to 0%
+// SampleTimer.attach_us(&SampleAndStore, 250); // LA: Scope has its own times
+ SampleTimer.attach(&SampleAndStore, cf_SCOPeriod_s); // LA: Avvia l'OscilloScopio con TimeBase x [s]
+ MotionTimer.attach(&MotionHandler, cf_MOTPeriod_s); // LA: Avvia il Motion con TimeBase x [s]
+ PWM_PB3.period(cf_PWMPeriod_s); // LA: Avvia il PWM con TimeBase x [s]
+ PWM_PB3.write((float) 0.0); // Set to 0%
// LA: Motion (1st) Setup
//
@@ -134,19 +190,24 @@
// POS Mode
// ========
//
- in_PosizionatoreSW.b_ServoLock = true;
- in_PosizionatoreSW.rtServoLock_Q = false;
+// in_PosizionatoreSW.b_ServoLock = true;
+// in_PosizionatoreSW.rtServoLock_Q = false;
//
- in_PosizionatoreSW.i64_TargetPosition = 3200; // [ui]
+ in_PosizionatoreSW.i64_TargetPosition = ci64_TargetPOS; // [ui]
in_PosizionatoreSW.i64_ActualPosition = Stabilus322699.getPulses(); //
- in_PosizionatoreSW.i64_AccelerationWindow = 64; // LA: Spazio concesso all'accelerazione.
- in_PosizionatoreSW.i64_DecelerationWindow = 512; // Spazio concesso alla decelerazione, è prioritario rispetto all'accelerazione.
- in_PosizionatoreSW.i64_diToleranceWindow = 10; // Finestra di Tolleranza
+// in_PosizionatoreSW.i64_AccelerationWindow = 32; // LA: Spazio concesso all'accelerazione.
+// in_PosizionatoreSW.i64_DecelerationWindow = 256; // Spazio concesso alla decelerazione, è prioritario rispetto all'accelerazione.
+ in_PosizionatoreSW.i64_AccelerationWindow = 2048; // LA: Spazio concesso all'accelerazione.
+ in_PosizionatoreSW.i64_DecelerationWindow = 2048; // Spazio concesso alla decelerazione, è prioritario rispetto all'accelerazione.
+ in_PosizionatoreSW.i64_diToleranceWindow = 16; // Finestra di Tolleranza
//
in_PosizionatoreSW.f_MaximumSpeed_x100_FW = 16.0; // % of "i32_Max_Speed"
in_PosizionatoreSW.f_MaximumSpeed_x100_BW = 60.0; //
- in_PosizionatoreSW.f_ServoLockSpeed_x100_FW = 4.8; // Riferimento di velocità minima a cui (appena) si muove l'asse [verso FW]
- in_PosizionatoreSW.f_ServoLockSpeed_x100_BW = 18.0; // Riferimento di velocità minima a cui (appena) si muove l'asse [verso BW]
+// in_PosizionatoreSW.f_ServoLockSpeed_x100_FW = 4.8; // Riferimento di velocità minima a cui (appena) si muove l'asse [verso FW]
+// in_PosizionatoreSW.f_ServoLockSpeed_x100_BW = 18.0; // Riferimento di velocità minima a cui (appena) si muove l'asse [verso BW]
+ in_PosizionatoreSW.f_ServoLockSpeed_x100_FW = 5.2; // Riferimento di velocità minima a cui (appena) si muove l'asse [verso FW]
+// in_PosizionatoreSW.f_ServoLockSpeed_x100_FW = 4.8; // Riferimento di velocità minima a cui (appena) si muove l'asse [verso FW]
+ in_PosizionatoreSW.f_ServoLockSpeed_x100_BW = 22.0; // Riferimento di velocità minima a cui (appena) si muove l'asse [verso BW]
// JOG Mode
// ========
@@ -160,7 +221,6 @@
in_PosizionatoreSW.f_JogSpeed_x100_FW = (in_PosizionatoreSW.f_MaximumSpeed_x100_FW/ 2); // LA: JOG's the Half of Max POS's Speed
in_PosizionatoreSW.f_JogSpeed_x100_BW = (in_PosizionatoreSW.f_MaximumSpeed_x100_BW/ 2); //
-
// LA: Color RGB Component(s)
// ======================
//
@@ -194,36 +254,6 @@
in_PosizionatoreSW.rtServoLock_Q = true;
while (1) {
- float f_PWMPercent;
-
- in_PosizionatoreSW.i64_ActualPosition = Stabilus322699.getPulses();
- //
- PosizionatoreSW (in_PosizionatoreSW, out_PosizionatoreSW);
- in_PosizionatoreSW.rtServoLock_Q = false;
-
- // int64_t i64_StartPosition;
- // int64_t i64_Distance;
- // bool b_Accelerating; // LA: bACPos_Accelerating
- // bool b_MaxSpeedReached;
- // bool b_Decelerating; // bACPos_Decelerating
- // bool b_InPosition;
- // bool b_InToleranceFW;
- // bool b_InToleranceBW;
-
- // int32_t i32_ATVSpeed;
- f_PWMPercent = ((float)out_PosizionatoreSW.i32_ATVSpeed)/ (float)in_PosizionatoreSW.i32_Max_Speed; // LA: In Range (float) 0.. 1
- PWM_PB3.write((float) 1.0- f_PWMPercent); // Set to x%
-
- // bool b_ATVDirectionFW;
- rDIR_FWD = out_PosizionatoreSW.b_ATVDirectionFW;
-
- // bool b_ATVDirectionBW;
-
- // bool b_STW1_On;
- // bool b_STW1_NoCStop;
- // bool b_STW1_NoQStop;
- // bool b_STW1_Enable;
- rENA_Off = !out_PosizionatoreSW.b_STW1_Enable;
// LA: Scope, Theory of operation.
// ===========================
@@ -242,37 +272,51 @@
ADC12_IN9.read(), // 8
ADC12_IN15.read(), // 9
- (f_PWMPercent* 100) // 10
+ (f_PWMPercent* 100), // 10
+
+ i32_Velocity, // 11
+ i32_Acceleration, // 12
+ i32_Jerk // 13
+
);
- LCM_PlotVector (
+ LCM_PlotScope (
Scale2RGBColor (0, 0, 0), // Back: Black
Scale2RGBColor (31, 0, 0) // Fore: Red
);
+ LCM_PlotSpeed (
+ Scale2RGBColor (0, 0, 0), // Back: Black
+ Scale2RGBColor (31, 0, 0) // Fore: Red
+ );
if (out_PosizionatoreSW.b_InPosition)
if (in_PosizionatoreSW.i64_TargetPosition > 0)
in_PosizionatoreSW.i64_TargetPosition = 0;
else
- in_PosizionatoreSW.i64_TargetPosition = 3200;
+ in_PosizionatoreSW.i64_TargetPosition = ci64_TargetPOS;
}
}
void LCM_ShowTactics(
- int32_t i32_Pulses,
+ int64_t i64_Pulses,
int32_t i32_ATVSpeed,
- float f_ai0000_Aux,
- float f_ai0001_Aux,
- float f_ai0002_Aux,
- float f_ai0003_Aux,
- float f_ai0004_Aux,
- float f_ai0005_Aux
+ //
+ float f_ai0000_Aux,
+ float f_ai0001_Aux,
+ float f_ai0002_Aux,
+ float f_ai0003_Aux,
+ float f_ai0004_Aux,
+ float f_ai0005_Aux,
+ //
+ int32_t i32_Velocity,
+ int32_t i32_Acceleration,
+ int32_t i32_Jerk
) {
char StringText[MAX_CHAR_PER_LINE+ 1]; // don't forget the /0 (end of string)
-static int32_t Pulses_Prec;
+static int64_t i64_Pulses_Prec;
static uint32_t ms_0002_prec;
static float f_ai0000_prec;
@@ -282,13 +326,18 @@
static float f_ai0004_prec;
static float f_ai0005_prec;
- if (i32_Pulses != Pulses_Prec) {
+static uint32_t i32_Velocity_prec;
+static uint32_t i32_Acceleration_prec;
+static uint32_t i32_Jerk_prec;
+
+ if (i64_Pulses != i64_Pulses_Prec) {
sprintf (StringText,
- "Pulses: %d ", i32_Pulses);
+ "Pulses: %d ", (int32_t)i64_Pulses);
LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 1), StringText);
- Pulses_Prec = i32_Pulses;
+ i64_Pulses_Prec = i64_Pulses;
}
+/*
if (i32_ATVSpeed != ms_0002_prec) {
sprintf (StringText,
"Speed[ui]: %d ", i32_ATVSpeed);
@@ -337,13 +386,30 @@
LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 10), StringText);
f_ai0005_prec = f_ai0005_Aux;
}
+*/
+ if (i32_Velocity != i32_Velocity_prec) {
+ sprintf (StringText,
+ "Vel[ui/10ms]: %d ", i32_Velocity); //, fVelocity);
+ LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 11), StringText);
+ i32_Velocity_prec = i32_Velocity;
+ }
+ if (i32_Acceleration != i32_Acceleration_prec) {
+ sprintf (StringText,
+ "Acc[ui/10ms^2]: %d ", i32_Acceleration); //, fAcceleration);
+ LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 12), StringText);
+ i32_Acceleration_prec = i32_Acceleration;
+ }
+ if (i32_Jerk != i32_Jerk_prec) {
+ sprintf (StringText,
+ "Jerk: %d ", i32_Jerk); //, fJerk);
+ LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 13), StringText);
+ i32_Jerk_prec = i32_Jerk;
+ }
}
-//static void SampleAndStore (void) {
-void SampleAndStore (void) {
-//static int32_t i32_Pulses;
+static void SampleAndStore (void) {
int16_t i16_SampleIndex;
-//float f_SampleAux;
+int64_t i64_SampleIndex;
// af_PlotSamples[240- 1] = ADC12_IN9.read();
af_PlotSamples[240- 1] = (float) Stabilus322699.getChannelA() * 0.33f;
@@ -351,7 +417,103 @@
for (i16_SampleIndex = 0; i16_SampleIndex < (240- 1); i16_SampleIndex++)
af_PlotSamples[i16_SampleIndex] = af_PlotSamples[i16_SampleIndex+ 1];
-// i32_Pulses++;
+ // LA: Position's Graph Section
+ // ========================
+ //
+ i64_SampleIndex = Stabilus322699.getPulses();
+ ai32_POS2VelGraph[i64_SampleIndex] = i32_Velocity;
+ ai32_POS2AccGraph[i64_SampleIndex] = i32_Acceleration;
+ ai32_POS2JrkGraph[i64_SampleIndex] = i32_Jerk;
}
-// getDmTft().setPixel (0,0,1);
+static void MotionHandler (void) {
+static int16_t i16_Index = 0;
+
+ // LA: Retrieve Actual Position
+ //
+ in_PosizionatoreSW.i64_ActualPosition = Stabilus322699.getPulses();
+
+ // LA: Execute Motion
+ //
+ PosizionatoreSW (in_PosizionatoreSW, out_PosizionatoreSW);
+ in_PosizionatoreSW.rtServoLock_Q = false;
+
+ // LA: Handle PostServo
+ //
+
+ // int64_t i64_StartPosition;
+ // int64_t i64_Distance;
+ // bool b_Accelerating; // LA: bACPos_Accelerating
+ // bool b_MaxSpeedReached;
+ // bool b_Decelerating; // bACPos_Decelerating
+ // bool b_InPosition;
+ // bool b_InToleranceFW;
+ // bool b_InToleranceBW;
+
+ // int32_t i32_ATVSpeed;
+ f_PWMPercent = ((float)out_PosizionatoreSW.i32_ATVSpeed)/ (float)in_PosizionatoreSW.i32_Max_Speed; // LA: In Range (float) 0.. 1
+ PWM_PB3.write((float) 1.0- f_PWMPercent); // Set to x%
+
+ // bool b_ATVDirectionFW;
+ rDIR_FWD = out_PosizionatoreSW.b_ATVDirectionFW;
+
+ // bool b_ATVDirectionBW;
+
+ // bool b_STW1_On;
+ // bool b_STW1_NoCStop;
+ // bool b_STW1_NoQStop;
+ // bool b_STW1_Enable;
+ rENA_Off = !out_PosizionatoreSW.b_STW1_Enable;
+
+ // LA: Update Motion Dynamic References
+ // ================================
+ //
+ // Per avere maggiore granularità delle misure, l'acquisizione viene fatta ogni 10 interventi
+ // Se il motion gira a 10ms, i riferimenti dinamici saranno calcolati ogni 100
+ //
+ if (i16_Index == 0) {
+ static uint32_t ui32_PreviousStep_ms;
+ uint32_t ui32_ActualStepSampled_ms;
+ uint32_t ui32_PassedActual_ms;
+ //
+ float fPassedActual_sxs;
+
+ // LA: Generazione del millisecondo Attuale
+ // ====================================
+ //
+ // Invoca il timer di sistema (TimersTimerValue) e lo confronta col suo precedente.
+ // Una volta elaborato e "scevrato" l'eventuale "Rollover" la sezione ritorna "ui32_PassedActual_ms_Local".
+ // "ui32_PassedActual_ms_Local" rappresenta i [ms] passati tra una istanza e l'altra
+ //
+ ui32_ActualStepSampled_ms = TimersTimerValue(); // Freezes the Actual Sample.
+ if (ui32_ActualStepSampled_ms >= ui32_PreviousStep_ms)
+ ui32_PassedActual_ms = (ui32_ActualStepSampled_ms- ui32_PreviousStep_ms); // Result => Actual- Previous
+ else
+ ui32_PassedActual_ms = ui32_ActualStepSampled_ms+ (0x7fffffff- ui32_PreviousStep_ms); // Result => Actual+ (Rollover- Previous)
+ ui32_PreviousStep_ms = ui32_ActualStepSampled_ms; // Store(s)&Hold(s) actual msSample
+ fPassedActual_sxs = ((float) 1000.0/ (float) ui32_PassedActual_ms); // Steps Any [s]
+
+ i32_Velocity = (int32_t) (in_PosizionatoreSW.i64_ActualPosition- i64_Position_Prec); // LA: Velocity in [ui/10ms]
+ i64_Position_Prec = in_PosizionatoreSW.i64_ActualPosition;
+ i32_Acceleration = (i32_Velocity- i32_Velocity_Prec); // LA: Acceleration in [ui/10ms^2]
+ i32_Velocity_Prec = i32_Velocity;
+ i32_Jerk = (i32_Acceleration- i32_Acceleration_Prec); // LA: Jerk
+ i32_Acceleration_Prec = i32_Acceleration;
+
+ fVelocity = (float) i32_Velocity * fPassedActual_sxs; // Velocity in [ui/s]
+ fAcceleration = (float) i32_Acceleration * fPassedActual_sxs; // Acceleration in [ui/s^2]
+ fJerk = (float) i32_Jerk * fPassedActual_sxs; // Jerk
+ }
+ i16_Index++;
+ if (i16_Index >= 10)
+ i16_Index = 0;
+
+/*
+ // LA: Position's Graph Section
+ // ========================
+ //
+ ai32_POS2VelGraph[in_PosizionatoreSW.i64_ActualPosition] = i32_Velocity;
+ ai32_POS2AccGraph[in_PosizionatoreSW.i64_ActualPosition] = i32_Acceleration;
+ ai32_POS2JrkGraph[in_PosizionatoreSW.i64_ActualPosition] = i32_Jerk;
+*/
+}