public static void InstructionGeneration()
{
var p1 = new Point3D(0, 0, 0);
var p2 = new Point3D(0, 100, 0);
var p3 = new Point3D(1000, 5000, 0);
var s1 = new ToolPathSegment(p1, p2, MotionMode.IsLinear);
var s2 = new ToolPathSegment(p2, p3, MotionMode.IsLinear);
var start = DateTime.Now;
var instructions = PlanStreamerContext.GenerateInstructions(new[] { s1, s2 }, 10);
var end = DateTime.Now;
Console.WriteLine((end - start).TotalSeconds);
Console.ReadKey();
var xInstructions = new List <StepInstrution>();
var yInstructions = new List <StepInstrution>();
foreach (Axes instruction in instructions)
{
xInstructions.Add(instruction.InstructionU);
yInstructions.Add(instruction.InstructionV);
Console.WriteLine($"{instruction.InstructionU} {instruction.InstructionV}");
}
var ranges = PathSpeedLimitCalculator.AccelerationRangesMetric.Select(r => Speed.FromMilimetersPerSecond(r).ToDeltaT()).ToArray();
var previousRange = ranges[0];
foreach (var range in ranges)
{
Console.WriteLine($"{range} {previousRange - range}");
previousRange = range;
}
}
internal void AddSegment(ToolPathSegment segment)
{
if (_lastAddedSegment != null)
{
var edgeLimit = PathSpeedLimitCalculator.CalculateEdgeLimit(_lastAddedSegment, segment);
_edgeLimits[segment] = edgeLimit;
}
_lastAddedSegment = segment;
var segmentInfo = new SegmentPlanningInfo(segment);
_workSegments.Enqueue(segmentInfo);
foreach (var limitCalculator in _openLimitCalculators)
{
limitCalculator.AddLookaheadSegments(new[] { segment }, _edgeLimits);
}
_openLimitCalculators.Add(segmentInfo.LimitCalculator);
foreach (var limitCalculator in _openLimitCalculators.ToArray())
{
if (!limitCalculator.NeedsMoreFollowingSegments)
{
_openLimitCalculators.Remove(limitCalculator);
}
}
}
public ToolPathSegmentSlicer(ToolPathSegment segment)
{
Segment = segment;
toSteps(segment.Start, out var sX, out var sY, out var sZ);
toSteps(segment.End, out var eX, out var eY, out var eZ);
var segmentV = segment.End - segment.Start;
_totalLength = segmentV.Length;
var se = segment.End;
var ss = segment.Start;
_totalX = eX - sX;
_totalY = eY - sY;
_totalZ = eZ - sZ;
var ratio = segmentV;
ratio.Normalize();
_x = new ChannelSlicer(_totalLength, _totalX, ratio.X);
_y = new ChannelSlicer(_totalLength, _totalY, ratio.Y);
_z = new ChannelSlicer(_totalLength, _totalZ, ratio.Z);
}
public PathSpeedLimitCalculator(ToolPathSegment activeSegment)
{
ActiveSegment = activeSegment;
_lookaheadDistance = 0.0;
_cornerLimit = Configuration.ReverseSafeSpeed.ToMetric();
_optimisticCornerLimit = Configuration.MaxPlaneSpeed.ToMetric();
}
public static void CalculateRatios(ToolPathSegment segment, out double rX, out double rY, out double rZ)
{
var v = segment.End - segment.Start;
v.Normalize();
rX = v.X;
rY = v.Y;
rZ = v.Z;
}
public static double CalculateEdgeLimit(ToolPathSegment segment1, ToolPathSegment segment2)
{
if (segment1 == null)
{
return(Configuration.ReverseSafeSpeed.ToMetric());
}
CalculateRatios(segment1, out var rX1, out var rY1, out var rZ1);
CalculateRatios(segment2, out var rX2, out var rY2, out var rZ2);
var limitX = GetAxisLimit(rX1, rX2);
var limitY = GetAxisLimit(rY1, rY2);
var limitZ = GetAxisLimit(rZ1, rZ2);
return(Math.Min(Math.Min(limitX, limitY), limitZ));
}
public static void CornerLimits()
{
var p1 = new Point3D(0, 0, 0);
var p2 = new Point3D(0, 100, 0);
var p3 = new Point3D(100, 500, 0);
var s1 = new ToolPathSegment(p1, p2, MotionMode.IsLinear);
var s2 = new ToolPathSegment(p2, p3, MotionMode.IsLinear);
var limitCalculator = new PathSpeedLimitCalculator(s1);
var l1 = limitCalculator.GetLimit(0);
var l2 = limitCalculator.GetLimit(0.99);
var l3 = limitCalculator.GetLimit(0.999);
var cornerLimit = PathSpeedLimitCalculator.CalculateEdgeLimit(s1, s2);
var edgeLimits = new Dictionary <ToolPathSegment, double>();
edgeLimits[s2] = cornerLimit;
limitCalculator.AddLookaheadSegments(new[] { s2 }, edgeLimits);
var al1 = limitCalculator.GetLimit(0);
var al2 = limitCalculator.GetLimit(0.99);
var al3 = limitCalculator.GetLimit(0.999);
}
public static void LineSlicing()
{
Configuration.EnableRouterMode();
System.Globalization.CultureInfo customCulture = (System.Globalization.CultureInfo)Thread.CurrentThread.CurrentCulture.Clone();
customCulture.NumberFormat.NumberDecimalSeparator = ".";
Thread.CurrentThread.CurrentCulture = customCulture;
SystemUtilities.PreventSleepMode();
var speed1Ticks = 900;
var speed2Ticks = 10000;
var timeGrainTicks = 3000;
var lengthMm = 100;
var v = new Vector(
ControllerCNC.Primitives.Speed.FromDeltaT(speed1Ticks).ToMetric(),
ControllerCNC.Primitives.Speed.FromDeltaT(speed2Ticks).ToMetric()
);
var timeGrain = 1.0 * timeGrainTicks / Configuration.TimerFrequency;
var speed = v.Length;
v = v * lengthMm;
var segment = new ToolPathSegment(new Point3D(0, 0, 0), new Point3D(v.X, v.Y, 0), MotionMode.IsLinearRapid);
var logger = new StepLogger(".");
var slicer = new ToolPathSegmentSlicer(segment);
for (var i = 0; i < 15; ++i)
{
var instruction = slicer.Slice(speed, timeGrain);
logger.LogInstruction(instruction);
}
logger.Flush();
}
internal SegmentPlanningInfo(ToolPathSegment segment)
{
Segment = segment;
LimitCalculator = new PathSpeedLimitCalculator(segment);
}
private void _stream(DriverCNC2 driver)
{
var context = new PlanStreamerContext();
foreach (var part in PlanParts)
{
var s = part.StartPoint;
var e = part.EndPoint;
var segment = new ToolPathSegment(new Point3D(s.X, s.Y, s.Z), new Point3D(e.X, e.Y, e.Z), MotionMode.IsLinear);
context.AddSegment(segment);
}
var zeroCompatibleSpeed = Configuration.ReverseSafeSpeed.ToMetric();
var instructionBuffer = new Queue <InstructionCNC>();
var maxPlannedInstructionCount = 5;
while (!context.IsComplete)
{
while (_stop && context.CurrentSpeed <= zeroCompatibleSpeed)
{
_isStreaming = false;
Thread.Sleep(10);
}
_isStreaming = true;
if (driver.IncompleteInstructionCount >= maxPlannedInstructionCount)
{
//wait some time so we are not spinning like crazy
Thread.Sleep(1);
continue;
}
do
{
double speed;
lock (_L_speed)
speed = _stream_cuttingSpeed;
if (_stop)
{
speed = zeroCompatibleSpeed;
}
var instruction = context.GenerateNextInstruction(speed);
instructionBuffer.Enqueue(instruction);
} while (driver.IncompleteInstructionCount == 0 && !context.IsComplete && instructionBuffer.Count < maxPlannedInstructionCount);
while (instructionBuffer.Count > 0)
{
var instruction = instructionBuffer.Dequeue();
driver.SEND(instruction);
}
}
while (driver.IncompleteInstructionCount > 0)
{
//wait until all instructions are completed
Thread.Sleep(10);
}
StreamingIsComplete?.Invoke();
}
private void _stream(DriverCNC2 driver)
{
PlanStreamerContext intermContext = null;
_context = new PlanStreamerContext(true);
foreach (var part in PlanParts)
{
var s = part.StartPoint;
var e = part.EndPoint;
var segment = new ToolPathSegment(new Point3D(s.X, s.Y, s.Z), new Point3D(e.X, e.Y, e.Z), MotionMode.IsLinear);
_context.AddSegment(segment);
}
var zeroCompatibleSpeed = Configuration.ReverseSafeSpeed.ToMetric();
var instructionBuffer = new Queue <InstructionCNC>();
var maxPlannedInstructionCount = 5;
var currentContext = _context;
while (!currentContext.IsComplete || intermContext != null)
{
if (intermContext != null && intermContext.IsComplete)
{
intermContext = null;
currentContext = _context;
continue;
}
while (_stop && _context.CurrentSpeed <= zeroCompatibleSpeed)
{
_isStreaming = false;
Thread.Sleep(10);
}
_isStreaming = true;
if (IsChangingPosition)
{
// create intermediate context which will transfer machine to the new position
var np = CurrentStreamPosition.As3Dmm();
var cp = driver.CurrentState.As3Dstep().As3Dmm();
var cpTransition = new Point3Dmm(cp.X, cp.Y, _transitionLevel);
var npTransition = new Point3Dmm(np.X, np.Y, _transitionLevel);
intermContext = new PlanStreamerContext(false);
intermContext.AddSegment(new ToolPathSegment(cp.As3D(), cpTransition.As3D(), MotionMode.IsLinear));
intermContext.AddSegment(new ToolPathSegment(cpTransition.As3D(), npTransition.As3D(), MotionMode.IsLinear));
intermContext.AddSegment(new ToolPathSegment(npTransition.As3D(), np.As3D(), MotionMode.IsLinear));
currentContext = intermContext;
IsChangingPosition = false;
}
if (driver.IncompleteInstructionCount >= maxPlannedInstructionCount)
{
//wait some time so we are not spinning like crazy
Thread.Sleep(1);
continue;
}
do
{
double speed;
lock (_L_speed)
{
speed = _stream_cuttingSpeed;
}
if (_stop)
{
speed = zeroCompatibleSpeed;
}
var instruction = currentContext.GenerateNextInstruction(speed, stopRemainingTimeLockstep: _stop);
instructionBuffer.Enqueue(instruction);
} while (driver.IncompleteInstructionCount == 0 && !currentContext.IsComplete && instructionBuffer.Count < maxPlannedInstructionCount);
while (instructionBuffer.Count > 0)
{
var instruction = instructionBuffer.Dequeue();
if (!driver.SEND(instruction))
{
throw new InvalidOperationException("Instruction was not accepted. (Probably over bounds?). Progress: " + Progress * 100);
}
}
}
while (driver.IncompleteInstructionCount > 0)
{
//wait until all instructions are completed
Thread.Sleep(10);
}
StreamingIsComplete?.Invoke();
}
