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); }