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