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);
}
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);
}
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());
}
public void SetPosition(int newPosition)
{
var steps = newPosition - _currentPosition;
var builder = new Planning.PlanBuilder();
//builder.AddRampedSteps(steps, Constants.FastestDeltaT);
builder.AddConstantSpeedTransitionXY(steps, steps, Configuration.ReverseSafeSpeed);
builder.DuplicateXYtoUV();
_cnc.SEND(builder.Build());
//position setting is blocking for now
while (_cnc.IncompleteInstructionCount > 0)
{
System.Threading.Thread.Sleep(1);
}
_currentPosition = newPosition;
}
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);
}
