Luca Antolini
Velocity Closed Loop Dynamic error correction
Dependencies: mbed QEI PID DmTftLibraryEx
Diff: SWPos/SWPos.cpp
- Revision:
- 39:be7055a0e9a4
- Parent:
- 33:f77aa3ecf87d
--- a/SWPos/SWPos.cpp Tue Apr 12 07:55:59 2022 +0000
+++ b/SWPos/SWPos.cpp Fri Apr 15 07:01:02 2022 +0000
@@ -74,12 +74,14 @@
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 float f_ServoLockSpeed_FW;
+static float f_ServoLockSpeed_BW;
static double d_X1;
static double d_X2;
@@ -94,12 +96,14 @@
static double d_n;
static double d_t;
-static int32_t i32_ActualSpeed;
+static float f_ActualSpeed_Local;
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 float f_MaximumSpeed_FW;
+static float f_MaximumSpeed_BW;
+
+static float f_MaximumSpeed_Local;
static int64_t i64_StartPosition_Local;
static int64_t i64_Distance_Local;
static bool b_GoingFW;
@@ -108,13 +112,16 @@
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;
@@ -168,9 +175,9 @@
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
-
+ f_MaximumJogSpeed_xW = (((in.f_JogSpeed_x100_FW)* (in.f_Max_Speed- f_ActualSpeed_Local)/ 100.0f)); // 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;
@@ -179,11 +186,11 @@
// 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
+ i32_Aux_ms2Acc = (i32_Aux_ms2Acc- ui32_PassedActual_ms_Local); // LA: Ms Passed @ this Trip
+ f_ActualSpeed_Local = ((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
+ f_ActualSpeed_Local = ((in.f_JogSpeed_x100_FW)* (in.f_Max_Speed- in.f_ZeroSpeed)/ 100.0f); // LA: Maximum Speed Reached
b_Accelerating_Local = false;
b_Decelerating_Local = false;
@@ -199,7 +206,7 @@
// 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_MaximumJogSpeed_xW = (((in.f_JogSpeed_x100_FW)* (float)(f_ActualSpeed_Local- in.f_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;
@@ -210,22 +217,22 @@
// 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
+ i32_Aux_ms2Dec = (i32_Aux_ms2Dec- ui32_PassedActual_ms_Local); // LA: Ms Passed @ this Trip
+ f_ActualSpeed_Local = (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; //
+ b_GoingFW = true; // LA: Moves ...
+ b_GoingBW = false; //
}
else {
- i32_ActualSpeed = in.i32_ZeroSpeed; // LA: Zero Speed Reached
+ f_ActualSpeed_Local = in.f_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; //
+ b_JogFW_Prec_Local = false; // LA: Move is Terminated, NOW
+ b_GoingFW = false; //
+ b_GoingBW = false; //
}
}
}
@@ -238,8 +245,8 @@
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
+ f_MaximumJogSpeed_xW = (((in.f_JogSpeed_x100_BW)* (in.f_Max_Speed- f_ActualSpeed_Local)/ 100.0f)); // 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;
@@ -249,15 +256,15 @@
// 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
+ i32_Aux_ms2Acc = (i32_Aux_ms2Acc- ui32_PassedActual_ms_Local); // LA: Ms Passed @ this Trip
+ f_ActualSpeed_Local = ((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;
+ f_ActualSpeed_Local = ((in.f_JogSpeed_x100_BW)* (in.f_Max_Speed- in.f_ZeroSpeed)/ 100.0f); // LA: Maximum Speed Reached
+
+ b_Accelerating_Local = false;
+ b_Decelerating_Local = false;
+ out.b_MaxSpeedReached = true;
}
b_GoingBW = true; // LA: Moves ...
@@ -270,9 +277,9 @@
// 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
-
+ f_MaximumJogSpeed_xW = (((in.f_JogSpeed_x100_BW)* (f_ActualSpeed_Local- in.f_ZeroSpeed)/ 100.0f)); // 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;
@@ -281,22 +288,22 @@
// 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
+ i32_Aux_ms2Dec = (i32_Aux_ms2Dec- ui32_PassedActual_ms_Local); // LA: Ms Passed @ this Trip
+ f_ActualSpeed_Local = (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; //
+ b_GoingBW = true; // LA: Moves ...
+ b_GoingFW = false; //
}
else {
- i32_ActualSpeed = in.i32_ZeroSpeed; // LA: Zero Speed Reached
+ f_ActualSpeed_Local = in.f_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; //
+ b_JogBW_Prec_Local = false; // LA: Move is Terminated, NOW
+ b_GoingBW = false; //
+ b_GoingFW = false; //
}
}
}
@@ -328,11 +335,11 @@
// 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));
-
+ f_MaximumSpeed_FW = (((in.f_MaximumSpeed_x100_FW)* (in.f_Max_Speed- in.f_ZeroSpeed)/ 100.0f));
+ f_ServoLockSpeed_FW = (((in.f_ServoLockSpeed_x100_FW)* (in.f_Max_Speed- in.f_ZeroSpeed)/ 100.0f));
+ f_MaximumSpeed_BW = (((in.f_MaximumSpeed_x100_BW)* (in.f_Max_Speed- in.f_ZeroSpeed)/ 100.0f));
+ f_ServoLockSpeed_BW = (((in.f_ServoLockSpeed_x100_BW)* (in.f_Max_Speed- in.f_ZeroSpeed)/ 100.0f));
+
// LA: Verifica (STATICA) del Profilo (Trapezio o Triangolo)
//
if (i64_Distance_Local < (in.i64_AccelerationWindow+ in.i64_DecelerationWindow)) {
@@ -368,8 +375,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)i32_ServoLockSpeed_FW;
- d_Y2 = (double)i32_MaximumSpeed_FW;
+ d_Y1 = (double)f_ServoLockSpeed_FW;
+ d_Y2 = (double)f_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
@@ -400,9 +407,9 @@
d_X1 = (double)(in.i64_TargetPosition- in.i64_DecelerationWindow);
d_X2 = (double)in.i64_TargetPosition;
- d_Y1 = (double)i32_MaximumSpeed_FW;
+ d_Y1 = (double)f_MaximumSpeed_FW;
//d_Y2 = (double)in.i32_ZeroSpeed;
- d_Y2 = (double)i32_ServoLockSpeed_FW;
+ d_Y2 = (double)f_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
@@ -430,7 +437,7 @@
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);
+ f_MaximumSpeed_Local = (float)((d_m* ((d_t- d_q)/ (d_m- d_n)))+ d_q);
}
else {
@@ -463,8 +470,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)i32_ServoLockSpeed_BW;
- d_Y2 = (double)i32_MaximumSpeed_BW;
+ d_Y1 = (double)f_ServoLockSpeed_BW;
+ d_Y2 = (double)f_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
@@ -497,9 +504,9 @@
d_X1 = (double)(in.i64_TargetPosition + in.i64_DecelerationWindow);
d_X2 = (double)(in.i64_TargetPosition);
- d_Y1 = (double)(i32_MaximumSpeed_BW);
+ d_Y1 = (double)(f_MaximumSpeed_BW);
//d_Y2 = (double)(in.i32_ZeroSpeed);
- d_Y2 = (double)(i32_ServoLockSpeed_BW);
+ d_Y2 = (double)(f_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
@@ -527,7 +534,7 @@
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);
+ f_MaximumSpeed_Local = (float)((d_m* ((d_t- d_q)/ (d_m- d_n)))+ d_q);
}
}
else {
@@ -538,9 +545,9 @@
i64_DecelerationWindow_Local = in.i64_DecelerationWindow;
if (in.i64_ActualPosition < in.i64_TargetPosition)
- i32_MaximumSpeed_Local = i32_MaximumSpeed_FW; // LA: Going FW
+ f_MaximumSpeed_Local = f_MaximumSpeed_FW; // LA: Going FW
else
- i32_MaximumSpeed_Local = i32_MaximumSpeed_BW; // LA: Going BW
+ f_MaximumSpeed_Local = f_MaximumSpeed_BW; // LA: Going BW
}
b_GoingFW = false;
@@ -563,7 +570,7 @@
// LA: Stop the Motion
//
- i32_ActualSpeed = in.i32_ZeroSpeed;
+ f_ActualSpeed_Local = in.f_ZeroSpeed;
b_GoingFW = false;
b_GoingBW = false;
@@ -645,11 +652,11 @@
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_Y1 = (double)f_ServoLockSpeed_FW;
+ d_Y2 = (double)f_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_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);
@@ -658,8 +665,7 @@
b_AuxCalculateProfile_000 = true;
}
-
- i32_ActualSpeed = (int)((d_m * (double)in.i64_ActualPosition)+ d_q);
+ f_ActualSpeed_Local = (float)((d_m * (double)in.i64_ActualPosition)+ d_q);
}
// Case is: MiddleWalking @ Planned-Full Speed
@@ -670,14 +676,14 @@
(in.i64_ActualPosition < (in.i64_TargetPosition - i64_DecelerationWindow_Local))
) {
- out.b_Accelerating = false;
- out.b_Decelerating = false;
- out.b_MaxSpeedReached = true;
+ out.b_Accelerating = false;
+ out.b_Decelerating = false;
+ out.b_MaxSpeedReached = true;
- out.b_InPosition = false;
- out.b_InToleranceFW = false;
+ out.b_InPosition = false;
+ out.b_InToleranceFW = false;
- i32_ActualSpeed = i32_MaximumSpeed_Local;
+ f_ActualSpeed_Local = f_MaximumSpeed_Local;
}
// Case is: Need to Decelerate, up to the Tolerance Window
@@ -717,9 +723,9 @@
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_Y1 = (double)f_ActualSpeed_Local; // LA: Maximum Speed Planned MIGHT have NOT been Reached
//d_Y2 = (double)in.i32_ZeroSpeed;
- d_Y2 = (double)i32_ServoLockSpeed_FW;
+ d_Y2 = (double)f_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
@@ -731,23 +737,22 @@
b_AuxCalculateProfile_001 = true;
}
-
- i32_ActualSpeed = abs((int)((d_m * (double)in.i64_ActualPosition)+ d_q));
+ f_ActualSpeed_Local = abs((float)((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_Accelerating = false;
+ out.b_Decelerating = false;
+ out.b_MaxSpeedReached = false;
- out.b_InPosition = true;
- out.b_InToleranceFW = true;
+ out.b_InPosition = true;
+ out.b_InToleranceFW = true;
- i64_StartPosition_Local = in.i64_ActualPosition;
- i32_ActualSpeed = i32_ServoLockSpeed_FW;
+ i64_StartPosition_Local = in.i64_ActualPosition;
+ f_ActualSpeed_Local = f_ServoLockSpeed_FW;
}
}
@@ -797,8 +802,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)i32_ServoLockSpeed_BW;
- d_Y2 = (double)i32_MaximumSpeed_Local;
+ d_Y1 = (double)f_ServoLockSpeed_BW;
+ d_Y2 = (double)f_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
@@ -810,8 +815,7 @@
b_AuxCalculateProfile_002 = true;
}
-
- i32_ActualSpeed = (int)((d_m * (double)in.i64_ActualPosition)+ d_q);
+ f_ActualSpeed_Local = (float)((d_m * (double)in.i64_ActualPosition)+ d_q);
}
// Case is: MiddleWalking @ Planned-Full Speed
@@ -822,14 +826,14 @@
(in.i64_ActualPosition > (in.i64_TargetPosition+ i64_DecelerationWindow_Local))
) {
- out.b_Accelerating = false;
- out.b_Decelerating = false;
- out.b_MaxSpeedReached = true;
+ out.b_Accelerating = false;
+ out.b_Decelerating = false;
+ out.b_MaxSpeedReached = true;
- out.b_InPosition = false;
- out.b_InToleranceBW = false;
+ out.b_InPosition = false;
+ out.b_InToleranceBW = false;
- i32_ActualSpeed = i32_MaximumSpeed_Local;
+ f_ActualSpeed_Local = f_MaximumSpeed_Local;
}
// Case is: Need to Decelerate, up to the Tolerance Window
@@ -869,9 +873,9 @@
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_Y1 = (double)f_ActualSpeed_Local; // LA: Maximum Speed Planned MIGHT have NOT been Reached
//d_Y2 = (double)in.i32_ZeroSpeed;
- d_Y2 = (double)i32_ServoLockSpeed_BW;
+ d_Y2 = (double)f_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
@@ -883,7 +887,7 @@
b_AuxCalculateProfile_003 = true;
}
- i32_ActualSpeed = abs((int)((d_m* (double)(in.i64_ActualPosition))+ d_q));
+ f_ActualSpeed_Local = abs((float)((d_m* (double)(in.i64_ActualPosition))+ d_q));
}
// Case is: Tolerance Reached while Going BW
@@ -898,7 +902,7 @@
out.b_InToleranceBW = true;
i64_StartPosition_Local = in.i64_ActualPosition;
- i32_ActualSpeed = i32_ServoLockSpeed_BW;
+ f_ActualSpeed_Local = f_ServoLockSpeed_BW;
}
}
@@ -919,7 +923,7 @@
out.b_InPosition = true;
i64_StartPosition_Local = in.i64_ActualPosition;
- i32_ActualSpeed = in.i32_ZeroSpeed;
+ f_ActualSpeed_Local = in.f_ZeroSpeed;
}
// ===========================
@@ -935,19 +939,19 @@
// 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;
+ if (f_ActualSpeed_Local > f_MaximumSpeed_FW)
+ f_ActualSpeed_Local = f_MaximumSpeed_FW;
+ if (f_ActualSpeed_Local < f_ServoLockSpeed_FW)
+ f_ActualSpeed_Local = f_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;
+ if (f_ActualSpeed_Local > f_MaximumSpeed_BW)
+ f_ActualSpeed_Local = f_MaximumSpeed_BW;
+ if (f_ActualSpeed_Local < f_ServoLockSpeed_BW)
+ f_ActualSpeed_Local = f_ServoLockSpeed_BW;
}
}
@@ -962,7 +966,7 @@
// Se dovesse uscire, il sistema lo riporterebbe in tolleranza (o cercherebbe di farlo) a "ServolockSpeed".
//
- i32_ActualSpeed = in.i32_ZeroSpeed;
+ f_ActualSpeed_Local = in.f_ZeroSpeed;
}
}
}
@@ -987,7 +991,7 @@
b_AuxCalculateProfile_002 = false;
b_AuxCalculateProfile_003 = false;
- i32_ActualSpeed = in.i32_ZeroSpeed;
+ f_ActualSpeed_Local = in.f_ZeroSpeed;
}
}
}
@@ -1011,7 +1015,7 @@
b_AuxCalculateProfile_002 = false;
b_AuxCalculateProfile_003 = false;
- i32_ActualSpeed = in.i32_ZeroSpeed;
+ f_ActualSpeed_Local = in.f_ZeroSpeed;
}
// ===================
@@ -1020,7 +1024,7 @@
// ===================
// ===================
- out.i32_ATVSpeed = i32_ActualSpeed;
+ out.f_ATVSpeed = f_ActualSpeed_Local;
out.b_ATVDirectionFW = b_GoingFW;
out.b_ATVDirectionBW = b_GoingBW;