mirror of
https://github.com/sinseman44/PyCNC.git
synced 2026-01-12 02:40:04 +00:00
501 lines
20 KiB
Python
501 lines
20 KiB
Python
from __future__ import division
|
|
|
|
import cnc.logging_config as logging_config
|
|
from cnc import hal
|
|
from cnc.pulses import *
|
|
from cnc.coordinates import *
|
|
from cnc.heater import *
|
|
from cnc.enums import *
|
|
from cnc.watchdog 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
|
|
"""
|
|
AUTO_FAN_ON = AUTO_FAN_ON
|
|
|
|
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._local = None
|
|
self._convertCoordinates = 0
|
|
self._absoluteCoordinates = 0
|
|
self._plane = None
|
|
self._fan_state = False
|
|
self._pen_state = False
|
|
self._heaters = dict()
|
|
self.reset()
|
|
hal.init()
|
|
self.watchdog = HardwareWatchdog()
|
|
|
|
def release(self):
|
|
""" Free all resources.
|
|
"""
|
|
self._spindle(0)
|
|
for h in self._heaters:
|
|
self._heaters[h].stop()
|
|
self._fan(False)
|
|
hal.deinit()
|
|
|
|
def reset(self):
|
|
""" Reinitialize all program configurable thing.
|
|
"""
|
|
self._velocity = min(MAX_VELOCITY_MM_PER_MIN_X,
|
|
MAX_VELOCITY_MM_PER_MIN_Y,
|
|
MAX_VELOCITY_MM_PER_MIN_Z,
|
|
MAX_VELOCITY_MM_PER_MIN_E)
|
|
self._spindle_rpm = 1000
|
|
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 _fan(self, state):
|
|
hal.fan_control(state)
|
|
self._fan_state = state
|
|
|
|
def _pen(self, state):
|
|
hal.pen_control(state)
|
|
self._pen_state = state
|
|
|
|
def _heat(self, heater, temperature, wait):
|
|
# check if sensor is ok
|
|
if heater == HEATER_EXTRUDER:
|
|
measure = hal.get_extruder_temperature
|
|
control = hal.extruder_heater_control
|
|
coefficients = EXTRUDER_PID
|
|
elif heater == HEATER_BED:
|
|
measure = hal.get_bed_temperature
|
|
control = hal.bed_heater_control
|
|
coefficients = BED_PID
|
|
else:
|
|
raise GMachineException("unknown heater")
|
|
try:
|
|
measure()
|
|
except (IOError, OSError):
|
|
raise GMachineException("can not measure temperature")
|
|
if heater in self._heaters:
|
|
self._heaters[heater].stop()
|
|
del self._heaters[heater]
|
|
if temperature != 0:
|
|
if heater == HEATER_EXTRUDER and self.AUTO_FAN_ON:
|
|
self._fan(True)
|
|
self._heaters[heater] = Heater(temperature, coefficients, measure,
|
|
control)
|
|
if wait:
|
|
self._heaters[heater].wait()
|
|
|
|
def __check_delta(self, delta):
|
|
pos = self._position + delta
|
|
logging.debug("_check_delta : pos init = {} - delta = {} - pos = {}".format(self._position, delta, pos))
|
|
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("[CHECK DELTA] out of effective area")
|
|
|
|
# noinspection PyMethodMayBeStatic
|
|
def __check_velocity(self, max_velocity):
|
|
if max_velocity.x > MAX_VELOCITY_MM_PER_MIN_X \
|
|
or max_velocity.y > MAX_VELOCITY_MM_PER_MIN_Y \
|
|
or max_velocity.z > MAX_VELOCITY_MM_PER_MIN_Z \
|
|
or max_velocity.e > MAX_VELOCITY_MM_PER_MIN_E:
|
|
raise GMachineException("out of maximum speed")
|
|
|
|
def _move_linear(self, delta, velocity, safe_zero=False):
|
|
if not safe_zero:
|
|
delta = delta.round(round(1.0 / STEPPER_PULSES_PER_MM_X, 2),
|
|
round(1.0 / STEPPER_PULSES_PER_MM_Y, 2),
|
|
round(1.0 / STEPPER_PULSES_PER_MM_Z, 2),
|
|
round(1.0 / STEPPER_PULSES_PER_MM_E, 2))
|
|
if delta.is_zero():
|
|
return
|
|
self.__check_delta(delta)
|
|
|
|
gen = PulseGeneratorLinear(delta, velocity)
|
|
self.__check_velocity(gen.max_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:
|
|
if da - ra == 0:
|
|
ea = 0
|
|
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("[ADJUST CIRCLE] out of effective area")
|
|
pq = q
|
|
return ea, eb
|
|
|
|
def _move_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)
|
|
self.__check_velocity(gen.max_velocity())
|
|
# if finish coords is not on circle, move some distance linearly
|
|
linear_delta = delta - circle_end
|
|
linear_gen = None
|
|
if not linear_delta.is_zero():
|
|
logging.info("Moving additionally {} to finish circle command".
|
|
format(linear_delta))
|
|
linear_gen = PulseGeneratorLinear(linear_delta, velocity)
|
|
self.__check_velocity(linear_gen.max_velocity())
|
|
# do movements
|
|
hal.move(gen)
|
|
if linear_gen is not None:
|
|
hal.move(linear_gen)
|
|
# save position
|
|
self._position = self._position + circle_end + linear_delta
|
|
|
|
def safe_zero(self, x=True, y=True, z=True):
|
|
""" Move head to zero position safely.
|
|
:param x: boolean, move X axis to zero
|
|
:param y: boolean, move Y axis to zero
|
|
:param z: boolean, move Z axis to zero
|
|
"""
|
|
logging.debug("[SAFE_ZERO] : X : {} - Y : {} - Z : {}".format(x,y,z))
|
|
if x and not y:
|
|
self._move_linear(Coordinates(-self._position.x, 0, 0, 0),
|
|
MAX_VELOCITY_MM_PER_MIN_X, True)
|
|
elif y and not x:
|
|
self._move_linear(Coordinates(0, -self._position.y, 0, 0),
|
|
MAX_VELOCITY_MM_PER_MIN_X, True)
|
|
elif x and y:
|
|
d = Coordinates(-self._position.x, -self._position.y, 0, 0)
|
|
logging.debug("[SAFE_ZERO] : d = {}".format(d))
|
|
self._move_linear(d, min(MAX_VELOCITY_MM_PER_MIN_X,
|
|
MAX_VELOCITY_MM_PER_MIN_Y),
|
|
True)
|
|
if z:
|
|
d = Coordinates(0, 0, -self._position.z, 0)
|
|
self._move_linear(d, MAX_VELOCITY_MM_PER_MIN_Z, True)
|
|
|
|
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 fan_state(self):
|
|
""" Check if fan is on.
|
|
:return True if fan is on, False otherwise.
|
|
"""
|
|
return self._fan_state
|
|
|
|
def __get_target_temperature(self, heater):
|
|
if heater not in self._heaters:
|
|
return 0
|
|
return self._heaters[heater].target_temperature()
|
|
|
|
def extruder_target_temperature(self):
|
|
""" Return desired extruder temperature.
|
|
:return Temperature in Celsius, 0 if disabled.
|
|
"""
|
|
return self.__get_target_temperature(HEATER_EXTRUDER)
|
|
|
|
def bed_target_temperature(self):
|
|
""" Return desired bed temperature.
|
|
:return Temperature in Celsius, 0 if disabled.
|
|
"""
|
|
return self.__get_target_temperature(HEATER_BED)
|
|
|
|
def do_command(self, gcode):
|
|
""" Perform action.
|
|
:param gcode: GCode object which represent one gcode line
|
|
:return String if any answer require, None otherwise.
|
|
"""
|
|
if gcode is None:
|
|
return None
|
|
answer = None
|
|
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._local,
|
|
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)
|
|
logging.debug("Velocity = {}".format(velocity))
|
|
radius = gcode.radius(Coordinates(0.0, 0.0, 0.0, 0.0),
|
|
self._convertCoordinates)
|
|
# check parameters
|
|
if velocity < MIN_VELOCITY_MM_PER_MIN and velocity > 1:
|
|
raise GMachineException("feed speed too low")
|
|
elif velocity == 1:
|
|
velocity = MIN_VELOCITY_MM_PER_MIN
|
|
# select command and run it
|
|
if c == 'G0': # rapid move
|
|
vl = max(MAX_VELOCITY_MM_PER_MIN_X,
|
|
MAX_VELOCITY_MM_PER_MIN_Y,
|
|
MAX_VELOCITY_MM_PER_MIN_Z,
|
|
MAX_VELOCITY_MM_PER_MIN_E)
|
|
l = delta.length()
|
|
if l > 0:
|
|
proportion = abs(delta) / l
|
|
if proportion.x > 0:
|
|
v = int(MAX_VELOCITY_MM_PER_MIN_X / proportion.x)
|
|
if v < vl:
|
|
vl = v
|
|
if proportion.y > 0:
|
|
v = int(MAX_VELOCITY_MM_PER_MIN_Y / proportion.y)
|
|
if v < vl:
|
|
vl = v
|
|
if proportion.z > 0:
|
|
v = int(MAX_VELOCITY_MM_PER_MIN_Z / proportion.z)
|
|
if v < vl:
|
|
vl = v
|
|
if proportion.e > 0:
|
|
v = int(MAX_VELOCITY_MM_PER_MIN_E / proportion.e)
|
|
if v < vl:
|
|
vl = v
|
|
self._move_linear(delta, vl)
|
|
elif c == 'G1': # linear interpolation
|
|
self._move_linear(delta, velocity)
|
|
elif c == 'G2': # circular interpolation, clockwise
|
|
self._move_circular(delta, radius, velocity, CW)
|
|
elif c == 'G3': # circular interpolation, counterclockwise
|
|
self._move_circular(delta, radius, velocity, CCW)
|
|
elif c == 'G4': # delay in s
|
|
if not gcode.has('P'):
|
|
raise GMachineException("P is not specified")
|
|
pause = gcode.get('P', 0)
|
|
if pause < 0:
|
|
raise GMachineException("bad delay")
|
|
hal.join()
|
|
time.sleep(pause / 1000)
|
|
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
|
|
axises = gcode.has('X'), gcode.has('Y'), gcode.has('Z')
|
|
if axises == (False, False, False):
|
|
axises = True, True, True
|
|
self.safe_zero(*axises)
|
|
hal.join()
|
|
if not hal.calibrate(*axises):
|
|
raise GMachineException("failed to calibrate")
|
|
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
|
|
if gcode.has_coordinates():
|
|
self._local = self._position - gcode.coordinates(
|
|
Coordinates(self._position.x - self._local.x,
|
|
self._position.y - self._local.y,
|
|
self._position.z - self._local.z,
|
|
self._position.e - self._local.e),
|
|
self._convertCoordinates)
|
|
else:
|
|
self._local = self._position
|
|
elif c == 'M3': # spindle on
|
|
spindle_rpm = gcode.get('S', self._spindle_rpm)
|
|
if spindle_rpm < 0 or spindle_rpm > SPINDLE_MAX_RPM:
|
|
raise GMachineException("bad spindle speed")
|
|
self._spindle(spindle_rpm)
|
|
self._spindle_rpm = spindle_rpm
|
|
elif c == 'M5': # spindle off
|
|
self._spindle(0)
|
|
elif c == 'M2' or c == 'M30': # program finish, reset everything.
|
|
self.reset()
|
|
elif c == 'M84': # disable motors
|
|
hal.disable_steppers()
|
|
# extruder and bed heaters control
|
|
elif c == 'M104' or c == 'M109' or c == 'M140' or c == 'M190':
|
|
if c == 'M104' or c == 'M109':
|
|
heater = HEATER_EXTRUDER
|
|
elif c == 'M140' or c == 'M190':
|
|
heater = HEATER_BED
|
|
else:
|
|
raise Exception("Unexpected heater command")
|
|
wait = c == 'M109' or c == 'M190'
|
|
if not gcode.has("S"):
|
|
raise GMachineException("temperature is not specified")
|
|
t = gcode.get('S', 0)
|
|
if ((heater == HEATER_EXTRUDER and t > EXTRUDER_MAX_TEMPERATURE) or
|
|
(heater == HEATER_BED and t > BED_MAX_TEMPERATURE) or
|
|
t < MIN_TEMPERATURE) and t != 0:
|
|
raise GMachineException("bad temperature")
|
|
self._heat(heater, t, wait)
|
|
elif c == 'M105': # get temperature
|
|
try:
|
|
et = hal.get_extruder_temperature()
|
|
except (IOError, OSError):
|
|
et = None
|
|
try:
|
|
bt = hal.get_bed_temperature()
|
|
except (IOError, OSError):
|
|
bt = None
|
|
if et is None and bt is None:
|
|
raise GMachineException("can not measure temperature")
|
|
answer = "E:{} B:{}".format(et, bt)
|
|
elif c == 'M106': # fan control
|
|
if gcode.get('S', 1) != 0:
|
|
self._fan(True)
|
|
else:
|
|
self._fan(False)
|
|
elif c == 'M107': # turn off fan
|
|
self._fan(False)
|
|
elif c == 'M111': # enable debug
|
|
logging_config.debug_enable()
|
|
elif c == 'M114': # get current position
|
|
hal.join()
|
|
p = self.position()
|
|
answer = "X:{} Y:{} Z:{} E:{}".format(p.x, p.y, p.z, p.e)
|
|
elif c == 'M117': # display a message
|
|
hal.set_message("TEST")
|
|
elif c == 'M300': # play sound normaly or control z axis
|
|
logging.debug("gcode = {}".format(gcode.get('S')))
|
|
if gcode.get('S') == 30:
|
|
self._pen(True)
|
|
elif gcode.get('S') == 50:
|
|
self._pen(False)
|
|
else:
|
|
raise GMachineException("Not supported, use M300 S30 or M300 S50")
|
|
elif c is None: # command not specified(ie just F was passed)
|
|
pass
|
|
# commands below are added just for compatibility
|
|
elif c == 'M82': # absolute mode for extruder
|
|
if not self._absoluteCoordinates:
|
|
raise GMachineException("Not supported, use G90/G91")
|
|
elif c == 'M83': # relative mode for extruder
|
|
if self._absoluteCoordinates:
|
|
raise GMachineException("Not supported, use G90/G91")
|
|
else:
|
|
raise GMachineException("unknown command")
|
|
# save parameters on success
|
|
self._velocity = velocity
|
|
logging.debug("position {}".format(self._position))
|
|
return answer
|