Luca Antolini
/
AAA_Stabilus322699_LA0030
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: mbed QEI PID DmTftLibraryEx
Revision 39:be7055a0e9a4, committed 2022-04-15
- Comitter:
- lex9296
- Date:
- Fri Apr 15 07:01:02 2022 +0000
- Parent:
- 38:72394e4c35f8
- Commit message:
- Retroazione di velocita e correzione
Changed in this revision
--- 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;
--- a/SWPos/SWPos.h Tue Apr 12 07:55:59 2022 +0000
+++ b/SWPos/SWPos.h Fri Apr 15 07:01:02 2022 +0000
@@ -3,8 +3,8 @@
struct in_sPosizionatoreSW {
bool b_AxisPowered;
bool b_ACPos_Homed;
- int32_t i32_Max_Speed;
- int32_t i32_ZeroSpeed;
+ float f_Max_Speed;
+ float f_ZeroSpeed;
bool b_JogMode;
bool b_JogFW;
@@ -32,25 +32,25 @@
};
struct out_sPosizionatoreSW {
- int64_t i64_StartPosition;
- int64_t i64_Distance;
+ int64_t i64_StartPosition;
+ int64_t i64_Distance;
- bool b_Accelerating; // LA: bACPos_Accelerating
- bool b_MaxSpeedReached;
+ bool b_Accelerating; // LA: bACPos_Accelerating
+ bool b_MaxSpeedReached;
//
- bool b_Decelerating; // bACPos_Decelerating
- bool b_InPosition;
- bool b_InToleranceFW;
- bool b_InToleranceBW;
+ bool b_Decelerating; // bACPos_Decelerating
+ bool b_InPosition;
+ bool b_InToleranceFW;
+ bool b_InToleranceBW;
- int32_t i32_ATVSpeed;
- bool b_ATVDirectionFW;
- bool b_ATVDirectionBW;
+ float f_ATVSpeed;
+ bool b_ATVDirectionFW;
+ bool b_ATVDirectionBW;
//
- bool b_STW1_On;
- bool b_STW1_NoCStop;
- bool b_STW1_NoQStop;
- bool b_STW1_Enable;
+ bool b_STW1_On;
+ bool b_STW1_NoCStop;
+ bool b_STW1_NoQStop;
+ bool b_STW1_Enable;
// LA: Disposizioni transitorie e finali
//
--- a/main.cpp Tue Apr 12 07:55:59 2022 +0000
+++ b/main.cpp Fri Apr 15 07:01:02 2022 +0000
@@ -22,6 +22,26 @@
// La retroazione sulla Velocità (Calcolata ogni "cf_MOTPeriod_s") è eseguita, nello stesso intervallo, da "PID_VelocityClosedLoop"
// Il parametro di Riferimento è la Velocity Calcolata
// Il Fattore di correzione è il PWM in uscita
+// Con la versione Pubblicata 0022 Il Doppio PID è correttamente funzionante.
+//
+// LA: 12 Aprile 2022, Introduco la retroazione ad anello Avanzata
+// ===========================================================
+//
+// Provo ad escludere il PID (I PID, attualmente son due) a favore di un controllo di retroazione più incisivo.
+// L'attuale PID non tiene conto del verso di correzione, mentre nel moto attuale mi aspetto maggiori difficoltà a frenare che ad
+// accelerare.
+// PQM introduco un controllo (in retroazione di proporzionalità all'errore) che possa diversificarsi tra Accelerazione e FRENO.
+// i.e. deve essere più incisivo quando frena, meno quando accelera.
+//
+// 15/4: Retroazione Avanzata e correzione dinamica dell'errore.
+//
+// In base all'errore calcolato sulla velocità, opero una retroazione semplice per far tornare il sistema in stabilità.
+// Ad ogni "chiamata" del motion correggo l'uscita nel verso suggerito dall'errore di un fattore ad esso proporzionale.
+// Ad oggi è il risultato migliore.
+//
+// Committo
+// Forco
+// Pubblico la 0030
//
#include "QEI.h"
@@ -29,6 +49,7 @@
#include "Timers.h"
#include "Eeprom.h"
+#include "math.h"
#define MAX_CHAR_PER_LINE 28
#define TEXT_ROW_SPACING 16
@@ -43,58 +64,30 @@
int64_t ci64_TargetPOS = 3096; // Used also Outside
-#include "PID.h"
-
-/*
-#define Lx 0.0001
-#define Ku 0.32200//0.922000
-#define Pu 0.0125
-#define Kp Ku*0.6
-#define Ti Pu*0.5
-#define Td Pu*0.125
-//
-#define P Kp
-#define I Kp/Ti
-#define D Kp*Td
-#define Mstate 6
-#define SorT 5
-#define Databit 0
-#define TIME 0.0125
-*/
-
-// Kc, Ti, Td, interval
-//PID PID_VelocityClosedLoop_FW (0.4, 0.0, /*1.0E-32,*/ 1.0E-9, cf_MOTPeriod_s);
-//PID PID_VelocityClosedLoop_BW (0.41, 0.0, /*1.0E-32,*/ 1.0E-9, cf_MOTPeriod_s);
-//PID PID_VelocityClosedLoop_FW (0.55, 0.0, /*1.0E-32,*/ 1.0E-9, cf_MOTPeriod_s);
-//PID PID_VelocityClosedLoop_BW (0.55, 0.0, /*1.0E-32,*/ 1.0E-2, cf_MOTPeriod_s);
-PID PID_VelocityClosedLoop_FW (0.55, 0.0, 0.0, cf_MOTPeriod_s);
-PID PID_VelocityClosedLoop_BW (0.55, 0.0, 0.0, cf_MOTPeriod_s);
-
// LA: LCM_ShowTactics
// ===============
//
+/*
void LCM_ShowTactics(
- int64_t i64_Pulses,
- int32_t i32_ATVSpeed,
- //
- float f_ai0000_Aux,
- float f_ai0001_Aux,
- float f_ai0002_Aux,
- float f_ai0003_Aux,
- float f_ai0004_Aux,
- float f_ai0005_Aux,
- //
- int32_t i32_Velocity,
- int32_t i32_Acceleration,
- int32_t i32_Jerk
-
+ int64_t i64_Pulses,
+ float f_ATVSpeed,
+ float f_ai0000_Aux,
+ float f_ai0001_Aux,
+ float f_ai0002_Aux,
+ float f_ai0003_Aux,
+ float f_ai0004_Aux,
+ float f_ai0005_Aux,
+ int32_t i32_Velocity,
+ int32_t i32_Acceleration,
+ int32_t i32_Jerk
);
+*/
// LA: SampleAndStore
// LA: SampleTimer ==
// ==============
//
-static void SampleAndStore (void);
+//static void SampleAndStore (void);
Ticker SampleTimer; // LA: To Sample 1AI any 'x'ms
float af_PlotSamples[240]; // LA: "Horiz" Plot Array
@@ -115,12 +108,18 @@
// LA: MotionTimer =
// =============
//
-static void MotionHandler (void);
+//static void MotionHandler (void);
Ticker MotionTimer; // LA: Gestisce il rilevamento (RT) di Velocità, Accelerazione e Jerk
// Esegue il Movimento Programmato
-float f_PWMPercent; // Deprecated
-float fPID_Error; //
+float f_PWMPercent; //
+float F_VelocityLoopFB_FW; // LA: Correzione Dinamica FW
+float F_VelocityLoopFB_BW; // LA: Correzione Dinamica BW
+//float fPID_Error; //
+
+float fMOT_VelocityAct; // 0.0= 0%, 1.0= 100%
+float fMOT_VelocityReq; // 0.0= 0%, 1.0= 100%
+float fMOT_VelocityError; //
float fVelocity;
float fAcceleration;
@@ -197,197 +196,28 @@
HAL_NVIC_SystemReset( );
}
-// =======
-// =======
-// Main(s)
-// =======
-// =======
-//
-int main (void){
-
- rDIR_FWD = true; // LA: Actuator Extends
- rENA_Off = true; // LA: Drive Power is Off
- //
- in_PosizionatoreSW.b_ServoLock = false;
- in_PosizionatoreSW.rtServoLock_Q = false;
-
- EepromInit(); // LA: Inizializza la EEProm
- TimersInit(); // LA: Parte il Timer a 1ms
-
- // LA: FactoryReset se "userButton" premuto all'avvio
- //
- if (userButton == 0) {
- FactoryReset();
- }
- DisplayDriverInit();
-
- SampleTimer.attach(&SampleAndStore, cf_SCOPeriod_s); // LA: Avvia l'OscilloScopio con TimeBase x [s]
- MotionTimer.attach(&MotionHandler, cf_MOTPeriod_s); // LA: Avvia il Motion con TimeBase x [s]
- PWM_PB3.period(cf_PWMPeriod_s); // LA: Avvia il PWM con TimeBase x [s]
- PWM_PB3.write((float) 0.0); // Set to 0%
-
- // LA: Motion (1st) Setup
- //
- in_PosizionatoreSW.b_AxisPowered = true;
- in_PosizionatoreSW.b_ACPos_Homed = true;
- in_PosizionatoreSW.i32_Max_Speed = 20; // [ui/ms]
- in_PosizionatoreSW.i32_ZeroSpeed = 0; //
-
- // POS Mode
- // ========
- //
-// in_PosizionatoreSW.b_ServoLock = true;
-// in_PosizionatoreSW.rtServoLock_Q = false;
- //
- in_PosizionatoreSW.i64_TargetPosition = ci64_TargetPOS; // [ui]
- in_PosizionatoreSW.i64_ActualPosition = Stabilus322699.getPulses(); //
- in_PosizionatoreSW.i64_AccelerationWindow = 512; // LA: Spazio concesso all'accelerazione.
- in_PosizionatoreSW.i64_DecelerationWindow = 1024; // Spazio concesso alla decelerazione, è prioritario rispetto all'accelerazione.
- in_PosizionatoreSW.i64_diToleranceWindow = 16; // Finestra di Tolleranza
- //
- in_PosizionatoreSW.f_MaximumSpeed_x100_FW = 100.0f; // % of "i32_Max_Speed"
- in_PosizionatoreSW.f_MaximumSpeed_x100_BW = 100.0f; //
- in_PosizionatoreSW.f_ServoLockSpeed_x100_FW = 25.0f; // Riferimento di velocità minima a cui (appena) si muove l'asse [verso FW]
-// in_PosizionatoreSW.f_ServoLockSpeed_x100_BW = 50.0f; // Riferimento di velocità minima a cui (appena) si muove l'asse [verso BW]
- in_PosizionatoreSW.f_ServoLockSpeed_x100_BW = 48.0f; // Riferimento di velocità minima a cui (appena) si muove l'asse [verso BW]
-
- // JOG Mode
- // ========
- //
- in_PosizionatoreSW.b_JogMode = false;
- in_PosizionatoreSW.b_JogFW = false;
- in_PosizionatoreSW.b_JogBW = false;
- in_PosizionatoreSW.i32_JogAccel_ms = 500; // [ms]
- in_PosizionatoreSW.i32_JogDecel_ms = 250; //
- //
- in_PosizionatoreSW.f_JogSpeed_x100_FW = (in_PosizionatoreSW.f_MaximumSpeed_x100_FW/ 2); // LA: JOG's the Half of Max POS's Speed
- in_PosizionatoreSW.f_JogSpeed_x100_BW = (in_PosizionatoreSW.f_MaximumSpeed_x100_BW/ 2); //
-
- // Velocity Loop PID
- // =================
- //
- // Input Speed (ref)= 0.. 512[ui/s]
- PID_VelocityClosedLoop_FW.setInputLimits(0.0f, (float)in_PosizionatoreSW.i32_Max_Speed);
- PID_VelocityClosedLoop_BW.setInputLimits(0.0f, (float)in_PosizionatoreSW.i32_Max_Speed);
- // Output PWM (ref)= 0.. 1
- PID_VelocityClosedLoop_FW.setOutputLimits(0.0f, 1.0f);
- PID_VelocityClosedLoop_BW.setOutputLimits(0.0f, 1.0f);
-
- // If there's a bias.
-// PID_VelocityClosedLoop.setBias(0.3);
-// PID_VelocityClosedLoop.setMode(AUTO_MODE);
- PID_VelocityClosedLoop_FW.setMode(MANUAL_MODE);
- PID_VelocityClosedLoop_BW.setMode(MANUAL_MODE);
-
- // LA: Color RGB Component(s)
- // ======================
- //
- // RED 0000 1000 0000 0000 min 0x0800 02048
- // 1111 1000 0000 0000 max 0xf800 63488
- //
- // GREEN 0000 0000 0010 0000 min 0x0020 00032
- // 0000 0111 1110 0000 max 0x07e0 02016
- //
- // BLUE 0000 0000 0000 0001 min 0x0001 00001
- // 0000 0000 0001 1111 max 0x001f 00031
- //
- // La componente ROSSA ha 5 bit di escursione (0.. 31),
- // La componente VERDE ha 6 bit di escursione (0.. 63),
- // La componente BLU ha 5 bit di escursione (0.. 31),
- //
- // Le componenti RGB di "Color" sono quindi scritte negli appropriati registri come segue:
- //
- // writeReg(RED, (Color & 0xf800) >> 11);
- // writeReg(GREEN, (Color & 0x07e0) >> 5);
- // writeReg(BLUE, (Color & 0x001f));
- //
- LCM_SetTextColor(Scale2RGBColor (0, 0, 0), Scale2RGBColor (31, 0, 0)); // LA: Red on Black
- LCM_ClearScreen (Scale2RGBColor (0, 0, 0)); // Black Background
- LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 0), "You Start Me Up ..."); // Intro Text
-
-// rDIR_FWD = false; // Collapse
-// rENA_Off = false; // Power On
-
- in_PosizionatoreSW.b_ServoLock = true;
- in_PosizionatoreSW.rtServoLock_Q = true;
-
- while (1) {
-
- // LA: Scope, Theory of operation.
- // ===========================
- //
- // 1) Sample a Value @ any Step
- // 2) Store @ the correct ms
- // 3) Plot the current Section of the Sampling Vector
- //
- LCM_ShowTactics (
- Stabilus322699.getPulses(), // Row 1
-
- out_PosizionatoreSW.i32_ATVSpeed, // 3
- adc_temp.read(), // 4
- adc_vbat.read(), // 5
- adc_vref.read(), // 6
-
- ADC12_IN9.read(), // 8
- ADC12_IN15.read(), // 9
- (f_PWMPercent* 100), // 10
-
- i32_Velocity, // 11
- i32_Acceleration, // 12
- i32_Jerk // 13
-
- );
-
- LCM_PlotScope (
- Scale2RGBColor (0, 0, 0), // Back: Black
- Scale2RGBColor (31, 0, 0) // Fore: Red
- );
-
- LCM_PlotSpeed (
- Scale2RGBColor (0, 0, 0), // Back: Black
- Scale2RGBColor (31, 0, 0) // Fore: Red
- );
-
- if (out_PosizionatoreSW.b_InPosition)
- if (in_PosizionatoreSW.i64_TargetPosition > 0)
- in_PosizionatoreSW.i64_TargetPosition = 0;
- else
- in_PosizionatoreSW.i64_TargetPosition = ci64_TargetPOS;
- }
-}
-
void LCM_ShowTactics(
int64_t i64_Pulses,
- int32_t i32_ATVSpeed,
- //
+ float f_ATVSpeed,
float f_ai0000_Aux,
float f_ai0001_Aux,
float f_ai0002_Aux,
float f_ai0003_Aux,
float f_ai0004_Aux,
float f_ai0005_Aux,
- //
int32_t i32_Velocity,
int32_t i32_Acceleration,
int32_t i32_Jerk
) {
char StringText[MAX_CHAR_PER_LINE+ 1]; // don't forget the /0 (end of string)
-
static int64_t i64_Pulses_Prec;
-static uint32_t ms_0002_prec;
-
-static float f_ai0000_prec;
-static float f_ai0001_prec;
-static float f_ai0002_prec;
-static float f_ai0003_prec;
-static float f_ai0004_prec;
static float f_ai0005_prec;
static float f_ai0006_prec;
-
+//static float f_ai0007_prec;
+static float f_ai0008_prec;
static uint32_t i32_Velocity_prec;
-static uint32_t i32_Acceleration_prec;
-static uint32_t i32_Jerk_prec;
+static float f_ATVSpeed_prec;
if (i64_Pulses != i64_Pulses_Prec) {
sprintf (StringText,
@@ -398,44 +228,41 @@
if (i32_Velocity != i32_Velocity_prec) {
sprintf (StringText,
- "Velocity[ui/ms]: %d ", i32_Velocity); //, fVelocity);
- LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 3), StringText);
+ "Velocity[ui/ms]: %d ", i32_Velocity);
+ LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 2), StringText);
i32_Velocity_prec = i32_Velocity;
}
- if (out_PosizionatoreSW.i32_ATVSpeed != i32_Acceleration_prec) {
+ if (fMOT_VelocityAct != f_ATVSpeed_prec) {
sprintf (StringText,
- "ATVSpeed[ui/ms]: %d ", out_PosizionatoreSW.i32_ATVSpeed); //, fAcceleration);
+ "VAct[0..1]: %f ", fMOT_VelocityAct);
+ LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 3), StringText);
+ f_ATVSpeed_prec = fMOT_VelocityAct;
+ }
+ if (fMOT_VelocityReq != f_ai0008_prec) {
+ sprintf (StringText,
+ "VReq[0..1]: %f ", fMOT_VelocityReq);
LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 4), StringText);
- i32_Acceleration_prec = out_PosizionatoreSW.i32_ATVSpeed;
+ f_ai0008_prec = fMOT_VelocityReq;
}
if (f_PWMPercent != f_ai0005_prec) {
sprintf (StringText,
- "PID_FB Compute: %f ", f_PWMPercent); //, fJerk);
+ "PWM [0..1]: %f ", f_PWMPercent);
LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 5), StringText);
f_ai0005_prec = f_PWMPercent;
}
- if (fPID_Error != f_ai0006_prec) {
- sprintf (StringText,
- "PID_FB Error: %f ", fPID_Error); //, fJerk);
- LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 6), StringText);
- f_ai0006_prec = fPID_Error;
- }
-
-/*
- if (i32_Acceleration != i32_Acceleration_prec) {
+ if (fMOT_VelocityError != f_ai0006_prec) {
sprintf (StringText,
- "Acc[ui/10ms^2]: %d ", i32_Acceleration); //, fAcceleration);
-// LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 12), StringText);
- LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 4), StringText);
- i32_Acceleration_prec = i32_Acceleration;
+ "Err [0..1): %f ", fMOT_VelocityError);
+ LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 6), StringText);
+ f_ai0006_prec = fMOT_VelocityError;
}
- if (i32_Jerk != i32_Jerk_prec) {
+/*
+ if (F_VelocityLoopFB != f_ai0007_prec) {
sprintf (StringText,
- "Jerk: %d ", i32_Jerk); //, fJerk);
-// LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 13), StringText);
- LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 5), StringText);
- i32_Jerk_prec = i32_Jerk;
+ "Corr[0..1]: %f ", F_VelocityLoopFB); //, fJerk);
+ LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 7), StringText);
+ f_ai0007_prec = F_VelocityLoopFB;
}
*/
}
@@ -460,7 +287,6 @@
}
static void MotionHandler (void) {
-//static int16_t i16_Index = 0;
static uint32_t ui32_PreviousStep_ms;
uint32_t ui32_ActualStepSampled_ms;
uint32_t ui32_PassedActual_ms;
@@ -528,36 +354,218 @@
i32_Jerk = (i32_Acceleration- i32_Acceleration_Prec); // LA: Jerk
i32_Acceleration_Prec = i32_Acceleration;
- fVelocity = (float) i32_Velocity * fPassedActual_sxs; // Velocity in [ui/s]
- fAcceleration = (float) i32_Acceleration * fPassedActual_sxs; // Acceleration in [ui/s^2]
- fJerk = (float) i32_Jerk * fPassedActual_sxs; // Jerk
+ fVelocity = (float)i32_Velocity * fPassedActual_sxs; // Velocity in [ui/s]
+ fAcceleration = (float)i32_Acceleration * fPassedActual_sxs; // Acceleration in [ui/s^2]
+ fJerk = (float)i32_Jerk * fPassedActual_sxs; // Jerk
+
+ // LA: Elaborazione dell'Uscita di Controllo della velocità
+ // ====================================================
+ //
+ // Verifica della velocità raggiunta.
+ // Calcolo dell'ERRORE.
+ // Elaborazione e applicazione della correzione.
+ //
- // LA: PID Compute Section
- // ===================
+ // LA: i32_Velocity, Velocità Attuale (Reale, rilevata)
+ // LA: out_PosizionatoreSW.i32_ATVSpeed, Velocità Richiesta (Elaborata in base al profilo)
+ //
+ // Si suppone che la velocità massima sia raggiungibile al massimo valore di f_PWMPercent (-> 1.0)
+ // Si suppone che la velocità minima sia raggiungibile al minimo valore di f_PWMPercent (-> 0.0)
//
+
+ // LA: Calcolo dell'Errore scalato 0..1
+ //
+ fMOT_VelocityAct = ((float)abs(i32_Velocity)/ in_PosizionatoreSW.f_Max_Speed); // 0.0= 0%, 1.0= 100%
+ fMOT_VelocityReq = (out_PosizionatoreSW.f_ATVSpeed/ in_PosizionatoreSW.f_Max_Speed); // 0.0= 0%, 1.0= 100%
+ fMOT_VelocityError = fMOT_VelocityReq- fMOT_VelocityAct; //
+
+// f_PWMPercent = out_PosizionatoreSW.f_ATVSpeed/ in_PosizionatoreSW.f_Max_Speed; // LA: In Range (float) 0.. 1
if (out_PosizionatoreSW.b_ATVDirectionFW) {
- PID_VelocityClosedLoop_BW.reset();
+ F_VelocityLoopFB_BW = 0.0f;
+
+ f_PWMPercent = (out_PosizionatoreSW.f_ATVSpeed/ in_PosizionatoreSW.f_Max_Speed)/ 4.0f; // FW is about 4:1 BW
+ f_PWMPercent += (fMOT_VelocityError* 0.1f);
- // Update the process variable.
- PID_VelocityClosedLoop_FW.setProcessValue((float)i32_Velocity);
- // Set Desired Value
- PID_VelocityClosedLoop_FW.setSetPoint((float)out_PosizionatoreSW.i32_ATVSpeed);
- // Release a new output.
-
- fPID_Error = PID_VelocityClosedLoop_FW.getError();
- f_PWMPercent = PID_VelocityClosedLoop_FW.compute();
+ /*
+ if (fMOT_VelocityError > 0.001f)
+ F_VelocityLoopFB_FW += 0.0002f;
+ if (fMOT_VelocityError < -0.001f)
+ F_VelocityLoopFB_FW -= 0.0002f;
+ */
+ F_VelocityLoopFB_FW += (0.004f* fMOT_VelocityError);
+ f_PWMPercent += F_VelocityLoopFB_FW;
}
else {
- PID_VelocityClosedLoop_FW.reset();
+ F_VelocityLoopFB_FW = 0.0f;
+
+ f_PWMPercent = (out_PosizionatoreSW.f_ATVSpeed/ in_PosizionatoreSW.f_Max_Speed)/ 1.4f;
+ f_PWMPercent += (fMOT_VelocityError* 0.1f);
+
+ /*
+ if (fMOT_VelocityError > 0.001f)
+ F_VelocityLoopFB_BW += 0.0005f;
+ if (fMOT_VelocityError < -0.001f)
+ F_VelocityLoopFB_BW -= 0.0005f;
+ */
+ F_VelocityLoopFB_BW += (0.01f* fMOT_VelocityError);
+ f_PWMPercent += F_VelocityLoopFB_BW;
+ }
+ if (f_PWMPercent > 1.0f)
+ f_PWMPercent = 1.0f;
+ if (f_PWMPercent < 0.0f)
+ f_PWMPercent = 0.0f;
+ PWM_PB3.write((float) 1.0- f_PWMPercent); // Set to x%
+}
+
+// =======
+// =======
+// Main(s)
+// =======
+// =======
+//
+int main (void){
+ rDIR_FWD = true; // LA: Actuator Extends
+ rENA_Off = true; // LA: Drive Power is Off
+ //
+ in_PosizionatoreSW.b_ServoLock = false;
+ in_PosizionatoreSW.rtServoLock_Q = false;
+
+ EepromInit(); // LA: Inizializza la EEProm
+ TimersInit(); // LA: Parte il Timer a 1ms
+
+ // LA: FactoryReset se "userButton" premuto all'avvio
+ //
+ if (userButton == 0) {
+ FactoryReset();
+ }
+ DisplayDriverInit();
+
+ SampleTimer.attach(&SampleAndStore, cf_SCOPeriod_s); // LA: Avvia l'OscilloScopio con TimeBase x [s]
+ MotionTimer.attach(&MotionHandler, cf_MOTPeriod_s); // LA: Avvia il Motion con TimeBase x [s]
+ PWM_PB3.period(cf_PWMPeriod_s); // LA: Avvia il PWM con TimeBase x [s]
+ PWM_PB3.write(0.0f); // Set to 0%
+
+// LA: Ok
+
+ // LA: Motion (1st) Setup
+ //
+ in_PosizionatoreSW.b_AxisPowered = true;
+ in_PosizionatoreSW.b_ACPos_Homed = true;
+ in_PosizionatoreSW.f_Max_Speed = 20.0f; // [ui/ms]
+ in_PosizionatoreSW.f_ZeroSpeed = 0.0f; //
+
+ // POS Mode
+ // ========
+ //
+// in_PosizionatoreSW.b_ServoLock = true;
+// in_PosizionatoreSW.rtServoLock_Q = false;
+ //
+ in_PosizionatoreSW.i64_TargetPosition = ci64_TargetPOS; // [ui]
+ in_PosizionatoreSW.i64_ActualPosition = Stabilus322699.getPulses(); //
+ in_PosizionatoreSW.i64_AccelerationWindow = 64; // LA: Spazio concesso alla rampa di Accelerazione
+ in_PosizionatoreSW.i64_DecelerationWindow = 1024; // Spazio concesso alla rampa di Decelerazione
+ in_PosizionatoreSW.i64_diToleranceWindow = 16; // Finestra di Tolleranza
+ //
+ in_PosizionatoreSW.f_MaximumSpeed_x100_FW = 100.0f; // % of "i32_Max_Speed"
+ in_PosizionatoreSW.f_MaximumSpeed_x100_BW = 100.0f; //
+// in_PosizionatoreSW.f_ServoLockSpeed_x100_FW = 16.0f; // Riferimento di velocità minima a cui (appena) si muove l'asse [verso FW]
+// in_PosizionatoreSW.f_ServoLockSpeed_x100_BW = 24.0f; // Riferimento di velocità minima a cui (appena) si muove l'asse [verso BW]
+ in_PosizionatoreSW.f_ServoLockSpeed_x100_FW = 24.0f; // Riferimento di velocità minima a cui (appena) si muove l'asse [verso FW]
+ in_PosizionatoreSW.f_ServoLockSpeed_x100_BW = 24.0f; // Riferimento di velocità minima a cui (appena) si muove l'asse [verso BW]
+
+// LA: Ok
- // Update the process variable.
- PID_VelocityClosedLoop_BW.setProcessValue((float)i32_Velocity);
- // Set Desired Value
- PID_VelocityClosedLoop_BW.setSetPoint((float)out_PosizionatoreSW.i32_ATVSpeed);
- // Release a new output.
+ // JOG Mode
+ // ========
+ //
+ in_PosizionatoreSW.b_JogMode = false;
+ in_PosizionatoreSW.b_JogFW = false;
+ in_PosizionatoreSW.b_JogBW = false;
+ in_PosizionatoreSW.i32_JogAccel_ms = 500; // [ms]
+ in_PosizionatoreSW.i32_JogDecel_ms = 250; //
+ //
+ in_PosizionatoreSW.f_JogSpeed_x100_FW = (in_PosizionatoreSW.f_MaximumSpeed_x100_FW/ 2); // LA: JOG's the Half of Max POS's Speed
+ in_PosizionatoreSW.f_JogSpeed_x100_BW = (in_PosizionatoreSW.f_MaximumSpeed_x100_BW/ 2); //
+
+// LA: Ok
+
+ // LA: Color RGB Component(s)
+ // ======================
+ //
+ // RED 0000 1000 0000 0000 min 0x0800 02048
+ // 1111 1000 0000 0000 max 0xf800 63488
+ //
+ // GREEN 0000 0000 0010 0000 min 0x0020 00032
+ // 0000 0111 1110 0000 max 0x07e0 02016
+ //
+ // BLUE 0000 0000 0000 0001 min 0x0001 00001
+ // 0000 0000 0001 1111 max 0x001f 00031
+ //
+ // La componente ROSSA ha 5 bit di escursione (0.. 31),
+ // La componente VERDE ha 6 bit di escursione (0.. 63),
+ // La componente BLU ha 5 bit di escursione (0.. 31),
+ //
+ // Le componenti RGB di "Color" sono quindi scritte negli appropriati registri come segue:
+ //
+ // writeReg(RED, (Color & 0xf800) >> 11);
+ // writeReg(GREEN, (Color & 0x07e0) >> 5);
+ // writeReg(BLUE, (Color & 0x001f));
+ //
+ LCM_SetTextColor(Scale2RGBColor (0, 0, 0), Scale2RGBColor (31, 0, 0)); // LA: Red on Black
+ LCM_ClearScreen (Scale2RGBColor (0, 0, 0)); // Black Background
+ LCM_DrawString (0, 0+ (TEXT_ROW_SPACING* 0), "You Start Me Up ..."); // Intro Text
+
+// rDIR_FWD = false; // Collapse
+// rENA_Off = false; // Power On
+
+ in_PosizionatoreSW.b_ServoLock = true;
+ in_PosizionatoreSW.rtServoLock_Q = true;
- fPID_Error = PID_VelocityClosedLoop_BW.getError();
- f_PWMPercent = PID_VelocityClosedLoop_BW.compute();
+// LA: Ok
+
+ while (1) {
+
+// LA: Ok
+
+ // LA: Scope, Theory of operation.
+ // ===========================
+ //
+ // 1) Sample a Value @ any Step
+ // 2) Store @ the correct ms
+ // 3) Plot the current Section of the Sampling Vector
+ //
+ LCM_ShowTactics (
+ Stabilus322699.getPulses(), // Row 1
+
+ out_PosizionatoreSW.f_ATVSpeed, // 3
+ adc_temp.read(), // 4
+ adc_vbat.read(), // 5
+ adc_vref.read(), // 6
+
+ ADC12_IN9.read(), // 8
+ ADC12_IN15.read(), // 9
+ (f_PWMPercent* 100), // 10
+
+ i32_Velocity, // 11
+ i32_Acceleration, // 12
+ i32_Jerk // 13
+
+ );
+
+ LCM_PlotScope (
+ Scale2RGBColor (0, 0, 0), // Back: Black
+ Scale2RGBColor (31, 0, 0) // Fore: Red
+ );
+
+ LCM_PlotSpeed (
+ Scale2RGBColor (0, 0, 0), // Back: Black
+ Scale2RGBColor (31, 0, 0) // Fore: Red
+ );
+
+ if (out_PosizionatoreSW.b_InPosition)
+ if (in_PosizionatoreSW.i64_TargetPosition > 0)
+ in_PosizionatoreSW.i64_TargetPosition = 0;
+ else
+ in_PosizionatoreSW.i64_TargetPosition = ci64_TargetPOS;
}
- PWM_PB3.write((float) 1.0- f_PWMPercent); // Set to x%
-}
+}
\ No newline at end of file