示例#1
0
        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);
        }
示例#2
0
        /// <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);
        }