Luca Antolini / Mbed 2 deprecated Stabilus322699_LA0005

New BASE release 4 Fork

Dependencies:   mbed QEI DmTftLibrary

SWPos/SWPos.cpp@26:bfca0c471a87, 2022-02-10 (annotated)

Committer:
lex9296
Date:
Thu Feb 10 13:46:17 2022 +0000
Revision:
26:bfca0c471a87
Parent:
23:b9d23a2f390e 
Last Commitment of LA0005 Fork

Who changed what in which revision?

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