/// <summary>
/// Creates the plan connecting coordinates by ramped lines.
/// </summary>
/// <param name="trajectory">Trajectory which plan will be created.</param>
/// <returns>The created plan.</returns>
public PlanBuilder CreateRampedPlan(Trajectory4D trajectory)
{
var planBuilder = new PlanBuilder();
iterateDistances(trajectory, (p, u, v, x, y) => planBuilder.AddRampedLineUVXY(u, v, x, y, Configuration.MaxPlaneAcceleration, Configuration.MaxPlaneSpeed));
return(planBuilder);
}
private double getRampTime(PlanPart3D part)
{
if (!_rampTimeCache.TryGetValue(part, out var time))
{
var builder = new PlanBuilder();
var diff = PlanBuilder3D.GetStepDiff(part.StartPoint, part.EndPoint);
builder.AddRampedLineUVXY(diff.U, diff.V, diff.X, diff.Y, part.AccelerationRamp, part.SpeedLimit);
var instructions = builder.Build();
var accumulator = 0.0;
foreach (var instruction in instructions)
{
var axes = instruction as Axes;
if (axes == null)
{
continue;
}
accumulator += axes.CalculateTotalTime() * 1.0 / Configuration.TimerFrequency;
}
_rampTimeCache[part] = time = accumulator;
}
return(time);
}
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());
}
