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