mirror of
https://github.com/sinseman44/PyCNC.git
synced 2026-08-31 00:33:13 +00:00
504 lines
17 KiB
Python
504 lines
17 KiB
Python
import time
|
|
|
|
from cnc.hal_raspberry import rpgpio
|
|
from cnc.pulses import *
|
|
from cnc.config import *
|
|
|
|
US_IN_SECONDS = 1000000
|
|
|
|
gpio = rpgpio.GPIO()
|
|
dma = rpgpio.DMAGPIO()
|
|
pwm = rpgpio.DMAPWM()
|
|
watchdog = rpgpio.DMAWatchdog()
|
|
|
|
class stepper:
|
|
''' double H bridge stepper motor driver '''
|
|
|
|
STEPPER_ENABLED = 0
|
|
STEPPER_DISABLED = 1
|
|
|
|
STATE_DIR_DIRECT = 0
|
|
STATE_DIR_INV = 1
|
|
|
|
AXE_X = 0
|
|
AXE_Y = 1
|
|
AXE_Z = 2
|
|
|
|
MODE_FULL = 0
|
|
MODE_HALF = 1
|
|
|
|
HALF_STEP_SEQ = [[1,0,0,0], # Phase A
|
|
[1,0,1,0], # Phase AB
|
|
[0,0,1,0], # Phase B
|
|
[0,1,1,0], # Phase A'B
|
|
[0,1,0,0], # Phase A'
|
|
[0,1,0,1], # Phase A'B'
|
|
[0,0,0,1], # Phase B'
|
|
[1,0,0,1]] # Phase AB'
|
|
|
|
FULL_STEP_SEQ = [[1,0,0,0], # Phase A
|
|
[0,0,1,0], # Phase B
|
|
[0,1,0,0], # Phase A'
|
|
[0,0,0,1]] # Phase B'
|
|
|
|
def __init__(self, axe=AXE_X, pins=[]):
|
|
''' contructor '''
|
|
self.state = self.STEPPER_DISABLED
|
|
self.dir = self.STATE_DIR_DIRECT
|
|
self.axe = axe
|
|
self.seq = self.FULL_STEP_SEQ[:]
|
|
self.pins = pins
|
|
self.step_number = 0
|
|
self.current_seq_num = 0
|
|
self.number_of_steps = 0
|
|
|
|
def init(self):
|
|
''' init gpios '''
|
|
self.state = self.STEPPER_ENABLED
|
|
for pin in self.pins:
|
|
#logging.debug("set pin {} out".format(pin))
|
|
gpio.init(pin, rpgpio.GPIO.MODE_OUTPUT)
|
|
|
|
def set_steps_number(self, number=0):
|
|
''' set the number of steps of the motor '''
|
|
self.number_of_steps = number
|
|
|
|
def set_mode(self, mode):
|
|
''' set operationnal mode '''
|
|
if mode > self.MODE_HALF:
|
|
raise ValueError
|
|
elif mode == self.MODE_FULL:
|
|
self.seq = self.FULL_STEP_SEQ[:]
|
|
if self.dir == self.STATE_DIR_INV:
|
|
self.seq.reverse()
|
|
elif mode == self.MODE_HALF:
|
|
self.seq = self.HALF_STEP_SEQ[:]
|
|
if self.dir == self.STATE_DIR_INV:
|
|
self.seq.reverse()
|
|
|
|
def get_mode(self):
|
|
''' get current operationnal mode '''
|
|
return self.seq
|
|
|
|
def set_dir(self, direction):
|
|
''' set direction '''
|
|
if direction > self.STATE_DIR_INV:
|
|
raise ValueError
|
|
elif direction == self.STATE_DIR_INV and self.dir == self.STATE_DIR_DIRECT:
|
|
#logging.debug("SET DIR INV")
|
|
self.dir = direction
|
|
#logging.debug("BEFORE: {}".format(self.seq))
|
|
self.seq.reverse()
|
|
#logging.debug("AFTER: {}".format(self.seq))
|
|
if self.current_seq_num > 0:
|
|
self.current_seq_num = len(self.seq) - 1 - self.current_seq_num
|
|
elif direction == self.STATE_DIR_DIRECT and self.dir == self.STATE_DIR_INV:
|
|
#logging.debug("SET DIR DIRECT")
|
|
self.dir = direction
|
|
#logging.debug("BEFORE: {}".format(self.seq))
|
|
self.seq.reverse()
|
|
#logging.debug("AFTER: {}".format(self.seq))
|
|
if self.current_seq_num > 0:
|
|
self.current_seq_num = len(self.seq) - 1 - self.current_seq_num
|
|
else:
|
|
logging.warning("direction already set")
|
|
|
|
def inc_seq_num(self):
|
|
''' increment current sequence number '''
|
|
if self.current_seq_num >= len(self.seq) - 1:
|
|
self.current_seq_num = 0
|
|
else:
|
|
self.current_seq_num += 1
|
|
|
|
def dec_seq_num(self):
|
|
''' decrement current sequence number '''
|
|
if self.current_seq_num == 0:
|
|
self.current_seq_num = len(self.seq) - 1
|
|
else:
|
|
self.current_seq_num -= 1
|
|
|
|
def get_current_seq(self):
|
|
''' get current sequence '''
|
|
return self.seq[self.current_seq_num]
|
|
|
|
def disable(self):
|
|
''' disable stepper '''
|
|
self.state = self.STEPPER_DISABLED
|
|
for pin in self.pins:
|
|
gpio.clear(pin)
|
|
|
|
def enable(self):
|
|
''' enable stepper '''
|
|
self.state = self.STEPPER_ENABLED
|
|
|
|
def get_cur_mask(self):
|
|
''' retreive current mask pins '''
|
|
mask = 0
|
|
# mask pins
|
|
for i, enable in enumerate(self.seq[self.current_seq_num]):
|
|
if enable:
|
|
mask += 1 << self.pins[i]
|
|
return mask
|
|
|
|
def get_cur_mask_and_inc_step(self):
|
|
''' retreive current mask pins and increment sequence '''
|
|
mask = self.get_cur_mask()
|
|
self.inc_seq_num()
|
|
return mask
|
|
|
|
def debug(self):
|
|
''' debug mode '''
|
|
logging.debug("state = {} - \
|
|
dir = {} - \
|
|
axe = {} - \
|
|
seq = {} - \
|
|
pins = {} - \
|
|
step_number = {} - \
|
|
current_seq_num = {} - \
|
|
number_of_steps = {}".format(self.state,
|
|
self.dir,
|
|
self.axe,
|
|
self.seq,
|
|
self.pins,
|
|
self.step_number,
|
|
self.current_seq_num,
|
|
self.number_of_steps))
|
|
|
|
|
|
stepper_x = stepper(axe=stepper.AXE_X, pins=STEPPER_STEP_PINS_X)
|
|
stepper_y = stepper(axe=stepper.AXE_Y, pins=STEPPER_STEP_PINS_Y)
|
|
|
|
def init():
|
|
""" Initialize GPIO pins and machine itself.
|
|
"""
|
|
#logging.info("initialisation of gpios ...")
|
|
# Init X stepper
|
|
stepper_x.init()
|
|
stepper_x.set_mode(mode = stepper.MODE_HALF)
|
|
stepper_x.set_steps_number(STEPPER_STEPS_PER_REV_X)
|
|
if STEPPER_INVERTED_X:
|
|
stepper_x.set_dir(stepper.STATE_DIR_INV)
|
|
stepper_x.enable()
|
|
#stepper_x.debug()
|
|
# Init Y stepper
|
|
stepper_y.init()
|
|
stepper_y.set_mode(mode = stepper.MODE_HALF)
|
|
stepper_y.set_steps_number(STEPPER_STEPS_PER_REV_Y)
|
|
if STEPPER_INVERTED_Y:
|
|
stepper_y.set_dir(stepper.STATE_DIR_INV)
|
|
stepper_y.enable()
|
|
#stepper_y.debug()
|
|
# Init EndStop
|
|
gpio.init(ENDSTOP_PIN_X, rpgpio.GPIO.MODE_INPUT_NOPULL)
|
|
gpio.init(ENDSTOP_PIN_Y, rpgpio.GPIO.MODE_INPUT_NOPULL)
|
|
# Init pen pin
|
|
gpio.init(PEN_PIN, rpgpio.GPIO.MODE_OUTPUT)
|
|
gpio.clear(PEN_PIN)
|
|
# Watchdog start
|
|
watchdog.start()
|
|
|
|
def spindle_control(percent):
|
|
""" Spindle control implementation.
|
|
:param percent: spindle speed in percent 0..100. If 0, stop the spindle.
|
|
"""
|
|
logging.debug("spindle control not implemented")
|
|
|
|
def fan_control(on_off):
|
|
"""
|
|
Cooling fan control.
|
|
:param on_off: boolean value if fan is enabled.
|
|
"""
|
|
logging.debug("fan control not implemented")
|
|
|
|
def pen_control(up_down):
|
|
"""
|
|
Pen control.
|
|
:param on_off: boolean value if pen is up or down.
|
|
"""
|
|
# waiting previous command is finished ...
|
|
while dma.is_active():
|
|
time.sleep(0.01)
|
|
|
|
if up_down:
|
|
#logging.info("Pen is up ...")
|
|
pwm.add_pin(PEN_PIN, 7)
|
|
time.sleep(0.25)
|
|
pwm.add_pin(PEN_PIN, 0)
|
|
else:
|
|
#logging.info("Pen is down ...")
|
|
pwm.add_pin(PEN_PIN, 12.5)
|
|
#pwm.add_pin(PEN_PIN, 9)
|
|
time.sleep(0.25)
|
|
pwm.add_pin(PEN_PIN, 0)
|
|
|
|
def extruder_heater_control(percent):
|
|
""" Extruder heater control.
|
|
:param percent: heater power in percent 0..100. 0 turns heater off.
|
|
"""
|
|
logging.debug("extruder heater control not implemented")
|
|
|
|
def bed_heater_control(percent):
|
|
""" Hot bed heater control.
|
|
:param percent: heater power in percent 0..100. 0 turns heater off.
|
|
"""
|
|
logging.debug("bed heater control not implemented")
|
|
|
|
def get_extruder_temperature():
|
|
""" Measure extruder temperature.
|
|
:return: temperature in Celsius.
|
|
"""
|
|
logging.debug("extruder temperature not implemented")
|
|
|
|
def get_bed_temperature():
|
|
""" Measure bed temperature.
|
|
:return: temperature in Celsius.
|
|
"""
|
|
logging.debug("bed temperature not implemented")
|
|
|
|
def disable_steppers():
|
|
""" Disable all steppers until any movement occurs.
|
|
"""
|
|
stepper_x.disable()
|
|
stepper_y.disable()
|
|
|
|
def set_message(msg):
|
|
""" Display message to lcd
|
|
"""
|
|
logging.debug("Message display not implemented")
|
|
|
|
def calibrate(x, y, z):
|
|
""" Move head to home position till end stop switch will be triggered.
|
|
Do not return till all procedures are completed.
|
|
:param x: boolean, True to calibrate X axis.
|
|
:param y: boolean, True to calibrate Y axis.
|
|
:param z: boolean, True to calibrate Z axis.
|
|
:return: boolean, True if all specified end stops were triggered.
|
|
"""
|
|
max_size = 0
|
|
mask = 0
|
|
if x:
|
|
max_size = max(max_size, TABLE_SIZE_X_MM * STEPPER_PULSES_PER_MM_X)
|
|
stepper_x.set_dir(stepper.STATE_DIR_DIRECT)
|
|
if y:
|
|
max_size = max(max_size, TABLE_SIZE_Y_MM * STEPPER_PULSES_PER_MM_Y)
|
|
stepper_y.set_dir(stepper.STATE_DIR_INV)
|
|
|
|
pulses_per_mm_avg = (STEPPER_PULSES_PER_MM_X + STEPPER_PULSES_PER_MM_Y) / 2.0
|
|
pulses_per_sec = CALIBRATION_VELOCITY_MM_PER_MIN / 60.0 * pulses_per_mm_avg
|
|
delay = int(US_IN_SECONDS / pulses_per_sec)
|
|
|
|
#logging.info("[CALIBRATE] num = {} pulses/mm".format(pulses_per_mm_avg))
|
|
#logging.info("[CALIBRATE] num = {} pulses/sec".format(pulses_per_sec))
|
|
#logging.info("[CALIBRATE] delay = {} us".format(delay))
|
|
|
|
dma.clear()
|
|
if not gpio.read(ENDSTOP_PIN_X):
|
|
# retreive current sequence to X mask pins
|
|
for _ in stepper_x.get_mode():
|
|
mask = 0
|
|
seq = stepper_x.get_current_seq()
|
|
mask = stepper_x.get_cur_mask_and_inc_step()
|
|
#logging.debug("[X AXIS] MASK = {:#032b} - SEQ = {}".format(mask, seq))
|
|
dma.add_pulse(mask, delay)
|
|
dma.finalize_stream()
|
|
dma.run(True)
|
|
while dma.is_active():
|
|
time.sleep(0.1)
|
|
x_endstop = gpio.read(ENDSTOP_PIN_X)
|
|
if x_endstop:
|
|
dma.stop()
|
|
dma.clear()
|
|
|
|
if not gpio.read(ENDSTOP_PIN_Y):
|
|
# retreive current sequence to mask Y pins
|
|
for _ in stepper_y.get_mode():
|
|
mask = 0
|
|
seq = stepper_y.get_current_seq()
|
|
mask = stepper_y.get_cur_mask_and_inc_step()
|
|
#logging.debug("[Y AXIS] MASK = {:#032b} - SEQ = {}".format(mask, seq))
|
|
dma.add_pulse(mask, delay)
|
|
|
|
dma.finalize_stream()
|
|
dma.run(True)
|
|
while dma.is_active():
|
|
time.sleep(0.1)
|
|
y_endstop = gpio.read(ENDSTOP_PIN_Y)
|
|
if y_endstop:
|
|
dma.stop()
|
|
dma.clear()
|
|
return True
|
|
|
|
def move(generator):
|
|
""" Move head to specified position
|
|
:param generator: PulseGenerator object.
|
|
"""
|
|
# Fill buffer right before currently running(previous sequence) dma
|
|
# this mode implements kind of round buffer, but protects if CPU is not
|
|
# powerful enough to calculate buffer in advance, faster then machine
|
|
# moving. In this case machine would safely paused between commands until
|
|
# calculation is done.
|
|
|
|
# G1 X50 F100 : permet de faire un mouvement de 50mm selon l'axe X lent (100mm/min soit 1.66mm/s).
|
|
|
|
# 4 control blocks per 32 bytes
|
|
bytes_per_iter = 4 * dma.control_block_size()
|
|
# prepare and run dma
|
|
dma.clear() # should just clear current address, but not stop current DMA
|
|
prev = 0
|
|
prevx = 0
|
|
prevy = 0
|
|
st = time.time()
|
|
current_cb = 0
|
|
k = 0
|
|
k0 = 0
|
|
idx = 0
|
|
flagx = False
|
|
flagy = False
|
|
for direction, tx, ty, tz, te in generator:
|
|
if current_cb is not None:
|
|
#logging.debug("{} + {} => result = {}".format(dma.current_address(), bytes_per_iter, dma.current_address() + bytes_per_iter))
|
|
while dma.current_address() + bytes_per_iter >= current_cb:
|
|
time.sleep(0.001)
|
|
current_cb = dma.current_control_block()
|
|
#logging.debug("current control block : {}".format(current_cb))
|
|
if current_cb is None:
|
|
k0 = k
|
|
st = time.time()
|
|
break # previous dma sequence has stopped
|
|
# logging.debug("[{}] direction: {} - tx: {} - ty: {} - tz: {} - te: {}".format(idx, direction, tx, ty, tz, te))
|
|
if direction: # set up directions
|
|
#logging.debug("[{}] direction: {} - tx: {} - ty: {} - tz: {} - te: {}".format(idx, direction, tx, ty, tz, te))
|
|
if tx > 0:
|
|
#logging.debug("TX Direct")
|
|
stepper_x.set_dir(stepper.STATE_DIR_DIRECT)
|
|
elif tx < 0:
|
|
#logging.debug("TX Inverse")
|
|
stepper_x.set_dir(stepper.STATE_DIR_INV)
|
|
if ty > 0:
|
|
#logging.debug("TY Direct")
|
|
stepper_y.set_dir(stepper.STATE_DIR_DIRECT)
|
|
elif ty < 0:
|
|
#logging.debug("TY Inverse")
|
|
stepper_y.set_dir(stepper.STATE_DIR_INV)
|
|
continue
|
|
|
|
mask_x = 0
|
|
mask_y = 0
|
|
mask = 0
|
|
kx = 0
|
|
ky = 0
|
|
if tx is not None and ty is not None:
|
|
# transform sec in microsec
|
|
kx = int(round(tx * US_IN_SECONDS))
|
|
ky = int(round(ty * US_IN_SECONDS))
|
|
# search min between kx and ky
|
|
k = min(kx, ky)
|
|
# logging.debug("[TX/TY] prev : {} - K : {} - diff : {}".format(prev, k, k - prev))
|
|
mask = stepper_x.get_cur_mask_and_inc_step()
|
|
mask += stepper_y.get_cur_mask_and_inc_step()
|
|
|
|
# set pulse with diff between current time and previous time
|
|
dma.add_pulse(mask, k - prev)
|
|
|
|
if kx - k > 0: # stay time to complete course for x axis
|
|
mask_x = stepper_x.get_cur_mask_and_inc_step()
|
|
# set pulse with diff between current time and previous time
|
|
dma.add_pulse(mask_x, (kx - k) - prev)
|
|
|
|
if ky - k > 0: # stay time to complete course for y axis
|
|
mask_y = stepper_y.get_cur_mask_and_inc_step()
|
|
# set pulse with diff between current time and previous time
|
|
dma.add_pulse(mask_y, (ky - k) - prev)
|
|
|
|
prev = k
|
|
|
|
elif tx is not None:
|
|
# transform sec in microsec
|
|
kx = int(round(tx * US_IN_SECONDS))
|
|
|
|
delayx = 0
|
|
if not prevx and not prevy:
|
|
# set minimum delay for the first time
|
|
#logging.debug("[TX] K : {:#08}us - DELAY : {:#08}us".format(kx, STEPPER_PULSE_LENGTH_US))
|
|
dma.add_delay(STEPPER_PULSE_LENGTH_US)
|
|
delayx = STEPPER_PULSE_LENGTH_US
|
|
else:
|
|
# retreive current sequence to mask pins
|
|
mask = stepper_x.get_cur_mask_and_inc_step()
|
|
if prevy:
|
|
mask += stepper_y.get_cur_mask()
|
|
if flagy:
|
|
#logging.debug("[TX] K : {:#08}us - DELAY : {:#08}us - MASK : {:#032b}".format(kx, kx - prevy, mask))
|
|
# set pulse with diff between current time and previous time
|
|
dma.add_pulse(mask, kx - prevy)
|
|
elif flagx:
|
|
#logging.debug("[TX] K : {:#08}us - DELAY : {:#08}us - MASK : {:#032b}".format(kx, kx - prevx, mask))
|
|
# set pulse with diff between current time and previous time
|
|
dma.add_pulse(mask, kx - prevx)
|
|
|
|
prevx = kx - delayx
|
|
flagx = True
|
|
flagy = False
|
|
|
|
elif ty is not None:
|
|
# transform sec in microsec
|
|
ky = int(round(ty * US_IN_SECONDS))
|
|
|
|
delayy=0
|
|
if not prevy and not prevx:
|
|
# set minimum delay for the first time
|
|
#logging.debug("[TY] K : {:#08}us - DELAY : {:#08}us".format(ky, STEPPER_PULSE_LENGTH_US))
|
|
dma.add_delay(STEPPER_PULSE_LENGTH_US)
|
|
delayy=STEPPER_PULSE_LENGTH_US
|
|
else:
|
|
# retreive current sequence to mask pins
|
|
mask = stepper_y.get_cur_mask_and_inc_step()
|
|
if prevx:
|
|
mask += stepper_x.get_cur_mask()
|
|
if flagx:
|
|
#logging.debug("[TY] K : {:#08}us - DELAY : {:#08}us - MASK : {:#032b}".format(ky, ky - prevx, mask))
|
|
# set pulse with diff between current time and previous time
|
|
dma.add_pulse(mask, ky - prevx)
|
|
elif flagy:
|
|
#logging.debug("[TY] K : {:#08}us - DELAY : {:#08}us - MASK : {:#032b}".format(ky, ky - prevy, mask))
|
|
# set pulse with diff between current time and previous time
|
|
dma.add_pulse(mask, ky - prevy)
|
|
prevy = ky - delayy
|
|
flagy = True
|
|
flagx = False
|
|
|
|
idx += 1
|
|
|
|
pt = time.time()
|
|
# after long command, we can fill short buffer, that why we may need to
|
|
# wait until long command finishes
|
|
while dma.is_active():
|
|
time.sleep(0.01)
|
|
dma.run(False)
|
|
|
|
logging.info("prepared in " + str(round(pt - st, 2)) + "s, estimated in "
|
|
+ str(round(generator.total_time_s(), 2)) + "s")
|
|
|
|
def join():
|
|
""" Wait till motors work.
|
|
"""
|
|
#logging.info("hal join()")
|
|
# wait till dma works
|
|
while dma.is_active():
|
|
time.sleep(0.01)
|
|
|
|
def deinit():
|
|
""" De-initialize hardware.
|
|
"""
|
|
#logging.info("deinit")
|
|
join()
|
|
disable_steppers()
|
|
pwm.remove_all()
|
|
gpio.clear(PEN_PIN)
|
|
watchdog.stop()
|
|
|
|
def watchdog_feed():
|
|
""" Feed hardware watchdog.
|
|
"""
|
|
watchdog.feed()
|