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