/// <summary> /// Creates the plan connecting coordinates constant speed without acceleration. /// </summary> /// <param name="trajectory">Trajectory which plan will be created.</param> /// <returns>The created plan.</returns> public PlanBuilder CreateConstantPlan(Trajectory4D trajectory, IEnumerable <Tuple <Speed, Speed> > segmentSpeeds = null) { if (segmentSpeeds == null) { segmentSpeeds = new Tuple <Speed, Speed> [0]; } var speeds = new Queue <Tuple <Speed, Speed> >(segmentSpeeds); var planBuilder = new PlanBuilder(); iterateDistances(trajectory, (p, u, v, x, y) => { var speedUV = _transitionSpeed; var speedXY = _transitionSpeed; if (speeds.Any()) { var speed = speeds.Dequeue(); speedUV = speed.Item1; speedXY = speed.Item2; } planBuilder.AddConstantSpeedTransitionUVXY(u, v, speedUV, x, y, speedXY); }); return(planBuilder); }
internal IEnumerable <InstructionCNC> NextRampInstructions() { if (_currentInstructionOffset != 0) { throw new NotSupportedException("Cannot generate next instruction when offset is present"); } var part = _plan[_currentIndex]; var diff = PlanBuilder3D.GetStepDiff(part.StartPoint, part.EndPoint); var builder = new PlanBuilder(); if (part.AccelerationRamp == null) { builder.AddConstantSpeedTransitionUVXY(diff.U, diff.V, part.SpeedLimit, diff.X, diff.Y, part.SpeedLimit); _currentConstantDistance += part.StartPoint.DistanceTo(part.EndPoint); } else { builder.AddRampedLineUVXY(diff.U, diff.V, diff.X, diff.Y, part.AccelerationRamp, part.SpeedLimit); _currentRampTime += getRampTime(part); } var plan = builder.Build(); ++_currentIndex; _currentInstructionOffset = 0; return(plan); }
public IEnumerable <InstructionCNC> Build() { var builder = new PlanBuilder(); foreach (var part in _plan) { var startSteps = GetPositionRev(part.StartPoint); var targetSteps = GetPositionRev(part.EndPoint); var diffU = ToStep(targetSteps.U - startSteps.U); var diffV = ToStep(targetSteps.V - startSteps.V); var diffX = ToStep(targetSteps.X - startSteps.X); var diffY = ToStep(targetSteps.Y - startSteps.Y); if (part.AccelerationRamp == null) { builder.AddConstantSpeedTransitionUVXY(diffU, diffV, part.SpeedLimit, diffX, diffY, part.SpeedLimit); } else { builder.AddRampedLineUVXY(diffU, diffV, diffX, diffY, part.AccelerationRamp, part.SpeedLimit); } } return(builder.Build()); }
internal IEnumerable <InstructionCNC> ShiftByConstantSpeed(double lengthLimit, Speed speed) { var part = _plan[_currentIndex]; var length = part.EndPoint.DistanceTo(part.StartPoint); if (length == 0) { _currentInstructionOffset = 0; ++_currentIndex; return(new InstructionCNC[0]); } var currentLength = Math.Min(length - _currentInstructionOffset, lengthLimit); var lastEnd = part.StartPoint.ShiftTo(part.EndPoint, _currentInstructionOffset / length); _currentInstructionOffset += currentLength; var currentEnd = part.StartPoint.ShiftTo(part.EndPoint, _currentInstructionOffset / length); var diff1 = PlanBuilder3D.GetStepDiff(lastEnd, part.EndPoint); var diff2 = PlanBuilder3D.GetStepDiff(currentEnd, part.EndPoint); if (_currentInstructionOffset >= length) { _currentInstructionOffset = 0; _currentConstantDistance += length; ++_currentIndex; } var builder = new PlanBuilder(); builder.AddConstantSpeedTransitionUVXY(diff1.U - diff2.U, diff1.V - diff2.V, speed, diff1.X - diff2.X, diff1.Y - diff2.Y, speed); var plan = builder.Build(); return(plan); }