TypeIIRMLStep1Profiles.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 
46 #include <TypeIIRMLStep1Profiles.h>
47 #include <TypeIIRMLMath.h>
48 #include <math.h>
49 
50 
51 //************************************************************************************
52 // ProfileStep1PosLinHldNegLin()
53 
54 double TypeIIRMLMath::ProfileStep1PosLinHldNegLin( const double &CurrentPosition
55  , const double &CurrentVelocity
56  , const double &TargetPosition
57  , const double &TargetVelocity
58  , const double &MaxVelocity
59  , const double &MaxAcceleration)
60 {
61  return( (2.0 * MaxAcceleration * (TargetPosition - CurrentPosition)
62  + pow2(CurrentVelocity)
63  + pow2(TargetVelocity)
64  + 2.0 * MaxVelocity * (MaxVelocity - CurrentVelocity
65  - TargetVelocity))
66  / (2.0 * MaxAcceleration * MaxVelocity) );
67 }
68 
69 
70 //************************************************************************************
71 // ProfileStep1PosLinNegLin()
72 
73 double TypeIIRMLMath::ProfileStep1PosLinNegLin( const double &CurrentPosition
74  , const double &CurrentVelocity
75  , const double &TargetPosition
76  , const double &TargetVelocity
77  , const double &MaxAcceleration)
78 {
79  return ((1.4142135623730950488016887242096980785696718753769480731766
80  * RMLSqrt(pow2(MaxAcceleration) * (2.0 * MaxAcceleration
81  * (TargetPosition - CurrentPosition) + pow2(CurrentVelocity)
82  + pow2(TargetVelocity))) - MaxAcceleration
83  * (CurrentVelocity + TargetVelocity))
84  / (pow2(MaxAcceleration)) );
85 }
86 
87 
88 //************************************************************************************
89 // ProfileStep1PosTriNegLin()
90 
91 double TypeIIRMLMath::ProfileStep1PosTriNegLin( const double &CurrentPosition
92  , const double &CurrentVelocity
93  , const double &TargetPosition
94  , const double &TargetVelocity
95  , const double &MaxAcceleration)
96 {
97  return ((1.4142135623730950488016887242096980785696718753769480731766
98  * RMLSqrt(pow2(MaxAcceleration) * (pow2(CurrentVelocity)
99  + pow2(TargetVelocity) + 2.0 * MaxAcceleration
100  * (TargetPosition - CurrentPosition )))
101  - MaxAcceleration * (CurrentVelocity + TargetVelocity))
102  / (pow2(MaxAcceleration)) );
103 }
104 
105 
106 //************************************************************************************
107 // ProfileStep1PosTrapNegLin()
108 
109 double TypeIIRMLMath::ProfileStep1PosTrapNegLin( const double &CurrentPosition
110  , const double &CurrentVelocity
111  , const double &TargetPosition
112  , const double &TargetVelocity
113  , const double &MaxVelocity
114  , const double &MaxAcceleration)
115 {
116  return ((TargetPosition - CurrentPosition) / MaxVelocity
117  + (0.5 * (pow2(CurrentVelocity) + pow2(TargetVelocity))
118  + MaxVelocity * (MaxVelocity - CurrentVelocity
119  - TargetVelocity)) / (MaxAcceleration * MaxVelocity));
120 }
121 
122 
123 //************************************************************************************
124 // ProfileStep1NegLinPosLin()
125 
126 double TypeIIRMLMath::ProfileStep1NegLinPosLin( const double &CurrentPosition
127  , const double &CurrentVelocity
128  , const double &TargetPosition
129  , const double &TargetVelocity
130  , const double &MaxAcceleration)
131 {
132  return ( (CurrentVelocity + TargetVelocity
133  - 1.4142135623730950488016887242096980785696718753769480731766
134  * RMLSqrt(2.0 * MaxAcceleration
135  * (CurrentPosition - TargetPosition)
136  + pow2(CurrentVelocity) + pow2(TargetVelocity)))
137  / MaxAcceleration );
138 }
139 
140 
141 //************************************************************************************
142 // IsSolutionForProfile_PosLinHldNegLin_Possible()
143 
144 bool TypeIIRMLMath::IsSolutionForProfile_PosLinHldNegLin_Possible( const double &CurrentPosition
145  , const double &CurrentVelocity
146  , const double &TargetPosition
147  , const double &TargetVelocity
148  , const double &MaxVelocity
149  , const double &MaxAcceleration)
150 {
151  // Check for t01 and vi
152  if ( (CurrentVelocity > MaxVelocity )
153  || (CurrentVelocity < 0.0 ) )
154  {
155  return (false);
156  }
157 
158  // Check for t12
159  if ( (2.0 * MaxAcceleration * (TargetPosition - CurrentPosition)
160  + pow2(CurrentVelocity) - 2.0 * pow2(MaxVelocity)
161  + pow2(TargetVelocity)) < -RML_VALID_SOLUTION_EPSILON)
162  {
163  return (false);
164  }
165 
166  // Check for t23 and vtrgt
167  if ( (TargetVelocity > MaxVelocity)
168  || (TargetVelocity < 0.0) )
169  {
170  return (false);
171  }
172 
173  return (true);
174 }
175 
176 
177 //************************************************************************************
178 // IsSolutionForProfile_PosLinNegLin_Possible()
179 
180 bool TypeIIRMLMath::IsSolutionForProfile_PosLinNegLin_Possible( const double &CurrentPosition
181  , const double &CurrentVelocity
182  , const double &TargetPosition
183  , const double &TargetVelocity
184  , const double &MaxVelocity
185  , const double &MaxAcceleration)
186 {
187  double BufferVariable = 0.0
188  , SqrtOfBufferVariable = 0.0
189  , PeakVelocity = 0.0;
190 
191  // Check for vi
192  if ( (CurrentVelocity > MaxVelocity )
193  || (CurrentVelocity < 0.0 ) )
194  {
195  return (false);
196  }
197 
198  // Check for vtrgt
199  if ( (TargetVelocity > MaxVelocity )
200  || (TargetVelocity < 0.0 ) )
201  {
202  return (false);
203  }
204 
205  BufferVariable = pow2(MaxAcceleration) * (2.0
206  * MaxAcceleration * (TargetPosition
207  - CurrentPosition) + pow2(CurrentVelocity)
208  + pow2(TargetVelocity));
209 
210  // Check for vpeak
211  if (BufferVariable < -RML_VALID_SOLUTION_EPSILON)
212  {
213  return (false);
214  }
215 
216  SqrtOfBufferVariable = RMLSqrt(BufferVariable);
217 
218  PeakVelocity = SqrtOfBufferVariable
219  / (1.4142135623730950488016887242096980785696718753769480731766
220  * MaxAcceleration);
221 
222  if ( (PeakVelocity > MaxVelocity )
223  || (PeakVelocity < CurrentVelocity )
224  || (PeakVelocity < TargetVelocity ) )
225  {
226  return (false);
227  }
228 
229  // Check for t01
230  if ( ((1.4142135623730950488016887242096980785696718753769480731766
231  * SqrtOfBufferVariable - 2.0 * MaxAcceleration
232  * CurrentVelocity) / (2.0 * pow2(MaxAcceleration)))
233  < -RML_VALID_SOLUTION_EPSILON)
234  {
235  return (false);
236  }
237 
238  // Check for t12
239  if ( ((1.4142135623730950488016887242096980785696718753769480731766
240  * SqrtOfBufferVariable - 2.0 * MaxAcceleration
241  * TargetVelocity) / (2.0 * pow2(MaxAcceleration)))
242  < -RML_VALID_SOLUTION_EPSILON)
243  {
244  return (false);
245  }
246 
247  return (true);
248 }
249 
250 
251 //************************************************************************************
252 // IsSolutionForProfile_PosTriNegLin_Possible()
253 
254 bool TypeIIRMLMath::IsSolutionForProfile_PosTriNegLin_Possible( const double &CurrentPosition
255  , const double &CurrentVelocity
256  , const double &TargetPosition
257  , const double &TargetVelocity
258  , const double &MaxVelocity
259  , const double &MaxAcceleration)
260 {
261  double BufferVariable = 0.0
262  , SqrtOfBufferVariable = 0.0
263  , PeakVelocity = 0.0;
264 
265  // Check for vi
266  if ( (CurrentVelocity > MaxVelocity )
267  || (CurrentVelocity < 0.0 ) )
268  {
269  return (false);
270  }
271 
272  // Check for t23 and vtrgt
273  if ( (TargetVelocity < -MaxVelocity )
274  || (TargetVelocity > 0.0 ) )
275  {
276  return (false);
277  }
278 
279  BufferVariable = pow2(MaxAcceleration) * (2.0
280  * MaxAcceleration * (TargetPosition
281  - CurrentPosition) + pow2(CurrentVelocity)
282  + pow2(TargetVelocity));
283 
284  // Check for vpeak
285  if (BufferVariable < -RML_VALID_SOLUTION_EPSILON)
286  {
287  return (false);
288  }
289 
290  SqrtOfBufferVariable = RMLSqrt(BufferVariable);
291 
292  PeakVelocity = SqrtOfBufferVariable
293  / (1.4142135623730950488016887242096980785696718753769480731766
294  * MaxAcceleration);
295 
296  if ( (PeakVelocity > MaxVelocity )
297  || (PeakVelocity < CurrentVelocity ) )
298  {
299  return (false);
300  }
301 
302  // Check for t01
303  if ( ((1.4142135623730950488016887242096980785696718753769480731766
304  * SqrtOfBufferVariable - 2.0 * MaxAcceleration
305  * CurrentVelocity) / (2.0 * pow2(MaxAcceleration)))
306  < -RML_VALID_SOLUTION_EPSILON)
307  {
308  return (false);
309  }
310 
311  // Check for t12
312  if ( ((1.4142135623730950488016887242096980785696718753769480731766
313  * SqrtOfBufferVariable - 2.0 * MaxAcceleration
314  * TargetVelocity) / (2.0 * pow2(MaxAcceleration)))
315  < -RML_VALID_SOLUTION_EPSILON)
316  {
317  return (false);
318  }
319 
320  return (true);
321 }
322 
323 
324 //************************************************************************************
325 // IsSolutionForProfile_PosTrapNegLin_Possible()
326 
327 bool TypeIIRMLMath::IsSolutionForProfile_PosTrapNegLin_Possible( const double &CurrentPosition
328  , const double &CurrentVelocity
329  , const double &TargetPosition
330  , const double &TargetVelocity
331  , const double &MaxVelocity
332  , const double &MaxAcceleration)
333 {
334  // Check for t01 and vi
335  if ( (CurrentVelocity > MaxVelocity )
336  || (CurrentVelocity < 0.0 ) )
337  {
338  return (false);
339  }
340 
341  // Check for t12
342  if ( (2.0 * MaxAcceleration * (TargetPosition - CurrentPosition)
343  + pow2(CurrentVelocity) - 2.0 * pow2(MaxVelocity)
344  + pow2(TargetVelocity)) < -RML_VALID_SOLUTION_EPSILON)
345  {
346  return (false);
347  }
348 
349  // Check for t34 and vtrgt
350  if ( (TargetVelocity < -MaxVelocity)
351  || (TargetVelocity > 0.0) )
352  {
353  return (false);
354  }
355 
356  return (true);
357 }
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::IsSolutionForProfile_PosLinNegLin_Possible
bool IsSolutionForProfile_PosLinNegLin_Possible(const double &CurrentPosition, const double &CurrentVelocity, const double &TargetPosition, const double &TargetVelocity, const double &MaxVelocity, const double &MaxAcceleration)
Checks whether a valid solution for the PosLinNegLin velocity profile would be possible.
Definition: TypeIIRMLStep1Profiles.cpp:180
TypeIIRMLMath.h
Header file for functions and definitions of constant values and macros.
TypeIIRMLMath::ProfileStep1NegLinPosLin
double ProfileStep1NegLinPosLin(const double &CurrentPosition, const double &CurrentVelocity, const double &TargetPosition, const double &TargetVelocity, const double &MaxAcceleration)
Calculates the execution time of the NegLinPosLin velocity profile.
Definition: TypeIIRMLStep1Profiles.cpp:126
RML_VALID_SOLUTION_EPSILON
#define RML_VALID_SOLUTION_EPSILON
Positive threshold value used during the check, whether a valid solution for a given profile is possi...
Definition: TypeIIRMLMath.h:97
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::IsSolutionForProfile_PosTriNegLin_Possible
bool IsSolutionForProfile_PosTriNegLin_Possible(const double &CurrentPosition, const double &CurrentVelocity, const double &TargetPosition, const double &TargetVelocity, const double &MaxVelocity, const double &MaxAcceleration)
Checks whether a valid solution for the PosTriNegLin velocity profile would be possible.
Definition: TypeIIRMLStep1Profiles.cpp:254
TypeIIRMLMath::RMLSqrt
double RMLSqrt(const double &Value)
Calculates the real square root of a given value. If the value is negative a value of almost zero wil...
Definition: TypeIIRMLMath.h:291
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
pow2
#define pow2(A)
A to the power of 2.
Definition: TypeIIRMLMath.h:273
TypeIIRMLMath::IsSolutionForProfile_PosLinHldNegLin_Possible
bool IsSolutionForProfile_PosLinHldNegLin_Possible(const double &CurrentPosition, const double &CurrentVelocity, const double &TargetPosition, const double &TargetVelocity, const double &MaxVelocity, const double &MaxAcceleration)
Checks whether a valid solution for the PosLinHldNegLin velocity profile would be possible...
Definition: TypeIIRMLStep1Profiles.cpp:144
TypeIIRMLStep1Profiles.h
Header file for the Step 1 motion profiles.
TypeIIRMLMath::IsSolutionForProfile_PosTrapNegLin_Possible
bool IsSolutionForProfile_PosTrapNegLin_Possible(const double &CurrentPosition, const double &CurrentVelocity, const double &TargetPosition, const double &TargetVelocity, const double &MaxVelocity, const double &MaxAcceleration)
Checks whether a valid solution for the PosTrapNegLin velocity profile would be possible.
Definition: TypeIIRMLStep1Profiles.cpp:327
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

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