void TypeIIRMLMath::TypeIIRMLDecisionTree1C( const double &CurrentPosition
, const double &CurrentVelocity
, const double &TargetPosition
, const double &TargetVelocity
, const double &MaxVelocity
, const double &MaxAcceleration
, double *AlternativeExecutionTime)
{
double ThisCurrentPosition = CurrentPosition
, ThisCurrentVelocity = CurrentVelocity
, ThisTargetPosition = TargetPosition
, ThisTargetVelocity = TargetVelocity ;
*AlternativeExecutionTime = 0.0;
// ********************************************************************
if (Decision_1C__001(ThisCurrentVelocity))
{
goto MDecision_1C__002;
}
else
{
NegateStep1( &ThisCurrentPosition
, &ThisCurrentVelocity
, &ThisTargetPosition
, &ThisTargetVelocity );
goto MDecision_1C__002;
}
// ********************************************************************
MDecision_1C__002:
if (Decision_1C__002( ThisCurrentVelocity
, MaxVelocity ))
{
goto MDecision_1C__003;
}
else
{
VToVMaxStep1( AlternativeExecutionTime
, &ThisCurrentPosition
, &ThisCurrentVelocity
, MaxVelocity
, MaxAcceleration );
goto MDecision_1C__003;
}
// ********************************************************************
MDecision_1C__003:
VToZeroStep1( AlternativeExecutionTime
, &ThisCurrentPosition
, &ThisCurrentVelocity
, MaxAcceleration );
NegateStep1( &ThisCurrentPosition
, &ThisCurrentVelocity
, &ThisTargetPosition
, &ThisTargetVelocity );
if (Decision_1C__003( ThisCurrentPosition
, ThisCurrentVelocity
, ThisTargetPosition
, ThisTargetVelocity
, MaxVelocity
, MaxAcceleration ))
{
*AlternativeExecutionTime += ProfileStep1PosTriNegLin( ThisCurrentPosition
, ThisCurrentVelocity
, ThisTargetPosition
, ThisTargetVelocity
, MaxAcceleration );
}
else
{
*AlternativeExecutionTime += ProfileStep1PosTrapNegLin( ThisCurrentPosition
, ThisCurrentVelocity
, ThisTargetPosition
, ThisTargetVelocity
, MaxVelocity
, MaxAcceleration );
}
// ********************************************************************
return;
}
void TypeIIRMLMath::TypeIIRMLDecisionTree1A( const double &CurrentPosition
, const double &CurrentVelocity
, const double &TargetPosition
, const double &TargetVelocity
, const double &MaxVelocity
, const double &MaxAcceleration
, Step1_Profile *AppliedProfile
, double *MinimalExecutionTime )
{
bool IntermediateInversion = false;
double ThisCurrentPosition = CurrentPosition
, ThisCurrentVelocity = CurrentVelocity
, ThisTargetPosition = TargetPosition
, ThisTargetVelocity = TargetVelocity ;
*MinimalExecutionTime = 0.0;
// ********************************************************************
if (Decision_1A__001(ThisCurrentVelocity))
{
goto MDecision_1A__002;
}
else
{
NegateStep1( &ThisCurrentPosition
, &ThisCurrentVelocity
, &ThisTargetPosition
, &ThisTargetVelocity );
goto MDecision_1A__002;
}
// ********************************************************************
MDecision_1A__002:
if (Decision_1A__002( ThisCurrentVelocity
, MaxVelocity ))
{
goto MDecision_1A__003;
}
else
{
VToVMaxStep1( MinimalExecutionTime
, &ThisCurrentPosition
, &ThisCurrentVelocity
, MaxVelocity
, MaxAcceleration );
goto MDecision_1A__003;
}
// ********************************************************************
MDecision_1A__003:
if (Decision_1A__003( ThisCurrentVelocity
, ThisTargetVelocity ))
{
goto MDecision_1A__004;
}
else
{
goto MDecision_1A__006;
}
// ********************************************************************
MDecision_1A__004:
if (Decision_1A__004( ThisCurrentPosition
, ThisCurrentVelocity
, ThisTargetPosition
, ThisTargetVelocity
, MaxAcceleration ))
{
goto MDecision_1A__005;
}
else
{
VToZeroStep1( MinimalExecutionTime
, &ThisCurrentPosition
, &ThisCurrentVelocity
, MaxAcceleration );
NegateStep1( &ThisCurrentPosition
, &ThisCurrentVelocity
, &ThisTargetPosition
, &ThisTargetVelocity );
IntermediateInversion = true;
goto MDecision_1A__009;
}
// ********************************************************************
MDecision_1A__005:
if (Decision_1A__005( ThisCurrentPosition
, ThisCurrentVelocity
, ThisTargetPosition
, ThisTargetVelocity
, MaxVelocity
, MaxAcceleration ))
{
*MinimalExecutionTime += ProfileStep1PosLinHldNegLin( ThisCurrentPosition
, ThisCurrentVelocity
, ThisTargetPosition
, ThisTargetVelocity
, MaxVelocity
, MaxAcceleration );
if (IntermediateInversion)
{
*AppliedProfile = Step1_Profile_NegLinHldPosLin;
}
else
{
*AppliedProfile = Step1_Profile_PosLinHldNegLin;
}
goto END_OF_THIS_FUNCTION;
}
else
{
*MinimalExecutionTime += ProfileStep1PosLinNegLin( ThisCurrentPosition
, ThisCurrentVelocity
, ThisTargetPosition
, ThisTargetVelocity
, MaxAcceleration );
if (IntermediateInversion)
{
*AppliedProfile = Step1_Profile_NegLinPosLin;
}
else
{
*AppliedProfile = Step1_Profile_PosLinNegLin;
}
goto END_OF_THIS_FUNCTION;
}
// ********************************************************************
MDecision_1A__006:
if (Decision_1A__006(ThisTargetVelocity))
{
goto MDecision_1A__007;
}
else
{
goto MDecision_1A__008;
}
// ********************************************************************
MDecision_1A__007:
if (Decision_1A__007( ThisCurrentPosition
, ThisCurrentVelocity
, ThisTargetPosition
, ThisTargetVelocity
, MaxAcceleration ))
{
VToZeroStep1( MinimalExecutionTime
, &ThisCurrentPosition
, &ThisCurrentVelocity
, MaxAcceleration );
NegateStep1( &ThisCurrentPosition
, &ThisCurrentVelocity
, &ThisTargetPosition
, &ThisTargetVelocity );
IntermediateInversion = true;
goto MDecision_1A__009;
}
else
{
goto MDecision_1A__005;
}
// ********************************************************************
MDecision_1A__008:
if (Decision_1A__008( ThisCurrentPosition
, ThisCurrentVelocity
, ThisTargetPosition
, ThisTargetVelocity
, MaxAcceleration ))
{
VToZeroStep1( MinimalExecutionTime
, &ThisCurrentPosition
, &ThisCurrentVelocity
, MaxAcceleration );
NegateStep1( &ThisCurrentPosition
, &ThisCurrentVelocity
, &ThisTargetPosition
, &ThisTargetVelocity );
IntermediateInversion = true;
goto MDecision_1A__005;
}
else
{
goto MDecision_1A__009;
}
// ********************************************************************
MDecision_1A__009:
if (Decision_1A__009( ThisCurrentPosition
, ThisCurrentVelocity
, ThisTargetPosition
, ThisTargetVelocity
, MaxVelocity
, MaxAcceleration ))
{
*MinimalExecutionTime += ProfileStep1PosTriNegLin( ThisCurrentPosition
, ThisCurrentVelocity
, ThisTargetPosition
, ThisTargetVelocity
, MaxAcceleration );
if (IntermediateInversion)
{
*AppliedProfile = Step1_Profile_NegTriPosLin;
}
else
{
*AppliedProfile = Step1_Profile_PosTriNegLin;
}
goto END_OF_THIS_FUNCTION;
}
else
{
*MinimalExecutionTime += ProfileStep1PosTrapNegLin( ThisCurrentPosition
, ThisCurrentVelocity
, ThisTargetPosition
, ThisTargetVelocity
, MaxVelocity
, MaxAcceleration );
if (IntermediateInversion)
{
*AppliedProfile = Step1_Profile_NegTrapPosLin;
}
else
{
*AppliedProfile = Step1_Profile_PosTrapNegLin;
}
goto END_OF_THIS_FUNCTION;
}
// ********************************************************************
END_OF_THIS_FUNCTION:
return;
}
