mirror of
https://github.com/sinseman44/PyCNC.git
synced 2026-01-12 02:40:04 +00:00
297 lines
12 KiB
Python
297 lines
12 KiB
Python
from __future__ import division
|
|
import time
|
|
|
|
import cnc.logging_config as logging_config
|
|
from cnc import hal
|
|
from cnc.pulses import *
|
|
from cnc.coordinates import *
|
|
|
|
|
|
class GMachineException(Exception):
|
|
""" Exceptions while processing gcode line.
|
|
"""
|
|
pass
|
|
|
|
|
|
class GMachine(object):
|
|
""" Main object which control and keep state of whole machine: steppers,
|
|
spindle, extruder etc
|
|
"""
|
|
def __init__(self):
|
|
""" Initialization.
|
|
"""
|
|
self._position = Coordinates(0.0, 0.0, 0.0, 0.0)
|
|
# init variables
|
|
self._velocity = 0
|
|
self._spindle_rpm = 0
|
|
self._pause = 0
|
|
self._local = None
|
|
self._convertCoordinates = 0
|
|
self._absoluteCoordinates = 0
|
|
self._plane = None
|
|
self.reset()
|
|
hal.init()
|
|
|
|
def release(self):
|
|
""" Return machine to original position and free all resources.
|
|
"""
|
|
self._spindle(0)
|
|
self.home()
|
|
hal.deinit()
|
|
|
|
def reset(self):
|
|
""" Reinitialize all program configurable thing.
|
|
"""
|
|
self._velocity = 1000
|
|
self._spindle_rpm = 1000
|
|
self._pause = 0
|
|
self._local = Coordinates(0.0, 0.0, 0.0, 0.0)
|
|
self._convertCoordinates = 1.0
|
|
self._absoluteCoordinates = True
|
|
self._plane = PLANE_XY
|
|
|
|
# noinspection PyMethodMayBeStatic
|
|
def _spindle(self, spindle_speed):
|
|
hal.join()
|
|
hal.spindle_control(100.0 * spindle_speed / SPINDLE_MAX_RPM)
|
|
|
|
def __check_delta(self, delta):
|
|
pos = self._position + delta
|
|
if not pos.is_in_aabb(Coordinates(0.0, 0.0, 0.0, 0.0),
|
|
Coordinates(TABLE_SIZE_X_MM, TABLE_SIZE_Y_MM,
|
|
TABLE_SIZE_Z_MM, 0)):
|
|
raise GMachineException("out of effective area")
|
|
|
|
def _move_linear(self, delta, velocity):
|
|
delta = delta.round(1.0 / STEPPER_PULSES_PER_MM_X,
|
|
1.0 / STEPPER_PULSES_PER_MM_Y,
|
|
1.0 / STEPPER_PULSES_PER_MM_Z,
|
|
1.0 / STEPPER_PULSES_PER_MM_E)
|
|
if delta.is_zero():
|
|
return
|
|
self.__check_delta(delta)
|
|
|
|
logging.info("Moving linearly {}".format(delta))
|
|
gen = PulseGeneratorLinear(delta, velocity)
|
|
hal.move(gen)
|
|
# save position
|
|
self._position = self._position + delta
|
|
|
|
@staticmethod
|
|
def __quarter(pa, pb):
|
|
if pa >= 0 and pb >= 0:
|
|
return 1
|
|
if pa < 0 and pb >= 0:
|
|
return 2
|
|
if pa < 0 and pb < 0:
|
|
return 3
|
|
if pa >= 0 and pb < 0:
|
|
return 4
|
|
|
|
def __adjust_circle(self, da, db, ra, rb, direction, pa, pb, ma, mb):
|
|
r = math.sqrt(ra * ra + rb * rb)
|
|
if r == 0:
|
|
raise GMachineException("circle radius is zero")
|
|
sq = self.__quarter(-ra, -rb)
|
|
if da == 0 and db == 0: # full circle
|
|
ea = da
|
|
eb = db
|
|
eq = 5 # mark as non-existing to check all
|
|
else:
|
|
b = (db - rb) / (da - ra)
|
|
ea = math.copysign(math.sqrt(r * r / (1.0 + abs(b))), da - ra)
|
|
eb = math.copysign(math.sqrt(r * r - ea * ea), db - rb)
|
|
eq = self.__quarter(ea, eb)
|
|
ea += ra
|
|
eb += rb
|
|
# iterate coordinates quarters and check if we fit table
|
|
q = sq
|
|
pq = q
|
|
for _ in range(0, 4):
|
|
if direction == CW:
|
|
q -= 1
|
|
else:
|
|
q += 1
|
|
if q <= 0:
|
|
q = 4
|
|
elif q >= 5:
|
|
q = 1
|
|
if q == eq:
|
|
break
|
|
is_raise = False
|
|
if (pq == 1 and q == 4) or (pq == 4 and q == 1):
|
|
is_raise = (pa + ra + r > ma)
|
|
elif (pq == 1 and q == 2) or (pq == 2 and q == 1):
|
|
is_raise = (pb + rb + r > mb)
|
|
elif (pq == 2 and q == 3) or (pq == 3 and q == 2):
|
|
is_raise = (pa + ra - r < 0)
|
|
elif (pq == 3 and q == 4) or (pq == 4 and q == 3):
|
|
is_raise = (pb + rb - r < 0)
|
|
if is_raise:
|
|
raise GMachineException("out of effective area")
|
|
pq = q
|
|
return ea, eb
|
|
|
|
def _circular(self, delta, radius, velocity, direction):
|
|
delta = delta.round(1.0 / STEPPER_PULSES_PER_MM_X,
|
|
1.0 / STEPPER_PULSES_PER_MM_Y,
|
|
1.0 / STEPPER_PULSES_PER_MM_Z,
|
|
1.0 / STEPPER_PULSES_PER_MM_E)
|
|
radius = radius.round(1.0 / STEPPER_PULSES_PER_MM_X,
|
|
1.0 / STEPPER_PULSES_PER_MM_Y,
|
|
1.0 / STEPPER_PULSES_PER_MM_Z,
|
|
1.0 / STEPPER_PULSES_PER_MM_E)
|
|
self.__check_delta(delta)
|
|
# get delta vector and put it on circle
|
|
circle_end = Coordinates(0, 0, 0, 0)
|
|
if self._plane == PLANE_XY:
|
|
circle_end.x, circle_end.y = \
|
|
self.__adjust_circle(delta.x, delta.y, radius.x, radius.y,
|
|
direction, self._position.x,
|
|
self._position.y, TABLE_SIZE_X_MM,
|
|
TABLE_SIZE_Y_MM)
|
|
circle_end.z = delta.z
|
|
elif self._plane == PLANE_YZ:
|
|
circle_end.y, circle_end.z = \
|
|
self.__adjust_circle(delta.y, delta.z, radius.y, radius.z,
|
|
direction, self._position.y,
|
|
self._position.z, TABLE_SIZE_Y_MM,
|
|
TABLE_SIZE_Z_MM)
|
|
circle_end.x = delta.x
|
|
elif self._plane == PLANE_ZX:
|
|
circle_end.z, circle_end.x = \
|
|
self.__adjust_circle(delta.z, delta.x, radius.z, radius.x,
|
|
direction, self._position.z,
|
|
self._position.x, TABLE_SIZE_Z_MM,
|
|
TABLE_SIZE_X_MM)
|
|
circle_end.y = delta.y
|
|
circle_end.e = delta.e
|
|
circle_end = circle_end.round(1.0 / STEPPER_PULSES_PER_MM_X,
|
|
1.0 / STEPPER_PULSES_PER_MM_Y,
|
|
1.0 / STEPPER_PULSES_PER_MM_Z,
|
|
1.0 / STEPPER_PULSES_PER_MM_E)
|
|
logging.info("Moving circularly {} {} {} with radius {}"
|
|
" and velocity {}".format(self._plane, circle_end,
|
|
direction, radius, velocity))
|
|
gen = PulseGeneratorCircular(circle_end, radius, self._plane, direction,
|
|
velocity)
|
|
hal.move(gen)
|
|
# if finish coords is not on circle, move some distance linearly
|
|
linear_delta = delta - circle_end
|
|
if not linear_delta.is_zero():
|
|
logging.info("Moving additionally {} to finish circle command".
|
|
format(linear_delta))
|
|
gen = PulseGeneratorLinear(linear_delta, velocity)
|
|
hal.move(gen)
|
|
# save position
|
|
self._position = self._position + circle_end + linear_delta
|
|
|
|
def home(self):
|
|
""" Move head to park position
|
|
"""
|
|
d = Coordinates(0, 0, -self._position.z, 0)
|
|
self._move_linear(d, STEPPER_MAX_VELOCITY_MM_PER_MIN)
|
|
d = Coordinates(-self._position.x, -self._position.y, 0, 0)
|
|
self._move_linear(d, STEPPER_MAX_VELOCITY_MM_PER_MIN)
|
|
|
|
def position(self):
|
|
""" Return current machine position (after the latest command)
|
|
Note that hal might still be moving motors and in this case
|
|
function will block until motors stops.
|
|
This function for tests only.
|
|
:return current position.
|
|
"""
|
|
hal.join()
|
|
return self._position
|
|
|
|
def plane(self):
|
|
""" Return current plane for circular interpolation. This function for
|
|
tests only.
|
|
:return current plane.
|
|
"""
|
|
return self._plane
|
|
|
|
def do_command(self, gcode):
|
|
""" Perform action.
|
|
:param gcode: GCode object which represent one gcode line
|
|
"""
|
|
if gcode is None:
|
|
return
|
|
logging.debug("got command " + str(gcode.params))
|
|
# read command
|
|
c = gcode.command()
|
|
if c is None and gcode.has_coordinates():
|
|
c = 'G1'
|
|
# read parameters
|
|
if self._absoluteCoordinates:
|
|
coord = gcode.coordinates(self._position, self._convertCoordinates)
|
|
coord = coord + self._local
|
|
delta = coord - self._position
|
|
else:
|
|
delta = gcode.coordinates(Coordinates(0.0, 0.0, 0.0, 0.0),
|
|
self._convertCoordinates)
|
|
# coord = self._position + delta
|
|
velocity = gcode.get('F', self._velocity)
|
|
spindle_rpm = gcode.get('S', self._spindle_rpm)
|
|
pause = gcode.get('P', self._pause)
|
|
radius = gcode.radius(Coordinates(0.0, 0.0, 0.0, 0.0),
|
|
self._convertCoordinates)
|
|
# check parameters
|
|
if velocity <= 0 or velocity > STEPPER_MAX_VELOCITY_MM_PER_MIN:
|
|
raise GMachineException("bad feed speed")
|
|
if spindle_rpm < 0 or spindle_rpm > SPINDLE_MAX_RPM:
|
|
raise GMachineException("bad spindle speed")
|
|
if pause < 0:
|
|
raise GMachineException("bad delay")
|
|
# select command and run it
|
|
if c == 'G0': # rapid move
|
|
self._move_linear(delta, STEPPER_MAX_VELOCITY_MM_PER_MIN)
|
|
elif c == 'G1': # linear interpolation
|
|
self._move_linear(delta, velocity)
|
|
elif c == 'G2': # circular interpolation, clockwise
|
|
self._circular(delta, radius, velocity, CW)
|
|
elif c == 'G3': # circular interpolation, counterclockwise
|
|
self._circular(delta, radius, velocity, CCW)
|
|
elif c == 'G4': # delay in s
|
|
hal.join()
|
|
time.sleep(pause)
|
|
elif c == 'G17': # XY plane select
|
|
self._plane = PLANE_XY
|
|
elif c == 'G18': # ZX plane select
|
|
self._plane = PLANE_ZX
|
|
elif c == 'G19': # YZ plane select
|
|
self._plane = PLANE_YZ
|
|
elif c == 'G20': # switch to inches
|
|
self._convertCoordinates = 25.4
|
|
elif c == 'G21': # switch to mm
|
|
self._convertCoordinates = 1.0
|
|
elif c == 'G28': # home
|
|
self.home()
|
|
elif c == 'G53': # switch to machine coords
|
|
self._local = Coordinates(0.0, 0.0, 0.0, 0.0)
|
|
elif c == 'G90': # switch to absolute coords
|
|
self._absoluteCoordinates = True
|
|
elif c == 'G91': # switch to relative coords
|
|
self._absoluteCoordinates = False
|
|
elif c == 'G92': # switch to local coords
|
|
self._local = self._position - \
|
|
gcode.coordinates(Coordinates(0.0, 0.0, 0.0, 0.0),
|
|
self._convertCoordinates)
|
|
elif c == 'M3': # spindle on
|
|
self._spindle(spindle_rpm)
|
|
elif c == 'M5': # spindle off
|
|
self._spindle(0)
|
|
elif c == 'M2' or c == 'M30': # program finish, reset everything.
|
|
self.reset()
|
|
elif c == 'M111': # enable debug
|
|
logging_config.debug_enable()
|
|
elif c is None: # command not specified(for example, just F was passed)
|
|
pass
|
|
else:
|
|
raise GMachineException("unknown command")
|
|
# save parameters on success
|
|
self._velocity = velocity
|
|
self._spindle_rpm = spindle_rpm
|
|
self._pause = pause
|
|
logging.debug("position {}".format(self._position))
|