gcode: Rework homing to use greenlets
Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
This commit is contained in:
parent
a6de1db94d
commit
89f5452ddb
|
@ -28,12 +28,6 @@ class CartKinematics:
|
||||||
for i in StepList:
|
for i in StepList:
|
||||||
s = self.steppers[i]
|
s = self.steppers[i]
|
||||||
s.mcu_stepper.set_position(int(newpos[i]*s.inv_step_dist + 0.5))
|
s.mcu_stepper.set_position(int(newpos[i]*s.inv_step_dist + 0.5))
|
||||||
def _get_homed_position(self, homing_state):
|
|
||||||
pos = [None]*3
|
|
||||||
for axis in homing_state.get_axes():
|
|
||||||
s = self.steppers[axis]
|
|
||||||
pos[axis] = s.position_endstop + s.get_homed_offset()*s.step_dist
|
|
||||||
return pos
|
|
||||||
def home(self, homing_state):
|
def home(self, homing_state):
|
||||||
# Each axis is homed independently and in order
|
# Each axis is homed independently and in order
|
||||||
for axis in homing_state.get_axes():
|
for axis in homing_state.get_axes():
|
||||||
|
@ -55,15 +49,17 @@ class CartKinematics:
|
||||||
homepos[axis] = s.position_endstop
|
homepos[axis] = s.position_endstop
|
||||||
coord = [None, None, None, None]
|
coord = [None, None, None, None]
|
||||||
coord[axis] = pos
|
coord[axis] = pos
|
||||||
homing_state.plan_home(list(coord), homepos, [s], s.homing_speed)
|
homing_state.home(list(coord), homepos, [s], s.homing_speed)
|
||||||
# Retract
|
# Retract
|
||||||
coord[axis] = rpos
|
coord[axis] = rpos
|
||||||
homing_state.plan_retract(list(coord), [s], s.homing_speed)
|
homing_state.retract(list(coord), s.homing_speed)
|
||||||
# Home again
|
# Home again
|
||||||
coord[axis] = r2pos
|
coord[axis] = r2pos
|
||||||
homing_state.plan_second_home(
|
homing_state.home(
|
||||||
list(coord), homepos, [s], s.homing_speed/2.0)
|
list(coord), homepos, [s], s.homing_speed/2.0, second_home=True)
|
||||||
homing_state.plan_calc_position(self._get_homed_position)
|
# Set final homed position
|
||||||
|
coord[axis] = s.position_endstop + s.get_homed_offset()*s.step_dist
|
||||||
|
homing_state.set_homed_position(coord)
|
||||||
def motor_off(self, move_time):
|
def motor_off(self, move_time):
|
||||||
self.limits = [(1.0, -1.0)] * 3
|
self.limits = [(1.0, -1.0)] * 3
|
||||||
for stepper in self.steppers:
|
for stepper in self.steppers:
|
||||||
|
|
|
@ -78,11 +78,6 @@ class DeltaKinematics:
|
||||||
pos = self._cartesian_to_actuator(newpos)
|
pos = self._cartesian_to_actuator(newpos)
|
||||||
for i in StepList:
|
for i in StepList:
|
||||||
self.steppers[i].mcu_stepper.set_position(pos[i])
|
self.steppers[i].mcu_stepper.set_position(pos[i])
|
||||||
def _get_homed_position(self, homing_state):
|
|
||||||
pos = [(s.mcu_stepper.commanded_position + s.get_homed_offset())
|
|
||||||
* s.step_dist
|
|
||||||
for s in self.steppers]
|
|
||||||
return self._actuator_to_cartesian(pos)
|
|
||||||
def home(self, homing_state):
|
def home(self, homing_state):
|
||||||
# All axes are homed simultaneously
|
# All axes are homed simultaneously
|
||||||
homing_state.set_axes([0, 1, 2])
|
homing_state.set_axes([0, 1, 2])
|
||||||
|
@ -92,16 +87,19 @@ class DeltaKinematics:
|
||||||
homepos = [0., 0., s.position_endstop, None]
|
homepos = [0., 0., s.position_endstop, None]
|
||||||
coord = list(homepos)
|
coord = list(homepos)
|
||||||
coord[2] = -1.5 * math.sqrt(self.arm_length2-self.max_xy2)
|
coord[2] = -1.5 * math.sqrt(self.arm_length2-self.max_xy2)
|
||||||
homing_state.plan_home(list(coord), homepos, self.steppers
|
homing_state.home(list(coord), homepos, self.steppers, s.homing_speed)
|
||||||
, s.homing_speed)
|
|
||||||
# Retract
|
# Retract
|
||||||
coord[2] = homepos[2] - s.homing_retract_dist
|
coord[2] = homepos[2] - s.homing_retract_dist
|
||||||
homing_state.plan_retract(list(coord), self.steppers, s.homing_speed)
|
homing_state.retract(list(coord), s.homing_speed)
|
||||||
# Home again
|
# Home again
|
||||||
coord[2] -= s.homing_retract_dist
|
coord[2] -= s.homing_retract_dist
|
||||||
homing_state.plan_second_home(list(coord), homepos, self.steppers
|
homing_state.home(list(coord), homepos, self.steppers
|
||||||
, s.homing_speed/2.0)
|
, s.homing_speed/2.0, second_home=True)
|
||||||
homing_state.plan_calc_position(self._get_homed_position)
|
# Set final homed position
|
||||||
|
coord = [(s.mcu_stepper.commanded_position + s.get_homed_offset())
|
||||||
|
* s.step_dist
|
||||||
|
for s in self.steppers]
|
||||||
|
homing_state.set_homed_position(self._actuator_to_cartesian(coord))
|
||||||
def motor_off(self, move_time):
|
def motor_off(self, move_time):
|
||||||
self.limit_xy2 = -1.
|
self.limit_xy2 = -1.
|
||||||
for stepper in self.steppers:
|
for stepper in self.steppers:
|
||||||
|
|
|
@ -260,14 +260,16 @@ class GCodeParser:
|
||||||
homing_state = homing.Homing(self.toolhead, axes)
|
homing_state = homing.Homing(self.toolhead, axes)
|
||||||
if self.is_fileinput:
|
if self.is_fileinput:
|
||||||
homing_state.set_no_verify_retract()
|
homing_state.set_no_verify_retract()
|
||||||
self.toolhead.home(homing_state)
|
try:
|
||||||
def axes_update(homing_state):
|
self.toolhead.home(homing_state)
|
||||||
newpos = self.toolhead.get_position()
|
except homing.EndstopError, e:
|
||||||
for axis in homing_state.get_axes():
|
self.toolhead.motor_off()
|
||||||
self.last_position[axis] = newpos[axis]
|
self.respond_error(str(e))
|
||||||
self.base_position[axis] = -self.homing_add[axis]
|
return
|
||||||
homing_state.plan_axes_update(axes_update)
|
newpos = self.toolhead.get_position()
|
||||||
self.set_busy(homing_state)
|
for axis in homing_state.get_axes():
|
||||||
|
self.last_position[axis] = newpos[axis]
|
||||||
|
self.base_position[axis] = -self.homing_add[axis]
|
||||||
def cmd_G90(self, params):
|
def cmd_G90(self, params):
|
||||||
# Use absolute coordinates
|
# Use absolute coordinates
|
||||||
self.absolutecoord = True
|
self.absolutecoord = True
|
||||||
|
|
|
@ -4,91 +4,54 @@
|
||||||
#
|
#
|
||||||
# This file may be distributed under the terms of the GNU GPLv3 license.
|
# This file may be distributed under the terms of the GNU GPLv3 license.
|
||||||
import logging
|
import logging
|
||||||
import mcu
|
|
||||||
|
|
||||||
class Homing:
|
class Homing:
|
||||||
def __init__(self, toolhead, changed_axes):
|
def __init__(self, toolhead, changed_axes):
|
||||||
self.toolhead = toolhead
|
self.toolhead = toolhead
|
||||||
self.changed_axes = changed_axes
|
self.changed_axes = changed_axes
|
||||||
self.verify_retract = True
|
self.verify_retract = True
|
||||||
|
|
||||||
self.eventtime = 0.
|
|
||||||
self.states = []
|
|
||||||
self.endstops = []
|
|
||||||
def set_no_verify_retract(self):
|
def set_no_verify_retract(self):
|
||||||
self.verify_retract = False
|
self.verify_retract = False
|
||||||
def set_axes(self, axes):
|
def set_axes(self, axes):
|
||||||
self.changed_axes = axes
|
self.changed_axes = axes
|
||||||
def get_axes(self):
|
def get_axes(self):
|
||||||
return self.changed_axes
|
return self.changed_axes
|
||||||
def plan_home(self, forcepos, movepos, steppers, speed):
|
def _fill_coord(self, coord):
|
||||||
self.states.append((self.do_home, (forcepos, movepos, steppers, speed)))
|
|
||||||
self.states.append((self.do_wait_endstop, (False,)))
|
|
||||||
def plan_second_home(self, forcepos, movepos, steppers, speed):
|
|
||||||
self.states.append((self.do_home, (forcepos, movepos, steppers, speed)))
|
|
||||||
self.states.append((self.do_wait_endstop, (self.verify_retract,)))
|
|
||||||
def plan_retract(self, newpos, steppers, speed):
|
|
||||||
self.states.append((self.do_retract, (newpos, steppers, speed)))
|
|
||||||
def plan_calc_position(self, callback):
|
|
||||||
self.states.append((self.do_calc_position, (callback,)))
|
|
||||||
def plan_axes_update(self, callback):
|
|
||||||
self.states.append((callback, (self,)))
|
|
||||||
def check_busy(self, eventtime):
|
|
||||||
self.eventtime = eventtime
|
|
||||||
while self.states:
|
|
||||||
first = self.states[0]
|
|
||||||
try:
|
|
||||||
ret = first[0](*first[1])
|
|
||||||
except EndstopError, e:
|
|
||||||
self.toolhead.motor_off()
|
|
||||||
raise
|
|
||||||
if ret:
|
|
||||||
return True
|
|
||||||
self.states.pop(0)
|
|
||||||
return False
|
|
||||||
|
|
||||||
def fill_coord(self, coord):
|
|
||||||
# Fill in any None entries in 'coord' with current toolhead position
|
# Fill in any None entries in 'coord' with current toolhead position
|
||||||
thcoord = list(self.toolhead.get_position())
|
thcoord = list(self.toolhead.get_position())
|
||||||
for i in range(len(coord)):
|
for i in range(len(coord)):
|
||||||
if coord[i] is not None:
|
if coord[i] is not None:
|
||||||
thcoord[i] = coord[i]
|
thcoord[i] = coord[i]
|
||||||
return thcoord
|
return thcoord
|
||||||
def do_retract(self, newpos, steppers, speed):
|
def retract(self, newpos, speed):
|
||||||
self.toolhead.move(self.fill_coord(newpos), speed)
|
self.toolhead.move(self._fill_coord(newpos), speed)
|
||||||
return False
|
def home(self, forcepos, movepos, steppers, speed, second_home=False):
|
||||||
def do_home(self, forcepos, movepos, steppers, speed):
|
|
||||||
# Alter kinematics class to think printer is at forcepos
|
# Alter kinematics class to think printer is at forcepos
|
||||||
self.toolhead.set_position(self.fill_coord(forcepos))
|
self.toolhead.set_position(self._fill_coord(forcepos))
|
||||||
# Start homing and issue move
|
# Start homing and issue move
|
||||||
print_time = self.toolhead.get_last_move_time()
|
print_time = self.toolhead.get_last_move_time()
|
||||||
|
endstops = []
|
||||||
for s in steppers:
|
for s in steppers:
|
||||||
es = s.enable_endstop_checking(print_time, s.step_dist / speed)
|
es = s.enable_endstop_checking(print_time, s.step_dist / speed)
|
||||||
self.endstops.append((s, es, s.mcu_stepper.get_mcu_position()))
|
endstops.append((s, es, s.mcu_stepper.get_mcu_position()))
|
||||||
self.toolhead.move(self.fill_coord(movepos), speed)
|
self.toolhead.move(self._fill_coord(movepos), speed)
|
||||||
move_end_print_time = self.toolhead.get_last_move_time()
|
move_end_print_time = self.toolhead.get_last_move_time()
|
||||||
self.toolhead.reset_print_time()
|
self.toolhead.reset_print_time()
|
||||||
for s, es, last_pos in self.endstops:
|
for s, es, last_pos in endstops:
|
||||||
es.home_finalize(es.print_to_mcu_time(move_end_print_time))
|
es.home_finalize(es.print_to_mcu_time(move_end_print_time))
|
||||||
return False
|
# Wait for endstops to trigger
|
||||||
def do_wait_endstop(self, verify_retract):
|
for s, es, last_pos in endstops:
|
||||||
# Check if endstops have triggered
|
|
||||||
while self.endstops:
|
|
||||||
stepper, es, last_pos = self.endstops[0]
|
|
||||||
try:
|
try:
|
||||||
if es.check_busy(self.eventtime):
|
es.home_wait()
|
||||||
return True
|
except es.error, e:
|
||||||
except mcu.error, e:
|
|
||||||
raise EndstopError("Failed to home stepper %s: %s" % (
|
raise EndstopError("Failed to home stepper %s: %s" % (
|
||||||
stepper.name, str(e)))
|
s.name, str(e)))
|
||||||
post_home_pos = stepper.mcu_stepper.get_mcu_position()
|
post_home_pos = s.mcu_stepper.get_mcu_position()
|
||||||
if verify_retract and last_pos == post_home_pos:
|
if second_home and self.verify_retract and last_pos == post_home_pos:
|
||||||
raise EndstopError("Endstop %s still triggered after retract" % (
|
raise EndstopError("Endstop %s still triggered after retract" % (
|
||||||
stepper.name,))
|
s.name,))
|
||||||
self.endstops.pop(0)
|
def set_homed_position(self, pos):
|
||||||
return False
|
self.toolhead.set_position(self._fill_coord(pos))
|
||||||
def do_calc_position(self, callback):
|
|
||||||
self.toolhead.set_position(self.fill_coord(callback(self)))
|
|
||||||
|
|
||||||
class EndstopError(Exception):
|
class EndstopError(Exception):
|
||||||
pass
|
pass
|
||||||
|
|
|
@ -111,6 +111,7 @@ class MCU_stepper:
|
||||||
return self.ffi_lib.stepcompress_get_errors(self._stepqueue)
|
return self.ffi_lib.stepcompress_get_errors(self._stepqueue)
|
||||||
|
|
||||||
class MCU_endstop:
|
class MCU_endstop:
|
||||||
|
error = error
|
||||||
RETRY_QUERY = 1.000
|
RETRY_QUERY = 1.000
|
||||||
def __init__(self, mcu, pin, stepper):
|
def __init__(self, mcu, pin, stepper):
|
||||||
self._mcu = mcu
|
self._mcu = mcu
|
||||||
|
@ -134,7 +135,7 @@ class MCU_endstop:
|
||||||
self._retry_query_ticks = int(self._mcu_freq * self.RETRY_QUERY)
|
self._retry_query_ticks = int(self._mcu_freq * self.RETRY_QUERY)
|
||||||
self._last_state = {}
|
self._last_state = {}
|
||||||
self.print_to_mcu_time = mcu.print_to_mcu_time
|
self.print_to_mcu_time = mcu.print_to_mcu_time
|
||||||
def home(self, mcu_time, rest_time):
|
def home_start(self, mcu_time, rest_time):
|
||||||
clock = int(mcu_time * self._mcu_freq)
|
clock = int(mcu_time * self._mcu_freq)
|
||||||
rest_ticks = int(rest_time * self._mcu_freq)
|
rest_ticks = int(rest_time * self._mcu_freq)
|
||||||
self._homing = True
|
self._homing = True
|
||||||
|
@ -149,10 +150,14 @@ class MCU_endstop:
|
||||||
self._mcu.serial.send_flush()
|
self._mcu.serial.send_flush()
|
||||||
self._stepper.note_stepper_stop()
|
self._stepper.note_stepper_stop()
|
||||||
self._home_timeout_clock = int(mcu_time * self._mcu_freq)
|
self._home_timeout_clock = int(mcu_time * self._mcu_freq)
|
||||||
|
def home_wait(self):
|
||||||
|
eventtime = time.time()
|
||||||
|
while self._check_busy(eventtime):
|
||||||
|
eventtime = self._mcu.pause(eventtime + 0.1)
|
||||||
def _handle_end_stop_state(self, params):
|
def _handle_end_stop_state(self, params):
|
||||||
logging.debug("end_stop_state %s" % (params,))
|
logging.debug("end_stop_state %s" % (params,))
|
||||||
self._last_state = params
|
self._last_state = params
|
||||||
def check_busy(self, eventtime):
|
def _check_busy(self, eventtime):
|
||||||
# Check if need to send an end_stop_query command
|
# Check if need to send an end_stop_query command
|
||||||
if self._mcu.is_fileoutput():
|
if self._mcu.is_fileoutput():
|
||||||
return False
|
return False
|
||||||
|
@ -161,7 +166,10 @@ class MCU_endstop:
|
||||||
if not self._homing:
|
if not self._homing:
|
||||||
return False
|
return False
|
||||||
if not self._last_state.get('homing', 0):
|
if not self._last_state.get('homing', 0):
|
||||||
self._stepper.set_mcu_position(self.get_last_position())
|
pos = self._last_state.get('pos', 0)
|
||||||
|
if self._stepper.get_invert_dir():
|
||||||
|
pos = -pos
|
||||||
|
self._stepper.set_mcu_position(pos)
|
||||||
self._homing = False
|
self._homing = False
|
||||||
return False
|
return False
|
||||||
if (self._mcu.serial.get_clock(last_sent_time)
|
if (self._mcu.serial.get_clock(last_sent_time)
|
||||||
|
@ -178,11 +186,6 @@ class MCU_endstop:
|
||||||
msg = self._query_cmd.encode(self._oid)
|
msg = self._query_cmd.encode(self._oid)
|
||||||
self._mcu.send(msg, cq=self._cmd_queue)
|
self._mcu.send(msg, cq=self._cmd_queue)
|
||||||
return True
|
return True
|
||||||
def get_last_position(self):
|
|
||||||
pos = self._last_state.get('pos', 0)
|
|
||||||
if self._stepper.get_invert_dir():
|
|
||||||
return -pos
|
|
||||||
return pos
|
|
||||||
def query_endstop(self, mcu_time):
|
def query_endstop(self, mcu_time):
|
||||||
clock = int(mcu_time * self._mcu_freq)
|
clock = int(mcu_time * self._mcu_freq)
|
||||||
self._homing = False
|
self._homing = False
|
||||||
|
@ -190,7 +193,7 @@ class MCU_endstop:
|
||||||
self._next_query_clock = clock
|
self._next_query_clock = clock
|
||||||
def query_endstop_wait(self):
|
def query_endstop_wait(self):
|
||||||
eventtime = time.time()
|
eventtime = time.time()
|
||||||
while self.check_busy(eventtime):
|
while self._check_busy(eventtime):
|
||||||
eventtime = self._mcu.pause(eventtime + 0.1)
|
eventtime = self._mcu.pause(eventtime + 0.1)
|
||||||
return self._last_state.get('pin', self._invert) ^ self._invert
|
return self._last_state.get('pin', self._invert) ^ self._invert
|
||||||
|
|
||||||
|
|
|
@ -75,7 +75,7 @@ class PrinterStepper:
|
||||||
self.need_motor_enable = not enable
|
self.need_motor_enable = not enable
|
||||||
def enable_endstop_checking(self, move_time, step_time):
|
def enable_endstop_checking(self, move_time, step_time):
|
||||||
mcu_time = self.mcu_endstop.print_to_mcu_time(move_time)
|
mcu_time = self.mcu_endstop.print_to_mcu_time(move_time)
|
||||||
self.mcu_endstop.home(mcu_time, step_time)
|
self.mcu_endstop.home_start(mcu_time, step_time)
|
||||||
return self.mcu_endstop
|
return self.mcu_endstop
|
||||||
def query_endstop(self, print_time):
|
def query_endstop(self, print_time):
|
||||||
mcu_time = self.mcu_endstop.print_to_mcu_time(print_time)
|
mcu_time = self.mcu_endstop.print_to_mcu_time(print_time)
|
||||||
|
@ -84,7 +84,7 @@ class PrinterStepper:
|
||||||
def get_homed_offset(self):
|
def get_homed_offset(self):
|
||||||
if not self.homing_stepper_phases or self.need_motor_enable:
|
if not self.homing_stepper_phases or self.need_motor_enable:
|
||||||
return 0
|
return 0
|
||||||
pos = self.mcu_endstop.get_last_position()
|
pos = self.mcu_stepper.get_mcu_position()
|
||||||
pos %= self.homing_stepper_phases
|
pos %= self.homing_stepper_phases
|
||||||
if self.homing_endstop_phase is None:
|
if self.homing_endstop_phase is None:
|
||||||
logging.info("Setting %s endstop phase to %d" % (
|
logging.info("Setting %s endstop phase to %d" % (
|
||||||
|
|
Loading…
Reference in New Issue