TypeIIRMLStep2IntermediateProfiles.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 #include <TypeIIRMLStep2IntermediateProfiles.h>
45 #include <TypeIIRMLPolynomial.h>
46 #include <TypeIIRMLMath.h>
47 #include <TypeIIRMLStep1IntermediateProfiles.h>
48 
49 using namespace TypeIIRMLMath;
50 
51 
52 //****************************************************************************
53 // NegateStep2()
54 
55 void TypeIIRMLMath::NegateStep2( double *ThisCurrentPosition
56  , double *ThisCurrentVelocity
57  , double *ThisTargetPosition
58  , double *ThisTargetVelocity
59  , bool *Inverted )
60 {
61  *ThisCurrentPosition = -(*ThisCurrentPosition ) ;
62  *ThisCurrentVelocity = -(*ThisCurrentVelocity ) ;
63  *ThisTargetPosition = -(*ThisTargetPosition ) ;
64  *ThisTargetVelocity = -(*ThisTargetVelocity ) ;
65  *Inverted = !(*Inverted) ;
66 
67  return;
68 }
69 
70 
71 //****************************************************************************
72 // VToVMaxStep2()
73 
74 void TypeIIRMLMath::VToVMaxStep2( double *ThisCurrentTime
75  , double *ThisCurrentPosition
76  , double *ThisCurrentVelocity
77  , const double &MaxVelocity
78  , const double &MaxAcceleration
79  , MotionPolynomials *PolynomialsLocal
80  , const bool &Inverted )
81 {
82  double TimeForCurrentStep = (*ThisCurrentVelocity - MaxVelocity)
83  / MaxAcceleration;
84 
85  if(Inverted)
86  {
87  PolynomialsLocal->PositionPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients((0.5 * MaxAcceleration), (-(*ThisCurrentVelocity)), (-(*ThisCurrentPosition)), (*ThisCurrentTime));
88  PolynomialsLocal->VelocityPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, MaxAcceleration , (-(*ThisCurrentVelocity)), (*ThisCurrentTime));
89  PolynomialsLocal->AccelerationPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, 0.0 , MaxAcceleration, (*ThisCurrentTime));
90  }
91  else
92  {
93  PolynomialsLocal->PositionPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients((0.5 * (-MaxAcceleration)), (*ThisCurrentVelocity), (*ThisCurrentPosition), (*ThisCurrentTime));
94  PolynomialsLocal->VelocityPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, (-MaxAcceleration) , (*ThisCurrentVelocity), (*ThisCurrentTime));
95  PolynomialsLocal->AccelerationPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, 0.0 , (-MaxAcceleration), (*ThisCurrentTime));
96  }
97 
98  PolynomialsLocal->PolynomialTimes[PolynomialsLocal->ValidPolynomials] = (*ThisCurrentTime) + TimeForCurrentStep;
99  PolynomialsLocal->ValidPolynomials++;
100 
101  //calculate values for the end of the current time interval
102  *ThisCurrentTime += (TimeForCurrentStep);
103  *ThisCurrentPosition += 0.5 * (*ThisCurrentVelocity + MaxVelocity)
104  * TimeForCurrentStep;
105  *ThisCurrentVelocity = MaxVelocity;
106 
107  return;
108 }
109 
110 
111 //****************************************************************************
112 // VToZeroStep2()
113 
114 void TypeIIRMLMath::VToZeroStep2( double *ThisCurrentTime
115  , double *ThisCurrentPosition
116  , double *ThisCurrentVelocity
117  , const double &MaxAcceleration
118  , MotionPolynomials *PolynomialsLocal
119  , const bool &Inverted )
120 {
121  double TimeForCurrentStep = *ThisCurrentVelocity / MaxAcceleration;
122 
123  if(Inverted)
124  {
125  PolynomialsLocal->PositionPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients((0.5 * MaxAcceleration), (-(*ThisCurrentVelocity)), (-(*ThisCurrentPosition)), (*ThisCurrentTime));
126  PolynomialsLocal->VelocityPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, MaxAcceleration , (-(*ThisCurrentVelocity)), (*ThisCurrentTime));
127  PolynomialsLocal->AccelerationPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, 0.0 , MaxAcceleration, (*ThisCurrentTime));
128  }
129  else
130  {
131  PolynomialsLocal->PositionPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients((0.5 * (-MaxAcceleration)), (*ThisCurrentVelocity), (*ThisCurrentPosition), (*ThisCurrentTime));
132  PolynomialsLocal->VelocityPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, (-MaxAcceleration) , (*ThisCurrentVelocity), (*ThisCurrentTime));
133  PolynomialsLocal->AccelerationPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, 0.0 , (-MaxAcceleration), (*ThisCurrentTime));
134  }
135 
136  PolynomialsLocal->PolynomialTimes[PolynomialsLocal->ValidPolynomials] = (*ThisCurrentTime) + TimeForCurrentStep;
137  PolynomialsLocal->ValidPolynomials++;
138 
139  //calculate values for the end of the current time interval
140  *ThisCurrentTime += (TimeForCurrentStep);
141  *ThisCurrentPosition += 0.5 * (*ThisCurrentVelocity) * TimeForCurrentStep;
142  *ThisCurrentVelocity = 0.0;
143 
144  return;
145 }
TypeIIRMLMath::MotionPolynomials::PolynomialTimes
double PolynomialTimes[MAXIMAL_NO_OF_POLYNOMIALS]
An array of ending times in seconds.
Definition: TypeIIRMLPolynomial.h:273
TypeIIRMLMath::VToVMaxStep2
void VToVMaxStep2(double *ThisCurrentTime, double *ThisCurrentPosition, double *ThisCurrentVelocity, const double &MaxVelocity, const double &MaxAcceleration, MotionPolynomials *PolynomialsLocal, const bool &Inverted)
One intermediate Step 2 trajectory segment: v -> vmax (NegLin)
Definition: TypeIIRMLStep2IntermediateProfiles.cpp:74
TypeIIRMLMath::MotionPolynomials::AccelerationPolynomial
TypeIIRMLPolynomial AccelerationPolynomial[MAXIMAL_NO_OF_POLYNOMIALS]
An array of acceleration polynomials.
Definition: TypeIIRMLPolynomial.h:318
TypeIIRMLMath::NegateStep2
void NegateStep2(double *ThisCurrentPosition, double *ThisCurrentVelocity, double *ThisTargetPosition, double *ThisTargetVelocity, bool *Inverted)
Negate input values.
Definition: TypeIIRMLStep2IntermediateProfiles.cpp:55
TypeIIRMLStep1IntermediateProfiles.h
Header file for intermediate profile segments of Step 1.
TypeIIRMLMath::MotionPolynomials
Three arrays of TypeIIRMLMath::TypeIIRMLPolynomial.
Definition: TypeIIRMLPolynomial.h:260
TypeIIRMLMath.h
Header file for functions and definitions of constant values and macros.
TypeIIRMLPolynomial.h
Header file for the class TypeIIRMLMath::TypeIIRMLPolynomial and the struct TypeIIRMLMath::MotionPoly...
TypeIIRMLMath::MotionPolynomials::VelocityPolynomial
TypeIIRMLPolynomial VelocityPolynomial[MAXIMAL_NO_OF_POLYNOMIALS]
An array of velocity polynomials.
Definition: TypeIIRMLPolynomial.h:303
TypeIIRMLMath::VToZeroStep2
void VToZeroStep2(double *ThisCurrentTime, double *ThisCurrentPosition, double *ThisCurrentVelocity, const double &MaxAcceleration, MotionPolynomials *PolynomialsLocal, const bool &Inverted)
One intermediate Step 2 trajectory segment: v -> 0 (NegLin)
Definition: TypeIIRMLStep2IntermediateProfiles.cpp:114
TypeIIRMLMath::TypeIIRMLPolynomial::SetCoefficients
void SetCoefficients(const double &Coeff2, const double &Coeff1, const double &Coeff0, const double &Diff)
Sets the coefficients of the polynomial object.
Definition: TypeIIRMLPolynomial.cpp:74
TypeIIRMLStep2IntermediateProfiles.h
Header file for intermediate profile segments of Step 2.
TypeIIRMLMath::MotionPolynomials::ValidPolynomials
unsigned char ValidPolynomials
The number of polynomials in use (0 ... MAXIMAL_NO_OF_POLYNOMIALS)
Definition: TypeIIRMLPolynomial.h:327
TypeIIRMLMath::MotionPolynomials::PositionPolynomial
TypeIIRMLPolynomial PositionPolynomial[MAXIMAL_NO_OF_POLYNOMIALS]
An array of position polynomials.
Definition: TypeIIRMLPolynomial.h:288

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