func (vm *Machine) setMoveMode(stmt *gcode.Block) {
if w, err := stmt.GetModalGroup("motionGroup"); err == nil {
if w != nil {
if w.Address != 'G' {
unknownCommand("motionGroup", w)
}
switch w.Command {
case 0:
vm.State.MoveMode = MoveModeRapid
case 1:
vm.State.MoveMode = MoveModeLinear
case 2:
vm.State.MoveMode = MoveModeCWArc
case 3:
vm.State.MoveMode = MoveModeCCWArc
case 80:
vm.State.MoveMode = MoveModeNone
default:
unknownCommand("motionGroup", w)
}
stmt.Remove(w)
}
} else {
propagate(err)
}
}
func (vm *Machine) setCoolant(stmt *gcode.Block) {
// TODO Handle M7 and M8 on same line
if w, err := stmt.GetModalGroup("coolantGroup"); err == nil {
if w != nil {
if w.Address != 'M' {
unknownCommand("coolantGroup", w)
}
switch w.Command {
case 7:
vm.State.MistCoolant = true
case 8:
vm.State.FloodCoolant = true
case 9:
vm.State.MistCoolant = false
vm.State.FloodCoolant = false
default:
unknownCommand("coolantGroup", w)
}
stmt.Remove(w)
}
} else {
propagate(err)
}
}
func (vm *Machine) feedRateMode(stmt *gcode.Block) {
if w, err := stmt.GetModalGroup("feedRateModeGroup"); err == nil {
if w != nil {
if w.Address != 'G' {
unknownCommand("feedRateModeGroup", w)
}
oldMode := vm.State.FeedMode
switch w.Command {
case 93:
vm.State.FeedMode = FeedModeInvTime
case 94:
vm.State.FeedMode = FeedModeUnitsMin
case 95:
vm.State.FeedMode = FeedModeUnitsRev
default:
unknownCommand("feedRateModeGroup", w)
}
if vm.State.FeedMode != oldMode {
// Ensure that feedrate is cleared
vm.State.Feedrate = 0
}
stmt.Remove(w)
}
} else {
propagate(err)
}
}
func (vm *Machine) postCheck(stmt *gcode.Block) {
for _, w := range stmt.Nodes {
if _, ok := w.(*gcode.Word); ok {
panic(fmt.Sprintf("Unsupported commands left in block: %s", stmt.Export(-1)))
}
}
}
func (vm *Machine) setSpindle(stmt *gcode.Block) {
if w, err := stmt.GetModalGroup("spindleGroup"); err == nil {
if w != nil {
if w.Address != 'M' {
unknownCommand("spindleGroup", w)
}
switch w.Command {
case 3:
vm.State.SpindleEnabled = true
vm.State.SpindleClockwise = true
case 4:
vm.State.SpindleEnabled = true
vm.State.SpindleClockwise = false
case 5:
vm.State.SpindleEnabled = false
default:
unknownCommand("spindleGroup", w)
}
stmt.Remove(w)
}
} else {
propagate(err)
}
}
func (vm *Machine) lineNumber(stmt *gcode.Block) {
if stmt.Length() > 0 {
if w, ok := stmt.Nodes[0].(*gcode.Word); ok {
if w.Address == 'N' {
// We just ignore and consume the line number
stmt.Remove(w)
}
}
}
}
func (vm *Machine) feedRate(stmt *gcode.Block) {
if val, err := stmt.GetWord('F'); err == nil {
if vm.Imperial {
val *= 25.4
}
vm.State.Feedrate = val
stmt.RemoveAddress('F')
} else if vm.State.FeedMode == FeedModeInvTime {
vm.State.Feedrate = -1
}
}
func (vm *Machine) setCoordinateSystem(stmt *gcode.Block) {
// TODO Implement coordinate system offsets!
if w, err := stmt.GetModalGroup("coordinateSystemGroup"); err == nil {
if w != nil {
if w.Address != 'G' {
unknownCommand("coordinateSystemGroup", w)
}
if vm.State.CutterCompensation != CutCompModeNone {
invalidCommand("coordinateSystemGroup", "coordinate system select", "Coordinate system change attempted with cutter compensation enabled")
}
switch w.Command {
case 54:
vm.CoordinateSystem.SelectCoordinateSystem(1)
case 55:
vm.CoordinateSystem.SelectCoordinateSystem(2)
case 56:
vm.CoordinateSystem.SelectCoordinateSystem(3)
case 57:
vm.CoordinateSystem.SelectCoordinateSystem(4)
case 58:
vm.CoordinateSystem.SelectCoordinateSystem(5)
case 59:
vm.CoordinateSystem.SelectCoordinateSystem(6)
case 59.1:
vm.CoordinateSystem.SelectCoordinateSystem(7)
case 59.2:
vm.CoordinateSystem.SelectCoordinateSystem(8)
case 59.3:
vm.CoordinateSystem.SelectCoordinateSystem(9)
default:
unknownCommand("coordinateSystemGroup", w)
}
stmt.Remove(w)
}
} else {
propagate(err)
}
}
func (vm *Machine) setUnits(stmt *gcode.Block) {
if w, err := stmt.GetModalGroup("unitsGroup"); err == nil {
if w != nil {
if w.Address != 'G' {
unknownCommand("unitsGroup", w)
}
switch w.Command {
case 20:
vm.Imperial = true
case 21:
vm.Imperial = false
default:
unknownCommand("unitsGroup", w)
}
stmt.Remove(w)
}
} else {
propagate(err)
}
}
func (vm *Machine) setArcDistanceMode(stmt *gcode.Block) {
if w, err := stmt.GetModalGroup("arcDistanceModeGroup"); err == nil {
if w != nil {
if w.Address != 'G' {
unknownCommand("arcDistanceModeGroup", w)
}
switch w.Command {
case 90.1:
vm.AbsoluteArc = true
case 91.1:
vm.AbsoluteArc = false
default:
unknownCommand("arcDistanceModeGroup", w)
}
stmt.Remove(w)
}
} else {
propagate(err)
}
}
func (vm *Machine) setStop(stmt *gcode.Block) {
// TODO implement
if w, err := stmt.GetModalGroup("stoppingGroup"); err == nil {
if w != nil {
if w.Address != 'M' {
unknownCommand("stoppingGroup", w)
}
switch w.Command {
case 2:
vm.Completed = true
case 30:
vm.Completed = true
default:
unknownCommand("stoppingGroup", w)
}
stmt.Remove(w)
}
} else {
propagate(err)
}
}
func (vm *Machine) setPlane(stmt *gcode.Block) {
if w, err := stmt.GetModalGroup("planeSelectionGroup"); err == nil {
if w != nil {
if w.Address != 'G' {
unknownCommand("planeSelectionGroup", w)
}
switch w.Command {
case 17:
vm.MovePlane = PlaneXY
case 18:
vm.MovePlane = PlaneXZ
case 19:
vm.MovePlane = PlaneYZ
default:
unknownCommand("planeSelectionGroup", w)
}
stmt.Remove(w)
}
} else {
propagate(err)
}
}
func (vm *Machine) toolChange(stmt *gcode.Block) {
if w, err := stmt.GetModalGroup("toolChangeGroup"); err == nil {
if w != nil {
if w.Address != 'M' {
unknownCommand("toolChangeGroup", w)
}
switch w.Command {
case 6:
if vm.NextTool == -1 {
panic("Toolchange attempted without a defined tool")
}
vm.State.Tool = vm.NextTool
default:
unknownCommand("toolChangeGroup", w)
}
stmt.Remove(w)
}
} else {
propagate(err)
}
}
func (vm *Machine) setCutterCompensation(stmt *gcode.Block) {
if w, err := stmt.GetModalGroup("cutterCompensationModeGroup"); err == nil {
if w != nil {
if w.Address != 'G' {
unknownCommand("cutterCompensationModeGroup", w)
}
switch w.Command {
case 40:
vm.State.CutterCompensation = CutCompModeNone
case 41:
vm.State.CutterCompensation = CutCompModeOuter
case 42:
vm.State.CutterCompensation = CutCompModeInner
default:
unknownCommand("cutterCompensationModeGroup", w)
}
stmt.Remove(w)
}
} else {
propagate(err)
}
}
func (vm *Machine) performMove(stmt *gcode.Block) {
if !stmt.IncludesOneOf('X', 'Y', 'Z') {
// Nothing to do
return
}
s := vm.State
if s.FeedMode == FeedModeInvTime && s.Feedrate == -1 && s.MoveMode != MoveModeRapid {
invalidCommand("motionGroup", "rapid", "Non-rapid inverse time feed mode move attempted without a set feedrate")
}
if vm.CoordinateSystem.OverrideActive() {
if s.CutterCompensation != CutCompModeNone {
invalidCommand("motionGroup", "move", "Coordinate override attempted with cutter compensation enabled")
}
if s.MoveMode == MoveModeCWArc || s.MoveMode == MoveModeCCWArc {
invalidCommand("motionGroup", "arc", "Coordinate override attempted for arc")
}
}
if s.MoveMode == MoveModeCWArc || s.MoveMode == MoveModeCCWArc {
// Arc
newX, newY, newZ, newI, newJ, newK := vm.calcPos(*stmt)
vm.arc(newX, newY, newZ, newI, newJ, newK, stmt.GetWordDefault('P', 1))
stmt.RemoveAddress('X', 'Y', 'Z', 'I', 'J', 'K', 'P')
} else if s.MoveMode == MoveModeLinear || s.MoveMode == MoveModeRapid {
// Line
newX, newY, newZ, _, _, _ := vm.calcPos(*stmt)
vm.move(newX, newY, newZ)
stmt.RemoveAddress('X', 'Y', 'Z')
} else {
invalidCommand("motionGroup", "move", fmt.Sprintf("Move attempted without an active move mode [%s]", stmt.Export(-1)))
}
}
func (vm *Machine) setToolLength(stmt *gcode.Block) {
if w, err := stmt.GetModalGroup("toolLengthGroup"); err == nil {
if w != nil {
if w.Address != 'G' {
unknownCommand("toolLengthGroup", w)
}
switch w.Command {
case 43:
// Ignore!
fmt.Fprintf(os.Stderr, "Warning: G43 issued and ignored.\n")
stmt.RemoveAddress('H')
default:
unknownCommand("toolLengthGroup", w)
}
stmt.Remove(w)
}
} else {
propagate(err)
}
}
// Calculates the absolute position of the given statement, including optional I, J, K parameters.
// Units are converted, and coordinate system applied unless overridden.
func (vm *Machine) calcPos(stmt gcode.Block) (newX, newY, newZ, newI, newJ, newK float64) {
pos := vm.curPos()
var err error
coordinateSystem := vm.CoordinateSystem.GetCoordinateSystem()
if vm.CoordinateSystem.OverrideActive() {
oldAbsolute := vm.AbsoluteMove
vm.AbsoluteMove = true
defer func() {
vm.AbsoluteMove = oldAbsolute
}()
}
if newX, err = stmt.GetWord('X'); err != nil {
newX = pos.X
} else {
if vm.Imperial {
newX *= 25.4
}
if !vm.AbsoluteMove {
newX += pos.X
} else {
newX += coordinateSystem.X
}
}
if newY, err = stmt.GetWord('Y'); err != nil {
newY = pos.Y
} else {
if vm.Imperial {
newY *= 25.4
}
if !vm.AbsoluteMove {
newY += pos.Y
} else {
newY += coordinateSystem.Y
}
}
if newZ, err = stmt.GetWord('Z'); err != nil {
newZ = pos.Z
} else {
if vm.Imperial {
newZ *= 25.4
}
if !vm.AbsoluteMove {
newZ += pos.Z
} else {
newZ += coordinateSystem.Z
}
}
newI = stmt.GetWordDefault('I', 0.0)
newJ = stmt.GetWordDefault('J', 0.0)
newK = stmt.GetWordDefault('K', 0.0)
if vm.Imperial {
newI *= 25.4
newJ *= 25.4
newK *= 25.4
}
if !vm.AbsoluteArc {
newI += pos.X
newJ += pos.Y
newK += pos.Z
} else {
newI += coordinateSystem.X
newJ += coordinateSystem.Y
newZ += coordinateSystem.Z
}
return newX, newY, newZ, newI, newJ, newK
}
func (vm *Machine) spindleSpeed(stmt *gcode.Block) {
if val, err := stmt.GetWord('S'); err == nil {
vm.State.SpindleSpeed = val
stmt.RemoveAddress('S')
}
}
func (vm *Machine) nextTool(stmt *gcode.Block) {
if val, err := stmt.GetWord('T'); err == nil {
vm.NextTool = int(val)
stmt.RemoveAddress('T')
}
}
func (vm *Machine) nonModals(stmt *gcode.Block) {
if w, err := stmt.GetModalGroup("nonModalGroup"); err == nil {
if w != nil {
if w.Address != 'G' {
unknownCommand("nonModalGroup", w)
}
switch w.Command {
case 4:
if val, err := stmt.GetWord('P'); err == nil {
if val < 0 {
invalidCommand("nonModalGroup", "dwell", "P word negative")
}
vm.dwell(val)
} else {
invalidCommand("nonModalGroup", "dwell", "P word not specified or specified multiple times")
}
stmt.RemoveAddress('P')
case 10:
if val, err := stmt.GetWord('L'); err == nil {
if val == 2 {
// Set coordinate system offsets
if cs, err := stmt.GetWord('P'); err == nil {
cs := int(cs)
x, y, z := stmt.GetWordDefault('X', 0), stmt.GetWordDefault('Y', 0), stmt.GetWordDefault('Z', 0)
x, y, z = vm.axesToMetric(x, y, z)
vm.CoordinateSystem.SetCoordinateSystem(x, y, z, cs)
stmt.RemoveAddress('X', 'Y', 'Z')
} else {
invalidCommand("nonModalGroup", "coordinate system configuration", "P word not specified or specified multiple times")
}
stmt.RemoveAddress('P')
}
} else {
invalidCommand("nonModalGroup", "G10 configuration", "L word not specified or specified multiple times")
}
stmt.RemoveAddress('L')
case 28:
oldMode := vm.State.MoveMode
vm.State.MoveMode = MoveModeRapid
if stmt.IncludesOneOf('X', 'Y', 'Z') {
newX, newY, newZ, _, _, _ := vm.calcPos(*stmt)
vm.move(newX, newY, newZ)
stmt.RemoveAddress('X', 'Y', 'Z')
}
vm.move(vm.StoredPos1.X, vm.StoredPos1.Y, vm.StoredPos1.Z)
vm.State.MoveMode = oldMode
case 28.1:
pos := vm.curPos()
vm.StoredPos1 = pos.Vector()
case 30:
oldMode := vm.State.MoveMode
vm.State.MoveMode = MoveModeRapid
if stmt.IncludesOneOf('X', 'Y', 'Z') {
newX, newY, newZ, _, _, _ := vm.calcPos(*stmt)
vm.move(newX, newY, newZ)
stmt.RemoveAddress('X', 'Y', 'Z')
}
vm.move(vm.StoredPos2.X, vm.StoredPos2.Y, vm.StoredPos2.Z)
vm.State.MoveMode = oldMode
case 30.1:
pos := vm.curPos()
vm.StoredPos2 = pos.Vector()
case 53:
vm.CoordinateSystem.Override()
case 92:
if stmt.IncludesOneOf('X', 'Y', 'Z') {
cp := vm.curPos()
x, y, z := stmt.GetWordDefault('X', 0), stmt.GetWordDefault('Y', 0), stmt.GetWordDefault('Z', 0)
x, y, z = vm.axesToMetric(x, y, z)
vm.CoordinateSystem.DisableOffset()
x, y, z = vm.CoordinateSystem.ApplyCoordinateSystem(x, y, z)
diffX, diffY, diffZ := cp.X-x, cp.Y-y, cp.Z-z
vm.CoordinateSystem.SetOffset(diffX, diffY, diffZ)
vm.CoordinateSystem.EnableOffset()
stmt.RemoveAddress('X', 'Y', 'Z')
} else {
invalidCommand("nonModalGroup", "G92 configuration", "No axis words specified")
}
case 92.1:
vm.CoordinateSystem.EraseOffset()
case 92.2:
vm.CoordinateSystem.DisableOffset()
case 92.3:
vm.CoordinateSystem.EnableOffset()
default:
unknownCommand("nonModalGroup", w)
}
stmt.Remove(w)
}
} else {
propagate(err)
}
}