Luca Antolini
Stabilus 322699 wDoublePID, ErrorGetter
Dependencies: mbed QEI PID DmTftLibraryEx
Diff: SWPos/SWPos.cpp
- Revision:
- 33:f77aa3ecf87d
- Parent:
- 32:1be3d79ff4db
--- 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.