public static double GetBrakingDistance(this SimpleP2PCalculator calculator, double t)
{
calculator.GetStatus(t, out _, out var a, out var v, out _);
var motionParameter = new MotionParameter(calculator.JerkMax, -calculator.JerkMax, calculator.AccelerationMax, -calculator.AccelerationMax);
var rampResult = ExtendedP2PCalculator.Calculate(a, v, 0, motionParameter);
return(rampResult.Length);
}
private bool CalculateProfile()
{
var velocityPoints = new List <VelocityPoint>()
{
new VelocityPoint(0, InitialAcceleration, InitialVelocity, null)
};
for (var i = 0; i < EffectiveConstraints.Count; i++)
{
var constraint = EffectiveConstraints[i];
var nextConstraint = EffectiveConstraints.ElementAtOrDefault(i + 1);
var a0 = i == 0 ? InitialAcceleration : 0.0;
var v0 = velocityPoints.Last().Velocity;
var v1 = constraint.MaximumVelocity;
var v2 = nextConstraint?.MaximumVelocity ?? 0.0;
var startDistance = velocityPoints.Max(v => v.Distance);
var situation = GetSituation(v0, constraint.MaximumVelocity, nextConstraint?.MaximumVelocity ?? 0);
if (situation == 3)
{
var distanceForAcc = ExtendedP2PCalculator.CalculateDistanceNeeded(a0, v0, v1, Parameters);
if (distanceForAcc < constraint.Length)
{
velocityPoints.Add(new VelocityPoint(startDistance + distanceForAcc, v1, constraint));
velocityPoints.Add(new VelocityPoint(startDistance + constraint.Length, v1, constraint));
}
else
{
if (MergeWithPreviousConstraint(constraint, i))
{
RemoveVelocityPointsOfLastConstraint(velocityPoints);
}
return(false);
}
}
else if (situation == 4 || situation == 5)
{
throw new JointMotionCalculationException($"Situation {situation} must not appear! Something went wrong before");
}
else if (situation == 7)
{
velocityPoints.Add(new VelocityPoint(startDistance + constraint.Length, v1, constraint));
}
else if (situation == 8)
{
var brakeDistance = ExtendedP2PCalculator.CalculateDistanceNeeded(v1, v2, Parameters);
if (brakeDistance <= constraint.Length)
{
velocityPoints.Add(new VelocityPoint(startDistance + (constraint.Length - brakeDistance), v1, constraint));
velocityPoints.Add(new VelocityPoint(startDistance + constraint.Length, v2, constraint));
}
else
{
if (MergeWithPreviousConstraint(constraint, i))
{
RemoveVelocityPointsOfLastConstraint(velocityPoints);
}
return(false);
}
}
else if (!IterativlyFindSteppedDownVelocity(a0, v0, constraint, v2, startDistance, velocityPoints))
{
switch (situation)
{
case 1:
MergeWithNextConstraint(constraint, i);
break;
case 2:
case 6:
if (MergeWithPreviousConstraint(constraint, i))
{
RemoveVelocityPointsOfLastConstraint(velocityPoints);
}
break;
default:
throw new JointMotionCalculationException($"Unhandled situation id {situation}");
}
return(false);
}
}
// remove ProfilePoints with same distance and velocity
velocityPoints = velocityPoints.DistinctBy(pp => new { pp.Distance, pp.Velocity }).ToList();
// calculate ramp results and times
var rampResults = new List <ExtendedRampCalculationResult>();
var times = new List <double>();
var timeSum = 0.0;
for (var i = 0; i < velocityPoints.Count - 1; i++)
{
var pFrom = velocityPoints[i];
var pTo = velocityPoints[i + 1];
var ramp = new ExtendedRampCalculationResult(ExtendedP2PCalculator.Calculate(pFrom.Acceleration, pFrom.Velocity, pTo.Velocity, Parameters), pFrom.Distance, timeSum);
var duration = ramp.Direction == RampDirection.Constant ? (pTo.Distance - pFrom.Distance) / pTo.Velocity : ramp.TotalDuration;
if (ramp.Direction == RampDirection.Constant)
{
ramp.Length = pTo.Distance - pFrom.Distance;
ramp.TotalDuration = duration;
}
rampResults.Add(ramp);
if (ramp.Direction != RampDirection.Constant && Math.Abs(ramp.Length - (pTo.Distance - pFrom.Distance)) > 1e-8)
{
throw new JointMotionCalculationException($"Calculated distance differs from velocityPoints distance", InputSet);
}
if (double.IsNaN(duration) || double.IsInfinity(duration))
{
throw new JointMotionCalculationException($"Invalid duration ({duration}) on point {i} at {pFrom.Distance}", InputSet);
}
timeSum += duration;
times.Add(timeSum);
}
VelocityProfilePoints = velocityPoints;
RampResults = rampResults;
TimesAtVelocityPoints = times;
TotalDuration = timeSum;
if (double.IsNaN(TotalDuration) || double.IsInfinity(TotalDuration))
{
throw new JointMotionCalculationException($"Invalid TotalDuration ({TotalDuration})");
}
return(true);
}
