Luca Antolini
Velocity Closed Loop Dynamic error correction
Dependencies: mbed QEI PID DmTftLibraryEx
SWPos/SWPos.cpp@39:be7055a0e9a4, 2022-04-15 (annotated)
- Committer:
- lex9296
- Date:
- Fri Apr 15 07:01:02 2022 +0000
- Revision:
- 39:be7055a0e9a4
- Parent:
- 33:f77aa3ecf87d
Retroazione di velocita e correzione
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| lex9296 | 23:b9d23a2f390e | 1 | |
| lex9296 | 23:b9d23a2f390e | 2 | /* LA: Theory of Operation. |
| lex9296 | 23:b9d23a2f390e | 3 | // ======================== |
| lex9296 | 23:b9d23a2f390e | 4 | // |
| lex9296 | 23:b9d23a2f390e | 5 | // once Encoder/Axis is Homed and Cycle Condition's Sussist, AC_Pos_Positioning is alloweed to take Control. |
| lex9296 | 23:b9d23a2f390e | 6 | // This is done by keeping "STW1_Control" true. |
| lex9296 | 23:b9d23a2f390e | 7 | // |
| lex9296 | 23:b9d23a2f390e | 8 | // if Servolock is off, The Axis is then Set to "Free Wheel Axis" |
| lex9296 | 23:b9d23a2f390e | 9 | // Otherwise, the System Keeps Actual Position (ServLock works inside the Tolerance window, if there's move), |
| lex9296 | 23:b9d23a2f390e | 10 | // or a New Move is Started if Target is != Actual Position. |
| lex9296 | 23:b9d23a2f390e | 11 | // |
| lex9296 | 23:b9d23a2f390e | 12 | // if ServoLock is true and movement is alloweed, after the positioning will have Actual = Target, Tolerance |
| lex9296 | 23:b9d23a2f390e | 13 | // flags active and a constant control to keep this. |
| lex9296 | 23:b9d23a2f390e | 14 | // |
| lex9296 | 23:b9d23a2f390e | 15 | // if "Axis" is Not Homed, All Positioning Flag(s) are kept clear |
| lex9296 | 23:b9d23a2f390e | 16 | */ |
| lex9296 | 23:b9d23a2f390e | 17 | |
| lex9296 | 23:b9d23a2f390e | 18 | /* LA: Verifica di coerenza del Profilo: |
| lex9296 | 23:b9d23a2f390e | 19 | // ================================= |
| lex9296 | 23:b9d23a2f390e | 20 | // |
| lex9296 | 23:b9d23a2f390e | 21 | // Se Lo spazio di Accelerazione sommato a quello di Decelerazione, ad INIZIO MOVIMENTO, supera la distanza (in Modulo) da percorrere, |
| lex9296 | 23:b9d23a2f390e | 22 | // allora il profilo và corretto (Non è più un trapezio ma diventa un triangolo). |
| lex9296 | 23:b9d23a2f390e | 23 | // |
| lex9296 | 23:b9d23a2f390e | 24 | // Per correggere il profilo occorre: |
| lex9296 | 23:b9d23a2f390e | 25 | // |
| lex9296 | 23:b9d23a2f390e | 26 | // 1) Calcolare le equazioni di entrambe le rette (Accelerazione e Decelerazione) |
| lex9296 | 23:b9d23a2f390e | 27 | // 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à) |
| lex9296 | 23:b9d23a2f390e | 28 | // 3) Sostituire all'Accelerazione il valore "Ordinata"- Punto di Partenza (Verificando il segno dell'operazione in base al verso) |
| lex9296 | 23:b9d23a2f390e | 29 | // 4) Sostituire alla Decelerazione il valore Destinazione - "Ordinata" (Verificando il segno dell'operazione in base al verso) |
| lex9296 | 23:b9d23a2f390e | 30 | // 5) Sostituire alla Velocità di Movimento l'"Ascissa" |
| lex9296 | 23:b9d23a2f390e | 31 | // |
| lex9296 | 23:b9d23a2f390e | 32 | // Avendo già le equazioni di RettaxAcc e RettaxDec: |
| lex9296 | 23:b9d23a2f390e | 33 | // ================================================= |
| lex9296 | 23:b9d23a2f390e | 34 | // |
| lex9296 | 23:b9d23a2f390e | 35 | // RettaxAcc: Y = (m * X)+ q |
| lex9296 | 23:b9d23a2f390e | 36 | // RettaxDec: Y = (n * X)+ t |
| lex9296 | 23:b9d23a2f390e | 37 | // |
| lex9296 | 23:b9d23a2f390e | 38 | // 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. |
| lex9296 | 23:b9d23a2f390e | 39 | // Questo permetterà la corretta esecuzione del "Triangolo", col vertice alla "velocità massima calcolata" e rampe teoriche equivalenti a quelle programmate. |
| lex9296 | 23:b9d23a2f390e | 40 | // |
| lex9296 | 23:b9d23a2f390e | 41 | // **** |
| lex9296 | 23:b9d23a2f390e | 42 | // **** |
| lex9296 | 23:b9d23a2f390e | 43 | // **** |
| lex9296 | 23:b9d23a2f390e | 44 | // **** |
| lex9296 | 23:b9d23a2f390e | 45 | // |
| lex9296 | 23:b9d23a2f390e | 46 | // Se non ho il "tempo" per calcolarlo uso un trucco: |
| lex9296 | 23:b9d23a2f390e | 47 | // Se il profilo è da correggere (Acc+ Dec > Distance) carico in ActualSpeed la Velocità di Servolock e lascio che l'asse si muova alla "Minima". |
| lex9296 | 23:b9d23a2f390e | 48 | // |
| lex9296 | 33:f77aa3ecf87d | 49 | // xx/xx/2021: JOG Movement. |
| lex9296 | 33:f77aa3ecf87d | 50 | // Ho introdotto la modalità di JOG dell'asse, in cui le rampe, anzichè in Spazio son gestite a Tempo. |
| lex9296 | 33:f77aa3ecf87d | 51 | // PQM rilevo l'attuale tempo di scansione (tra una chiamata e l'altra del posizionatore) e adeguo gli inteventi sulla pendenza |
| lex9296 | 33:f77aa3ecf87d | 52 | // in maniera conseguente. |
| lex9296 | 33:f77aa3ecf87d | 53 | // Per usare il JOG non è necessario che l'home sia fatto, solo che l'asse sia abilitato. |
| lex9296 | 33:f77aa3ecf87d | 54 | // Il JOG ha prelazione sul Posizionamento: se è attivo il posizionamento è mutualmente escluso (Anche se Servolock è "TRUE"). |
| lex9296 | 33:f77aa3ecf87d | 55 | // |
| lex9296 | 33:f77aa3ecf87d | 56 | // 11/04/2022: L'individuazione del "Triangolo" ed i relativi calcoli sono STATICI, effettuati PRIMA dell'inizio del movimento. |
| lex9296 | 33:f77aa3ecf87d | 57 | // PQM le equazioni base vanno create usando i limiti TEORICI (MaxSpeed) non quelli realmente raggiunti (ActualSpeed). |
| lex9296 | 33:f77aa3ecf87d | 58 | // |
| lex9296 | 23:b9d23a2f390e | 59 | */ |
| lex9296 | 23:b9d23a2f390e | 60 | |
| lex9296 | 23:b9d23a2f390e | 61 | // LA: Includes |
| lex9296 | 23:b9d23a2f390e | 62 | #include "mbed.h" |
| lex9296 | 23:b9d23a2f390e | 63 | #include <stdio.h> |
| lex9296 | 23:b9d23a2f390e | 64 | #include <stdlib.h> |
| lex9296 | 23:b9d23a2f390e | 65 | |
| lex9296 | 23:b9d23a2f390e | 66 | #include "SWPos.h" |
| lex9296 | 23:b9d23a2f390e | 67 | #include "Timers.h" |
| lex9296 | 23:b9d23a2f390e | 68 | |
| lex9296 | 23:b9d23a2f390e | 69 | //in_sPosizionatoreSW in_PosizionatoreSW; |
| lex9296 | 23:b9d23a2f390e | 70 | //out_sPosizionatoreSW out_PosizionatoreSW; |
| lex9296 | 23:b9d23a2f390e | 71 | |
| lex9296 | 23:b9d23a2f390e | 72 | // LA: Basic Function's Integration |
| lex9296 | 23:b9d23a2f390e | 73 | void PosizionatoreSW (const in_sPosizionatoreSW &in, out_sPosizionatoreSW &out) { |
| lex9296 | 23:b9d23a2f390e | 74 | static bool InProgress = false; |
| lex9296 | 23:b9d23a2f390e | 75 | |
| lex9296 | 23:b9d23a2f390e | 76 | static int64_t i64_TargetPosition_Prec; |
| lex9296 | 39:be7055a0e9a4 | 77 | // |
| lex9296 | 23:b9d23a2f390e | 78 | static bool b_AuxCalculateProfile_003; |
| lex9296 | 23:b9d23a2f390e | 79 | static bool b_AuxCalculateProfile_002; |
| lex9296 | 23:b9d23a2f390e | 80 | static bool b_AuxCalculateProfile_001; |
| lex9296 | 23:b9d23a2f390e | 81 | static bool b_AuxCalculateProfile_000; |
| lex9296 | 39:be7055a0e9a4 | 82 | |
| lex9296 | 39:be7055a0e9a4 | 83 | static float f_ServoLockSpeed_FW; |
| lex9296 | 39:be7055a0e9a4 | 84 | static float f_ServoLockSpeed_BW; |
| lex9296 | 23:b9d23a2f390e | 85 | |
| lex9296 | 23:b9d23a2f390e | 86 | static double d_X1; |
| lex9296 | 23:b9d23a2f390e | 87 | static double d_X2; |
| lex9296 | 23:b9d23a2f390e | 88 | static double d_Y1; |
| lex9296 | 23:b9d23a2f390e | 89 | static double d_Y2; |
| lex9296 | 23:b9d23a2f390e | 90 | static double d_Y2_meno_Y1; |
| lex9296 | 23:b9d23a2f390e | 91 | static double d_X2_meno_X1; |
| lex9296 | 23:b9d23a2f390e | 92 | static double d_X2_per_Y1; |
| lex9296 | 23:b9d23a2f390e | 93 | static double d_X1_per_Y2; |
| lex9296 | 23:b9d23a2f390e | 94 | static double d_m; |
| lex9296 | 23:b9d23a2f390e | 95 | static double d_q; |
| lex9296 | 23:b9d23a2f390e | 96 | static double d_n; |
| lex9296 | 23:b9d23a2f390e | 97 | static double d_t; |
| lex9296 | 23:b9d23a2f390e | 98 | |
| lex9296 | 39:be7055a0e9a4 | 99 | static float f_ActualSpeed_Local; |
| lex9296 | 23:b9d23a2f390e | 100 | static int64_t i64_AccelerationWindow_Local; |
| lex9296 | 23:b9d23a2f390e | 101 | static int64_t i64_DecelerationWindow_Local; |
| lex9296 | 39:be7055a0e9a4 | 102 | |
| lex9296 | 39:be7055a0e9a4 | 103 | static float f_MaximumSpeed_FW; |
| lex9296 | 39:be7055a0e9a4 | 104 | static float f_MaximumSpeed_BW; |
| lex9296 | 39:be7055a0e9a4 | 105 | |
| lex9296 | 39:be7055a0e9a4 | 106 | static float f_MaximumSpeed_Local; |
| lex9296 | 23:b9d23a2f390e | 107 | static int64_t i64_StartPosition_Local; |
| lex9296 | 23:b9d23a2f390e | 108 | static int64_t i64_Distance_Local; |
| lex9296 | 23:b9d23a2f390e | 109 | static bool b_GoingFW; |
| lex9296 | 23:b9d23a2f390e | 110 | static bool b_GoingBW; |
| lex9296 | 23:b9d23a2f390e | 111 | static bool b_Accelerating_Local; |
| lex9296 | 23:b9d23a2f390e | 112 | static bool b_Decelerating_Local; |
| lex9296 | 23:b9d23a2f390e | 113 | static bool b_JogFW_Prec_Local; |
| lex9296 | 23:b9d23a2f390e | 114 | static bool b_JogBW_Prec_Local; |
| lex9296 | 39:be7055a0e9a4 | 115 | |
| lex9296 | 23:b9d23a2f390e | 116 | static int32_t i32_Aux_ms2Acc; |
| lex9296 | 23:b9d23a2f390e | 117 | static int32_t i32_Aux_ms2Dec; |
| lex9296 | 39:be7055a0e9a4 | 118 | |
| lex9296 | 23:b9d23a2f390e | 119 | static float f_Aux_AccelAnyms; |
| lex9296 | 23:b9d23a2f390e | 120 | static float f_Aux_DecelAnyms; |
| lex9296 | 23:b9d23a2f390e | 121 | static float f_MaximumJogSpeed_xW; |
| lex9296 | 23:b9d23a2f390e | 122 | |
| lex9296 | 23:b9d23a2f390e | 123 | static uint32_t ui32_PreviousStep_ms_Local; |
| lex9296 | 39:be7055a0e9a4 | 124 | |
| lex9296 | 23:b9d23a2f390e | 125 | uint32_t ui32_ActualStepSampled_ms_Local; |
| lex9296 | 23:b9d23a2f390e | 126 | uint32_t ui32_PassedActual_ms_Local; |
| lex9296 | 23:b9d23a2f390e | 127 | |
| lex9296 | 23:b9d23a2f390e | 128 | if (InProgress) |
| lex9296 | 23:b9d23a2f390e | 129 | return; |
| lex9296 | 23:b9d23a2f390e | 130 | else { |
| lex9296 | 23:b9d23a2f390e | 131 | InProgress = true; |
| lex9296 | 23:b9d23a2f390e | 132 | |
| lex9296 | 23:b9d23a2f390e | 133 | // LA: Generazione del millisecondo Attuale |
| lex9296 | 23:b9d23a2f390e | 134 | // ==================================== |
| lex9296 | 23:b9d23a2f390e | 135 | // |
| lex9296 | 23:b9d23a2f390e | 136 | // Invoca il timer di sistema (TimersTimerValue) e lo confronta col suo precedente. |
| lex9296 | 23:b9d23a2f390e | 137 | // Una volta elaborato e "scevrato" l'eventuale "Rollover" la sezione ritorna "ui32_PassedActual_ms_Local". |
| lex9296 | 23:b9d23a2f390e | 138 | // "ui32_PassedActual_ms_Local" rappresenta i [ms] passati tra una chiamata e l'altra del Posizionatore SW. |
| lex9296 | 23:b9d23a2f390e | 139 | // |
| lex9296 | 23:b9d23a2f390e | 140 | ui32_ActualStepSampled_ms_Local = TimersTimerValue(); // Freezes the Actual Sample. |
| lex9296 | 23:b9d23a2f390e | 141 | if (ui32_ActualStepSampled_ms_Local >= ui32_PreviousStep_ms_Local) |
| lex9296 | 23:b9d23a2f390e | 142 | ui32_PassedActual_ms_Local = (ui32_ActualStepSampled_ms_Local- ui32_PreviousStep_ms_Local); // Result => Actual- Previous |
| lex9296 | 23:b9d23a2f390e | 143 | else |
| lex9296 | 23:b9d23a2f390e | 144 | ui32_PassedActual_ms_Local = ui32_ActualStepSampled_ms_Local+ (0x7fffffff- ui32_PreviousStep_ms_Local); // Result => Actual+ (Rollover- Previous) |
| lex9296 | 23:b9d23a2f390e | 145 | // |
| lex9296 | 23:b9d23a2f390e | 146 | ui32_PreviousStep_ms_Local = ui32_ActualStepSampled_ms_Local; // Store(s)&Hold(s) actual msSample |
| lex9296 | 23:b9d23a2f390e | 147 | |
| lex9296 | 23:b9d23a2f390e | 148 | // LA: Test pourposes ... |
| lex9296 | 23:b9d23a2f390e | 149 | // |
| lex9296 | 23:b9d23a2f390e | 150 | out.ui32_PreviousStep_ms = ui32_PreviousStep_ms_Local; |
| lex9296 | 23:b9d23a2f390e | 151 | out.ui32_ActualStepSampled_ms = ui32_ActualStepSampled_ms_Local; |
| lex9296 | 23:b9d23a2f390e | 152 | out.ui32_PassedActual_ms = ui32_PassedActual_ms_Local; |
| lex9296 | 23:b9d23a2f390e | 153 | |
| lex9296 | 23:b9d23a2f390e | 154 | // LA: Valutazione della Distanza (Rimanente) |
| lex9296 | 23:b9d23a2f390e | 155 | // ====================================== |
| lex9296 | 23:b9d23a2f390e | 156 | // |
| lex9296 | 23:b9d23a2f390e | 157 | if (in.i64_ActualPosition > in.i64_TargetPosition) |
| lex9296 | 23:b9d23a2f390e | 158 | i64_Distance_Local = (in.i64_ActualPosition- in.i64_TargetPosition); |
| lex9296 | 23:b9d23a2f390e | 159 | else |
| lex9296 | 23:b9d23a2f390e | 160 | i64_Distance_Local = (in.i64_TargetPosition- in.i64_ActualPosition); |
| lex9296 | 23:b9d23a2f390e | 161 | |
| lex9296 | 23:b9d23a2f390e | 162 | // LA: Entering SWPositioner |
| lex9296 | 23:b9d23a2f390e | 163 | // ===================== |
| lex9296 | 23:b9d23a2f390e | 164 | // |
| lex9296 | 23:b9d23a2f390e | 165 | if (in.b_AxisPowered) { |
| lex9296 | 23:b9d23a2f390e | 166 | if (in.b_JogMode) { |
| lex9296 | 23:b9d23a2f390e | 167 | |
| lex9296 | 23:b9d23a2f390e | 168 | // JOG Mode Engaged |
| lex9296 | 23:b9d23a2f390e | 169 | // |
| lex9296 | 23:b9d23a2f390e | 170 | if (in.b_JogFW) { |
| lex9296 | 23:b9d23a2f390e | 171 | if (!b_JogFW_Prec_Local) { |
| lex9296 | 23:b9d23a2f390e | 172 | |
| lex9296 | 23:b9d23a2f390e | 173 | // JOG Mode FW "Just" Engaged |
| lex9296 | 23:b9d23a2f390e | 174 | // |
| lex9296 | 23:b9d23a2f390e | 175 | b_JogFW_Prec_Local = in.b_JogFW; |
| lex9296 | 23:b9d23a2f390e | 176 | i32_Aux_ms2Acc = in.i32_JogAccel_ms; |
| lex9296 | 23:b9d23a2f390e | 177 | // |
| lex9296 | 39:be7055a0e9a4 | 178 | f_MaximumJogSpeed_xW = (((in.f_JogSpeed_x100_FW)* (in.f_Max_Speed- f_ActualSpeed_Local)/ 100.0f)); // LA: Speed to be Reached |
| lex9296 | 39:be7055a0e9a4 | 179 | f_Aux_AccelAnyms = (f_MaximumJogSpeed_xW/ (float)in.i32_JogAccel_ms); // LA: Any ms Increment o'Speed |
| lex9296 | 39:be7055a0e9a4 | 180 | |
| lex9296 | 23:b9d23a2f390e | 181 | b_Accelerating_Local = true; |
| lex9296 | 23:b9d23a2f390e | 182 | b_Decelerating_Local = false; |
| lex9296 | 23:b9d23a2f390e | 183 | out.b_MaxSpeedReached = false; |
| lex9296 | 23:b9d23a2f390e | 184 | } |
| lex9296 | 23:b9d23a2f390e | 185 | |
| lex9296 | 23:b9d23a2f390e | 186 | // JOG Move FW |
| lex9296 | 23:b9d23a2f390e | 187 | // |
| lex9296 | 23:b9d23a2f390e | 188 | if (i32_Aux_ms2Acc > 0) { |
| lex9296 | 39:be7055a0e9a4 | 189 | i32_Aux_ms2Acc = (i32_Aux_ms2Acc- ui32_PassedActual_ms_Local); // LA: Ms Passed @ this Trip |
| lex9296 | 39:be7055a0e9a4 | 190 | f_ActualSpeed_Local = ((float)(in.i32_JogAccel_ms- i32_Aux_ms2Acc)* f_Aux_AccelAnyms); // LA: Acc Checkpoint |
| lex9296 | 23:b9d23a2f390e | 191 | } |
| lex9296 | 23:b9d23a2f390e | 192 | else { |
| lex9296 | 39:be7055a0e9a4 | 193 | f_ActualSpeed_Local = ((in.f_JogSpeed_x100_FW)* (in.f_Max_Speed- in.f_ZeroSpeed)/ 100.0f); // LA: Maximum Speed Reached |
| lex9296 | 23:b9d23a2f390e | 194 | |
| lex9296 | 23:b9d23a2f390e | 195 | b_Accelerating_Local = false; |
| lex9296 | 23:b9d23a2f390e | 196 | b_Decelerating_Local = false; |
| lex9296 | 23:b9d23a2f390e | 197 | out.b_MaxSpeedReached = true; |
| lex9296 | 23:b9d23a2f390e | 198 | } |
| lex9296 | 23:b9d23a2f390e | 199 | b_GoingFW = true; // LA: Moves ... |
| lex9296 | 23:b9d23a2f390e | 200 | b_GoingBW = false; // |
| lex9296 | 23:b9d23a2f390e | 201 | } |
| lex9296 | 23:b9d23a2f390e | 202 | else { |
| lex9296 | 23:b9d23a2f390e | 203 | if (b_JogFW_Prec_Local) { |
| lex9296 | 23:b9d23a2f390e | 204 | if (!b_Decelerating_Local) { |
| lex9296 | 23:b9d23a2f390e | 205 | |
| lex9296 | 23:b9d23a2f390e | 206 | // JOG Mode FW "Just" Released |
| lex9296 | 23:b9d23a2f390e | 207 | // |
| lex9296 | 23:b9d23a2f390e | 208 | i32_Aux_ms2Dec = in.i32_JogDecel_ms; |
| lex9296 | 39:be7055a0e9a4 | 209 | f_MaximumJogSpeed_xW = (((in.f_JogSpeed_x100_FW)* (float)(f_ActualSpeed_Local- in.f_ZeroSpeed)/ 100)); // LA: Speed to be Reached |
| lex9296 | 23:b9d23a2f390e | 210 | f_Aux_DecelAnyms = (f_MaximumJogSpeed_xW/ (float)in.i32_JogDecel_ms); // LA: Any ms Increment o'Speed |
| lex9296 | 23:b9d23a2f390e | 211 | |
| lex9296 | 23:b9d23a2f390e | 212 | b_Accelerating_Local = false; |
| lex9296 | 23:b9d23a2f390e | 213 | b_Decelerating_Local = true; |
| lex9296 | 23:b9d23a2f390e | 214 | out.b_MaxSpeedReached = false; |
| lex9296 | 23:b9d23a2f390e | 215 | } |
| lex9296 | 23:b9d23a2f390e | 216 | |
| lex9296 | 23:b9d23a2f390e | 217 | // JOG Move FW, Decelerating to Zero |
| lex9296 | 23:b9d23a2f390e | 218 | // |
| lex9296 | 23:b9d23a2f390e | 219 | if (i32_Aux_ms2Dec > 0) { |
| lex9296 | 39:be7055a0e9a4 | 220 | i32_Aux_ms2Dec = (i32_Aux_ms2Dec- ui32_PassedActual_ms_Local); // LA: Ms Passed @ this Trip |
| lex9296 | 39:be7055a0e9a4 | 221 | f_ActualSpeed_Local = (f_MaximumJogSpeed_xW- (float)(in.i32_JogDecel_ms- i32_Aux_ms2Dec)* f_Aux_DecelAnyms); // LA: Dec Checkpoint |
| lex9296 | 23:b9d23a2f390e | 222 | |
| lex9296 | 39:be7055a0e9a4 | 223 | b_GoingFW = true; // LA: Moves ... |
| lex9296 | 39:be7055a0e9a4 | 224 | b_GoingBW = false; // |
| lex9296 | 23:b9d23a2f390e | 225 | } |
| lex9296 | 23:b9d23a2f390e | 226 | else { |
| lex9296 | 39:be7055a0e9a4 | 227 | f_ActualSpeed_Local = in.f_ZeroSpeed; // LA: Zero Speed Reached |
| lex9296 | 23:b9d23a2f390e | 228 | |
| lex9296 | 23:b9d23a2f390e | 229 | b_Accelerating_Local = false; |
| lex9296 | 23:b9d23a2f390e | 230 | b_Decelerating_Local = false; |
| lex9296 | 23:b9d23a2f390e | 231 | out.b_MaxSpeedReached = false; |
| lex9296 | 23:b9d23a2f390e | 232 | // |
| lex9296 | 39:be7055a0e9a4 | 233 | b_JogFW_Prec_Local = false; // LA: Move is Terminated, NOW |
| lex9296 | 39:be7055a0e9a4 | 234 | b_GoingFW = false; // |
| lex9296 | 39:be7055a0e9a4 | 235 | b_GoingBW = false; // |
| lex9296 | 23:b9d23a2f390e | 236 | } |
| lex9296 | 23:b9d23a2f390e | 237 | } |
| lex9296 | 23:b9d23a2f390e | 238 | } |
| lex9296 | 23:b9d23a2f390e | 239 | |
| lex9296 | 23:b9d23a2f390e | 240 | if (in.b_JogBW) { |
| lex9296 | 23:b9d23a2f390e | 241 | if (!b_JogBW_Prec_Local) { |
| lex9296 | 23:b9d23a2f390e | 242 | |
| lex9296 | 23:b9d23a2f390e | 243 | // JOG Mode BW "Just" Engaged |
| lex9296 | 23:b9d23a2f390e | 244 | // |
| lex9296 | 23:b9d23a2f390e | 245 | b_JogBW_Prec_Local = in.b_JogBW; |
| lex9296 | 23:b9d23a2f390e | 246 | i32_Aux_ms2Acc = in.i32_JogAccel_ms; |
| lex9296 | 23:b9d23a2f390e | 247 | // |
| lex9296 | 39:be7055a0e9a4 | 248 | f_MaximumJogSpeed_xW = (((in.f_JogSpeed_x100_BW)* (in.f_Max_Speed- f_ActualSpeed_Local)/ 100.0f)); // LA: Speed to be Reached |
| lex9296 | 39:be7055a0e9a4 | 249 | f_Aux_AccelAnyms = (f_MaximumJogSpeed_xW/ (float)in.i32_JogAccel_ms); // LA: Any ms Increment o'Speed |
| lex9296 | 23:b9d23a2f390e | 250 | |
| lex9296 | 23:b9d23a2f390e | 251 | b_Accelerating_Local = true; |
| lex9296 | 23:b9d23a2f390e | 252 | b_Decelerating_Local = false; |
| lex9296 | 23:b9d23a2f390e | 253 | out.b_MaxSpeedReached = false; |
| lex9296 | 23:b9d23a2f390e | 254 | } |
| lex9296 | 23:b9d23a2f390e | 255 | |
| lex9296 | 23:b9d23a2f390e | 256 | // JOG Move BW |
| lex9296 | 23:b9d23a2f390e | 257 | // |
| lex9296 | 23:b9d23a2f390e | 258 | if (i32_Aux_ms2Acc > 0) { |
| lex9296 | 39:be7055a0e9a4 | 259 | i32_Aux_ms2Acc = (i32_Aux_ms2Acc- ui32_PassedActual_ms_Local); // LA: Ms Passed @ this Trip |
| lex9296 | 39:be7055a0e9a4 | 260 | f_ActualSpeed_Local = ((float)(in.i32_JogAccel_ms- i32_Aux_ms2Acc)* f_Aux_AccelAnyms); // LA: Acc Checkpoint |
| lex9296 | 23:b9d23a2f390e | 261 | } |
| lex9296 | 23:b9d23a2f390e | 262 | else { |
| lex9296 | 39:be7055a0e9a4 | 263 | f_ActualSpeed_Local = ((in.f_JogSpeed_x100_BW)* (in.f_Max_Speed- in.f_ZeroSpeed)/ 100.0f); // LA: Maximum Speed Reached |
| lex9296 | 39:be7055a0e9a4 | 264 | |
| lex9296 | 39:be7055a0e9a4 | 265 | b_Accelerating_Local = false; |
| lex9296 | 39:be7055a0e9a4 | 266 | b_Decelerating_Local = false; |
| lex9296 | 39:be7055a0e9a4 | 267 | out.b_MaxSpeedReached = true; |
| lex9296 | 23:b9d23a2f390e | 268 | } |
| lex9296 | 23:b9d23a2f390e | 269 | |
| lex9296 | 23:b9d23a2f390e | 270 | b_GoingBW = true; // LA: Moves ... |
| lex9296 | 23:b9d23a2f390e | 271 | b_GoingFW = false; // |
| lex9296 | 23:b9d23a2f390e | 272 | } |
| lex9296 | 23:b9d23a2f390e | 273 | else { |
| lex9296 | 23:b9d23a2f390e | 274 | if (b_JogBW_Prec_Local) { |
| lex9296 | 23:b9d23a2f390e | 275 | if (!b_Decelerating_Local) { |
| lex9296 | 23:b9d23a2f390e | 276 | |
| lex9296 | 23:b9d23a2f390e | 277 | // JOG Mode BW "Just" Released |
| lex9296 | 23:b9d23a2f390e | 278 | // |
| lex9296 | 23:b9d23a2f390e | 279 | i32_Aux_ms2Dec = in.i32_JogDecel_ms; |
| lex9296 | 39:be7055a0e9a4 | 280 | f_MaximumJogSpeed_xW = (((in.f_JogSpeed_x100_BW)* (f_ActualSpeed_Local- in.f_ZeroSpeed)/ 100.0f)); // LA: Speed to be Reached |
| lex9296 | 39:be7055a0e9a4 | 281 | f_Aux_DecelAnyms = (f_MaximumJogSpeed_xW/ (float)(in.i32_JogDecel_ms)); // LA: Any ms Increment o'Speed |
| lex9296 | 39:be7055a0e9a4 | 282 | |
| lex9296 | 23:b9d23a2f390e | 283 | b_Accelerating_Local = false; |
| lex9296 | 23:b9d23a2f390e | 284 | b_Decelerating_Local = true; |
| lex9296 | 23:b9d23a2f390e | 285 | out.b_MaxSpeedReached = false; |
| lex9296 | 23:b9d23a2f390e | 286 | } |
| lex9296 | 23:b9d23a2f390e | 287 | |
| lex9296 | 23:b9d23a2f390e | 288 | // JOG Move FW, Decelerating to Zero |
| lex9296 | 23:b9d23a2f390e | 289 | // |
| lex9296 | 23:b9d23a2f390e | 290 | if (i32_Aux_ms2Dec > 0) { |
| lex9296 | 39:be7055a0e9a4 | 291 | i32_Aux_ms2Dec = (i32_Aux_ms2Dec- ui32_PassedActual_ms_Local); // LA: Ms Passed @ this Trip |
| lex9296 | 39:be7055a0e9a4 | 292 | f_ActualSpeed_Local = (f_MaximumJogSpeed_xW- (float)(in.i32_JogDecel_ms- i32_Aux_ms2Dec)* f_Aux_DecelAnyms); // LA: Dec Checkpoint |
| lex9296 | 23:b9d23a2f390e | 293 | |
| lex9296 | 39:be7055a0e9a4 | 294 | b_GoingBW = true; // LA: Moves ... |
| lex9296 | 39:be7055a0e9a4 | 295 | b_GoingFW = false; // |
| lex9296 | 23:b9d23a2f390e | 296 | } |
| lex9296 | 23:b9d23a2f390e | 297 | else { |
| lex9296 | 39:be7055a0e9a4 | 298 | f_ActualSpeed_Local = in.f_ZeroSpeed; // LA: Zero Speed Reached |
| lex9296 | 23:b9d23a2f390e | 299 | |
| lex9296 | 23:b9d23a2f390e | 300 | b_Accelerating_Local = false; |
| lex9296 | 23:b9d23a2f390e | 301 | b_Decelerating_Local = false; |
| lex9296 | 23:b9d23a2f390e | 302 | out.b_MaxSpeedReached = false; |
| lex9296 | 23:b9d23a2f390e | 303 | // |
| lex9296 | 39:be7055a0e9a4 | 304 | b_JogBW_Prec_Local = false; // LA: Move is Terminated, NOW |
| lex9296 | 39:be7055a0e9a4 | 305 | b_GoingBW = false; // |
| lex9296 | 39:be7055a0e9a4 | 306 | b_GoingFW = false; // |
| lex9296 | 23:b9d23a2f390e | 307 | } |
| lex9296 | 23:b9d23a2f390e | 308 | } |
| lex9296 | 23:b9d23a2f390e | 309 | } |
| lex9296 | 23:b9d23a2f390e | 310 | out.b_Accelerating = b_Accelerating_Local; |
| lex9296 | 23:b9d23a2f390e | 311 | out.b_Decelerating = b_Decelerating_Local; |
| lex9296 | 23:b9d23a2f390e | 312 | } |
| lex9296 | 23:b9d23a2f390e | 313 | |
| lex9296 | 23:b9d23a2f390e | 314 | else { |
| lex9296 | 23:b9d23a2f390e | 315 | // !in.b_JogMode |
| lex9296 | 23:b9d23a2f390e | 316 | |
| lex9296 | 23:b9d23a2f390e | 317 | // JOG Mode NOT Engaged |
| lex9296 | 23:b9d23a2f390e | 318 | // Axis Powered |
| lex9296 | 23:b9d23a2f390e | 319 | // |
| lex9296 | 23:b9d23a2f390e | 320 | b_JogFW_Prec_Local = false; |
| lex9296 | 23:b9d23a2f390e | 321 | b_JogBW_Prec_Local = false; |
| lex9296 | 23:b9d23a2f390e | 322 | |
| lex9296 | 23:b9d23a2f390e | 323 | if (in.b_ACPos_Homed) { |
| lex9296 | 23:b9d23a2f390e | 324 | if ( |
| lex9296 | 23:b9d23a2f390e | 325 | (in.rtServoLock_Q && (in.i64_TargetPosition != in.i64_ActualPosition)) || |
| lex9296 | 23:b9d23a2f390e | 326 | (in.b_ServoLock && (in.i64_TargetPosition != i64_TargetPosition_Prec)) |
| lex9296 | 23:b9d23a2f390e | 327 | ) { |
| lex9296 | 23:b9d23a2f390e | 328 | |
| lex9296 | 23:b9d23a2f390e | 329 | // LA: An Issue to the Motion to Start is then Present & Valid |
| lex9296 | 23:b9d23a2f390e | 330 | // |
| lex9296 | 23:b9d23a2f390e | 331 | i64_TargetPosition_Prec = in.i64_TargetPosition; |
| lex9296 | 23:b9d23a2f390e | 332 | i64_StartPosition_Local = in.i64_ActualPosition; |
| lex9296 | 23:b9d23a2f390e | 333 | |
| lex9296 | 23:b9d23a2f390e | 334 | // wAccelerationWindow è già la Finestra di Accelerazione |
| lex9296 | 23:b9d23a2f390e | 335 | // wDecelerationWindow è già la Finestra di Decelerazione |
| lex9296 | 23:b9d23a2f390e | 336 | // wToleranceWindow è già la Finestra di Tolleranza di Posizionamento |
| lex9296 | 23:b9d23a2f390e | 337 | // |
| lex9296 | 39:be7055a0e9a4 | 338 | f_MaximumSpeed_FW = (((in.f_MaximumSpeed_x100_FW)* (in.f_Max_Speed- in.f_ZeroSpeed)/ 100.0f)); |
| lex9296 | 39:be7055a0e9a4 | 339 | f_ServoLockSpeed_FW = (((in.f_ServoLockSpeed_x100_FW)* (in.f_Max_Speed- in.f_ZeroSpeed)/ 100.0f)); |
| lex9296 | 39:be7055a0e9a4 | 340 | f_MaximumSpeed_BW = (((in.f_MaximumSpeed_x100_BW)* (in.f_Max_Speed- in.f_ZeroSpeed)/ 100.0f)); |
| lex9296 | 39:be7055a0e9a4 | 341 | f_ServoLockSpeed_BW = (((in.f_ServoLockSpeed_x100_BW)* (in.f_Max_Speed- in.f_ZeroSpeed)/ 100.0f)); |
| lex9296 | 39:be7055a0e9a4 | 342 | |
| lex9296 | 33:f77aa3ecf87d | 343 | // LA: Verifica (STATICA) del Profilo (Trapezio o Triangolo) |
| lex9296 | 23:b9d23a2f390e | 344 | // |
| lex9296 | 23:b9d23a2f390e | 345 | if (i64_Distance_Local < (in.i64_AccelerationWindow+ in.i64_DecelerationWindow)) { |
| lex9296 | 23:b9d23a2f390e | 346 | |
| lex9296 | 23:b9d23a2f390e | 347 | // LA: Attenzione, il Profilo è Triangolare |
| lex9296 | 23:b9d23a2f390e | 348 | // |
| lex9296 | 23:b9d23a2f390e | 349 | if (in.i64_ActualPosition < in.i64_TargetPosition) { |
| lex9296 | 23:b9d23a2f390e | 350 | |
| lex9296 | 23:b9d23a2f390e | 351 | // LA: Going FW |
| lex9296 | 23:b9d23a2f390e | 352 | // LA: Calcolare Entrambi i Profili, |
| lex9296 | 23:b9d23a2f390e | 353 | // Trovare il Punto di Intersezione |
| lex9296 | 23:b9d23a2f390e | 354 | // Aggiornare Acc/Dec/VMax in Accordo |
| lex9296 | 23:b9d23a2f390e | 355 | |
| lex9296 | 23:b9d23a2f390e | 356 | // Punto 1) Ricavo Y = mX+ q |
| lex9296 | 23:b9d23a2f390e | 357 | // ================ |
| lex9296 | 23:b9d23a2f390e | 358 | // |
| lex9296 | 23:b9d23a2f390e | 359 | // Retta x due punti, partendo da (wStartPosition, i32_ZeroSpeed) |
| lex9296 | 23:b9d23a2f390e | 360 | // (x1, Y1) |
| lex9296 | 23:b9d23a2f390e | 361 | // x Giungere a (wStartPosition+ wAccelerationWindow, MaximumSpeed) |
| lex9296 | 23:b9d23a2f390e | 362 | // (x2, Y2) |
| lex9296 | 23:b9d23a2f390e | 363 | // |
| lex9296 | 23:b9d23a2f390e | 364 | // Y = mX + q |
| lex9296 | 23:b9d23a2f390e | 365 | // |
| lex9296 | 23:b9d23a2f390e | 366 | // X = wActualPosition |
| lex9296 | 23:b9d23a2f390e | 367 | // Y = ActualSpeed |
| lex9296 | 23:b9d23a2f390e | 368 | // |
| lex9296 | 23:b9d23a2f390e | 369 | // m = (y2- y1)/(x2- x1) |
| lex9296 | 23:b9d23a2f390e | 370 | // q = ((x2* y1)- (x1* y2))/ (x2- x1) |
| lex9296 | 23:b9d23a2f390e | 371 | // |
| lex9296 | 23:b9d23a2f390e | 372 | // ================================== |
| lex9296 | 23:b9d23a2f390e | 373 | // ================================== |
| lex9296 | 23:b9d23a2f390e | 374 | |
| lex9296 | 23:b9d23a2f390e | 375 | d_X1 = (double)i64_StartPosition_Local; |
| lex9296 | 23:b9d23a2f390e | 376 | d_X2 = (double)(i64_StartPosition_Local+ in.i64_AccelerationWindow); |
| lex9296 | 33:f77aa3ecf87d | 377 | //d_Y1 = (double)in.i32_ZeroSpeed; |
| lex9296 | 39:be7055a0e9a4 | 378 | d_Y1 = (double)f_ServoLockSpeed_FW; |
| lex9296 | 39:be7055a0e9a4 | 379 | d_Y2 = (double)f_MaximumSpeed_FW; |
| lex9296 | 23:b9d23a2f390e | 380 | |
| lex9296 | 23:b9d23a2f390e | 381 | d_Y2_meno_Y1 = (d_Y2- d_Y1); // LA: From Zero to Max |
| lex9296 | 23:b9d23a2f390e | 382 | d_X2_meno_X1 = (d_X2- d_X1); // LA: Acceleration EndPoint |
| lex9296 | 23:b9d23a2f390e | 383 | d_X2_per_Y1 = (d_X2* d_Y1); |
| lex9296 | 23:b9d23a2f390e | 384 | d_X1_per_Y2 = (d_X1* d_Y2); |
| lex9296 | 23:b9d23a2f390e | 385 | |
| lex9296 | 23:b9d23a2f390e | 386 | d_m = (d_Y2_meno_Y1)/ (d_X2_meno_X1); |
| lex9296 | 23:b9d23a2f390e | 387 | d_q = ((d_X2_per_Y1)- (d_X1_per_Y2))/ (d_X2_meno_X1); |
| lex9296 | 23:b9d23a2f390e | 388 | |
| lex9296 | 23:b9d23a2f390e | 389 | // Punto 2) Ricavo Y = nX+ t |
| lex9296 | 23:b9d23a2f390e | 390 | // ================ |
| lex9296 | 23:b9d23a2f390e | 391 | // |
| lex9296 | 23:b9d23a2f390e | 392 | // Retta x due punti, partendo da (wTargetPosition- wDecelerationWindow, MaximumSpeed) |
| lex9296 | 23:b9d23a2f390e | 393 | // (x1, Y1) |
| lex9296 | 23:b9d23a2f390e | 394 | // x Giungere a (wTargetPosition, i32_ZeroSpeed) |
| lex9296 | 23:b9d23a2f390e | 395 | // (x2, Y2) |
| lex9296 | 23:b9d23a2f390e | 396 | // |
| lex9296 | 23:b9d23a2f390e | 397 | // Y = nX + t |
| lex9296 | 23:b9d23a2f390e | 398 | // |
| lex9296 | 23:b9d23a2f390e | 399 | // X = wActualPosition |
| lex9296 | 23:b9d23a2f390e | 400 | // Y = ActualSpeed |
| lex9296 | 23:b9d23a2f390e | 401 | // |
| lex9296 | 23:b9d23a2f390e | 402 | // n = (y2- y1)/(x2- x1) |
| lex9296 | 23:b9d23a2f390e | 403 | // t = ((x2* y1)- (x1* y2))/ (x2- x1) |
| lex9296 | 23:b9d23a2f390e | 404 | // |
| lex9296 | 23:b9d23a2f390e | 405 | // ================================== |
| lex9296 | 23:b9d23a2f390e | 406 | // ================================== |
| lex9296 | 23:b9d23a2f390e | 407 | |
| lex9296 | 23:b9d23a2f390e | 408 | d_X1 = (double)(in.i64_TargetPosition- in.i64_DecelerationWindow); |
| lex9296 | 23:b9d23a2f390e | 409 | d_X2 = (double)in.i64_TargetPosition; |
| lex9296 | 39:be7055a0e9a4 | 410 | d_Y1 = (double)f_MaximumSpeed_FW; |
| lex9296 | 33:f77aa3ecf87d | 411 | //d_Y2 = (double)in.i32_ZeroSpeed; |
| lex9296 | 39:be7055a0e9a4 | 412 | d_Y2 = (double)f_ServoLockSpeed_FW; |
| lex9296 | 23:b9d23a2f390e | 413 | |
| lex9296 | 23:b9d23a2f390e | 414 | d_Y2_meno_Y1 = (d_Y2- d_Y1); // LA: From Max to Zero |
| lex9296 | 23:b9d23a2f390e | 415 | d_X2_meno_X1 = (d_X2- d_X1); // LA: Deceleration EndPoint |
| lex9296 | 23:b9d23a2f390e | 416 | d_X2_per_Y1 = (d_X2* d_Y1); |
| lex9296 | 23:b9d23a2f390e | 417 | d_X1_per_Y2 = (d_X1* d_Y2); |
| lex9296 | 23:b9d23a2f390e | 418 | |
| lex9296 | 23:b9d23a2f390e | 419 | d_n = (d_Y2_meno_Y1)/ (d_X2_meno_X1); |
| lex9296 | 23:b9d23a2f390e | 420 | d_t = ((d_X2_per_Y1)- (d_X1_per_Y2))/ (d_X2_meno_X1); |
| lex9296 | 23:b9d23a2f390e | 421 | |
| lex9296 | 23:b9d23a2f390e | 422 | // Punto 3) Rilevo il punto di Intersezione x la X |
| lex9296 | 23:b9d23a2f390e | 423 | // Ricavo conseguentemente Y dall'equazione |
| lex9296 | 23:b9d23a2f390e | 424 | // ======================================== |
| lex9296 | 23:b9d23a2f390e | 425 | // |
| lex9296 | 23:b9d23a2f390e | 426 | // X = (t- q)/ (m- n) |
| lex9296 | 23:b9d23a2f390e | 427 | // Y = mX+ q (o Y = nX+ t, che in quel punto è equivalente ...) |
| lex9296 | 23:b9d23a2f390e | 428 | // |
| lex9296 | 23:b9d23a2f390e | 429 | // Y Rappresenterà la Massima Velocità raggiungibile, con le attuali pendenze |
| lex9296 | 23:b9d23a2f390e | 430 | // X Rappresenta il punto a cui ridurre Accelerazioni e Decelerazioni di profilo. |
| lex9296 | 23:b9d23a2f390e | 431 | // |
| lex9296 | 23:b9d23a2f390e | 432 | // PQM: |
| lex9296 | 23:b9d23a2f390e | 433 | // |
| lex9296 | 23:b9d23a2f390e | 434 | // Accwindow = ((t- q)/ (m- n))- StartPosition |
| lex9296 | 23:b9d23a2f390e | 435 | // Decwindow = FinishPosition- ((t- q)/ (m- n)) |
| lex9296 | 23:b9d23a2f390e | 436 | // MaxSpeed = (m* ((t- q)/ (m- n))) + q |
| lex9296 | 23:b9d23a2f390e | 437 | |
| lex9296 | 23:b9d23a2f390e | 438 | i64_AccelerationWindow_Local = (long)(((d_t- d_q)/ (d_m- d_n))- (double)i64_StartPosition_Local); |
| lex9296 | 33:f77aa3ecf87d | 439 | i64_DecelerationWindow_Local = (long)((double)i64_TargetPosition_Prec- ((d_t- d_q)/ (d_m- d_n))); |
| lex9296 | 39:be7055a0e9a4 | 440 | f_MaximumSpeed_Local = (float)((d_m* ((d_t- d_q)/ (d_m- d_n)))+ d_q); |
| lex9296 | 23:b9d23a2f390e | 441 | } |
| lex9296 | 23:b9d23a2f390e | 442 | |
| lex9296 | 23:b9d23a2f390e | 443 | else { |
| lex9296 | 23:b9d23a2f390e | 444 | |
| lex9296 | 23:b9d23a2f390e | 445 | // uui64_ActualPosition >= uui64_TargetPosition |
| lex9296 | 23:b9d23a2f390e | 446 | // LA: Going BW |
| lex9296 | 23:b9d23a2f390e | 447 | // LA: Calcolare Entrambi i Profili, |
| lex9296 | 23:b9d23a2f390e | 448 | // Trovare il Punto di Intersezione |
| lex9296 | 23:b9d23a2f390e | 449 | // Aggiornare Acc/Dec/VMax in Accordo |
| lex9296 | 23:b9d23a2f390e | 450 | |
| lex9296 | 23:b9d23a2f390e | 451 | // Punto 1) Ricavo Y = mX+ q |
| lex9296 | 23:b9d23a2f390e | 452 | // ================ |
| lex9296 | 23:b9d23a2f390e | 453 | // |
| lex9296 | 23:b9d23a2f390e | 454 | // Retta x due punti, partendo da (wStartPosition, i32_ZeroSpeed) |
| lex9296 | 23:b9d23a2f390e | 455 | // (x1, Y1) |
| lex9296 | 23:b9d23a2f390e | 456 | // x Giungere a (wStartPosition- wAccelerationWindow, MaximumSpeed) |
| lex9296 | 23:b9d23a2f390e | 457 | // (x2, Y2) |
| lex9296 | 23:b9d23a2f390e | 458 | // |
| lex9296 | 23:b9d23a2f390e | 459 | // Y = mX + q |
| lex9296 | 23:b9d23a2f390e | 460 | // |
| lex9296 | 23:b9d23a2f390e | 461 | // X = wActualPosition |
| lex9296 | 23:b9d23a2f390e | 462 | // Y = ActualSpeed |
| lex9296 | 23:b9d23a2f390e | 463 | // |
| lex9296 | 23:b9d23a2f390e | 464 | // m = (y2- y1)/(x2- x1) |
| lex9296 | 23:b9d23a2f390e | 465 | // q = ((x2* y1)- (x1* y2))/ (x2- x1) |
| lex9296 | 23:b9d23a2f390e | 466 | // |
| lex9296 | 23:b9d23a2f390e | 467 | // ================================== |
| lex9296 | 23:b9d23a2f390e | 468 | // ================================== |
| lex9296 | 23:b9d23a2f390e | 469 | |
| lex9296 | 23:b9d23a2f390e | 470 | d_X1 = (double)i64_StartPosition_Local; |
| lex9296 | 23:b9d23a2f390e | 471 | d_X2 = (double)(i64_StartPosition_Local- in.i64_AccelerationWindow); |
| lex9296 | 33:f77aa3ecf87d | 472 | //d_Y1 = (double)in.i32_ZeroSpeed; |
| lex9296 | 39:be7055a0e9a4 | 473 | d_Y1 = (double)f_ServoLockSpeed_BW; |
| lex9296 | 39:be7055a0e9a4 | 474 | d_Y2 = (double)f_MaximumSpeed_BW; |
| lex9296 | 23:b9d23a2f390e | 475 | |
| lex9296 | 23:b9d23a2f390e | 476 | d_Y2_meno_Y1 = (d_Y2- d_Y1); // LA: From Zero to Max |
| lex9296 | 23:b9d23a2f390e | 477 | d_X2_meno_X1 = (d_X2- d_X1); // LA: Acceleration EndPoint |
| lex9296 | 23:b9d23a2f390e | 478 | d_X2_per_Y1 = (d_X2* d_Y1); |
| lex9296 | 23:b9d23a2f390e | 479 | d_X1_per_Y2 = (d_X1* d_Y2); |
| lex9296 | 23:b9d23a2f390e | 480 | |
| lex9296 | 23:b9d23a2f390e | 481 | d_m = (d_Y2_meno_Y1)/ (d_X2_meno_X1); |
| lex9296 | 23:b9d23a2f390e | 482 | d_q = ((d_X2_per_Y1)- (d_X1_per_Y2))/ (d_X2_meno_X1); |
| lex9296 | 23:b9d23a2f390e | 483 | |
| lex9296 | 23:b9d23a2f390e | 484 | b_AuxCalculateProfile_002 = true; |
| lex9296 | 23:b9d23a2f390e | 485 | |
| lex9296 | 23:b9d23a2f390e | 486 | // Punto 2) Ricavo Y = nX+ t |
| lex9296 | 23:b9d23a2f390e | 487 | // ================ |
| lex9296 | 23:b9d23a2f390e | 488 | // |
| lex9296 | 23:b9d23a2f390e | 489 | // Retta x due punti, partendo da (wTargetPosition+ wDecelerationWindow, MaximumSpeed) |
| lex9296 | 23:b9d23a2f390e | 490 | // (x1, Y1) |
| lex9296 | 23:b9d23a2f390e | 491 | // x Giungere a (wTargetPosition, i32_ZeroSpeed) |
| lex9296 | 23:b9d23a2f390e | 492 | // (x2, Y2) |
| lex9296 | 23:b9d23a2f390e | 493 | // |
| lex9296 | 23:b9d23a2f390e | 494 | // Y = mX + q |
| lex9296 | 23:b9d23a2f390e | 495 | // |
| lex9296 | 23:b9d23a2f390e | 496 | // X = wActualPosition |
| lex9296 | 23:b9d23a2f390e | 497 | // Y = ActualSpeed |
| lex9296 | 23:b9d23a2f390e | 498 | // |
| lex9296 | 23:b9d23a2f390e | 499 | // m = (y2- y1)/(x2- x1) |
| lex9296 | 23:b9d23a2f390e | 500 | // q = ((x2* y1)- (x1* y2))/ (x2- x1) |
| lex9296 | 23:b9d23a2f390e | 501 | // |
| lex9296 | 23:b9d23a2f390e | 502 | // ================================== |
| lex9296 | 23:b9d23a2f390e | 503 | // ================================== |
| lex9296 | 23:b9d23a2f390e | 504 | |
| lex9296 | 23:b9d23a2f390e | 505 | d_X1 = (double)(in.i64_TargetPosition + in.i64_DecelerationWindow); |
| lex9296 | 23:b9d23a2f390e | 506 | d_X2 = (double)(in.i64_TargetPosition); |
| lex9296 | 39:be7055a0e9a4 | 507 | d_Y1 = (double)(f_MaximumSpeed_BW); |
| lex9296 | 33:f77aa3ecf87d | 508 | //d_Y2 = (double)(in.i32_ZeroSpeed); |
| lex9296 | 39:be7055a0e9a4 | 509 | d_Y2 = (double)(f_ServoLockSpeed_BW); |
| lex9296 | 23:b9d23a2f390e | 510 | |
| lex9296 | 23:b9d23a2f390e | 511 | d_Y2_meno_Y1 = (d_Y2- d_Y1); // LA: From Max to Zero |
| lex9296 | 23:b9d23a2f390e | 512 | d_X2_meno_X1 = (d_X2- d_X1); // LA: Deceleration EndPoint |
| lex9296 | 23:b9d23a2f390e | 513 | d_X2_per_Y1 = (d_X2* d_Y1); |
| lex9296 | 23:b9d23a2f390e | 514 | d_X1_per_Y2 = (d_X1* d_Y2); |
| lex9296 | 23:b9d23a2f390e | 515 | |
| lex9296 | 23:b9d23a2f390e | 516 | d_n = (d_Y2_meno_Y1)/ (d_X2_meno_X1); |
| lex9296 | 23:b9d23a2f390e | 517 | d_t = ((d_X2_per_Y1)- (d_X1_per_Y2))/ (d_X2_meno_X1); |
| lex9296 | 23:b9d23a2f390e | 518 | |
| lex9296 | 23:b9d23a2f390e | 519 | // Punto 3) Rilevo il punto di Intersezione x la X |
| lex9296 | 23:b9d23a2f390e | 520 | // Ricavo conseguentemente Y dall'equazione |
| lex9296 | 23:b9d23a2f390e | 521 | // ======================================== |
| lex9296 | 23:b9d23a2f390e | 522 | // |
| lex9296 | 23:b9d23a2f390e | 523 | // X = (t- q)/ (m- n) |
| lex9296 | 23:b9d23a2f390e | 524 | // Y = mX+ q (o Y = nX+ t, che in quel punto è equivalente ...) |
| lex9296 | 23:b9d23a2f390e | 525 | // |
| lex9296 | 23:b9d23a2f390e | 526 | // Y Rappresenterà la Massima Velocità raggiungibile, con le attuali pendenze |
| lex9296 | 23:b9d23a2f390e | 527 | // X Rappresenta il punto a cui ridurre Accelerazioni e Decelerazioni di profilo. |
| lex9296 | 23:b9d23a2f390e | 528 | // |
| lex9296 | 23:b9d23a2f390e | 529 | // PQM: |
| lex9296 | 23:b9d23a2f390e | 530 | // |
| lex9296 | 23:b9d23a2f390e | 531 | // Accwindow = StartPosition- ((t- q)/ (m- n)) |
| lex9296 | 23:b9d23a2f390e | 532 | // Decwindow = ((t- q)/ (m- n))- FinishPosition |
| lex9296 | 23:b9d23a2f390e | 533 | // MaxSpeed = (m* ((t- q)/ (m- n))) + q |
| lex9296 | 23:b9d23a2f390e | 534 | |
| lex9296 | 23:b9d23a2f390e | 535 | i64_AccelerationWindow_Local = (long)((double)i64_StartPosition_Local- ((d_t- d_q)/ (d_m- d_n))); |
| lex9296 | 33:f77aa3ecf87d | 536 | i64_DecelerationWindow_Local = (long)(((d_t- d_q)/ (d_m- d_n))- (double)i64_TargetPosition_Prec); |
| lex9296 | 39:be7055a0e9a4 | 537 | f_MaximumSpeed_Local = (float)((d_m* ((d_t- d_q)/ (d_m- d_n)))+ d_q); |
| lex9296 | 23:b9d23a2f390e | 538 | } |
| lex9296 | 23:b9d23a2f390e | 539 | } |
| lex9296 | 23:b9d23a2f390e | 540 | else { |
| lex9296 | 23:b9d23a2f390e | 541 | |
| lex9296 | 23:b9d23a2f390e | 542 | // LA: il Profilo è Trapeziodale, Nullaltro da fare che assegnare i valori ai contenitori locali. |
| lex9296 | 23:b9d23a2f390e | 543 | // |
| lex9296 | 23:b9d23a2f390e | 544 | i64_AccelerationWindow_Local = in.i64_AccelerationWindow; |
| lex9296 | 23:b9d23a2f390e | 545 | i64_DecelerationWindow_Local = in.i64_DecelerationWindow; |
| lex9296 | 23:b9d23a2f390e | 546 | |
| lex9296 | 23:b9d23a2f390e | 547 | if (in.i64_ActualPosition < in.i64_TargetPosition) |
| lex9296 | 39:be7055a0e9a4 | 548 | f_MaximumSpeed_Local = f_MaximumSpeed_FW; // LA: Going FW |
| lex9296 | 23:b9d23a2f390e | 549 | else |
| lex9296 | 39:be7055a0e9a4 | 550 | f_MaximumSpeed_Local = f_MaximumSpeed_BW; // LA: Going BW |
| lex9296 | 23:b9d23a2f390e | 551 | } |
| lex9296 | 23:b9d23a2f390e | 552 | |
| lex9296 | 23:b9d23a2f390e | 553 | b_GoingFW = false; |
| lex9296 | 23:b9d23a2f390e | 554 | out.b_InToleranceFW = false; |
| lex9296 | 23:b9d23a2f390e | 555 | b_GoingBW = false; |
| lex9296 | 23:b9d23a2f390e | 556 | out.b_InToleranceBW = false; |
| lex9296 | 23:b9d23a2f390e | 557 | |
| lex9296 | 23:b9d23a2f390e | 558 | out.b_Accelerating = false; |
| lex9296 | 23:b9d23a2f390e | 559 | out.b_MaxSpeedReached = false; |
| lex9296 | 23:b9d23a2f390e | 560 | out.b_Decelerating = false; |
| lex9296 | 23:b9d23a2f390e | 561 | out.b_InPosition = false; |
| lex9296 | 23:b9d23a2f390e | 562 | |
| lex9296 | 23:b9d23a2f390e | 563 | b_AuxCalculateProfile_000 = false; |
| lex9296 | 23:b9d23a2f390e | 564 | b_AuxCalculateProfile_001 = false; |
| lex9296 | 23:b9d23a2f390e | 565 | b_AuxCalculateProfile_002 = false; |
| lex9296 | 23:b9d23a2f390e | 566 | b_AuxCalculateProfile_003 = false; |
| lex9296 | 23:b9d23a2f390e | 567 | } |
| lex9296 | 23:b9d23a2f390e | 568 | |
| lex9296 | 23:b9d23a2f390e | 569 | if (!in.b_ServoLock) { |
| lex9296 | 23:b9d23a2f390e | 570 | |
| lex9296 | 23:b9d23a2f390e | 571 | // LA: Stop the Motion |
| lex9296 | 23:b9d23a2f390e | 572 | // |
| lex9296 | 39:be7055a0e9a4 | 573 | f_ActualSpeed_Local = in.f_ZeroSpeed; |
| lex9296 | 23:b9d23a2f390e | 574 | b_GoingFW = false; |
| lex9296 | 23:b9d23a2f390e | 575 | b_GoingBW = false; |
| lex9296 | 23:b9d23a2f390e | 576 | |
| lex9296 | 23:b9d23a2f390e | 577 | out.b_STW1_On = false; |
| lex9296 | 23:b9d23a2f390e | 578 | out.b_STW1_NoCStop = false; |
| lex9296 | 23:b9d23a2f390e | 579 | out.b_STW1_NoQStop = false; |
| lex9296 | 23:b9d23a2f390e | 580 | out.b_STW1_Enable = false; |
| lex9296 | 23:b9d23a2f390e | 581 | |
| lex9296 | 23:b9d23a2f390e | 582 | // LA: i64_TargetPosition_Prec è ritentiva, per cui è bene inizializzarla != uui64_TargetPosition. |
| lex9296 | 23:b9d23a2f390e | 583 | // |
| lex9296 | 23:b9d23a2f390e | 584 | if (in.i64_TargetPosition > 0) // LA: E' Possibile Decrementare senza rollare? |
| lex9296 | 23:b9d23a2f390e | 585 | i64_TargetPosition_Prec = in.i64_TargetPosition- 1; // LA: Si, Di certo Diverso, Di certo Coerente. |
| lex9296 | 23:b9d23a2f390e | 586 | else |
| lex9296 | 23:b9d23a2f390e | 587 | i64_TargetPosition_Prec = in.i64_TargetPosition+ 1; // LA: No, Incremento. Di certo è Diverso e Coerente. |
| lex9296 | 23:b9d23a2f390e | 588 | |
| lex9296 | 23:b9d23a2f390e | 589 | // ======================== |
| lex9296 | 23:b9d23a2f390e | 590 | // ======================== |
| lex9296 | 23:b9d23a2f390e | 591 | // Axis is Now "Free Wheel" |
| lex9296 | 23:b9d23a2f390e | 592 | // ======================== |
| lex9296 | 23:b9d23a2f390e | 593 | // ======================== |
| lex9296 | 23:b9d23a2f390e | 594 | |
| lex9296 | 23:b9d23a2f390e | 595 | } |
| lex9296 | 23:b9d23a2f390e | 596 | |
| lex9296 | 23:b9d23a2f390e | 597 | else { |
| lex9296 | 23:b9d23a2f390e | 598 | |
| lex9296 | 23:b9d23a2f390e | 599 | // LA: Issue to <Start the Motion> |
| lex9296 | 23:b9d23a2f390e | 600 | // LA: Keep the present Motion <Active> |
| lex9296 | 23:b9d23a2f390e | 601 | // |
| lex9296 | 23:b9d23a2f390e | 602 | out.b_STW1_On = true; |
| lex9296 | 23:b9d23a2f390e | 603 | out.b_STW1_NoCStop = true; |
| lex9296 | 23:b9d23a2f390e | 604 | out.b_STW1_NoQStop = true; |
| lex9296 | 23:b9d23a2f390e | 605 | out.b_STW1_Enable = true; |
| lex9296 | 23:b9d23a2f390e | 606 | |
| lex9296 | 23:b9d23a2f390e | 607 | // Positioner |
| lex9296 | 23:b9d23a2f390e | 608 | // |
| lex9296 | 23:b9d23a2f390e | 609 | if (in.i64_ActualPosition < in.i64_TargetPosition) { |
| lex9296 | 23:b9d23a2f390e | 610 | |
| lex9296 | 23:b9d23a2f390e | 611 | // LA: Going FW |
| lex9296 | 23:b9d23a2f390e | 612 | // |
| lex9296 | 23:b9d23a2f390e | 613 | out.b_InToleranceBW = false; |
| lex9296 | 23:b9d23a2f390e | 614 | b_GoingFW = true; |
| lex9296 | 23:b9d23a2f390e | 615 | b_GoingBW = false; |
| lex9296 | 23:b9d23a2f390e | 616 | |
| lex9296 | 23:b9d23a2f390e | 617 | // Case is: Acceleration is Alloweed |
| lex9296 | 23:b9d23a2f390e | 618 | // ======================== |
| lex9296 | 23:b9d23a2f390e | 619 | // |
| lex9296 | 23:b9d23a2f390e | 620 | if ( |
| lex9296 | 23:b9d23a2f390e | 621 | (in.i64_ActualPosition >= i64_StartPosition_Local) && |
| lex9296 | 23:b9d23a2f390e | 622 | (in.i64_ActualPosition < (i64_StartPosition_Local+ i64_AccelerationWindow_Local)) |
| lex9296 | 23:b9d23a2f390e | 623 | ) { |
| lex9296 | 23:b9d23a2f390e | 624 | |
| lex9296 | 23:b9d23a2f390e | 625 | out.b_Accelerating = true; |
| lex9296 | 23:b9d23a2f390e | 626 | out.b_Decelerating = false; |
| lex9296 | 23:b9d23a2f390e | 627 | out.b_MaxSpeedReached = false; |
| lex9296 | 23:b9d23a2f390e | 628 | |
| lex9296 | 23:b9d23a2f390e | 629 | out.b_InPosition = false; |
| lex9296 | 23:b9d23a2f390e | 630 | out.b_InToleranceFW = false; |
| lex9296 | 23:b9d23a2f390e | 631 | |
| lex9296 | 23:b9d23a2f390e | 632 | // Line Profile [m, q] is to be Calculated only once |
| lex9296 | 23:b9d23a2f390e | 633 | // |
| lex9296 | 23:b9d23a2f390e | 634 | if (! b_AuxCalculateProfile_000) { |
| lex9296 | 23:b9d23a2f390e | 635 | |
| lex9296 | 23:b9d23a2f390e | 636 | // Retta x due punti, partendo da (wStartPosition, i32_ZeroSpeed) |
| lex9296 | 23:b9d23a2f390e | 637 | // (x1, Y1) |
| lex9296 | 23:b9d23a2f390e | 638 | // x Giungere a (wStartPosition+ wAccelerationWindow, MaximumSpeed) |
| lex9296 | 23:b9d23a2f390e | 639 | // (x2, Y2) |
| lex9296 | 23:b9d23a2f390e | 640 | // |
| lex9296 | 23:b9d23a2f390e | 641 | // Y = mX + q |
| lex9296 | 23:b9d23a2f390e | 642 | // |
| lex9296 | 23:b9d23a2f390e | 643 | // X = wActualPosition |
| lex9296 | 23:b9d23a2f390e | 644 | // Y = ActualSpeed |
| lex9296 | 23:b9d23a2f390e | 645 | // |
| lex9296 | 23:b9d23a2f390e | 646 | // m = (y2- y1)/(x2- x1) |
| lex9296 | 23:b9d23a2f390e | 647 | // q = ((x2* y1)- (x1* y2))/ (x2- x1) |
| lex9296 | 23:b9d23a2f390e | 648 | // |
| lex9296 | 23:b9d23a2f390e | 649 | // ================================== |
| lex9296 | 23:b9d23a2f390e | 650 | // ================================== |
| lex9296 | 23:b9d23a2f390e | 651 | |
| lex9296 | 23:b9d23a2f390e | 652 | d_X1 = (double)i64_StartPosition_Local; |
| lex9296 | 23:b9d23a2f390e | 653 | d_X2 = (double)(i64_StartPosition_Local+ i64_AccelerationWindow_Local); |
| lex9296 | 33:f77aa3ecf87d | 654 | //d_Y1 = (double)in.i32_ZeroSpeed; |
| lex9296 | 39:be7055a0e9a4 | 655 | d_Y1 = (double)f_ServoLockSpeed_FW; |
| lex9296 | 39:be7055a0e9a4 | 656 | d_Y2 = (double)f_MaximumSpeed_Local; |
| lex9296 | 23:b9d23a2f390e | 657 | |
| lex9296 | 23:b9d23a2f390e | 658 | d_Y2_meno_Y1 = (d_Y2- d_Y1); // LA: From Zero to Max |
| lex9296 | 39:be7055a0e9a4 | 659 | d_X2_meno_X1 = (d_X2- d_X1); // LA: Acceleration EndPoint |
| lex9296 | 23:b9d23a2f390e | 660 | d_X2_per_Y1 = (d_X2* d_Y1); |
| lex9296 | 23:b9d23a2f390e | 661 | d_X1_per_Y2 = (d_X1* d_Y2); |
| lex9296 | 23:b9d23a2f390e | 662 | |
| lex9296 | 23:b9d23a2f390e | 663 | d_m = (d_Y2_meno_Y1)/ (d_X2_meno_X1); |
| lex9296 | 23:b9d23a2f390e | 664 | d_q = ((d_X2_per_Y1)- (d_X1_per_Y2))/ (d_X2_meno_X1); |
| lex9296 | 23:b9d23a2f390e | 665 | |
| lex9296 | 23:b9d23a2f390e | 666 | b_AuxCalculateProfile_000 = true; |
| lex9296 | 23:b9d23a2f390e | 667 | } |
| lex9296 | 39:be7055a0e9a4 | 668 | f_ActualSpeed_Local = (float)((d_m * (double)in.i64_ActualPosition)+ d_q); |
| lex9296 | 23:b9d23a2f390e | 669 | } |
| lex9296 | 23:b9d23a2f390e | 670 | |
| lex9296 | 23:b9d23a2f390e | 671 | // Case is: MiddleWalking @ Planned-Full Speed |
| lex9296 | 23:b9d23a2f390e | 672 | // ================================== |
| lex9296 | 23:b9d23a2f390e | 673 | // |
| lex9296 | 23:b9d23a2f390e | 674 | if ( |
| lex9296 | 23:b9d23a2f390e | 675 | (in.i64_ActualPosition >= (i64_StartPosition_Local+ i64_AccelerationWindow_Local)) && |
| lex9296 | 23:b9d23a2f390e | 676 | (in.i64_ActualPosition < (in.i64_TargetPosition - i64_DecelerationWindow_Local)) |
| lex9296 | 23:b9d23a2f390e | 677 | ) { |
| lex9296 | 23:b9d23a2f390e | 678 | |
| lex9296 | 39:be7055a0e9a4 | 679 | out.b_Accelerating = false; |
| lex9296 | 39:be7055a0e9a4 | 680 | out.b_Decelerating = false; |
| lex9296 | 39:be7055a0e9a4 | 681 | out.b_MaxSpeedReached = true; |
| lex9296 | 23:b9d23a2f390e | 682 | |
| lex9296 | 39:be7055a0e9a4 | 683 | out.b_InPosition = false; |
| lex9296 | 39:be7055a0e9a4 | 684 | out.b_InToleranceFW = false; |
| lex9296 | 23:b9d23a2f390e | 685 | |
| lex9296 | 39:be7055a0e9a4 | 686 | f_ActualSpeed_Local = f_MaximumSpeed_Local; |
| lex9296 | 23:b9d23a2f390e | 687 | } |
| lex9296 | 23:b9d23a2f390e | 688 | |
| lex9296 | 23:b9d23a2f390e | 689 | // Case is: Need to Decelerate, up to the Tolerance Window |
| lex9296 | 23:b9d23a2f390e | 690 | // ============================================== |
| lex9296 | 23:b9d23a2f390e | 691 | // |
| lex9296 | 23:b9d23a2f390e | 692 | if ( |
| lex9296 | 23:b9d23a2f390e | 693 | (in.i64_ActualPosition >= (in.i64_TargetPosition- i64_DecelerationWindow_Local)) && |
| lex9296 | 23:b9d23a2f390e | 694 | (in.i64_ActualPosition < (in.i64_TargetPosition- in.i64_diToleranceWindow)) |
| lex9296 | 23:b9d23a2f390e | 695 | ) { |
| lex9296 | 23:b9d23a2f390e | 696 | |
| lex9296 | 23:b9d23a2f390e | 697 | out.b_Accelerating = false; |
| lex9296 | 23:b9d23a2f390e | 698 | out.b_Decelerating = true; |
| lex9296 | 23:b9d23a2f390e | 699 | out.b_MaxSpeedReached = false; |
| lex9296 | 23:b9d23a2f390e | 700 | |
| lex9296 | 23:b9d23a2f390e | 701 | out.b_InPosition = false; |
| lex9296 | 23:b9d23a2f390e | 702 | out.b_InToleranceFW = false; |
| lex9296 | 23:b9d23a2f390e | 703 | |
| lex9296 | 23:b9d23a2f390e | 704 | // Line Profile [m, q] is to be Calculated only once |
| lex9296 | 23:b9d23a2f390e | 705 | // |
| lex9296 | 23:b9d23a2f390e | 706 | if (! b_AuxCalculateProfile_001) { |
| lex9296 | 23:b9d23a2f390e | 707 | |
| lex9296 | 23:b9d23a2f390e | 708 | // Retta x due punti, partendo da (wTargetPosition- wDecelerationWindow, MaximumSpeed) |
| lex9296 | 23:b9d23a2f390e | 709 | // (x1, Y1) |
| lex9296 | 23:b9d23a2f390e | 710 | // x Giungere a (wTargetPosition, i32_ZeroSpeed) |
| lex9296 | 23:b9d23a2f390e | 711 | // (x2, Y2) |
| lex9296 | 23:b9d23a2f390e | 712 | // |
| lex9296 | 23:b9d23a2f390e | 713 | // Y = mX + q |
| lex9296 | 23:b9d23a2f390e | 714 | // |
| lex9296 | 23:b9d23a2f390e | 715 | // X = wActualPosition |
| lex9296 | 23:b9d23a2f390e | 716 | // Y = ActualSpeed |
| lex9296 | 23:b9d23a2f390e | 717 | // |
| lex9296 | 23:b9d23a2f390e | 718 | // m = (y2- y1)/(x2- x1) |
| lex9296 | 23:b9d23a2f390e | 719 | // q = ((x2* y1)- (x1* y2))/ (x2- x1) |
| lex9296 | 23:b9d23a2f390e | 720 | // |
| lex9296 | 23:b9d23a2f390e | 721 | // ================================== |
| lex9296 | 23:b9d23a2f390e | 722 | // ================================== |
| lex9296 | 23:b9d23a2f390e | 723 | |
| lex9296 | 23:b9d23a2f390e | 724 | d_X1 = (double)(in.i64_TargetPosition- i64_DecelerationWindow_Local); |
| lex9296 | 23:b9d23a2f390e | 725 | d_X2 = (double)in.i64_TargetPosition; |
| lex9296 | 39:be7055a0e9a4 | 726 | d_Y1 = (double)f_ActualSpeed_Local; // LA: Maximum Speed Planned MIGHT have NOT been Reached |
| lex9296 | 33:f77aa3ecf87d | 727 | //d_Y2 = (double)in.i32_ZeroSpeed; |
| lex9296 | 39:be7055a0e9a4 | 728 | d_Y2 = (double)f_ServoLockSpeed_FW; |
| lex9296 | 23:b9d23a2f390e | 729 | |
| lex9296 | 23:b9d23a2f390e | 730 | d_Y2_meno_Y1 = (d_Y2- d_Y1); // LA: From Max to Zero |
| lex9296 | 23:b9d23a2f390e | 731 | d_X2_meno_X1 = (d_X2- d_X1); // LA: Deceleration EndPoint |
| lex9296 | 23:b9d23a2f390e | 732 | d_X2_per_Y1 = (d_X2* d_Y1); |
| lex9296 | 23:b9d23a2f390e | 733 | d_X1_per_Y2 = (d_X1* d_Y2); |
| lex9296 | 23:b9d23a2f390e | 734 | |
| lex9296 | 23:b9d23a2f390e | 735 | d_m = (d_Y2_meno_Y1)/ (d_X2_meno_X1); |
| lex9296 | 23:b9d23a2f390e | 736 | d_q = ((d_X2_per_Y1)- (d_X1_per_Y2))/ (d_X2_meno_X1); |
| lex9296 | 23:b9d23a2f390e | 737 | |
| lex9296 | 23:b9d23a2f390e | 738 | b_AuxCalculateProfile_001 = true; |
| lex9296 | 23:b9d23a2f390e | 739 | } |
| lex9296 | 39:be7055a0e9a4 | 740 | f_ActualSpeed_Local = abs((float)((d_m * (double)in.i64_ActualPosition)+ d_q)); |
| lex9296 | 23:b9d23a2f390e | 741 | } |
| lex9296 | 23:b9d23a2f390e | 742 | |
| lex9296 | 23:b9d23a2f390e | 743 | // Case is: Tolerance Reached while Going FW |
| lex9296 | 23:b9d23a2f390e | 744 | // ================================ |
| lex9296 | 23:b9d23a2f390e | 745 | // |
| lex9296 | 23:b9d23a2f390e | 746 | if (in.i64_ActualPosition >= (in.i64_TargetPosition- in.i64_diToleranceWindow)) { |
| lex9296 | 39:be7055a0e9a4 | 747 | out.b_Accelerating = false; |
| lex9296 | 39:be7055a0e9a4 | 748 | out.b_Decelerating = false; |
| lex9296 | 39:be7055a0e9a4 | 749 | out.b_MaxSpeedReached = false; |
| lex9296 | 23:b9d23a2f390e | 750 | |
| lex9296 | 39:be7055a0e9a4 | 751 | out.b_InPosition = true; |
| lex9296 | 39:be7055a0e9a4 | 752 | out.b_InToleranceFW = true; |
| lex9296 | 23:b9d23a2f390e | 753 | |
| lex9296 | 39:be7055a0e9a4 | 754 | i64_StartPosition_Local = in.i64_ActualPosition; |
| lex9296 | 39:be7055a0e9a4 | 755 | f_ActualSpeed_Local = f_ServoLockSpeed_FW; |
| lex9296 | 23:b9d23a2f390e | 756 | } |
| lex9296 | 23:b9d23a2f390e | 757 | } |
| lex9296 | 23:b9d23a2f390e | 758 | |
| lex9296 | 23:b9d23a2f390e | 759 | else if (in.i64_ActualPosition > in.i64_TargetPosition) { |
| lex9296 | 23:b9d23a2f390e | 760 | |
| lex9296 | 23:b9d23a2f390e | 761 | // LA: Going Bw |
| lex9296 | 23:b9d23a2f390e | 762 | // |
| lex9296 | 23:b9d23a2f390e | 763 | out.b_InToleranceFW = false; |
| lex9296 | 23:b9d23a2f390e | 764 | b_GoingBW = true; |
| lex9296 | 23:b9d23a2f390e | 765 | b_GoingFW = false; |
| lex9296 | 23:b9d23a2f390e | 766 | |
| lex9296 | 23:b9d23a2f390e | 767 | // Case is: Acceleration is Alloweed |
| lex9296 | 23:b9d23a2f390e | 768 | // ======================== |
| lex9296 | 23:b9d23a2f390e | 769 | // |
| lex9296 | 23:b9d23a2f390e | 770 | if ( |
| lex9296 | 23:b9d23a2f390e | 771 | (in.i64_ActualPosition <= i64_StartPosition_Local) && |
| lex9296 | 23:b9d23a2f390e | 772 | (in.i64_ActualPosition > (i64_StartPosition_Local- i64_AccelerationWindow_Local)) |
| lex9296 | 23:b9d23a2f390e | 773 | ) { |
| lex9296 | 23:b9d23a2f390e | 774 | |
| lex9296 | 23:b9d23a2f390e | 775 | out.b_Accelerating = true; |
| lex9296 | 23:b9d23a2f390e | 776 | out.b_Decelerating = false; |
| lex9296 | 23:b9d23a2f390e | 777 | out.b_MaxSpeedReached = false; |
| lex9296 | 23:b9d23a2f390e | 778 | |
| lex9296 | 23:b9d23a2f390e | 779 | out.b_InPosition = false; |
| lex9296 | 23:b9d23a2f390e | 780 | out.b_InToleranceBW = false; |
| lex9296 | 23:b9d23a2f390e | 781 | |
| lex9296 | 23:b9d23a2f390e | 782 | // Line Profile [m, q] is to be Calculated only once |
| lex9296 | 23:b9d23a2f390e | 783 | // |
| lex9296 | 23:b9d23a2f390e | 784 | if (! b_AuxCalculateProfile_002) { |
| lex9296 | 23:b9d23a2f390e | 785 | |
| lex9296 | 23:b9d23a2f390e | 786 | // Retta x due punti, partendo da (wStartPosition, i32_ZeroSpeed) |
| lex9296 | 23:b9d23a2f390e | 787 | // (x1, Y1) |
| lex9296 | 23:b9d23a2f390e | 788 | // x Giungere a (wStartPosition- wAccelerationWindow, MaximumSpeed) |
| lex9296 | 23:b9d23a2f390e | 789 | // (x2, Y2) |
| lex9296 | 23:b9d23a2f390e | 790 | // |
| lex9296 | 23:b9d23a2f390e | 791 | // Y = mX + q |
| lex9296 | 23:b9d23a2f390e | 792 | // |
| lex9296 | 23:b9d23a2f390e | 793 | // X = wActualPosition |
| lex9296 | 23:b9d23a2f390e | 794 | // Y = ActualSpeed |
| lex9296 | 23:b9d23a2f390e | 795 | // |
| lex9296 | 23:b9d23a2f390e | 796 | // m = (y2- y1)/(x2- x1) |
| lex9296 | 23:b9d23a2f390e | 797 | // q = ((x2* y1)- (x1* y2))/ (x2- x1) |
| lex9296 | 23:b9d23a2f390e | 798 | // |
| lex9296 | 23:b9d23a2f390e | 799 | // ================================== |
| lex9296 | 23:b9d23a2f390e | 800 | // ================================== |
| lex9296 | 23:b9d23a2f390e | 801 | |
| lex9296 | 23:b9d23a2f390e | 802 | d_X1 = (double)i64_StartPosition_Local; |
| lex9296 | 23:b9d23a2f390e | 803 | d_X2 = (double)(i64_StartPosition_Local- i64_AccelerationWindow_Local); |
| lex9296 | 33:f77aa3ecf87d | 804 | //d_Y1 = (double)in.i32_ZeroSpeed; |
| lex9296 | 39:be7055a0e9a4 | 805 | d_Y1 = (double)f_ServoLockSpeed_BW; |
| lex9296 | 39:be7055a0e9a4 | 806 | d_Y2 = (double)f_MaximumSpeed_Local; |
| lex9296 | 33:f77aa3ecf87d | 807 | |
| lex9296 | 23:b9d23a2f390e | 808 | d_Y2_meno_Y1 = (d_Y2- d_Y1); // LA: From Zero to Max |
| lex9296 | 23:b9d23a2f390e | 809 | d_X2_meno_X1 = (d_X2- d_X1); // LA: Acceleration EndPoint |
| lex9296 | 23:b9d23a2f390e | 810 | d_X2_per_Y1 = (d_X2* d_Y1); |
| lex9296 | 23:b9d23a2f390e | 811 | d_X1_per_Y2 = (d_X1* d_Y2); |
| lex9296 | 23:b9d23a2f390e | 812 | |
| lex9296 | 23:b9d23a2f390e | 813 | d_m = (d_Y2_meno_Y1)/ (d_X2_meno_X1); |
| lex9296 | 23:b9d23a2f390e | 814 | d_q = ((d_X2_per_Y1)- (d_X1_per_Y2))/ (d_X2_meno_X1); |
| lex9296 | 23:b9d23a2f390e | 815 | |
| lex9296 | 23:b9d23a2f390e | 816 | b_AuxCalculateProfile_002 = true; |
| lex9296 | 23:b9d23a2f390e | 817 | } |
| lex9296 | 39:be7055a0e9a4 | 818 | f_ActualSpeed_Local = (float)((d_m * (double)in.i64_ActualPosition)+ d_q); |
| lex9296 | 23:b9d23a2f390e | 819 | } |
| lex9296 | 23:b9d23a2f390e | 820 | |
| lex9296 | 23:b9d23a2f390e | 821 | // Case is: MiddleWalking @ Planned-Full Speed |
| lex9296 | 23:b9d23a2f390e | 822 | // ================================== |
| lex9296 | 23:b9d23a2f390e | 823 | // |
| lex9296 | 23:b9d23a2f390e | 824 | if ( |
| lex9296 | 23:b9d23a2f390e | 825 | (in.i64_ActualPosition <= (i64_StartPosition_Local- i64_AccelerationWindow_Local)) && |
| lex9296 | 23:b9d23a2f390e | 826 | (in.i64_ActualPosition > (in.i64_TargetPosition+ i64_DecelerationWindow_Local)) |
| lex9296 | 23:b9d23a2f390e | 827 | ) { |
| lex9296 | 23:b9d23a2f390e | 828 | |
| lex9296 | 39:be7055a0e9a4 | 829 | out.b_Accelerating = false; |
| lex9296 | 39:be7055a0e9a4 | 830 | out.b_Decelerating = false; |
| lex9296 | 39:be7055a0e9a4 | 831 | out.b_MaxSpeedReached = true; |
| lex9296 | 23:b9d23a2f390e | 832 | |
| lex9296 | 39:be7055a0e9a4 | 833 | out.b_InPosition = false; |
| lex9296 | 39:be7055a0e9a4 | 834 | out.b_InToleranceBW = false; |
| lex9296 | 23:b9d23a2f390e | 835 | |
| lex9296 | 39:be7055a0e9a4 | 836 | f_ActualSpeed_Local = f_MaximumSpeed_Local; |
| lex9296 | 23:b9d23a2f390e | 837 | } |
| lex9296 | 23:b9d23a2f390e | 838 | |
| lex9296 | 23:b9d23a2f390e | 839 | // Case is: Need to Decelerate, up to the Tolerance Window |
| lex9296 | 23:b9d23a2f390e | 840 | // ============================================== |
| lex9296 | 23:b9d23a2f390e | 841 | // |
| lex9296 | 23:b9d23a2f390e | 842 | if ( |
| lex9296 | 23:b9d23a2f390e | 843 | (in.i64_ActualPosition <= (in.i64_TargetPosition+ i64_DecelerationWindow_Local)) && |
| lex9296 | 23:b9d23a2f390e | 844 | (in.i64_ActualPosition > (in.i64_TargetPosition+ in.i64_diToleranceWindow)) |
| lex9296 | 23:b9d23a2f390e | 845 | ) { |
| lex9296 | 23:b9d23a2f390e | 846 | |
| lex9296 | 23:b9d23a2f390e | 847 | out.b_Accelerating = false; |
| lex9296 | 23:b9d23a2f390e | 848 | out.b_Decelerating = true; |
| lex9296 | 23:b9d23a2f390e | 849 | out.b_MaxSpeedReached = false; |
| lex9296 | 23:b9d23a2f390e | 850 | |
| lex9296 | 23:b9d23a2f390e | 851 | out.b_InPosition = false; |
| lex9296 | 23:b9d23a2f390e | 852 | out.b_InToleranceBW = false; |
| lex9296 | 23:b9d23a2f390e | 853 | |
| lex9296 | 23:b9d23a2f390e | 854 | // Line Profile [m, q] is to be Calculated only once |
| lex9296 | 23:b9d23a2f390e | 855 | // |
| lex9296 | 23:b9d23a2f390e | 856 | if (! b_AuxCalculateProfile_003) { |
| lex9296 | 23:b9d23a2f390e | 857 | |
| lex9296 | 23:b9d23a2f390e | 858 | // Retta x due punti, partendo da (wTargetPosition+ wDecelerationWindow, MaximumSpeed) |
| lex9296 | 23:b9d23a2f390e | 859 | // (x1, Y1) |
| lex9296 | 23:b9d23a2f390e | 860 | // x Giungere a (wTargetPosition, i32_ZeroSpeed) |
| lex9296 | 23:b9d23a2f390e | 861 | // (x2, Y2) |
| lex9296 | 23:b9d23a2f390e | 862 | // |
| lex9296 | 23:b9d23a2f390e | 863 | // Y = mX + q |
| lex9296 | 23:b9d23a2f390e | 864 | // |
| lex9296 | 23:b9d23a2f390e | 865 | // X = wActualPosition |
| lex9296 | 23:b9d23a2f390e | 866 | // Y = ActualSpeed |
| lex9296 | 23:b9d23a2f390e | 867 | // |
| lex9296 | 23:b9d23a2f390e | 868 | // m = (y2- y1)/(x2- x1) |
| lex9296 | 23:b9d23a2f390e | 869 | // q = ((x2* y1)- (x1* y2))/ (x2- x1) |
| lex9296 | 23:b9d23a2f390e | 870 | // |
| lex9296 | 23:b9d23a2f390e | 871 | // ================================== |
| lex9296 | 23:b9d23a2f390e | 872 | // ================================== |
| lex9296 | 23:b9d23a2f390e | 873 | |
| lex9296 | 23:b9d23a2f390e | 874 | d_X1 = (double)(in.i64_TargetPosition + i64_DecelerationWindow_Local); |
| lex9296 | 23:b9d23a2f390e | 875 | d_X2 = (double)in.i64_TargetPosition; |
| lex9296 | 39:be7055a0e9a4 | 876 | d_Y1 = (double)f_ActualSpeed_Local; // LA: Maximum Speed Planned MIGHT have NOT been Reached |
| lex9296 | 33:f77aa3ecf87d | 877 | //d_Y2 = (double)in.i32_ZeroSpeed; |
| lex9296 | 39:be7055a0e9a4 | 878 | d_Y2 = (double)f_ServoLockSpeed_BW; |
| lex9296 | 23:b9d23a2f390e | 879 | |
| lex9296 | 23:b9d23a2f390e | 880 | d_Y2_meno_Y1 = (d_Y2- d_Y1); // LA: From Max to Zero |
| lex9296 | 23:b9d23a2f390e | 881 | d_X2_meno_X1 = (d_X2- d_X1); // LA: Deceleration EndPoint |
| lex9296 | 23:b9d23a2f390e | 882 | d_X2_per_Y1 = (d_X2* d_Y1); |
| lex9296 | 23:b9d23a2f390e | 883 | d_X1_per_Y2 = (d_X1* d_Y2); |
| lex9296 | 23:b9d23a2f390e | 884 | |
| lex9296 | 23:b9d23a2f390e | 885 | d_m = (d_Y2_meno_Y1)/ (d_X2_meno_X1); |
| lex9296 | 23:b9d23a2f390e | 886 | d_q = ((d_X2_per_Y1)- (d_X1_per_Y2))/ (d_X2_meno_X1); |
| lex9296 | 23:b9d23a2f390e | 887 | |
| lex9296 | 23:b9d23a2f390e | 888 | b_AuxCalculateProfile_003 = true; |
| lex9296 | 23:b9d23a2f390e | 889 | } |
| lex9296 | 39:be7055a0e9a4 | 890 | f_ActualSpeed_Local = abs((float)((d_m* (double)(in.i64_ActualPosition))+ d_q)); |
| lex9296 | 23:b9d23a2f390e | 891 | } |
| lex9296 | 23:b9d23a2f390e | 892 | |
| lex9296 | 23:b9d23a2f390e | 893 | // Case is: Tolerance Reached while Going BW |
| lex9296 | 23:b9d23a2f390e | 894 | // ================================ |
| lex9296 | 23:b9d23a2f390e | 895 | // |
| lex9296 | 23:b9d23a2f390e | 896 | if (in.i64_ActualPosition <= (in.i64_TargetPosition + in.i64_diToleranceWindow)) { |
| lex9296 | 23:b9d23a2f390e | 897 | out.b_Accelerating = false; |
| lex9296 | 23:b9d23a2f390e | 898 | out.b_Decelerating = false; |
| lex9296 | 23:b9d23a2f390e | 899 | out.b_MaxSpeedReached = false; |
| lex9296 | 23:b9d23a2f390e | 900 | |
| lex9296 | 23:b9d23a2f390e | 901 | out.b_InPosition = true; |
| lex9296 | 23:b9d23a2f390e | 902 | out.b_InToleranceBW = true; |
| lex9296 | 23:b9d23a2f390e | 903 | |
| lex9296 | 23:b9d23a2f390e | 904 | i64_StartPosition_Local = in.i64_ActualPosition; |
| lex9296 | 39:be7055a0e9a4 | 905 | f_ActualSpeed_Local = f_ServoLockSpeed_BW; |
| lex9296 | 23:b9d23a2f390e | 906 | } |
| lex9296 | 23:b9d23a2f390e | 907 | } |
| lex9296 | 23:b9d23a2f390e | 908 | |
| lex9296 | 23:b9d23a2f390e | 909 | else { |
| lex9296 | 23:b9d23a2f390e | 910 | |
| lex9296 | 23:b9d23a2f390e | 911 | // LA: In Position |
| lex9296 | 23:b9d23a2f390e | 912 | // =========== |
| lex9296 | 23:b9d23a2f390e | 913 | // |
| lex9296 | 23:b9d23a2f390e | 914 | b_GoingBW = false; |
| lex9296 | 23:b9d23a2f390e | 915 | out.b_InToleranceBW = true; |
| lex9296 | 23:b9d23a2f390e | 916 | b_GoingFW = false; |
| lex9296 | 23:b9d23a2f390e | 917 | out.b_InToleranceFW = true; |
| lex9296 | 23:b9d23a2f390e | 918 | |
| lex9296 | 23:b9d23a2f390e | 919 | out.b_Accelerating = false; |
| lex9296 | 23:b9d23a2f390e | 920 | out.b_Decelerating = false; |
| lex9296 | 23:b9d23a2f390e | 921 | out.b_MaxSpeedReached = false; |
| lex9296 | 23:b9d23a2f390e | 922 | |
| lex9296 | 23:b9d23a2f390e | 923 | out.b_InPosition = true; |
| lex9296 | 23:b9d23a2f390e | 924 | |
| lex9296 | 23:b9d23a2f390e | 925 | i64_StartPosition_Local = in.i64_ActualPosition; |
| lex9296 | 39:be7055a0e9a4 | 926 | f_ActualSpeed_Local = in.f_ZeroSpeed; |
| lex9296 | 23:b9d23a2f390e | 927 | } |
| lex9296 | 23:b9d23a2f390e | 928 | |
| lex9296 | 23:b9d23a2f390e | 929 | // =========================== |
| lex9296 | 23:b9d23a2f390e | 930 | // =========================== |
| lex9296 | 23:b9d23a2f390e | 931 | // LA: Managing the Speed Limit(s) |
| lex9296 | 23:b9d23a2f390e | 932 | // =========================== |
| lex9296 | 23:b9d23a2f390e | 933 | // =========================== |
| lex9296 | 23:b9d23a2f390e | 934 | |
| lex9296 | 23:b9d23a2f390e | 935 | if (i64_Distance_Local >= in.i64_diToleranceWindow) { |
| lex9296 | 23:b9d23a2f390e | 936 | // i64_Distance_Local >= in.i64_diToleranceWindow |
| lex9296 | 23:b9d23a2f390e | 937 | |
| lex9296 | 23:b9d23a2f390e | 938 | if (in.i64_ActualPosition < in.i64_TargetPosition) { |
| lex9296 | 23:b9d23a2f390e | 939 | |
| lex9296 | 23:b9d23a2f390e | 940 | // LA: Going FW |
| lex9296 | 23:b9d23a2f390e | 941 | // |
| lex9296 | 39:be7055a0e9a4 | 942 | if (f_ActualSpeed_Local > f_MaximumSpeed_FW) |
| lex9296 | 39:be7055a0e9a4 | 943 | f_ActualSpeed_Local = f_MaximumSpeed_FW; |
| lex9296 | 39:be7055a0e9a4 | 944 | if (f_ActualSpeed_Local < f_ServoLockSpeed_FW) |
| lex9296 | 39:be7055a0e9a4 | 945 | f_ActualSpeed_Local = f_ServoLockSpeed_FW; |
| lex9296 | 23:b9d23a2f390e | 946 | } |
| lex9296 | 23:b9d23a2f390e | 947 | else { |
| lex9296 | 23:b9d23a2f390e | 948 | |
| lex9296 | 23:b9d23a2f390e | 949 | // LA: Going BW |
| lex9296 | 23:b9d23a2f390e | 950 | // |
| lex9296 | 39:be7055a0e9a4 | 951 | if (f_ActualSpeed_Local > f_MaximumSpeed_BW) |
| lex9296 | 39:be7055a0e9a4 | 952 | f_ActualSpeed_Local = f_MaximumSpeed_BW; |
| lex9296 | 39:be7055a0e9a4 | 953 | if (f_ActualSpeed_Local < f_ServoLockSpeed_BW) |
| lex9296 | 39:be7055a0e9a4 | 954 | f_ActualSpeed_Local = f_ServoLockSpeed_BW; |
| lex9296 | 23:b9d23a2f390e | 955 | } |
| lex9296 | 23:b9d23a2f390e | 956 | } |
| lex9296 | 23:b9d23a2f390e | 957 | |
| lex9296 | 23:b9d23a2f390e | 958 | else { |
| lex9296 | 23:b9d23a2f390e | 959 | // i64_Distance_Local < in.i64_diToleranceWindow |
| lex9296 | 23:b9d23a2f390e | 960 | |
| lex9296 | 33:f77aa3ecf87d | 961 | // LA: Attenzione, questo esegue un <eventuale> OVERRIDE alle assegnazioni precedenti. |
| lex9296 | 33:f77aa3ecf87d | 962 | // =============================================================================== |
| lex9296 | 23:b9d23a2f390e | 963 | // |
| lex9296 | 23:b9d23a2f390e | 964 | // Se il controllo di distanza vede che l'asse è in Anello di tolleranza forza a Zerospeed la velocità. |
| lex9296 | 23:b9d23a2f390e | 965 | // Il comportamento conseguente è che l'asse rimane in quiete, senza correnti applicate, fintanto che resta all'interno della finestra. |
| lex9296 | 23:b9d23a2f390e | 966 | // Se dovesse uscire, il sistema lo riporterebbe in tolleranza (o cercherebbe di farlo) a "ServolockSpeed". |
| lex9296 | 23:b9d23a2f390e | 967 | // |
| lex9296 | 23:b9d23a2f390e | 968 | |
| lex9296 | 39:be7055a0e9a4 | 969 | f_ActualSpeed_Local = in.f_ZeroSpeed; |
| lex9296 | 23:b9d23a2f390e | 970 | } |
| lex9296 | 23:b9d23a2f390e | 971 | } |
| lex9296 | 23:b9d23a2f390e | 972 | } |
| lex9296 | 23:b9d23a2f390e | 973 | |
| lex9296 | 23:b9d23a2f390e | 974 | else { |
| lex9296 | 23:b9d23a2f390e | 975 | // !in.b_ACPos_Homed |
| lex9296 | 23:b9d23a2f390e | 976 | |
| lex9296 | 23:b9d23a2f390e | 977 | // LA: if Not Homed, Positioning Flag(s) are Kept Clear |
| lex9296 | 23:b9d23a2f390e | 978 | // |
| lex9296 | 32:1be3d79ff4db | 979 | b_GoingFW = false; |
| lex9296 | 32:1be3d79ff4db | 980 | out.b_InToleranceFW = false; |
| lex9296 | 32:1be3d79ff4db | 981 | b_GoingBW = false; |
| lex9296 | 32:1be3d79ff4db | 982 | out.b_InToleranceBW = false; |
| lex9296 | 23:b9d23a2f390e | 983 | |
| lex9296 | 32:1be3d79ff4db | 984 | out.b_Accelerating = false; |
| lex9296 | 32:1be3d79ff4db | 985 | out.b_MaxSpeedReached = false; |
| lex9296 | 32:1be3d79ff4db | 986 | out.b_Decelerating = false; |
| lex9296 | 32:1be3d79ff4db | 987 | out.b_InPosition = false; |
| lex9296 | 23:b9d23a2f390e | 988 | |
| lex9296 | 23:b9d23a2f390e | 989 | b_AuxCalculateProfile_000 = false; |
| lex9296 | 23:b9d23a2f390e | 990 | b_AuxCalculateProfile_001 = false; |
| lex9296 | 23:b9d23a2f390e | 991 | b_AuxCalculateProfile_002 = false; |
| lex9296 | 23:b9d23a2f390e | 992 | b_AuxCalculateProfile_003 = false; |
| lex9296 | 23:b9d23a2f390e | 993 | |
| lex9296 | 39:be7055a0e9a4 | 994 | f_ActualSpeed_Local = in.f_ZeroSpeed; |
| lex9296 | 23:b9d23a2f390e | 995 | } |
| lex9296 | 23:b9d23a2f390e | 996 | } |
| lex9296 | 23:b9d23a2f390e | 997 | } |
| lex9296 | 23:b9d23a2f390e | 998 | else { |
| lex9296 | 23:b9d23a2f390e | 999 | // !in.b_AxisPowered |
| lex9296 | 23:b9d23a2f390e | 1000 | |
| lex9296 | 23:b9d23a2f390e | 1001 | // LA: if Not Powered, Motion Flag(s) are Kept Clear |
| lex9296 | 23:b9d23a2f390e | 1002 | // |
| lex9296 | 32:1be3d79ff4db | 1003 | b_GoingFW = false; |
| lex9296 | 32:1be3d79ff4db | 1004 | out.b_InToleranceFW = false; |
| lex9296 | 32:1be3d79ff4db | 1005 | b_GoingBW = false; |
| lex9296 | 32:1be3d79ff4db | 1006 | out.b_InToleranceBW = false; |
| lex9296 | 23:b9d23a2f390e | 1007 | |
| lex9296 | 32:1be3d79ff4db | 1008 | out.b_Accelerating = false; |
| lex9296 | 23:b9d23a2f390e | 1009 | out.b_MaxSpeedReached = false; |
| lex9296 | 32:1be3d79ff4db | 1010 | out.b_Decelerating = false; |
| lex9296 | 32:1be3d79ff4db | 1011 | out.b_InPosition = false; |
| lex9296 | 23:b9d23a2f390e | 1012 | |
| lex9296 | 23:b9d23a2f390e | 1013 | b_AuxCalculateProfile_000 = false; |
| lex9296 | 23:b9d23a2f390e | 1014 | b_AuxCalculateProfile_001 = false; |
| lex9296 | 23:b9d23a2f390e | 1015 | b_AuxCalculateProfile_002 = false; |
| lex9296 | 23:b9d23a2f390e | 1016 | b_AuxCalculateProfile_003 = false; |
| lex9296 | 23:b9d23a2f390e | 1017 | |
| lex9296 | 39:be7055a0e9a4 | 1018 | f_ActualSpeed_Local = in.f_ZeroSpeed; |
| lex9296 | 23:b9d23a2f390e | 1019 | } |
| lex9296 | 23:b9d23a2f390e | 1020 | |
| lex9296 | 23:b9d23a2f390e | 1021 | // =================== |
| lex9296 | 23:b9d23a2f390e | 1022 | // =================== |
| lex9296 | 23:b9d23a2f390e | 1023 | // LA: Finally, RAW Output |
| lex9296 | 23:b9d23a2f390e | 1024 | // =================== |
| lex9296 | 23:b9d23a2f390e | 1025 | // =================== |
| lex9296 | 23:b9d23a2f390e | 1026 | |
| lex9296 | 39:be7055a0e9a4 | 1027 | out.f_ATVSpeed = f_ActualSpeed_Local; |
| lex9296 | 23:b9d23a2f390e | 1028 | out.b_ATVDirectionFW = b_GoingFW; |
| lex9296 | 23:b9d23a2f390e | 1029 | out.b_ATVDirectionBW = b_GoingBW; |
| lex9296 | 23:b9d23a2f390e | 1030 | |
| lex9296 | 23:b9d23a2f390e | 1031 | out.i64_StartPosition = i64_StartPosition_Local; |
| lex9296 | 23:b9d23a2f390e | 1032 | out.i64_Distance = i64_Distance_Local; |
| lex9296 | 23:b9d23a2f390e | 1033 | } |
| lex9296 | 23:b9d23a2f390e | 1034 | InProgress = false; |
| lex9296 | 23:b9d23a2f390e | 1035 | } |