Stabilus 322699 wDoublePID
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.