From 92f81d51f4a88dd6f32a2507e8c3d754c62f4b03 Mon Sep 17 00:00:00 2001 From: Kevin O'Connor Date: Mon, 25 Jul 2016 23:47:30 -0400 Subject: [PATCH] homing: Move low-level coordinate manipulation to kinematic class Rework the code so that the kinematic class (currently just cartesian.py) has more control over the homing process. Signed-off-by: Kevin O'Connor --- klippy/cartesian.py | 31 ++++++++++++++++--- klippy/gcode.py | 24 +++++++-------- klippy/homing.py | 75 ++++++++++++++++++--------------------------- klippy/toolhead.py | 4 +-- 4 files changed, 70 insertions(+), 64 deletions(-) diff --git a/klippy/cartesian.py b/klippy/cartesian.py index 67c68be5..f061f673 100644 --- a/klippy/cartesian.py +++ b/klippy/cartesian.py @@ -33,11 +33,34 @@ class CartKinematics: accel_factor = min([self.steppers[i].max_accel / abs(axes_d[i]) for i in StepList if axes_d[i]]) return velocity_factor * move_d, accel_factor * move_d - def home(self, toolhead, axis): + def home(self, toolhead, axes): # Each axis is homed independently and in order - homing_state = homing.Homing(toolhead, self.steppers) # XXX - for a in axis: - homing_state.plan_home(a) + homing_state = homing.Homing(toolhead, axes) + for axis in axes: + s = self.steppers[axis] + # Determine moves + if s.homing_positive_dir: + pos = s.position_endstop + 1.5*( + s.position_min - s.position_endstop) + rpos = s.position_endstop - s.homing_retract_dist + r2pos = rpos - s.homing_retract_dist + else: + pos = s.position_endstop + 1.5*( + s.position_max - s.position_endstop) + rpos = s.position_endstop + s.homing_retract_dist + r2pos = rpos + s.homing_retract_dist + # Initial homing + homepos = [None, None, None, None] + homepos[axis] = s.position_endstop + coord = [None, None, None, None] + coord[axis] = pos + homing_state.plan_home(list(coord), homepos, [s], s.homing_speed) + # Retract + coord[axis] = rpos + homing_state.plan_move(list(coord), s.homing_speed) + # Home again + coord[axis] = r2pos + homing_state.plan_home(list(coord), homepos, [s], s.homing_speed/2.0) return homing_state def motor_off(self, move_time): for stepper in self.steppers: diff --git a/klippy/gcode.py b/klippy/gcode.py index fd377c1c..ad05b0d5 100644 --- a/klippy/gcode.py +++ b/klippy/gcode.py @@ -227,19 +227,19 @@ class GCodeParser: pass def cmd_G28(self, params): # Move to origin - axis = [] - for a in 'XYZ': - if a in params: - axis.append(self.axis2pos[a]) - if not axis: - axis = [0, 1, 2] - busy_handler = self.toolhead.home(axis) - def axis_update(axis): + axes = [] + for axis in 'XYZ': + if axis in params: + axes.append(self.axis2pos[axis]) + if not axes: + axes = [0, 1, 2] + busy_handler = self.toolhead.home(axes) + def axes_update(axes): newpos = self.toolhead.get_position() - for a in axis: - self.last_position[a] = newpos[a] - self.base_position[a] = -self.homing_add[a] - busy_handler.plan_axis_update(axis_update) + for axis in axes: + self.last_position[axis] = newpos[axis] + self.base_position[axis] = -self.homing_add[axis] + busy_handler.plan_axes_update(axes_update) self.set_busy(busy_handler) def cmd_G90(self, params): # Use absolute coordinates diff --git a/klippy/homing.py b/klippy/homing.py index 8eb0d065..8bcf4a24 100644 --- a/klippy/homing.py +++ b/klippy/homing.py @@ -7,36 +7,19 @@ import logging class Homing: - def __init__(self, toolhead, steppers): + def __init__(self, toolhead, changed_axes): self.toolhead = toolhead - self.steppers = steppers + self.changed_axes = changed_axes self.states = [] self.endstops = [] - self.changed_axis = [] - def plan_home(self, axis, precise=False): - s = self.steppers[axis] - state = self.states - self.changed_axis.append(axis) - speed = s.homing_speed - if s.homing_positive_dir: - pos = s.position_endstop + 1.5*(s.position_min - s.position_endstop) - rpos = s.position_endstop - s.homing_retract_dist - r2pos = rpos - s.homing_retract_dist - else: - pos = s.position_endstop + 1.5*(s.position_max - s.position_endstop) - rpos = s.position_endstop + s.homing_retract_dist - r2pos = rpos + s.homing_retract_dist - # Initial homing - state.append((self.do_home, ({axis: pos}, speed))) - state.append((self.do_wait_endstop, ({axis: 1},))) - # Retract - state.append((self.do_move, ({axis: rpos}, speed))) - # Home again - state.append((self.do_home, ({axis: r2pos}, speed/2.0))) - state.append((self.do_wait_endstop, ({axis: 1},))) - def plan_axis_update(self, callback): - self.states.append((callback, (self.changed_axis,))) + 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, ())) + def plan_move(self, newpos, speed): + self.states.append((self.do_move, (newpos, speed))) + def plan_axes_update(self, callback): + self.states.append((callback, (self.changed_axes,))) def check_busy(self, eventtime): while self.states: first = self.states[0] @@ -46,34 +29,34 @@ class Homing: self.states.pop(0) return False - def do_move(self, axis_pos, speed): - # Issue a move command to axis_pos - newpos = self.toolhead.get_position() - for axis, pos in axis_pos.items(): - newpos[axis] = pos - self.toolhead.move(newpos, speed) + def fill_coord(self, coord): + # Fill in any None entries in 'coord' with current toolhead position + thcoord = self.toolhead.get_position() + coord = list(coord) + for i in range(len(coord)): + if coord[i] is None: + coord[i] = thcoord[i] + return coord + def do_move(self, newpos, speed): + # Issue a move command + self.toolhead.move(self.fill_coord(newpos), speed) return False - def do_home(self, axis_pos, speed): - # Alter kinematics class to think printer is at axis_pos - newpos = self.toolhead.get_position() - forcepos = list(newpos) - for axis, pos in axis_pos.items(): - newpos[axis] = self.steppers[axis].position_endstop - forcepos[axis] = pos - self.toolhead.set_position(forcepos) + def do_home(self, forcepos, movepos, steppers, speed): + # Alter kinematics class to think printer is at forcepos + self.toolhead.set_position(self.fill_coord(forcepos)) # Start homing and issue move print_time = self.toolhead.get_last_move_time() - for axis in axis_pos: - hz = speed * self.steppers[axis].inv_step_dist - es = self.steppers[axis].enable_endstop_checking(print_time, hz) + for s in steppers: + hz = speed * s.inv_step_dist + es = s.enable_endstop_checking(print_time, hz) self.endstops.append(es) - self.toolhead.move(newpos, speed) + self.toolhead.move(self.fill_coord(movepos), speed) self.toolhead.reset_print_time() for es in self.endstops: es.home_finalize() return False - def do_wait_endstop(self, axis_wait): - # Check if axis_wait endstops have triggered + def do_wait_endstop(self): + # Check if endstops have triggered for es in self.endstops: if es.is_homing(): return True diff --git a/klippy/toolhead.py b/klippy/toolhead.py index d0cd86a5..35d296e1 100644 --- a/klippy/toolhead.py +++ b/klippy/toolhead.py @@ -253,8 +253,8 @@ class ToolHead: speed = min(speed, self.max_xy_speed) move = Move(self, newpos, move_d, axes_d, speed, self.max_xy_accel) self.move_queue.add_move(move) - def home(self, axis): - return self.kin.home(self, axis) + def home(self, axes): + return self.kin.home(self, axes) def dwell(self, delay): self.get_last_move_time() self.update_move_time(delay)