Luca Antolini
New BASE release 4 Fork
Dependencies: mbed QEI DmTftLibrary
Diff: SWPos/SWPos.cpp
- Revision:
- 23:b9d23a2f390e
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/SWPos/SWPos.cpp Thu Feb 10 09:39:01 2022 +0000
@@ -0,0 +1,1019 @@
+
+/* LA: Theory of Operation.
+// ========================
+//
+// once Encoder/Axis is Homed and Cycle Condition's Sussist, AC_Pos_Positioning is alloweed to take Control.
+// This is done by keeping "STW1_Control" true.
+//
+// if Servolock is off, The Axis is then Set to "Free Wheel Axis"
+// Otherwise, the System Keeps Actual Position (ServLock works inside the Tolerance window, if there's move),
+// or a New Move is Started if Target is != Actual Position.
+//
+// if ServoLock is true and movement is alloweed, after the positioning will have Actual = Target, Tolerance
+// flags active and a constant control to keep this.
+//
+// if "Axis" is Not Homed, All Positioning Flag(s) are kept clear
+*/
+
+/* LA: Verifica di coerenza del Profilo:
+// =================================
+//
+// Se Lo spazio di Accelerazione sommato a quello di Decelerazione, ad INIZIO MOVIMENTO, supera la distanza (in Modulo) da percorrere,
+// allora il profilo và corretto (Non è più un trapezio ma diventa un triangolo).
+//
+// Per correggere il profilo occorre:
+//
+// 1) Calcolare le equazioni di entrambe le rette (Accelerazione e Decelerazione)
+// 2) Ricavare il punto di intersezione delle due rette (La cui "Ascissa" sarà la velocità massima raggiungibile, l'"Ordinata" il punto a cui la raggiungerà)
+// 3) Sostituire all'Accelerazione il valore "Ordinata"- Punto di Partenza (Verificando il segno dell'operazione in base al verso)
+// 4) Sostituire alla Decelerazione il valore Destinazione - "Ordinata" (Verificando il segno dell'operazione in base al verso)
+// 5) Sostituire alla Velocità di Movimento l'"Ascissa"
+//
+// Avendo già le equazioni di RettaxAcc e RettaxDec:
+// =================================================
+//
+// RettaxAcc: Y = (m * X)+ q
+// RettaxDec: Y = (n * X)+ t
+//
+// Il punto X a cui le Y si equivalgono è X = (t- q)/ (m- n), la Y è ricavabile, indifferentemente, da entrambe le equazioni al punto X.
+// Questo permetterà la corretta esecuzione del "Triangolo", col vertice alla "velocità massima calcolata" e rampe teoriche equivalenti a quelle programmate.
+//
+// ****
+// ****
+// ****
+// ****
+//
+// 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".
+//
+*/
+
+// LA: Includes
+#include "mbed.h"
+#include <stdio.h>
+#include <stdlib.h>
+
+#include "SWPos.h"
+#include "Timers.h"
+
+//in_sPosizionatoreSW in_PosizionatoreSW;
+//out_sPosizionatoreSW out_PosizionatoreSW;
+
+// LA: Basic Function's Integration
+void PosizionatoreSW (const in_sPosizionatoreSW &in, out_sPosizionatoreSW &out) {
+static bool InProgress = false;
+
+static int64_t i64_TargetPosition_Prec;
+static bool b_AuxCalculateProfile_003;
+static bool b_AuxCalculateProfile_002;
+static bool b_AuxCalculateProfile_001;
+static bool b_AuxCalculateProfile_000;
+static int32_t i32_ServoLockSpeed_FW;
+static int32_t i32_ServoLockSpeed_BW;
+
+static double d_X1;
+static double d_X2;
+static double d_Y1;
+static double d_Y2;
+static double d_Y2_meno_Y1;
+static double d_X2_meno_X1;
+static double d_X2_per_Y1;
+static double d_X1_per_Y2;
+static double d_m;
+static double d_q;
+static double d_n;
+static double d_t;
+
+static int32_t i32_ActualSpeed;
+static int64_t i64_AccelerationWindow_Local;
+static int64_t i64_DecelerationWindow_Local;
+static int32_t i32_MaximumSpeed_FW;
+static int32_t i32_MaximumSpeed_BW;
+static int32_t i32_MaximumSpeed_Local;
+static int64_t i64_StartPosition_Local;
+static int64_t i64_Distance_Local;
+static bool b_GoingFW;
+static bool b_GoingBW;
+static bool b_Accelerating_Local;
+static bool b_Decelerating_Local;
+static bool b_JogFW_Prec_Local;
+static bool b_JogBW_Prec_Local;
+static int32_t i32_Aux_ms2Acc;
+static int32_t i32_Aux_ms2Dec;
+static float f_Aux_AccelAnyms;
+static float f_Aux_DecelAnyms;
+static float f_MaximumJogSpeed_xW;
+
+static uint32_t ui32_PreviousStep_ms_Local;
+uint32_t ui32_ActualStepSampled_ms_Local;
+uint32_t ui32_PassedActual_ms_Local;
+
+ if (InProgress)
+ return;
+ else {
+ InProgress = true;
+
+ // 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 chiamata e l'altra del Posizionatore SW.
+ //
+ ui32_ActualStepSampled_ms_Local = TimersTimerValue(); // Freezes the Actual Sample.
+ if (ui32_ActualStepSampled_ms_Local >= ui32_PreviousStep_ms_Local)
+ ui32_PassedActual_ms_Local = (ui32_ActualStepSampled_ms_Local- ui32_PreviousStep_ms_Local); // Result => Actual- Previous
+ else
+ ui32_PassedActual_ms_Local = ui32_ActualStepSampled_ms_Local+ (0x7fffffff- ui32_PreviousStep_ms_Local); // Result => Actual+ (Rollover- Previous)
+ //
+ ui32_PreviousStep_ms_Local = ui32_ActualStepSampled_ms_Local; // Store(s)&Hold(s) actual msSample
+
+ // LA: Test pourposes ...
+ //
+ out.ui32_PreviousStep_ms = ui32_PreviousStep_ms_Local;
+ out.ui32_ActualStepSampled_ms = ui32_ActualStepSampled_ms_Local;
+ out.ui32_PassedActual_ms = ui32_PassedActual_ms_Local;
+
+ // LA: Valutazione della Distanza (Rimanente)
+ // ======================================
+ //
+ if (in.i64_ActualPosition > in.i64_TargetPosition)
+ i64_Distance_Local = (in.i64_ActualPosition- in.i64_TargetPosition);
+ else
+ i64_Distance_Local = (in.i64_TargetPosition- in.i64_ActualPosition);
+
+ // LA: Entering SWPositioner
+ // =====================
+ //
+ if (in.b_AxisPowered) {
+ if (in.b_JogMode) {
+
+ // JOG Mode Engaged
+ //
+ if (in.b_JogFW) {
+ if (!b_JogFW_Prec_Local) {
+
+ // JOG Mode FW "Just" Engaged
+ //
+ b_JogFW_Prec_Local = in.b_JogFW;
+ i32_Aux_ms2Acc = in.i32_JogAccel_ms;
+ //
+ f_MaximumJogSpeed_xW = (((in.f_JogSpeed_x100_FW)* (float)(in.i32_Max_Speed- i32_ActualSpeed)/ 100)); // LA: Speed to be Reached
+ f_Aux_AccelAnyms = (f_MaximumJogSpeed_xW/ (float)in.i32_JogAccel_ms); // LA: Any ms Increment o'Speed
+
+ b_Accelerating_Local = true;
+ b_Decelerating_Local = false;
+ out.b_MaxSpeedReached = false;
+ }
+
+ // JOG Move FW
+ //
+ if (i32_Aux_ms2Acc > 0) {
+ i32_Aux_ms2Acc = (i32_Aux_ms2Acc- ui32_PassedActual_ms_Local); // LA: Ms Passed @ this Trip
+ i32_ActualSpeed = (int32_t)((float)(in.i32_JogAccel_ms- i32_Aux_ms2Acc)* f_Aux_AccelAnyms); // LA: Acc Checkpoint
+ }
+ else {
+ i32_ActualSpeed = (int32_t)((in.f_JogSpeed_x100_FW)* (float)(in.i32_Max_Speed- in.i32_ZeroSpeed)/ 100); // LA: Maximum Speed Reached
+
+ b_Accelerating_Local = false;
+ b_Decelerating_Local = false;
+ out.b_MaxSpeedReached = true;
+ }
+ b_GoingFW = true; // LA: Moves ...
+ b_GoingBW = false; //
+ }
+ else {
+ if (b_JogFW_Prec_Local) {
+ if (!b_Decelerating_Local) {
+
+ // JOG Mode FW "Just" Released
+ //
+ i32_Aux_ms2Dec = in.i32_JogDecel_ms;
+ f_MaximumJogSpeed_xW = (((in.f_JogSpeed_x100_FW)* (float)(i32_ActualSpeed- in.i32_ZeroSpeed)/ 100)); // LA: Speed to be Reached
+ f_Aux_DecelAnyms = (f_MaximumJogSpeed_xW/ (float)in.i32_JogDecel_ms); // LA: Any ms Increment o'Speed
+
+ b_Accelerating_Local = false;
+ b_Decelerating_Local = true;
+ out.b_MaxSpeedReached = false;
+ }
+
+ // JOG Move FW, Decelerating to Zero
+ //
+ if (i32_Aux_ms2Dec > 0) {
+ i32_Aux_ms2Dec = (i32_Aux_ms2Dec- ui32_PassedActual_ms_Local); // LA: Ms Passed @ this Trip
+ i32_ActualSpeed = (int32_t)(f_MaximumJogSpeed_xW- (float)(in.i32_JogDecel_ms- i32_Aux_ms2Dec)* f_Aux_DecelAnyms); // LA: Dec Checkpoint
+
+ b_GoingFW = true; // LA: Moves ...
+ b_GoingBW = false; //
+ }
+ else {
+ i32_ActualSpeed = in.i32_ZeroSpeed; // LA: Zero Speed Reached
+
+ b_Accelerating_Local = false;
+ b_Decelerating_Local = false;
+ out.b_MaxSpeedReached = false;
+ //
+ b_JogFW_Prec_Local = false; // LA: Move is Terminated, NOW
+ b_GoingFW = false; //
+ b_GoingBW = false; //
+ }
+ }
+ }
+
+ if (in.b_JogBW) {
+ if (!b_JogBW_Prec_Local) {
+
+ // JOG Mode BW "Just" Engaged
+ //
+ b_JogBW_Prec_Local = in.b_JogBW;
+ i32_Aux_ms2Acc = in.i32_JogAccel_ms;
+ //
+ f_MaximumJogSpeed_xW = (((in.f_JogSpeed_x100_BW)* (float)(in.i32_Max_Speed- i32_ActualSpeed)/ 100)); // LA: Speed to be Reached
+ f_Aux_AccelAnyms = (f_MaximumJogSpeed_xW/ (float)in.i32_JogAccel_ms); // LA: Any ms Increment o'Speed
+
+ b_Accelerating_Local = true;
+ b_Decelerating_Local = false;
+ out.b_MaxSpeedReached = false;
+ }
+
+ // JOG Move BW
+ //
+ if (i32_Aux_ms2Acc > 0) {
+ i32_Aux_ms2Acc = (i32_Aux_ms2Acc- ui32_PassedActual_ms_Local); // LA: Ms Passed @ this Trip
+ i32_ActualSpeed = (int32_t)((float)(in.i32_JogAccel_ms- i32_Aux_ms2Acc)* f_Aux_AccelAnyms); // LA: Acc Checkpoint
+ }
+ else {
+ i32_ActualSpeed = (int32_t)((in.f_JogSpeed_x100_BW)* (float)(in.i32_Max_Speed- in.i32_ZeroSpeed)/ 100); // LA: Maximum Speed Reached
+
+ b_Accelerating_Local = false;
+ b_Decelerating_Local = false;
+ out.b_MaxSpeedReached = true;
+ }
+
+ b_GoingBW = true; // LA: Moves ...
+ b_GoingFW = false; //
+ }
+ else {
+ if (b_JogBW_Prec_Local) {
+ if (!b_Decelerating_Local) {
+
+ // JOG Mode BW "Just" Released
+ //
+ i32_Aux_ms2Dec = in.i32_JogDecel_ms;
+ f_MaximumJogSpeed_xW = (((in.f_JogSpeed_x100_BW)* (float)(i32_ActualSpeed- in.i32_ZeroSpeed)/ 100)); // LA: Speed to be Reached
+ f_Aux_DecelAnyms = (f_MaximumJogSpeed_xW/ (float)(in.i32_JogDecel_ms)); // LA: Any ms Increment o'Speed
+
+ b_Accelerating_Local = false;
+ b_Decelerating_Local = true;
+ out.b_MaxSpeedReached = false;
+ }
+
+ // JOG Move FW, Decelerating to Zero
+ //
+ if (i32_Aux_ms2Dec > 0) {
+ i32_Aux_ms2Dec = (i32_Aux_ms2Dec- ui32_PassedActual_ms_Local); // LA: Ms Passed @ this Trip
+ i32_ActualSpeed = (int32_t)(f_MaximumJogSpeed_xW- (float)(in.i32_JogDecel_ms- i32_Aux_ms2Dec)* f_Aux_DecelAnyms); // LA: Dec Checkpoint
+
+ b_GoingBW = true; // LA: Moves ...
+ b_GoingFW = false; //
+ }
+ else {
+ i32_ActualSpeed = in.i32_ZeroSpeed; // LA: Zero Speed Reached
+
+ b_Accelerating_Local = false;
+ b_Decelerating_Local = false;
+ out.b_MaxSpeedReached = false;
+ //
+ b_JogBW_Prec_Local = false; // LA: Move is Terminated, NOW
+ b_GoingBW = false; //
+ b_GoingFW = false; //
+ }
+ }
+ }
+ out.b_Accelerating = b_Accelerating_Local;
+ out.b_Decelerating = b_Decelerating_Local;
+ }
+
+ else {
+ // !in.b_JogMode
+
+ // JOG Mode NOT Engaged
+ // Axis Powered
+ //
+ b_JogFW_Prec_Local = false;
+ b_JogBW_Prec_Local = false;
+
+ if (in.b_ACPos_Homed) {
+ if (
+ (in.rtServoLock_Q && (in.i64_TargetPosition != in.i64_ActualPosition)) ||
+ (in.b_ServoLock && (in.i64_TargetPosition != i64_TargetPosition_Prec))
+ ) {
+
+ // LA: An Issue to the Motion to Start is then Present & Valid
+ //
+ i64_TargetPosition_Prec = in.i64_TargetPosition;
+ i64_StartPosition_Local = in.i64_ActualPosition;
+
+ // wAccelerationWindow è già la Finestra di Accelerazione
+ // wDecelerationWindow è già la Finestra di Decelerazione
+ // wToleranceWindow è già la Finestra di Tolleranza di Posizionamento
+ //
+ i32_MaximumSpeed_FW = (int32_t)(((in.f_MaximumSpeed_x100_FW)* (float)(in.i32_Max_Speed- in.i32_ZeroSpeed)/ 100));
+ i32_ServoLockSpeed_FW = (int32_t)(((in.f_ServoLockSpeed_x100_FW)* (float)(in.i32_Max_Speed- in.i32_ZeroSpeed)/ 100));
+ 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)
+ //
+ if (i64_Distance_Local < (in.i64_AccelerationWindow+ in.i64_DecelerationWindow)) {
+
+ // LA: Attenzione, il Profilo è Triangolare
+ //
+ if (in.i64_ActualPosition < in.i64_TargetPosition) {
+
+ // LA: Going FW
+ // LA: Calcolare Entrambi i Profili,
+ // Trovare il Punto di Intersezione
+ // Aggiornare Acc/Dec/VMax in Accordo
+
+ // Punto 1) Ricavo Y = mX+ q
+ // ================
+ //
+ // Retta x due punti, partendo da (wStartPosition, i32_ZeroSpeed)
+ // (x1, Y1)
+ // x Giungere a (wStartPosition+ wAccelerationWindow, MaximumSpeed)
+ // (x2, Y2)
+ //
+ // Y = mX + q
+ //
+ // X = wActualPosition
+ // Y = ActualSpeed
+ //
+ // m = (y2- y1)/(x2- x1)
+ // q = ((x2* y1)- (x1* y2))/ (x2- x1)
+ //
+ // ==================================
+ // ==================================
+
+ d_X1 = (double)i64_StartPosition_Local;
+ d_X2 = (double)(i64_StartPosition_Local+ in.i64_AccelerationWindow);
+ d_Y1 = (double)in.i32_ZeroSpeed;
+ d_Y2 = (double)i32_MaximumSpeed_FW;
+
+ 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);
+ d_X1_per_Y2 = (d_X1* d_Y2);
+
+ d_m = (d_Y2_meno_Y1)/ (d_X2_meno_X1);
+ d_q = ((d_X2_per_Y1)- (d_X1_per_Y2))/ (d_X2_meno_X1);
+
+ // Punto 2) Ricavo Y = nX+ t
+ // ================
+ //
+ // Retta x due punti, partendo da (wTargetPosition- wDecelerationWindow, MaximumSpeed)
+ // (x1, Y1)
+ // x Giungere a (wTargetPosition, i32_ZeroSpeed)
+ // (x2, Y2)
+ //
+ // Y = nX + t
+ //
+ // X = wActualPosition
+ // Y = ActualSpeed
+ //
+ // n = (y2- y1)/(x2- x1)
+ // t = ((x2* y1)- (x1* y2))/ (x2- x1)
+ //
+ // ==================================
+ // ==================================
+
+ 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_Y2_meno_Y1 = (d_Y2- d_Y1); // LA: From Max to Zero
+ d_X2_meno_X1 = (d_X2- d_X1); // LA: Deceleration EndPoint
+ d_X2_per_Y1 = (d_X2* d_Y1);
+ d_X1_per_Y2 = (d_X1* d_Y2);
+
+ d_n = (d_Y2_meno_Y1)/ (d_X2_meno_X1);
+ d_t = ((d_X2_per_Y1)- (d_X1_per_Y2))/ (d_X2_meno_X1);
+
+ // Punto 3) Rilevo il punto di Intersezione x la X
+ // Ricavo conseguentemente Y dall'equazione
+ // ========================================
+ //
+ // X = (t- q)/ (m- n)
+ // Y = mX+ q (o Y = nX+ t, che in quel punto è equivalente ...)
+ //
+ // Y Rappresenterà la Massima Velocità raggiungibile, con le attuali pendenze
+ // X Rappresenta il punto a cui ridurre Accelerazioni e Decelerazioni di profilo.
+ //
+ // PQM:
+ //
+ // Accwindow = ((t- q)/ (m- n))- StartPosition
+ // Decwindow = FinishPosition- ((t- q)/ (m- n))
+ // 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)));
+ i32_MaximumSpeed_Local = (int32_t)((d_m* ((d_t- d_q)/ (d_m- d_n)))+ d_q);
+ }
+
+ else {
+
+ // uui64_ActualPosition >= uui64_TargetPosition
+ // LA: Going BW
+ // LA: Calcolare Entrambi i Profili,
+ // Trovare il Punto di Intersezione
+ // Aggiornare Acc/Dec/VMax in Accordo
+
+ // Punto 1) Ricavo Y = mX+ q
+ // ================
+ //
+ // Retta x due punti, partendo da (wStartPosition, i32_ZeroSpeed)
+ // (x1, Y1)
+ // x Giungere a (wStartPosition- wAccelerationWindow, MaximumSpeed)
+ // (x2, Y2)
+ //
+ // Y = mX + q
+ //
+ // X = wActualPosition
+ // Y = ActualSpeed
+ //
+ // m = (y2- y1)/(x2- x1)
+ // q = ((x2* y1)- (x1* y2))/ (x2- x1)
+ //
+ // ==================================
+ // ==================================
+
+ d_X1 = (double)i64_StartPosition_Local;
+ d_X2 = (double)(i64_StartPosition_Local- in.i64_AccelerationWindow);
+ d_Y1 = (double)in.i32_ZeroSpeed;
+ d_Y2 = (double)i32_MaximumSpeed_BW;
+
+ 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);
+ d_X1_per_Y2 = (d_X1* d_Y2);
+
+ d_m = (d_Y2_meno_Y1)/ (d_X2_meno_X1);
+ d_q = ((d_X2_per_Y1)- (d_X1_per_Y2))/ (d_X2_meno_X1);
+
+ b_AuxCalculateProfile_002 = true;
+
+ // Punto 2) Ricavo Y = nX+ t
+ // ================
+ //
+ // Retta x due punti, partendo da (wTargetPosition+ wDecelerationWindow, MaximumSpeed)
+ // (x1, Y1)
+ // x Giungere a (wTargetPosition, i32_ZeroSpeed)
+ // (x2, Y2)
+ //
+ // Y = mX + q
+ //
+ // X = wActualPosition
+ // Y = ActualSpeed
+ //
+ // m = (y2- y1)/(x2- x1)
+ // q = ((x2* y1)- (x1* y2))/ (x2- x1)
+ //
+ // ==================================
+ // ==================================
+
+ 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_Y2_meno_Y1 = (d_Y2- d_Y1); // LA: From Max to Zero
+ d_X2_meno_X1 = (d_X2- d_X1); // LA: Deceleration EndPoint
+ d_X2_per_Y1 = (d_X2* d_Y1);
+ d_X1_per_Y2 = (d_X1* d_Y2);
+
+ d_n = (d_Y2_meno_Y1)/ (d_X2_meno_X1);
+ d_t = ((d_X2_per_Y1)- (d_X1_per_Y2))/ (d_X2_meno_X1);
+
+ // Punto 3) Rilevo il punto di Intersezione x la X
+ // Ricavo conseguentemente Y dall'equazione
+ // ========================================
+ //
+ // X = (t- q)/ (m- n)
+ // Y = mX+ q (o Y = nX+ t, che in quel punto è equivalente ...)
+ //
+ // Y Rappresenterà la Massima Velocità raggiungibile, con le attuali pendenze
+ // X Rappresenta il punto a cui ridurre Accelerazioni e Decelerazioni di profilo.
+ //
+ // PQM:
+ //
+ // Accwindow = StartPosition- ((t- q)/ (m- n))
+ // Decwindow = ((t- q)/ (m- n))- FinishPosition
+ // 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);
+ i32_MaximumSpeed_Local = (int32_t)((d_m* ((d_t- d_q)/ (d_m- d_n)))+ d_q);
+ }
+ }
+ else {
+
+ // LA: il Profilo è Trapeziodale, Nullaltro da fare che assegnare i valori ai contenitori locali.
+ //
+ i64_AccelerationWindow_Local = in.i64_AccelerationWindow;
+ i64_DecelerationWindow_Local = in.i64_DecelerationWindow;
+
+ if (in.i64_ActualPosition < in.i64_TargetPosition)
+ i32_MaximumSpeed_Local = i32_MaximumSpeed_FW; // LA: Going FW
+ else
+ i32_MaximumSpeed_Local = i32_MaximumSpeed_BW; // LA: Going BW
+ }
+
+ b_GoingFW = false;
+ out.b_InToleranceFW = false;
+ b_GoingBW = false;
+ out.b_InToleranceBW = false;
+
+ out.b_Accelerating = false;
+ out.b_MaxSpeedReached = false;
+ out.b_Decelerating = false;
+ out.b_InPosition = false;
+
+ b_AuxCalculateProfile_000 = false;
+ b_AuxCalculateProfile_001 = false;
+ b_AuxCalculateProfile_002 = false;
+ b_AuxCalculateProfile_003 = false;
+ }
+
+ if (!in.b_ServoLock) {
+
+ // LA: Stop the Motion
+ //
+ i32_ActualSpeed = in.i32_ZeroSpeed;
+ b_GoingFW = false;
+ b_GoingBW = false;
+
+ out.b_STW1_On = false;
+ out.b_STW1_NoCStop = false;
+ out.b_STW1_NoQStop = false;
+ out.b_STW1_Enable = false;
+
+ // LA: i64_TargetPosition_Prec è ritentiva, per cui è bene inizializzarla != uui64_TargetPosition.
+ //
+ if (in.i64_TargetPosition > 0) // LA: E' Possibile Decrementare senza rollare?
+ i64_TargetPosition_Prec = in.i64_TargetPosition- 1; // LA: Si, Di certo Diverso, Di certo Coerente.
+ else
+ i64_TargetPosition_Prec = in.i64_TargetPosition+ 1; // LA: No, Incremento. Di certo è Diverso e Coerente.
+
+ // ========================
+ // ========================
+ // Axis is Now "Free Wheel"
+ // ========================
+ // ========================
+
+ }
+
+ else {
+
+ // LA: Issue to <Start the Motion>
+ // LA: Keep the present Motion <Active>
+ //
+ out.b_STW1_On = true;
+ out.b_STW1_NoCStop = true;
+ out.b_STW1_NoQStop = true;
+ out.b_STW1_Enable = true;
+
+ // Positioner
+ //
+ if (in.i64_ActualPosition < in.i64_TargetPosition) {
+
+ // LA: Going FW
+ //
+ out.b_InToleranceBW = false;
+ b_GoingFW = true;
+ b_GoingBW = false;
+
+ // Case is: Acceleration is Alloweed
+ // ========================
+ //
+ if (
+ (in.i64_ActualPosition >= i64_StartPosition_Local) &&
+ (in.i64_ActualPosition < (i64_StartPosition_Local+ i64_AccelerationWindow_Local))
+ ) {
+
+ out.b_Accelerating = true;
+ out.b_Decelerating = false;
+ out.b_MaxSpeedReached = false;
+
+ out.b_InPosition = false;
+ out.b_InToleranceFW = false;
+
+ // Line Profile [m, q] is to be Calculated only once
+ //
+ if (! b_AuxCalculateProfile_000) {
+
+ // Retta x due punti, partendo da (wStartPosition, i32_ZeroSpeed)
+ // (x1, Y1)
+ // x Giungere a (wStartPosition+ wAccelerationWindow, MaximumSpeed)
+ // (x2, Y2)
+ //
+ // Y = mX + q
+ //
+ // X = wActualPosition
+ // Y = ActualSpeed
+ //
+ // m = (y2- y1)/(x2- x1)
+ // q = ((x2* y1)- (x1* y2))/ (x2- x1)
+ //
+ // ==================================
+ // ==================================
+
+ d_X1 = (double)i64_StartPosition_Local;
+ d_X2 = (double)(i64_StartPosition_Local+ i64_AccelerationWindow_Local);
+ d_Y1 = (double)in.i32_ZeroSpeed;
+ 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);
+ d_X1_per_Y2 = (d_X1* d_Y2);
+
+ d_m = (d_Y2_meno_Y1)/ (d_X2_meno_X1);
+ d_q = ((d_X2_per_Y1)- (d_X1_per_Y2))/ (d_X2_meno_X1);
+
+ b_AuxCalculateProfile_000 = true;
+ }
+
+ i32_ActualSpeed = (int)((d_m * (double)in.i64_ActualPosition)+ d_q);
+ }
+
+ // Case is: MiddleWalking @ Planned-Full Speed
+ // ==================================
+ //
+ if (
+ (in.i64_ActualPosition >= (i64_StartPosition_Local+ i64_AccelerationWindow_Local)) &&
+ (in.i64_ActualPosition < (in.i64_TargetPosition - i64_DecelerationWindow_Local))
+ ) {
+
+ out.b_Accelerating = false;
+ out.b_Decelerating = false;
+ out.b_MaxSpeedReached = true;
+
+ out.b_InPosition = false;
+ out.b_InToleranceFW = false;
+
+ i32_ActualSpeed = i32_MaximumSpeed_Local;
+ }
+
+ // Case is: Need to Decelerate, up to the Tolerance Window
+ // ==============================================
+ //
+ if (
+ (in.i64_ActualPosition >= (in.i64_TargetPosition- i64_DecelerationWindow_Local)) &&
+ (in.i64_ActualPosition < (in.i64_TargetPosition- in.i64_diToleranceWindow))
+ ) {
+
+ out.b_Accelerating = false;
+ out.b_Decelerating = true;
+ out.b_MaxSpeedReached = false;
+
+ out.b_InPosition = false;
+ out.b_InToleranceFW = false;
+
+ // Line Profile [m, q] is to be Calculated only once
+ //
+ if (! b_AuxCalculateProfile_001) {
+
+ // Retta x due punti, partendo da (wTargetPosition- wDecelerationWindow, MaximumSpeed)
+ // (x1, Y1)
+ // x Giungere a (wTargetPosition, i32_ZeroSpeed)
+ // (x2, Y2)
+ //
+ // Y = mX + q
+ //
+ // X = wActualPosition
+ // Y = ActualSpeed
+ //
+ // m = (y2- y1)/(x2- x1)
+ // q = ((x2* y1)- (x1* y2))/ (x2- x1)
+ //
+ // ==================================
+ // ==================================
+
+ 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_meno_Y1 = (d_Y2- d_Y1); // LA: From Max to Zero
+ d_X2_meno_X1 = (d_X2- d_X1); // LA: Deceleration EndPoint
+ d_X2_per_Y1 = (d_X2* d_Y1);
+ d_X1_per_Y2 = (d_X1* d_Y2);
+
+ d_m = (d_Y2_meno_Y1)/ (d_X2_meno_X1);
+ d_q = ((d_X2_per_Y1)- (d_X1_per_Y2))/ (d_X2_meno_X1);
+
+ b_AuxCalculateProfile_001 = true;
+ }
+
+ i32_ActualSpeed = abs((int)((d_m * (double)in.i64_ActualPosition)+ d_q));
+ }
+
+ // Case is: Tolerance Reached while Going FW
+ // ================================
+ //
+ if (in.i64_ActualPosition >= (in.i64_TargetPosition- in.i64_diToleranceWindow)) {
+ out.b_Accelerating = false;
+ out.b_Decelerating = false;
+ out.b_MaxSpeedReached = false;
+
+ out.b_InPosition = true;
+ out.b_InToleranceFW = true;
+
+ i64_StartPosition_Local = in.i64_ActualPosition;
+ i32_ActualSpeed = i32_ServoLockSpeed_FW;
+ }
+ }
+
+ else if (in.i64_ActualPosition > in.i64_TargetPosition) {
+
+ // LA: Going Bw
+ //
+ out.b_InToleranceFW = false;
+ b_GoingBW = true;
+ b_GoingFW = false;
+
+ // Case is: Acceleration is Alloweed
+ // ========================
+ //
+ if (
+ (in.i64_ActualPosition <= i64_StartPosition_Local) &&
+ (in.i64_ActualPosition > (i64_StartPosition_Local- i64_AccelerationWindow_Local))
+ ) {
+
+ out.b_Accelerating = true;
+ out.b_Decelerating = false;
+ out.b_MaxSpeedReached = false;
+
+ out.b_InPosition = false;
+ out.b_InToleranceBW = false;
+
+ // Line Profile [m, q] is to be Calculated only once
+ //
+ if (! b_AuxCalculateProfile_002) {
+
+ // Retta x due punti, partendo da (wStartPosition, i32_ZeroSpeed)
+ // (x1, Y1)
+ // x Giungere a (wStartPosition- wAccelerationWindow, MaximumSpeed)
+ // (x2, Y2)
+ //
+ // Y = mX + q
+ //
+ // X = wActualPosition
+ // Y = ActualSpeed
+ //
+ // m = (y2- y1)/(x2- x1)
+ // q = ((x2* y1)- (x1* y2))/ (x2- x1)
+ //
+ // ==================================
+ // ==================================
+
+ d_X1 = (double)i64_StartPosition_Local;
+ d_X2 = (double)(i64_StartPosition_Local- i64_AccelerationWindow_Local);
+ d_Y1 = (double)in.i32_ZeroSpeed;
+ 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);
+ d_X1_per_Y2 = (d_X1* d_Y2);
+
+ d_m = (d_Y2_meno_Y1)/ (d_X2_meno_X1);
+ d_q = ((d_X2_per_Y1)- (d_X1_per_Y2))/ (d_X2_meno_X1);
+
+ b_AuxCalculateProfile_002 = true;
+ }
+
+ i32_ActualSpeed = (int)((d_m * (double)in.i64_ActualPosition)+ d_q);
+ }
+
+ // Case is: MiddleWalking @ Planned-Full Speed
+ // ==================================
+ //
+ if (
+ (in.i64_ActualPosition <= (i64_StartPosition_Local- i64_AccelerationWindow_Local)) &&
+ (in.i64_ActualPosition > (in.i64_TargetPosition+ i64_DecelerationWindow_Local))
+ ) {
+
+ out.b_Accelerating = false;
+ out.b_Decelerating = false;
+ out.b_MaxSpeedReached = true;
+
+ out.b_InPosition = false;
+ out.b_InToleranceBW = false;
+
+ i32_ActualSpeed = i32_MaximumSpeed_Local;
+ }
+
+ // Case is: Need to Decelerate, up to the Tolerance Window
+ // ==============================================
+ //
+ if (
+ (in.i64_ActualPosition <= (in.i64_TargetPosition+ i64_DecelerationWindow_Local)) &&
+ (in.i64_ActualPosition > (in.i64_TargetPosition+ in.i64_diToleranceWindow))
+ ) {
+
+ out.b_Accelerating = false;
+ out.b_Decelerating = true;
+ out.b_MaxSpeedReached = false;
+
+ out.b_InPosition = false;
+ out.b_InToleranceBW = false;
+
+ // Line Profile [m, q] is to be Calculated only once
+ //
+ if (! b_AuxCalculateProfile_003) {
+
+ // Retta x due punti, partendo da (wTargetPosition+ wDecelerationWindow, MaximumSpeed)
+ // (x1, Y1)
+ // x Giungere a (wTargetPosition, i32_ZeroSpeed)
+ // (x2, Y2)
+ //
+ // Y = mX + q
+ //
+ // X = wActualPosition
+ // Y = ActualSpeed
+ //
+ // m = (y2- y1)/(x2- x1)
+ // q = ((x2* y1)- (x1* y2))/ (x2- x1)
+ //
+ // ==================================
+ // ==================================
+
+ 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_meno_Y1 = (d_Y2- d_Y1); // LA: From Max to Zero
+ d_X2_meno_X1 = (d_X2- d_X1); // LA: Deceleration EndPoint
+ d_X2_per_Y1 = (d_X2* d_Y1);
+ d_X1_per_Y2 = (d_X1* d_Y2);
+
+ d_m = (d_Y2_meno_Y1)/ (d_X2_meno_X1);
+ d_q = ((d_X2_per_Y1)- (d_X1_per_Y2))/ (d_X2_meno_X1);
+
+ b_AuxCalculateProfile_003 = true;
+ }
+
+ i32_ActualSpeed = abs((int)((d_m* (double)(in.i64_ActualPosition))+ d_q));
+ }
+
+ // Case is: Tolerance Reached while Going BW
+ // ================================
+ //
+ if (in.i64_ActualPosition <= (in.i64_TargetPosition + in.i64_diToleranceWindow)) {
+ out.b_Accelerating = false;
+ out.b_Decelerating = false;
+ out.b_MaxSpeedReached = false;
+
+ out.b_InPosition = true;
+ out.b_InToleranceBW = true;
+
+ i64_StartPosition_Local = in.i64_ActualPosition;
+ i32_ActualSpeed = i32_ServoLockSpeed_BW;
+ }
+ }
+
+ else {
+
+ // LA: In Position
+ // ===========
+ //
+ b_GoingBW = false;
+ out.b_InToleranceBW = true;
+ b_GoingFW = false;
+ out.b_InToleranceFW = true;
+
+ out.b_Accelerating = false;
+ out.b_Decelerating = false;
+ out.b_MaxSpeedReached = false;
+
+ out.b_InPosition = true;
+
+ i64_StartPosition_Local = in.i64_ActualPosition;
+ 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)
+ // ===========================
+ // ===========================
+
+ if (i64_Distance_Local >= in.i64_diToleranceWindow) {
+ // i64_Distance_Local >= in.i64_diToleranceWindow
+
+ if (in.i64_ActualPosition < in.i64_TargetPosition) {
+
+ // LA: Going FW
+ //
+ if (i32_ActualSpeed > i32_MaximumSpeed_FW)
+ i32_ActualSpeed = i32_MaximumSpeed_FW;
+ if (i32_ActualSpeed < i32_ServoLockSpeed_FW)
+ i32_ActualSpeed = i32_ServoLockSpeed_FW;
+ }
+ else {
+
+ // LA: Going BW
+ //
+ if (i32_ActualSpeed > i32_MaximumSpeed_BW)
+ i32_ActualSpeed = i32_MaximumSpeed_BW;
+ if (i32_ActualSpeed < i32_ServoLockSpeed_BW)
+ i32_ActualSpeed = i32_ServoLockSpeed_BW;
+ }
+ }
+
+ else {
+ // i64_Distance_Local < in.i64_diToleranceWindow
+
+ // LA: Attenzione, questo esegue un 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.
+ // Se dovesse uscire, il sistema lo riporterebbe in tolleranza (o cercherebbe di farlo) a "ServolockSpeed".
+ //
+
+ i32_ActualSpeed = in.i32_ZeroSpeed;
+ }
+ }
+ }
+
+ else {
+ // !in.b_ACPos_Homed
+
+ // LA: if Not Homed, Positioning Flag(s) are Kept Clear
+ //
+ b_GoingFW = false;
+ out.b_InToleranceFW = false;
+ b_GoingBW = false;
+ out.b_InToleranceBW = false;
+
+ out.b_Accelerating = false;
+ out.b_MaxSpeedReached = false;
+ out.b_Decelerating = false;
+ out.b_InPosition = false;
+
+ b_AuxCalculateProfile_000 = false;
+ b_AuxCalculateProfile_001 = false;
+ b_AuxCalculateProfile_002 = false;
+ b_AuxCalculateProfile_003 = false;
+
+ i32_ActualSpeed = in.i32_ZeroSpeed;
+ }
+ }
+ }
+ else {
+ // !in.b_AxisPowered
+
+ // LA: if Not Powered, Motion Flag(s) are Kept Clear
+ //
+ b_GoingFW = false;
+ out.b_InToleranceFW = false;
+ b_GoingBW = false;
+ out.b_InToleranceBW = false;
+
+ out.b_Accelerating = false;
+ out.b_MaxSpeedReached = false;
+ out.b_Decelerating = false;
+ out.b_InPosition = false;
+
+ b_AuxCalculateProfile_000 = false;
+ b_AuxCalculateProfile_001 = false;
+ b_AuxCalculateProfile_002 = false;
+ b_AuxCalculateProfile_003 = false;
+
+ i32_ActualSpeed = in.i32_ZeroSpeed;
+ }
+
+ // ===================
+ // ===================
+ // LA: Finally, RAW Output
+ // ===================
+ // ===================
+
+ out.i32_ATVSpeed = i32_ActualSpeed;
+ out.b_ATVDirectionFW = b_GoingFW;
+ out.b_ATVDirectionBW = b_GoingBW;
+
+ out.i64_StartPosition = i64_StartPosition_Local;
+ out.i64_Distance = i64_Distance_Local;
+ }
+ InProgress = false;
+}