internal override void BuildPlan(PlanBuilder3D builder, MillingWorkspacePanel workspace)
{
var reliefPoints = TransformedShapeDefinition;
builder.GotoZeroLevel();
var maxDepth = _millingDepth;
var upDown = true;
for (var x = 0; x < _width; ++x)
{
for (var i = 0; i < _height; ++i)
{
var y = upDown ? i : _height - i - 1;
var point = reliefPoints[x, y];
if (point.Z > maxDepth)
{
point = new Point3Dmm(point.X, point.Y, maxDepth);
}
builder.AddCuttingSpeedTransition(new Point2Dmm(point.X, point.Y), point.Z);
}
upDown = !upDown;
}
builder.GotoTransitionLevel();
builder.AddRampedLine(EntryPoint);
}
internal override void BuildPlan(PlanBuilder3D builder, MillingWorkspacePanel workspace)
{
/*
* for (var i = 0; i < 100; ++i)
* {
* builder.AddRampedLine(new Point3Dmm(PositionX , PositionY, builder.ZeroLevel + 5));
* builder.AddRampedLine(new Point3Dmm(PositionX, PositionY, builder.ZeroLevel));
* }
* return;
*/
var maxZ = _toolPath.Targets.Select(t => t.End.Z).Max();
var minZ = _toolPath.Targets.Select(t => t.End.Z).Min();
foreach (var target in _toolPath.Targets)
{
var p = target.End;
var p3Dmm = new Point3Dmm(p.X + PositionX, -p.Y + PositionY, -p.Z + builder.ZeroLevel);
if (target.MotionMode == MotionMode.IsLinearRapid)
{
//builder.AddRampedLine(new Point3Dmm(p3Dmm.X, p3Dmm.Y, builder.ZeroLevel));
builder.AddRampedLine(p3Dmm);
//builder.AddCuttingLine(p3Dmm);
}
else
{
builder.AddCuttingLine(p3Dmm);
}
}
builder.AddRampedLine(getEntryPoint());
}
internal override void BuildPlan(PlanBuilder3D builder, MillingWorkspacePanel workspace)
{
builder.GotoZeroLevel();
var maxDepth = _millingDepth;
var upDown = true;
var stepLength = 1.0;
var resolutionX = _shapeMetricSize.Width / stepLength;
var resolutionY = _shapeMetricSize.Height / stepLength;
for (var xi = 0.0; xi < resolutionX; xi += stepLength)
{
for (var yi = 0.0; yi < resolutionY; yi += stepLength)
{
var xRatio = 1.0 * xi / resolutionX;
var yRatio = 1.0 * yi / resolutionY;
var depth = _shapeMap.GetHeight(xRatio, yRatio) * _millingDepth;
var x = _shapeMetricSize.Width * xRatio;
var y = _shapeMetricSize.Height * yRatio;
if (!upDown)
{
y = _shapeMetricSize.Height - y;
}
var point = new Point3Dmm(PositionX + x, PositionY + y, depth);
builder.AddCuttingSpeedTransition(new Point2Dmm(point.X, point.Y), point.Z);
}
upDown = !upDown;
}
builder.GotoTransitionLevel();
builder.AddRampedLine(EntryPoint);
}
public PlanPart3D(Point3Dmm currentPoint, Point3Dmm endPoint, Acceleration accelerationRamp, Speed speedLimit)
{
StartPoint = currentPoint;
EndPoint = endPoint;
AccelerationRamp = accelerationRamp;
SpeedLimit = speedLimit;
}
internal HeadCNC(Color headColor, MillingWorkspacePanel parent)
{
var brush = new SolidColorBrush(headColor);
brush.Opacity = 0.8;
_headPen = new Pen(brush, 4.0);
_parent = parent;
Position = new Point3Dmm(0, 0, 0);
}
private void toSteps(Point3D p, out int x, out int y, out int z)
{
var pmm = new Point3Dmm(p.X, p.Y, p.Z);
var pStep = pmm.As3Dstep();
x = pStep.X;
y = pStep.Y;
z = pStep.Z;
}
private void moveTo(Point3Dmm target, Acceleration accelerationRamp, Speed speedLimit)
{
_currentLevel = target.Z;
if (_currentPoint.DistanceSquaredTo(target) > 0.0)
{
_plan.Add(new PlanPart3D(_currentPoint, target, accelerationRamp, speedLimit));
}
_currentPoint = target;
}
public static Point4Dstep GetStepDiff(Point3Dmm startPoint, Point3Dmm endPoint)
{
var startSteps = GetPositionRev(startPoint);
var targetSteps = GetPositionRev(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);
return(new Point4Dstep(diffU, diffV, diffX, diffY));
}
private void executePlan()
{
if (!Cnc.IsHomeCalibrated)
{
ShowError("Calibration is required!");
return;
}
disableChanges();
var state = Cnc.PlannedState;
var currentPosition = PlanBuilder3D.GetPosition(state);
var startPoint = Workspace.EntryPoint;
var start = new Point3Dmm(startPoint.PositionX, startPoint.PositionY, _zLevel);
var aboveStart = new Point3Dmm(start.X, start.Y, currentPosition.Z);
var builder = new PlanBuilder3D(currentPosition.Z, _zLevel, Workspace.CuttingSpeed, Configuration.MaxPlaneSpeed, Configuration.MaxPlaneAcceleration);
builder.SetPosition(currentPosition);
builder.AddRampedLine(aboveStart);
try
{
if (_zLevel <= aboveStart.Z)
{
throw new PlanningException("Level Z is above the current position.");
}
Workspace.BuildPlan(builder);
}
catch (PlanningException ex)
{
planCompleted();
ShowError(ex.Message);
return;
}
var plan = builder.Build();
if (!Cnc.Check(plan))
{
planCompleted();
ShowError("Plan overflows the workspace!");
return;
}
CheckEngineDialog.WaitForConfirmation(() =>
{
executePlan(builder);
});
}
private Point3Dmm getCurrentPosition()
{
var currentU = Cnc.EstimationU;
var currentV = Cnc.EstimationV;
var currentX = Cnc.EstimationX;
var currentY = Cnc.EstimationY;
var prePosition = PlanBuilder3D.GetPositionFromSteps(currentU, currentV, currentX, currentY);
var position = new Point3Dmm(prePosition.X, prePosition.Y, prePosition.Z);
return(position);
}
public void GotoZ(double zLevel)
{
var nextTarget = new Point3Dmm(_currentPoint.X, _currentPoint.Y, zLevel + _zeroLevel);
if (nextTarget.Z < _currentPoint.Z)
{
AddRampedLine(nextTarget);
}
else
{
AddCuttingLine(nextTarget);
}
}
/// <summary>
/// Rotates given point according to current rotation angle.
/// </summary>
protected Point3Dmm rotate(Point3Dmm point)
{
var c1 = 0.5;
var c2 = 0.5;
var centeredX = point.X - c1;
var centeredY = point.Y - c2;
var rotatedX = centeredX * _rotationCos - centeredY * _rotationSin;
var rotatedY = centeredY * _rotationCos + centeredX * _rotationSin;
return(new Point3Dmm(
rotatedX + c1, rotatedY + c2, point.Z
));
}
protected Point3Dmm[,] definitionTransformation(double[,] relief)
{
var result = new Point3Dmm[_width, _height];
for (var xi = 0; xi < _width; ++xi)
{
for (var yi = 0; yi < _height; ++yi)
{
var x = 1.0 * xi / _width;
var y = 1.0 * yi / _height;
var z = relief[xi, yi];
var definitionPoint = new Point3Dmm(x, y, z);
var point = rotate(definitionPoint);
point = new Point3Dmm(point.X * _shapeMetricSize.Width + PositionX, point.Y * _shapeMetricSize.Height + PositionY, point.Z);
result[xi, yi] = point;
}
}
return(result);
}
public void AddCuttingLine(Point3Dmm target)
{
moveTo(target, null, CuttingSpeed);
}
public void AddCuttingSpeedTransition(Point2Dmm target, double zLevel)
{
var zTarget = new Point3Dmm(target.C1, target.C2, zLevel + _zeroLevel);
moveTo(zTarget, null, CuttingSpeed);
}
private void executePlan()
{
if (!Cnc.IsHomeCalibrated)
{
ShowError("Calibration is required!");
return;
}
disableMotionCommands();
Workspace.DisableChanges();
var state = Cnc.PlannedState;
var currentPosition = PlanBuilder3D.GetPosition(state);
var startPoint = Workspace.EntryPoint;
var start = new Point3Dmm(startPoint.PositionX, startPoint.PositionY, _zLevel);
var aboveStart = new Point3Dmm(start.X, start.Y, currentPosition.Z);
var builder = new PlanBuilder3D(currentPosition.Z, _zLevel, Workspace.CuttingSpeed, Configuration.MaxPlaneSpeed, Configuration.MaxPlaneAcceleration);
builder.SetPosition(currentPosition);
builder.AddRampedLine(aboveStart);
try
{
if (_zLevel <= aboveStart.Z)
{
throw new PlanningException("Level Z is above the current position.");
}
Workspace.BuildPlan(builder);
}
catch (PlanningException ex)
{
planCompleted();
ShowError(ex.Message);
return;
}
var plan = builder.Build();
if (!Cnc.Check(plan))
{
planCompleted();
ShowError("Plan overflows the workspace!");
return;
}
CheckEngineDialog.WaitForConfirmation(() =>
{
/**/
builder.SetStreamingCuttingSpeed(getCuttingSpeed());
builder.StreamingIsComplete += planCompleted;/*/
* Cnc.SEND(plan);
* Cnc.OnInstructionQueueIsComplete += planCompleted;/**/
_streamingBuilder = builder;
_planStart = DateTime.Now;
builder.StreamInstructions(Cnc);
this.Focus();
});
}
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();
}
public void AddRampedLine(Point3Dmm target)
{
moveTo(target, PlaneAcceleration, TransitionSpeed);
}
public static Point4Dstep ToStepsRev(Point3Dmm point)
{
return(ToStepsRev(point.X, point.Y, point.Z));
}
public static Point4Dmm GetPositionRev(Point3Dmm point)
{
return(new Point4Dmm(point.Y, point.X, point.Y, point.Z));
}
protected Point ConvertToVisual(Point3Dmm point)
{
return(ConvertToVisual(new Point2Dmm(point.X, point.Y)));
}
public void SetPosition(Point3Dmm target)
{
_currentPoint = target;
}
