public void ProcessProgram(ActionProgram.ActionProgram program) { ExecutionProgram = program; if (program == null) { ExecutionTime = 0; return; } decimal time = 0; foreach (var action in program.Actions) { if (!(action.action is RTAction ma)) { continue; } if (!(ma.Command is IRTMoveCommand move)) { continue; } var len = move.Length; var feed = move.Options.Feed; var t = len / feed; time += t; } ExecutionTime = time; }
public CNCState.CNCState ProcessLineMove(decimal?X, decimal?Y, decimal?Z, bool fast, ActionProgram.ActionProgram program, CNCState.CNCState state) { if (X == null && Y == null && Z == null) { return(state); } state = state.BuildCopy(); Vector3 delta; Vector3 compensation; Vector3 targetPosition; (delta, compensation, targetPosition) = FindMovement(state, state.AxisState.TargetPosition, state.AxisState.Position, X, Y, Z); state.AxisState.TargetPosition = targetPosition; if (fast) { return(program.AddFastLineMovement(delta, compensation, state)); } else { return(program.AddLineMovement(delta, compensation, state.AxisState.Feed, state)); } }
public CNCState.CNCState ProcessToolStart(int tool_id, IReadOnlyDictionary <string, decimal> options, ActionProgram.ActionProgram program, CNCState.CNCState state) { state = state.BuildCopy(); IToolState toolState = state.ToolStates[tool_id]; if (toolState is SpindleState ss) { ss.SpindleSpeed = options["speed"]; if (options["ccw"] != 0) { ss.RotationState = SpindleState.SpindleRotationState.CounterClockwise; } else { ss.RotationState = SpindleState.SpindleRotationState.Clockwise; } } else if (toolState is BinaryState bs) { bs.Enabled = true; } return(CommitTool(tool_id, program, state)); }
public CNCState.CNCState ProcessToolChange(int tool, ActionProgram.ActionProgram program, CNCState.CNCState state) { var newstate = state.BuildCopy(); program.AddToolChange(tool); // change tool return(newstate); }
public CNCState.CNCState CoordinatesSystemSet(int cs, ActionProgram.ActionProgram program, CNCState.CNCState state) { state = state.BuildCopy(); state.AxisState.Params.CurrentCoordinateSystemIndex = cs; return(state); }
public CNCState.CNCState AddZProbe(ActionProgram.ActionProgram program, CNCState.CNCState state) { var after = state.BuildCopy(); after.AxisState.Position.z = 0; program.AddZProbe(state, after); program.AddAction(new SyncCoordinates(stateSyncManager, state.AxisState.Position), state, null); return(state); }
public CNCState.CNCState ProcessSyncToolStop(int tool_id, ActionProgram.ActionProgram program, CNCState.CNCState state) { var newstate = state.BuildCopy(); newstate.SyncToolState.Tool = tool_id; newstate.SyncToolState.Enabled = false; program.DisableRTTool(tool_id, state, newstate); return(newstate); }
public void ProcessProgram(ActionProgram.ActionProgram program) { List <MoveActionChain> chains = new List <MoveActionChain>(); MoveActionChain chain = new MoveActionChain(); var actions = program.Actions; Logger.Instance.Debug(this, "optimize", "begin optimization"); int index = 1; int na = actions.Count; foreach (var action in actions) { Logger.Instance.Debug(this, "optimize", string.Format("Process action #{0} / {1}", index, na)); var ma = action.action as RTAction; if (ma == null || ma.RequireFinish || ma.Command as IRTMoveCommand == null) { if (ma == null || ma.RequireFinish) { if (chain.Actions.Count > 0) { var subc = SplitChainByDirectionFlip(chain); chains.AddRange(subc); chain = new MoveActionChain(); } } } else { var cmd = ma.Command as IRTMoveCommand; chain.Actions.Add(cmd); } index += 1; } if (chain.Actions.Count > 0) { var subc = SplitChainByDirectionFlip(chain); chains.AddRange(subc); } index = 1; int nc = chains.Count; foreach (var c in chains) { Logger.Instance.Debug(this, "optimize", string.Format("Optimize chain #{0} / {1}. It has {2} actions", index, nc, c.Actions.Count)); OptimizeChain(c); index += 1; } Logger.Instance.Debug(this, "optimize", "ready optimization"); }
public CNCState.CNCState FinishProgram(ActionProgram.ActionProgram program, CNCState.CNCState state) { program.AddPlaceholder(state); Logger.Instance.Debug(this, "build", "finish build program"); Logger.Instance.Debug(this, "build", "move feed limit"); moveFeedLimiter.ProcessProgram(program); Logger.Instance.Debug(this, "build", "optimize program"); optimizer.ProcessProgram(program); Logger.Instance.Debug(this, "build", "calculate time of execution"); timeCalculator.ProcessProgram(program); Logger.Instance.Debug(this, "build", string.Format("program ready. Time = {0}", timeCalculator.ExecutionTime)); return(state); }
public CNCState.CNCState BeginProgram(ActionProgram.ActionProgram program, CNCState.CNCState state) { state = state.BuildCopy(); var currentPos = state.AxisState.Params.CurrentCoordinateSystem.ToLocal(state.AxisState.TargetPosition); state.VarsState.Vars["x"] = currentPos.x; state.VarsState.Vars["y"] = currentPos.y; state.VarsState.Vars["z"] = currentPos.z; Logger.Instance.Debug(this, "build", "start build program"); program.AddConfiguration(state); program.AddRTUnlock(state); return(state); }
public CNCState.CNCState ProcessToolStop(int tool_id, ActionProgram.ActionProgram program, CNCState.CNCState state) { state = state.BuildCopy(); IToolState toolState = state.ToolStates[tool_id]; if (toolState is SpindleState ss) { ss.RotationState = SpindleState.SpindleRotationState.Off; } else if (toolState is BinaryState bs) { bs.Enabled = false; } return(CommitTool(tool_id, program, state)); }
private CNCState.CNCState CommitTool(int tool_id, ActionProgram.ActionProgram program, CNCState.CNCState state) { IAction action = null; IDriver driver = tool_drivers[tool_id]; if (driver is N700E_driver n700e) { SpindleState ss = state.ToolStates[tool_id] as SpindleState; action = n700e.CreateAction(ss.RotationState, ss.SpindleSpeed); } else if (driver is GPIO_driver gpio) { BinaryState bs = state.ToolStates[tool_id] as BinaryState; action = gpio.CreateAction(bs.Enabled); } else if (driver is RawModbus_driver modbus) { BinaryState bs = state.ToolStates[tool_id] as BinaryState; action = modbus.CreateAction(bs.Enabled); } else if (driver is Dummy_driver dummy) { action = dummy.CreateAction(); } else { throw new InvalidOperationException("invalid driver: " + driver); } if (action != null) { program.AddAction(action, state, state); return(state); } else { return(state); } }
public CNCState.CNCState CoordinatesSet(decimal?X, decimal?Y, decimal?Z, ActionProgram.ActionProgram program, CNCState.CNCState state) { state = state.BuildCopy(); if (X != null) { state.AxisState.Params.CurrentCoordinateSystem.Offset.x = state.AxisState.Position.x - ConvertSizes(X.Value, state); } if (Y != null) { state.AxisState.Params.CurrentCoordinateSystem.Offset.y = state.AxisState.Position.y - ConvertSizes(Y.Value, state); } if (Z != null) { state.AxisState.Params.CurrentCoordinateSystem.Offset.z = state.AxisState.Position.z - ConvertSizes(Z.Value, state); } state.AxisState.TargetPosition = state.AxisState.Position; return(state); }
public void ProcessProgram(ActionProgram.ActionProgram program) { foreach (var action in program.Actions) { if (!(action.action is RTAction ma)) { continue; } if (ma.Command is RTLineMoveCommand linemovecmd) { Vector3 dir = linemovecmd.DirStart; // Equal to DirEnd var(maxfeed, maxacc) = MaxLineFeedAcc(dir); linemovecmd.Options.acceleration = maxacc; linemovecmd.Options.Feed = Math.Min(maxfeed, linemovecmd.Options.Feed); } if (ma.Command is RTArcMoveCommand arcmovecmd) { decimal R = arcmovecmd.R; decimal maxfeed = (decimal)Math.Sqrt((double)(R * config.max_acceleration)); arcmovecmd.Options.Feed = Math.Min(arcmovecmd.Options.Feed, maxfeed); } } }
public ExpectedTimeCalculator() { ExecutionTime = 0; ExecutionProgram = null; }
public CNCState.CNCState ProcessDrillingMove(decimal?X, decimal?Y, decimal?Z, decimal?R, decimal?Q, ActionProgram.ActionProgram program, CNCState.CNCState state) { state = state.BuildCopy(); // TODO Handle G17, 18, 19 Vector3 delta, compensation; var coordinateSystem = state.AxisState.Params.CurrentCoordinateSystem; if (R != null) { state.DrillingState.RetractHeightLocal = R.Value; } if (Z != null) { state.DrillingState.DrillHeightLocal = Z.Value; } if (Q != null) { state.DrillingState.PeckDepth = Math.Abs(Q.Value); } if (state.DrillingState.Peck && state.DrillingState.PeckDepth == 0) { throw new InvalidOperationException("Pecking with depth = 0"); } #region Positioning if (X == null && Y == null) { return(state); } Vector3 topPosition; (delta, compensation, topPosition) = FindMovement(state, state.AxisState.TargetPosition, state.AxisState.Position, X, Y, null); state.AxisState.TargetPosition = topPosition; state = program.AddFastLineMovement(delta, compensation, state); #endregion Positioning var absmode = state.AxisState.Absolute; state.AxisState.Absolute = true; #region R height Vector3 rPosition; (delta, compensation, rPosition) = FindMovement(state, state.AxisState.TargetPosition, state.AxisState.Position, null, null, state.DrillingState.RetractHeightLocal); state.AxisState.TargetPosition = rPosition; state = program.AddFastLineMovement(delta, compensation, state); #endregion R height // TODO: dwelling, etc Vector3 currentDrillPosition = state.AxisState.TargetPosition; Vector3 bottomPosition; (_, _, bottomPosition) = FindMovement(state, state.AxisState.TargetPosition, state.AxisState.Position, null, null, state.DrillingState.DrillHeightLocal); decimal startHeight = coordinateSystem.ToLocal(rPosition).z; decimal preparationHeight = startHeight; decimal currentHeight = startHeight; decimal finishHeight = coordinateSystem.ToLocal(bottomPosition).z; decimal peckMax; if (state.DrillingState.Peck) { peckMax = state.DrillingState.PeckDepth; } else { peckMax = decimal.MaxValue; } while (currentHeight != finishHeight) { decimal targetHeight; decimal deltaHeight = finishHeight - currentHeight; if (deltaHeight > peckMax) { deltaHeight = peckMax; } if (deltaHeight < -peckMax) { deltaHeight = -peckMax; } targetHeight = currentHeight + deltaHeight; #region preparation if (preparationHeight != startHeight) { (delta, compensation, currentDrillPosition) = FindMovement(state, state.AxisState.TargetPosition, state.AxisState.Position, null, null, preparationHeight); state.AxisState.TargetPosition = currentDrillPosition; state = program.AddFastLineMovement(delta, compensation, state); } #endregion #region drilling (delta, compensation, currentDrillPosition) = FindMovement(state, state.AxisState.TargetPosition, state.AxisState.Position, null, null, targetHeight); state.AxisState.TargetPosition = currentDrillPosition; state = program.AddLineMovement(delta, compensation, state.AxisState.Feed, state); #endregion #region retracting (delta, compensation, currentDrillPosition) = FindMovement(state, state.AxisState.TargetPosition, state.AxisState.Position, null, null, startHeight); state.AxisState.TargetPosition = currentDrillPosition; state = program.AddFastLineMovement(delta, compensation, state); #endregion preparationHeight = currentHeight; currentHeight = targetHeight; } #region Retract switch (state.DrillingState.RetractDepth) { case DrillingState.RetractDepthType.InitialHeight: delta = topPosition - state.AxisState.TargetPosition; compensation = state.AxisState.TargetPosition - state.AxisState.Position; state.AxisState.TargetPosition = topPosition; state = program.AddFastLineMovement(delta, compensation, state); break; case DrillingState.RetractDepthType.RHeight: break; default: throw new InvalidOperationException("Unknown retract depth state"); } #endregion // Restore mode state.AxisState.Absolute = absmode; return(state); }
public CNCState.CNCState ProcessArcMove(decimal?X, decimal?Y, decimal?Z, decimal?I, decimal?J, decimal?K, decimal?R, bool ccw, ActionProgram.ActionProgram program, CNCState.CNCState state) { if (X == null && Y == null && Z == null) { return(state); } state = state.BuildCopy(); Vector3 delta; Vector3 compensation; Vector3 targetPosition; (delta, compensation, targetPosition) = FindMovement(state, state.AxisState.TargetPosition, state.AxisState.Position, X, Y, Z); state.AxisState.TargetPosition = targetPosition; if (R == null && I == null && J == null && K == null) { decimal r; switch (state.AxisState.Axis) { case AxisState.Plane.XY: r = (new Vector2(delta.x, delta.y)).Length() / 2; break; case AxisState.Plane.YZ: r = (new Vector2(delta.y, delta.z)).Length() / 2; break; case AxisState.Plane.ZX: r = (new Vector2(delta.z, delta.x)).Length() / 2; break; default: throw new InvalidOperationException(); } return(program.AddArcMovement(delta, compensation, r, ccw, state.AxisState.Axis, state.AxisState.Feed, state)); } else if (R != null) { var r = ConvertSizes(R.Value, state); return(program.AddArcMovement(delta, compensation, r, ccw, state.AxisState.Axis, state.AxisState.Feed, state)); } else { decimal i = 0; decimal j = 0; decimal k = 0; if (I != null) { i = ConvertSizes(I.Value, state); } if (J != null) { j = ConvertSizes(J.Value, state); } if (K != null) { k = ConvertSizes(K.Value, state); } return(program.AddArcMovement(delta, compensation, new Vector3(i, j, k), ccw, state.AxisState.Axis, state.AxisState.Feed, state)); } }
public CNCState.CNCState AddPause(decimal dt, ActionProgram.ActionProgram program, CNCState.CNCState state) { program.AddDelay((int)dt, state); return(state); }