private Dictionary <Point4Dstep, double> createPointSpeeds(IEnumerable <Point4Dstep> points) { var junctionLimits = createJunctionLimits(points); var calculationPlan = new PlanBuilder(); var speedLimits = new Dictionary <Point4Dstep, double>(); var currentSpeedF = 1.0 * Configuration.TimerFrequency / Configuration.StartDeltaT; var stepAcceleration = 1; Point4Dstep previousPoint = null; foreach (var point in points.Reverse()) { var junctionLimitF = junctionLimits[point]; var newSpeedF = currentSpeedF + stepAcceleration; if (previousPoint != null) { var currentSpeed = new Speed((int)(currentSpeedF), Configuration.TimerFrequency); var junctionSpeed = new Speed((int)(junctionLimitF), Configuration.TimerFrequency); var stepsX = previousPoint.X - point.X; var stepsY = previousPoint.Y - point.Y; var newSpeed = calculationPlan.AddLineXY(stepsX, stepsY, currentSpeed, Configuration.MaxPlaneAcceleration, junctionSpeed); newSpeedF = 1.0 * newSpeed.StepCount / newSpeed.Ticks * Configuration.TimerFrequency; } currentSpeedF = Math.Min(newSpeedF, junctionLimitF); if (double.IsNaN(currentSpeedF)) { throw new NotSupportedException("invalid computation"); } speedLimits[point] = currentSpeedF; previousPoint = point; } previousPoint = null; currentSpeedF = 1.0 * Configuration.TimerFrequency / Configuration.StartDeltaT; foreach (var point in points) { var speedLimitF = speedLimits[point]; var newSpeedF = currentSpeedF + stepAcceleration; if (previousPoint != null) { var currentSpeed = new Speed((int)(currentSpeedF), Configuration.TimerFrequency); var limitSpeed = new Speed((int)(speedLimitF), Configuration.TimerFrequency); var stepsX = previousPoint.X - point.X; var stepsY = previousPoint.Y - point.Y; var newSpeed = calculationPlan.AddLineXY(stepsX, stepsY, currentSpeed, Configuration.MaxPlaneAcceleration, limitSpeed); newSpeedF = 1.0 * newSpeed.StepCount / newSpeed.Ticks * Configuration.TimerFrequency; } currentSpeedF = Math.Min(newSpeedF, speedLimitF); speedLimits[point] = currentSpeedF; previousPoint = point; //Debug.WriteLine(currentVelocity); } return(speedLimits); }
/// <summary> /// Creates the plan connecting coordinates by lines with continuous speed. /// </summary> /// <param name="trajectory">Trajectory which plan will be created.</param> /// <returns>The created plan.</returns> public PlanBuilder CreateContinuousPlan(Trajectory4D trajectory) { var pointSpeeds = createPointSpeeds(trajectory.Points); var planBuilder = new PlanBuilder(); var currentSpeed = Speed.Zero; iterateDistances(trajectory, (p, x, y) => { var targetSpeedF = pointSpeeds[p]; var targetSpeed = new Speed((int)(targetSpeedF), Configuration.TimerFrequency); planBuilder.AddLineXY(x, y, currentSpeed, Configuration.MaxPlaneAcceleration, targetSpeed); currentSpeed = targetSpeed; }); return(planBuilder); }