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