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