Luca Antolini
All the previous but the PID
Dependencies: mbed QEI PID DmTftLibraryEx
SWPos/SWPos.cpp
- Committer:
- lex9296
- Date:
- 2022-04-11
- Revision:
- 34:0522cebfe489
- Parent:
- 33:f77aa3ecf87d
File content as of revision 34:0522cebfe489:
/* 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".
//
// 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
#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 (STATICA) 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_Y1 = (double)i32_ServoLockSpeed_FW;
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_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
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_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);
}
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_Y1 = (double)i32_ServoLockSpeed_BW;
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_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
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_TargetPosition_Prec);
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_Y1 = (double)i32_ServoLockSpeed_FW;
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 = (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
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_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);
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 = (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
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;
}
// ===========================
// ===========================
// 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 <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.
// 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;
}