TypeIIRMLDecisionTree1A.cpp
Go to the documentation of this file.
1 // ---------------------- Doxygen info ----------------------
39 // ----------------------------------------------------------
40 // For a convenient reading of this file's source code,
41 // please use a tab width of four characters.
42 // ----------------------------------------------------------
43 
44 
45 #include <TypeIIRMLDecisionTree1A.h>
46 #include <TypeIIRMLStep1Profiles.h>
47 #include <TypeIIRMLMath.h>
48 #include <TypeIIRMLDecisions.h>
49 #include <TypeIIRMLStep1IntermediateProfiles.h>
50 #include <TypeIIRMLStep1Profiles.h>
51 
52 
53 //************************************************************************************
54 // TypeIIRMLDecisionTree1A()
55 
56 void TypeIIRMLMath::TypeIIRMLDecisionTree1A( const double &CurrentPosition
57  , const double &CurrentVelocity
58  , const double &TargetPosition
59  , const double &TargetVelocity
60  , const double &MaxVelocity
61  , const double &MaxAcceleration
62  , Step1_Profile *AppliedProfile
63  , double *MinimalExecutionTime )
64 {
65  bool IntermediateInversion = false;
66 
67  double ThisCurrentPosition = CurrentPosition
68  , ThisCurrentVelocity = CurrentVelocity
69  , ThisTargetPosition = TargetPosition
70  , ThisTargetVelocity = TargetVelocity ;
71 
72  *MinimalExecutionTime = 0.0;
73 
74  // ********************************************************************
75  if (Decision_1A__001(ThisCurrentVelocity))
76  {
77  goto MDecision_1A__002;
78  }
79  else
80  {
81  NegateStep1( &ThisCurrentPosition
82  , &ThisCurrentVelocity
83  , &ThisTargetPosition
84  , &ThisTargetVelocity );
85 
86  goto MDecision_1A__002;
87  }
88  // ********************************************************************
89 MDecision_1A__002:
90  if (Decision_1A__002( ThisCurrentVelocity
91  , MaxVelocity ))
92  {
93  goto MDecision_1A__003;
94  }
95  else
96  {
97  VToVMaxStep1( MinimalExecutionTime
98  , &ThisCurrentPosition
99  , &ThisCurrentVelocity
100  , MaxVelocity
101  , MaxAcceleration );
102 
103  goto MDecision_1A__003;
104  }
105  // ********************************************************************
106 MDecision_1A__003:
107  if (Decision_1A__003( ThisCurrentVelocity
108  , ThisTargetVelocity ))
109  {
110  goto MDecision_1A__004;
111  }
112  else
113  {
114  goto MDecision_1A__006;
115  }
116  // ********************************************************************
117 MDecision_1A__004:
118  if (Decision_1A__004( ThisCurrentPosition
119  , ThisCurrentVelocity
120  , ThisTargetPosition
121  , ThisTargetVelocity
122  , MaxAcceleration ))
123  {
124  goto MDecision_1A__005;
125  }
126  else
127  {
128  VToZeroStep1( MinimalExecutionTime
129  , &ThisCurrentPosition
130  , &ThisCurrentVelocity
131  , MaxAcceleration );
132 
133  NegateStep1( &ThisCurrentPosition
134  , &ThisCurrentVelocity
135  , &ThisTargetPosition
136  , &ThisTargetVelocity );
137 
138  IntermediateInversion = true;
139  goto MDecision_1A__009;
140  }
141  // ********************************************************************
142 MDecision_1A__005:
143  if (Decision_1A__005( ThisCurrentPosition
144  , ThisCurrentVelocity
145  , ThisTargetPosition
146  , ThisTargetVelocity
147  , MaxVelocity
148  , MaxAcceleration ))
149  {
150  *MinimalExecutionTime += ProfileStep1PosLinHldNegLin( ThisCurrentPosition
151  , ThisCurrentVelocity
152  , ThisTargetPosition
153  , ThisTargetVelocity
154  , MaxVelocity
155  , MaxAcceleration );
156  if (IntermediateInversion)
157  {
158  *AppliedProfile = Step1_Profile_NegLinHldPosLin;
159  }
160  else
161  {
162  *AppliedProfile = Step1_Profile_PosLinHldNegLin;
163  }
164 
165  goto END_OF_THIS_FUNCTION;
166  }
167  else
168  {
169  *MinimalExecutionTime += ProfileStep1PosLinNegLin( ThisCurrentPosition
170  , ThisCurrentVelocity
171  , ThisTargetPosition
172  , ThisTargetVelocity
173  , MaxAcceleration );
174  if (IntermediateInversion)
175  {
176  *AppliedProfile = Step1_Profile_NegLinPosLin;
177  }
178  else
179  {
180  *AppliedProfile = Step1_Profile_PosLinNegLin;
181  }
182 
183  goto END_OF_THIS_FUNCTION;
184  }
185  // ********************************************************************
186 MDecision_1A__006:
187  if (Decision_1A__006(ThisTargetVelocity))
188  {
189  goto MDecision_1A__007;
190  }
191  else
192  {
193  goto MDecision_1A__008;
194  }
195  // ********************************************************************
196 MDecision_1A__007:
197  if (Decision_1A__007( ThisCurrentPosition
198  , ThisCurrentVelocity
199  , ThisTargetPosition
200  , ThisTargetVelocity
201  , MaxAcceleration ))
202  {
203  VToZeroStep1( MinimalExecutionTime
204  , &ThisCurrentPosition
205  , &ThisCurrentVelocity
206  , MaxAcceleration );
207 
208  NegateStep1( &ThisCurrentPosition
209  , &ThisCurrentVelocity
210  , &ThisTargetPosition
211  , &ThisTargetVelocity );
212 
213  IntermediateInversion = true;
214  goto MDecision_1A__009;
215  }
216  else
217  {
218  goto MDecision_1A__005;
219  }
220  // ********************************************************************
221 MDecision_1A__008:
222  if (Decision_1A__008( ThisCurrentPosition
223  , ThisCurrentVelocity
224  , ThisTargetPosition
225  , ThisTargetVelocity
226  , MaxAcceleration ))
227  {
228  VToZeroStep1( MinimalExecutionTime
229  , &ThisCurrentPosition
230  , &ThisCurrentVelocity
231  , MaxAcceleration );
232 
233  NegateStep1( &ThisCurrentPosition
234  , &ThisCurrentVelocity
235  , &ThisTargetPosition
236  , &ThisTargetVelocity );
237 
238  IntermediateInversion = true;
239  goto MDecision_1A__005;
240  }
241  else
242  {
243  goto MDecision_1A__009;
244  }
245  // ********************************************************************
246 MDecision_1A__009:
247  if (Decision_1A__009( ThisCurrentPosition
248  , ThisCurrentVelocity
249  , ThisTargetPosition
250  , ThisTargetVelocity
251  , MaxVelocity
252  , MaxAcceleration ))
253  {
254  *MinimalExecutionTime += ProfileStep1PosTriNegLin( ThisCurrentPosition
255  , ThisCurrentVelocity
256  , ThisTargetPosition
257  , ThisTargetVelocity
258  , MaxAcceleration );
259  if (IntermediateInversion)
260  {
261  *AppliedProfile = Step1_Profile_NegTriPosLin;
262  }
263  else
264  {
265  *AppliedProfile = Step1_Profile_PosTriNegLin;
266  }
267 
268  goto END_OF_THIS_FUNCTION;
269  }
270  else
271  {
272  *MinimalExecutionTime += ProfileStep1PosTrapNegLin( ThisCurrentPosition
273  , ThisCurrentVelocity
274  , ThisTargetPosition
275  , ThisTargetVelocity
276  , MaxVelocity
277  , MaxAcceleration );
278  if (IntermediateInversion)
279  {
280  *AppliedProfile = Step1_Profile_NegTrapPosLin;
281  }
282  else
283  {
284  *AppliedProfile = Step1_Profile_PosTrapNegLin;
285  }
286 
287  goto END_OF_THIS_FUNCTION;
288  }
289  // ********************************************************************
290 END_OF_THIS_FUNCTION:
291 
292  return;
293 }
TypeIIRMLMath::Decision_1A__008
bool Decision_1A__008(const double &CurrentPosition, const double &CurrentVelocity, const double &TargetPosition, const double &TargetVelocity, const double &MaxAcceleration)
If v->0->vtrgt, is p>=ptrgt?
Definition: TypeIIRMLDecisions.cpp:138
TypeIIRMLStep1IntermediateProfiles.h
Header file for intermediate profile segments of Step 1.
TypeIIRMLMath::VToVMaxStep1
void VToVMaxStep1(double *TotalTime, double *ThisCurrentPosition, double *ThisCurrentVelocity, const double &MaxVelocity, const double &MaxAcceleration)
One intermediate Step 1 trajectory segment: v -> +vmax (NegLin)
Definition: TypeIIRMLStep1IntermediateProfiles.cpp:72
TypeIIRMLMath::ProfileStep1PosTriNegLin
double ProfileStep1PosTriNegLin(const double &CurrentPosition, const double &CurrentVelocity, const double &TargetPosition, const double &TargetVelocity, const double &MaxAcceleration)
Calculates the execution time of the PosTriNegLin velocity profile.
Definition: TypeIIRMLStep1Profiles.cpp:91
TypeIIRMLMath::TypeIIRMLDecisionTree1A
void TypeIIRMLDecisionTree1A(const double &CurrentPosition, const double &CurrentVelocity, const double &TargetPosition, const double &TargetVelocity, const double &MaxVelocity, const double &MaxAcceleration, Step1_Profile *AppliedProfile, double *MinimalExecutionTime)
This function contains the decision tree 1A of the Step 1 of the Type II On-Line Trajectory Generatio...
Definition: TypeIIRMLDecisionTree1A.cpp:56
TypeIIRMLMath::Decision_1A__004
bool Decision_1A__004(const double &CurrentPosition, const double &CurrentVelocity, const double &TargetPosition, const double &TargetVelocity, const double &MaxAcceleration)
If v->vtrgt, is p<=ptrgt?
Definition: TypeIIRMLDecisions.cpp:81
TypeIIRMLDecisions.h
Header file for decisions of the two decision trees of the Type II On-Line Trajectory Generation algo...
TypeIIRMLMath::NegateStep1
void NegateStep1(double *ThisCurrentPosition, double *ThisCurrentVelocity, double *ThisTargetPosition, double *ThisTargetVelocity)
Negate input values during Step 1.
Definition: TypeIIRMLStep1IntermediateProfiles.cpp:55
TypeIIRMLMath.h
Header file for functions and definitions of constant values and macros.
TypeIIRMLMath::ProfileStep1PosLinHldNegLin
double ProfileStep1PosLinHldNegLin(const double &CurrentPosition, const double &CurrentVelocity, const double &TargetPosition, const double &TargetVelocity, const double &MaxVelocity, const double &MaxAcceleration)
Calculates the execution time of the PosLinHldNegLin velocity profile.
Definition: TypeIIRMLStep1Profiles.cpp:54
TypeIIRMLMath::Decision_1A__003
bool Decision_1A__003(const double &CurrentVelocity, const double &TargetVelocity)
Is (vi <= vtrgt)?
Definition: TypeIIRMLDecisions.cpp:71
TypeIIRMLMath::VToZeroStep1
void VToZeroStep1(double *TotalTime, double *ThisCurrentPosition, double *ThisCurrentVelocity, const double &MaxAcceleration)
One intermediate Step 1 trajectory segment: v -> 0 (NegLin)
Definition: TypeIIRMLStep1IntermediateProfiles.cpp:93
TypeIIRMLMath::ProfileStep1PosTrapNegLin
double ProfileStep1PosTrapNegLin(const double &CurrentPosition, const double &CurrentVelocity, const double &TargetPosition, const double &TargetVelocity, const double &MaxVelocity, const double &MaxAcceleration)
Calculates the execution time of the PosTrapNegLin velocity profile.
Definition: TypeIIRMLStep1Profiles.cpp:109
TypeIIRMLMath::Decision_1A__002
bool Decision_1A__002(const double &CurrentVelocity, const double &MaxVelocity)
Is (vi <= +vmax)?
Definition: TypeIIRMLDecisions.cpp:61
TypeIIRMLMath::Decision_1A__006
bool Decision_1A__006(const double &TargetVelocity)
Is (vtrgt >= 0)?
Definition: TypeIIRMLDecisions.cpp:114
TypeIIRMLMath::Decision_1A__009
bool Decision_1A__009(const double &CurrentPosition, const double &CurrentVelocity, const double &TargetPosition, const double &TargetVelocity, const double &MaxVelocity, const double &MaxAcceleration)
If v->vmax->0->vtrgt, is p>=ptrgt?
Definition: TypeIIRMLDecisions.cpp:154
TypeIIRMLDecisionTree1A.h
Header file for the Step 1 decision tree 1A of the Type II On-Line Trajectory Generation algorithm...
TypeIIRMLStep1Profiles.h
Header file for the Step 1 motion profiles.
TypeIIRMLMath::Decision_1A__007
bool Decision_1A__007(const double &CurrentPosition, const double &CurrentVelocity, const double &TargetPosition, const double &TargetVelocity, const double &MaxAcceleration)
If v->vtrgt, is p>=ptrgt?
Definition: TypeIIRMLDecisions.cpp:123
TypeIIRMLMath::Decision_1A__005
bool Decision_1A__005(const double &CurrentPosition, const double &CurrentVelocity, const double &TargetPosition, const double &TargetVelocity, const double &MaxVelocity, const double &MaxAcceleration)
If v->vmax->vtrgt, is p<=ptrgt?
Definition: TypeIIRMLDecisions.cpp:96
TypeIIRMLMath::ProfileStep1PosLinNegLin
double ProfileStep1PosLinNegLin(const double &CurrentPosition, const double &CurrentVelocity, const double &TargetPosition, const double &TargetVelocity, const double &MaxAcceleration)
Calculates the execution time of the PosLinNegLin velocity profile.
Definition: TypeIIRMLStep1Profiles.cpp:73
TypeIIRMLMath::Decision_1A__001
bool Decision_1A__001(const double &CurrentVelocity)
Is (vi >= 0)?
Definition: TypeIIRMLDecisions.cpp:52
TypeIIRMLMath::Step1_Profile
Step1_Profile
Enumeration of all possible profiles of Step 1 (A, B, and C).
Definition: TypeIIRMLStep1Profiles.h:71

libreflexxestype2
Author(s):
autogenerated on Sat Nov 21 2020 03:17:34