/// <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);
}
/// <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);
}
/// <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)
{
var planBuilder = new PlanBuilder();
iterateDistances(trajectory, (p, x, y) => planBuilder.AddConstantSpeedTransitionXY(x, y, _transitionSpeed));
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);
}
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());
}
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);
}
/// <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);
}
internal PlanBuilder FillBuilder()
{
var builder = new PlanBuilder();
for (var i = 0; i < _pathPlansX.Count; ++i)
{
var planX = _pathPlansX[i];
var planY = _pathPlansY[i];
System.Diagnostics.Debug.WriteLine("PathTracer");
System.Diagnostics.Debug.WriteLine("\tX: " + planX);
System.Diagnostics.Debug.WriteLine("\tY: " + planY);
builder.AddXY(planX, planY);
}
return(builder);
}
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);
}
