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