From 89f5452ddbcaecfba7cee5a3ba73b2659746d63f Mon Sep 17 00:00:00 2001 From: Kevin O'Connor Date: Thu, 8 Dec 2016 18:12:20 -0500 Subject: [PATCH] gcode: Rework homing to use greenlets Signed-off-by: Kevin O'Connor --- klippy/cartesian.py | 18 +++++------ klippy/delta.py | 20 ++++++------ klippy/gcode.py | 18 ++++++----- klippy/homing.py | 75 ++++++++++++--------------------------------- klippy/mcu.py | 21 +++++++------ klippy/stepper.py | 4 +-- 6 files changed, 59 insertions(+), 97 deletions(-) diff --git a/klippy/cartesian.py b/klippy/cartesian.py index 5171297e..b2bd9661 100644 --- a/klippy/cartesian.py +++ b/klippy/cartesian.py @@ -28,12 +28,6 @@ class CartKinematics: for i in StepList: s = self.steppers[i] 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): # Each axis is homed independently and in order for axis in homing_state.get_axes(): @@ -55,15 +49,17 @@ class CartKinematics: homepos[axis] = s.position_endstop coord = [None, None, None, None] 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 coord[axis] = rpos - homing_state.plan_retract(list(coord), [s], s.homing_speed) + homing_state.retract(list(coord), s.homing_speed) # Home again coord[axis] = r2pos - homing_state.plan_second_home( - list(coord), homepos, [s], s.homing_speed/2.0) - homing_state.plan_calc_position(self._get_homed_position) + homing_state.home( + list(coord), homepos, [s], s.homing_speed/2.0, second_home=True) + # 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): self.limits = [(1.0, -1.0)] * 3 for stepper in self.steppers: diff --git a/klippy/delta.py b/klippy/delta.py index bbb5f6f0..96079d1e 100644 --- a/klippy/delta.py +++ b/klippy/delta.py @@ -78,11 +78,6 @@ class DeltaKinematics: pos = self._cartesian_to_actuator(newpos) for i in StepList: 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): # All axes are homed simultaneously homing_state.set_axes([0, 1, 2]) @@ -92,16 +87,19 @@ class DeltaKinematics: homepos = [0., 0., s.position_endstop, None] coord = list(homepos) coord[2] = -1.5 * math.sqrt(self.arm_length2-self.max_xy2) - homing_state.plan_home(list(coord), homepos, self.steppers - , s.homing_speed) + homing_state.home(list(coord), homepos, self.steppers, s.homing_speed) # Retract 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 coord[2] -= s.homing_retract_dist - homing_state.plan_second_home(list(coord), homepos, self.steppers - , s.homing_speed/2.0) - homing_state.plan_calc_position(self._get_homed_position) + homing_state.home(list(coord), homepos, self.steppers + , s.homing_speed/2.0, second_home=True) + # 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): self.limit_xy2 = -1. for stepper in self.steppers: diff --git a/klippy/gcode.py b/klippy/gcode.py index f280b43e..5dee80cf 100644 --- a/klippy/gcode.py +++ b/klippy/gcode.py @@ -260,14 +260,16 @@ class GCodeParser: homing_state = homing.Homing(self.toolhead, axes) if self.is_fileinput: homing_state.set_no_verify_retract() - self.toolhead.home(homing_state) - def axes_update(homing_state): - newpos = self.toolhead.get_position() - for axis in homing_state.get_axes(): - self.last_position[axis] = newpos[axis] - self.base_position[axis] = -self.homing_add[axis] - homing_state.plan_axes_update(axes_update) - self.set_busy(homing_state) + try: + self.toolhead.home(homing_state) + except homing.EndstopError, e: + self.toolhead.motor_off() + self.respond_error(str(e)) + return + newpos = self.toolhead.get_position() + 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): # Use absolute coordinates self.absolutecoord = True diff --git a/klippy/homing.py b/klippy/homing.py index b1126b57..1e06f0b9 100644 --- a/klippy/homing.py +++ b/klippy/homing.py @@ -4,91 +4,54 @@ # # This file may be distributed under the terms of the GNU GPLv3 license. import logging -import mcu class Homing: def __init__(self, toolhead, changed_axes): self.toolhead = toolhead self.changed_axes = changed_axes self.verify_retract = True - - self.eventtime = 0. - self.states = [] - self.endstops = [] def set_no_verify_retract(self): self.verify_retract = False def set_axes(self, axes): self.changed_axes = axes def get_axes(self): return self.changed_axes - def plan_home(self, forcepos, movepos, steppers, speed): - 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): + def _fill_coord(self, coord): # Fill in any None entries in 'coord' with current toolhead position thcoord = list(self.toolhead.get_position()) for i in range(len(coord)): if coord[i] is not None: thcoord[i] = coord[i] return thcoord - def do_retract(self, newpos, steppers, speed): - self.toolhead.move(self.fill_coord(newpos), speed) - return False - def do_home(self, forcepos, movepos, steppers, speed): + def retract(self, newpos, speed): + self.toolhead.move(self._fill_coord(newpos), speed) + def home(self, forcepos, movepos, steppers, speed, second_home=False): # 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 print_time = self.toolhead.get_last_move_time() + endstops = [] for s in steppers: es = s.enable_endstop_checking(print_time, s.step_dist / speed) - self.endstops.append((s, es, s.mcu_stepper.get_mcu_position())) - self.toolhead.move(self.fill_coord(movepos), speed) + endstops.append((s, es, s.mcu_stepper.get_mcu_position())) + self.toolhead.move(self._fill_coord(movepos), speed) move_end_print_time = self.toolhead.get_last_move_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)) - return False - def do_wait_endstop(self, verify_retract): - # Check if endstops have triggered - while self.endstops: - stepper, es, last_pos = self.endstops[0] + # Wait for endstops to trigger + for s, es, last_pos in endstops: try: - if es.check_busy(self.eventtime): - return True - except mcu.error, e: + es.home_wait() + except es.error, e: raise EndstopError("Failed to home stepper %s: %s" % ( - stepper.name, str(e))) - post_home_pos = stepper.mcu_stepper.get_mcu_position() - if verify_retract and last_pos == post_home_pos: + s.name, str(e))) + post_home_pos = s.mcu_stepper.get_mcu_position() + if second_home and self.verify_retract and last_pos == post_home_pos: raise EndstopError("Endstop %s still triggered after retract" % ( - stepper.name,)) - self.endstops.pop(0) - return False - def do_calc_position(self, callback): - self.toolhead.set_position(self.fill_coord(callback(self))) + s.name,)) + def set_homed_position(self, pos): + self.toolhead.set_position(self._fill_coord(pos)) class EndstopError(Exception): pass diff --git a/klippy/mcu.py b/klippy/mcu.py index 091222b2..ba441474 100644 --- a/klippy/mcu.py +++ b/klippy/mcu.py @@ -111,6 +111,7 @@ class MCU_stepper: return self.ffi_lib.stepcompress_get_errors(self._stepqueue) class MCU_endstop: + error = error RETRY_QUERY = 1.000 def __init__(self, mcu, pin, stepper): self._mcu = mcu @@ -134,7 +135,7 @@ class MCU_endstop: self._retry_query_ticks = int(self._mcu_freq * self.RETRY_QUERY) self._last_state = {} 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) rest_ticks = int(rest_time * self._mcu_freq) self._homing = True @@ -149,10 +150,14 @@ class MCU_endstop: self._mcu.serial.send_flush() self._stepper.note_stepper_stop() 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): logging.debug("end_stop_state %s" % (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 if self._mcu.is_fileoutput(): return False @@ -161,7 +166,10 @@ class MCU_endstop: if not self._homing: return False 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 return False if (self._mcu.serial.get_clock(last_sent_time) @@ -178,11 +186,6 @@ class MCU_endstop: msg = self._query_cmd.encode(self._oid) self._mcu.send(msg, cq=self._cmd_queue) 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): clock = int(mcu_time * self._mcu_freq) self._homing = False @@ -190,7 +193,7 @@ class MCU_endstop: self._next_query_clock = clock def query_endstop_wait(self): eventtime = time.time() - while self.check_busy(eventtime): + while self._check_busy(eventtime): eventtime = self._mcu.pause(eventtime + 0.1) return self._last_state.get('pin', self._invert) ^ self._invert diff --git a/klippy/stepper.py b/klippy/stepper.py index 038412ff..1c40e6c7 100644 --- a/klippy/stepper.py +++ b/klippy/stepper.py @@ -75,7 +75,7 @@ class PrinterStepper: self.need_motor_enable = not enable def enable_endstop_checking(self, move_time, step_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 def query_endstop(self, print_time): mcu_time = self.mcu_endstop.print_to_mcu_time(print_time) @@ -84,7 +84,7 @@ class PrinterStepper: def get_homed_offset(self): if not self.homing_stepper_phases or self.need_motor_enable: return 0 - pos = self.mcu_endstop.get_last_position() + pos = self.mcu_stepper.get_mcu_position() pos %= self.homing_stepper_phases if self.homing_endstop_phase is None: logging.info("Setting %s endstop phase to %d" % (