From 4ca190d3934d665e1bbc2b729e233530e689f034 Mon Sep 17 00:00:00 2001 From: Kevin O'Connor Date: Tue, 12 Nov 2019 13:55:50 -0500 Subject: [PATCH] stepper: Move MCU_stepper from mcu.py to stepper.py Signed-off-by: Kevin O'Connor --- klippy/extras/controller_fan.py | 2 +- klippy/extras/force_move.py | 8 +- klippy/extras/manual_stepper.py | 6 +- klippy/extras/stepper_enable.py | 20 ++--- klippy/mcu.py | 134 +----------------------------- klippy/pins.py | 2 +- klippy/stepper.py | 141 +++++++++++++++++++++++++++++++- 7 files changed, 160 insertions(+), 153 deletions(-) diff --git a/klippy/extras/controller_fan.py b/klippy/extras/controller_fan.py index 31c1e6a7..adf78ccc 100644 --- a/klippy/extras/controller_fan.py +++ b/klippy/extras/controller_fan.py @@ -38,7 +38,7 @@ class ControllerFan: power = 0. active = False for name in self.stepper_names: - active |= self.stepper_enable.is_motor_enabled(name) + active |= self.stepper_enable.lookup_enable(name).is_motor_enabled() for heater in self.heaters: _, target_temp = heater.get_temp(eventtime) if target_temp: diff --git a/klippy/extras/force_move.py b/klippy/extras/force_move.py index 70d5abf6..187ba91b 100644 --- a/klippy/extras/force_move.py +++ b/klippy/extras/force_move.py @@ -54,9 +54,10 @@ class ForceMove: toolhead = self.printer.lookup_object('toolhead') print_time = toolhead.get_last_move_time() stepper_enable = self.printer.lookup_object('stepper_enable') - was_enable = stepper_enable.is_motor_enabled(stepper.get_name()) + enable = stepper_enable.lookup_enable(stepper.get_name()) + was_enable = enable.is_motor_enabled() if not was_enable: - stepper_enable.motor_enable(stepper.get_name(), print_time) + enable.motor_enable(print_time) toolhead.dwell(STALL_TIME) return was_enable def restore_enable(self, stepper, was_enable): @@ -65,7 +66,8 @@ class ForceMove: toolhead.dwell(STALL_TIME) print_time = toolhead.get_last_move_time() stepper_enable = self.printer.lookup_object('stepper_enable') - stepper_enable.motor_disable(stepper.get_name(), print_time) + enable = stepper_enable.lookup_enable(stepper.get_name()) + enable.motor_disable(print_time) toolhead.dwell(STALL_TIME) def manual_move(self, stepper, dist, speed, accel=0.): toolhead = self.printer.lookup_object('toolhead') diff --git a/klippy/extras/manual_stepper.py b/klippy/extras/manual_stepper.py index cf597405..aa9c8049 100644 --- a/klippy/extras/manual_stepper.py +++ b/klippy/extras/manual_stepper.py @@ -49,10 +49,12 @@ class ManualStepper: stepper_enable = self.printer.lookup_object('stepper_enable') if enable: for s in self.steppers: - stepper_enable.motor_enable(s.get_name(), self.next_cmd_time) + se = stepper_enable.lookup_enable(s.get_name()) + se.motor_enable(self.next_cmd_time) else: for s in self.steppers: - stepper_enable.motor_disable(s.get_name(), self.next_cmd_time) + se = stepper_enable.lookup_enable(s.get_name()) + se.motor_disable(self.next_cmd_time) self.sync_print_time() def do_set_position(self, setpos): self.rail.set_position([setpos, 0., 0.]) diff --git a/klippy/extras/stepper_enable.py b/klippy/extras/stepper_enable.py index 8f028055..52910387 100644 --- a/klippy/extras/stepper_enable.py +++ b/klippy/extras/stepper_enable.py @@ -54,19 +54,21 @@ def lookup_enable_pin(ppins, pin_list): class EnableTracking: def __init__(self, printer, stepper, pin): self.stepper = stepper - self.is_motor_enabled = False + self.is_enabled = False self.stepper.add_active_callback(self.motor_enable) self.enable = lookup_enable_pin(printer.lookup_object('pins'), pin) def motor_enable(self, print_time): - if not self.is_motor_enabled: + if not self.is_enabled: self.enable.set_enable(print_time) - self.is_motor_enabled = True + self.is_enabled = True def motor_disable(self, print_time): - if self.is_motor_enabled: + if self.is_enabled: # Enable stepper on future stepper movement self.enable.set_disable(print_time) - self.is_motor_enabled = False + self.is_enabled = False self.stepper.add_active_callback(self.motor_enable) + def is_motor_enabled(self): + return self.is_enabled class PrinterStepperEnable: def __init__(self, config): @@ -95,12 +97,8 @@ class PrinterStepperEnable: def cmd_M18(self, params): # Turn off motors self.motor_off() - def is_motor_enabled(self, name): - return self.enable_lines[name].is_motor_enabled - def motor_enable(self, name, print_time): - self.enable_lines[name].motor_enable(print_time) - def motor_disable(self, name, print_time): - self.enable_lines[name].motor_disable(print_time) + def lookup_enable(self, name): + return self.enable_lines[name] def load_config(config): return PrinterStepperEnable(config) diff --git a/klippy/mcu.py b/klippy/mcu.py index 0a283452..4f045263 100644 --- a/klippy/mcu.py +++ b/klippy/mcu.py @@ -9,138 +9,6 @@ import serialhdl, pins, chelper, clocksync class error(Exception): pass -class MCU_stepper: - def __init__(self, mcu, pin_params): - self._mcu = mcu - self._oid = oid = self._mcu.create_oid() - self._mcu.register_config_callback(self._build_config) - self._step_pin = pin_params['pin'] - self._invert_step = pin_params['invert'] - self._dir_pin = self._invert_dir = None - self._mcu_position_offset = 0. - self._step_dist = 0. - self._min_stop_interval = 0. - self._reset_cmd_id = self._get_position_cmd = None - self._active_callbacks = [] - ffi_main, self._ffi_lib = chelper.get_ffi() - self._stepqueue = ffi_main.gc(self._ffi_lib.stepcompress_alloc(oid), - self._ffi_lib.stepcompress_free) - self._mcu.register_stepqueue(self._stepqueue) - self._stepper_kinematics = None - self._itersolve_generate_steps = self._ffi_lib.itersolve_generate_steps - self._itersolve_check_active = self._ffi_lib.itersolve_check_active - self._trapq = ffi_main.NULL - def get_mcu(self): - return self._mcu - def setup_dir_pin(self, pin_params): - if pin_params['chip'] is not self._mcu: - raise pins.error("Stepper dir pin must be on same mcu as step pin") - self._dir_pin = pin_params['pin'] - self._invert_dir = pin_params['invert'] - def setup_min_stop_interval(self, min_stop_interval): - self._min_stop_interval = min_stop_interval - def setup_step_distance(self, step_dist): - self._step_dist = step_dist - def setup_itersolve(self, alloc_func, *params): - ffi_main, ffi_lib = chelper.get_ffi() - sk = ffi_main.gc(getattr(ffi_lib, alloc_func)(*params), ffi_lib.free) - self.set_stepper_kinematics(sk) - def _build_config(self): - max_error = self._mcu.get_max_stepper_error() - min_stop_interval = max(0., self._min_stop_interval - max_error) - self._mcu.add_config_cmd( - "config_stepper oid=%d step_pin=%s dir_pin=%s" - " min_stop_interval=%d invert_step=%d" % ( - self._oid, self._step_pin, self._dir_pin, - self._mcu.seconds_to_clock(min_stop_interval), - self._invert_step)) - self._mcu.add_config_cmd( - "reset_step_clock oid=%d clock=0" % (self._oid,), is_init=True) - step_cmd_id = self._mcu.lookup_command_id( - "queue_step oid=%c interval=%u count=%hu add=%hi") - dir_cmd_id = self._mcu.lookup_command_id( - "set_next_step_dir oid=%c dir=%c") - self._reset_cmd_id = self._mcu.lookup_command_id( - "reset_step_clock oid=%c clock=%u") - self._get_position_cmd = self._mcu.lookup_command( - "stepper_get_position oid=%c") - self._ffi_lib.stepcompress_fill( - self._stepqueue, self._mcu.seconds_to_clock(max_error), - self._invert_dir, step_cmd_id, dir_cmd_id) - def get_oid(self): - return self._oid - def get_step_dist(self): - return self._step_dist - def is_dir_inverted(self): - return self._invert_dir - def calc_position_from_coord(self, coord): - return self._ffi_lib.itersolve_calc_position_from_coord( - self._stepper_kinematics, coord[0], coord[1], coord[2]) - def set_position(self, coord): - self.set_commanded_position(self.calc_position_from_coord(coord)) - def get_commanded_position(self): - return self._ffi_lib.itersolve_get_commanded_pos( - self._stepper_kinematics) - def set_commanded_position(self, pos): - self._mcu_position_offset += self.get_commanded_position() - pos - self._ffi_lib.itersolve_set_commanded_pos(self._stepper_kinematics, pos) - def get_mcu_position(self): - mcu_pos_dist = self.get_commanded_position() + self._mcu_position_offset - mcu_pos = mcu_pos_dist / self._step_dist - if mcu_pos >= 0.: - return int(mcu_pos + 0.5) - return int(mcu_pos - 0.5) - def set_stepper_kinematics(self, sk): - old_sk = self._stepper_kinematics - self._stepper_kinematics = sk - if sk is not None: - self._ffi_lib.itersolve_set_stepcompress( - sk, self._stepqueue, self._step_dist) - return old_sk - def note_homing_end(self, did_trigger=False): - ret = self._ffi_lib.stepcompress_reset(self._stepqueue, 0) - if ret: - raise error("Internal error in stepcompress") - data = (self._reset_cmd_id, self._oid, 0) - ret = self._ffi_lib.stepcompress_queue_msg( - self._stepqueue, data, len(data)) - if ret: - raise error("Internal error in stepcompress") - if not did_trigger or self._mcu.is_fileoutput(): - return - params = self._get_position_cmd.send_with_response( - [self._oid], response='stepper_position', response_oid=self._oid) - mcu_pos_dist = params['pos'] * self._step_dist - if self._invert_dir: - mcu_pos_dist = -mcu_pos_dist - self._ffi_lib.itersolve_set_commanded_pos( - self._stepper_kinematics, mcu_pos_dist - self._mcu_position_offset) - def set_trapq(self, tq): - if tq is None: - ffi_main, self._ffi_lib = chelper.get_ffi() - tq = ffi_main.NULL - self._ffi_lib.itersolve_set_trapq(self._stepper_kinematics, tq) - old_tq = self._trapq - self._trapq = tq - return old_tq - def add_active_callback(self, cb): - self._active_callbacks.append(cb) - def generate_steps(self, flush_time): - # Check for activity if necessary - if self._active_callbacks: - ret = self._itersolve_check_active(self._stepper_kinematics, - flush_time) - if ret: - cbs = self._active_callbacks - self._active_callbacks = [] - for cb in cbs: - cb(ret) - # Generate steps - ret = self._itersolve_generate_steps(self._stepper_kinematics, - flush_time) - if ret: - raise error("Internal error in stepcompress") - class MCU_endstop: class TimeoutError(Exception): pass @@ -699,7 +567,7 @@ class MCU: self.register_response(self._handle_mcu_stats, 'stats') # Config creation helpers def setup_pin(self, pin_type, pin_params): - pcs = {'stepper': MCU_stepper, 'endstop': MCU_endstop, + pcs = {'endstop': MCU_endstop, 'digital_out': MCU_digital_out, 'pwm': MCU_pwm, 'adc': MCU_adc} if pin_type not in pcs: raise pins.error("pin type %s not supported on mcu" % (pin_type,)) diff --git a/klippy/pins.py b/klippy/pins.py index a4b93762..4f2c1db2 100644 --- a/klippy/pins.py +++ b/klippy/pins.py @@ -251,7 +251,7 @@ class PrinterPins: self.active_pins[share_name] = pin_params return pin_params def setup_pin(self, pin_type, pin_desc): - can_invert = pin_type in ['stepper', 'endstop', 'digital_out', 'pwm'] + can_invert = pin_type in ['endstop', 'digital_out', 'pwm'] can_pullup = pin_type in ['endstop'] pin_params = self.lookup_pin(pin_desc, can_invert, can_pullup) return pin_params['chip'].setup_pin(pin_type, pin_params) diff --git a/klippy/stepper.py b/klippy/stepper.py index cc108768..24cc9fac 100644 --- a/klippy/stepper.py +++ b/klippy/stepper.py @@ -4,13 +4,149 @@ # # This file may be distributed under the terms of the GNU GPLv3 license. import math, logging, collections -import homing +import homing, chelper + +class error(Exception): + pass ###################################################################### # Steppers ###################################################################### +class MCU_stepper: + def __init__(self, pin_params): + self._mcu = pin_params['chip'] + self._oid = oid = self._mcu.create_oid() + self._mcu.register_config_callback(self._build_config) + self._step_pin = pin_params['pin'] + self._invert_step = pin_params['invert'] + self._dir_pin = self._invert_dir = None + self._mcu_position_offset = 0. + self._step_dist = 0. + self._min_stop_interval = 0. + self._reset_cmd_id = self._get_position_cmd = None + self._active_callbacks = [] + ffi_main, self._ffi_lib = chelper.get_ffi() + self._stepqueue = ffi_main.gc(self._ffi_lib.stepcompress_alloc(oid), + self._ffi_lib.stepcompress_free) + self._mcu.register_stepqueue(self._stepqueue) + self._stepper_kinematics = None + self._itersolve_generate_steps = self._ffi_lib.itersolve_generate_steps + self._itersolve_check_active = self._ffi_lib.itersolve_check_active + self._trapq = ffi_main.NULL + def get_mcu(self): + return self._mcu + def setup_dir_pin(self, pin_params): + if pin_params['chip'] is not self._mcu: + raise self._mcu.get_printer().config_error( + "Stepper dir pin must be on same mcu as step pin") + self._dir_pin = pin_params['pin'] + self._invert_dir = pin_params['invert'] + def setup_min_stop_interval(self, min_stop_interval): + self._min_stop_interval = min_stop_interval + def setup_step_distance(self, step_dist): + self._step_dist = step_dist + def setup_itersolve(self, alloc_func, *params): + ffi_main, ffi_lib = chelper.get_ffi() + sk = ffi_main.gc(getattr(ffi_lib, alloc_func)(*params), ffi_lib.free) + self.set_stepper_kinematics(sk) + def _build_config(self): + max_error = self._mcu.get_max_stepper_error() + min_stop_interval = max(0., self._min_stop_interval - max_error) + self._mcu.add_config_cmd( + "config_stepper oid=%d step_pin=%s dir_pin=%s" + " min_stop_interval=%d invert_step=%d" % ( + self._oid, self._step_pin, self._dir_pin, + self._mcu.seconds_to_clock(min_stop_interval), + self._invert_step)) + self._mcu.add_config_cmd( + "reset_step_clock oid=%d clock=0" % (self._oid,), is_init=True) + step_cmd_id = self._mcu.lookup_command_id( + "queue_step oid=%c interval=%u count=%hu add=%hi") + dir_cmd_id = self._mcu.lookup_command_id( + "set_next_step_dir oid=%c dir=%c") + self._reset_cmd_id = self._mcu.lookup_command_id( + "reset_step_clock oid=%c clock=%u") + self._get_position_cmd = self._mcu.lookup_command( + "stepper_get_position oid=%c") + self._ffi_lib.stepcompress_fill( + self._stepqueue, self._mcu.seconds_to_clock(max_error), + self._invert_dir, step_cmd_id, dir_cmd_id) + def get_oid(self): + return self._oid + def get_step_dist(self): + return self._step_dist + def is_dir_inverted(self): + return self._invert_dir + def calc_position_from_coord(self, coord): + return self._ffi_lib.itersolve_calc_position_from_coord( + self._stepper_kinematics, coord[0], coord[1], coord[2]) + def set_position(self, coord): + self.set_commanded_position(self.calc_position_from_coord(coord)) + def get_commanded_position(self): + return self._ffi_lib.itersolve_get_commanded_pos( + self._stepper_kinematics) + def set_commanded_position(self, pos): + self._mcu_position_offset += self.get_commanded_position() - pos + self._ffi_lib.itersolve_set_commanded_pos(self._stepper_kinematics, pos) + def get_mcu_position(self): + mcu_pos_dist = self.get_commanded_position() + self._mcu_position_offset + mcu_pos = mcu_pos_dist / self._step_dist + if mcu_pos >= 0.: + return int(mcu_pos + 0.5) + return int(mcu_pos - 0.5) + def set_stepper_kinematics(self, sk): + old_sk = self._stepper_kinematics + self._stepper_kinematics = sk + if sk is not None: + self._ffi_lib.itersolve_set_stepcompress( + sk, self._stepqueue, self._step_dist) + return old_sk + def note_homing_end(self, did_trigger=False): + ret = self._ffi_lib.stepcompress_reset(self._stepqueue, 0) + if ret: + raise error("Internal error in stepcompress") + data = (self._reset_cmd_id, self._oid, 0) + ret = self._ffi_lib.stepcompress_queue_msg( + self._stepqueue, data, len(data)) + if ret: + raise error("Internal error in stepcompress") + if not did_trigger or self._mcu.is_fileoutput(): + return + params = self._get_position_cmd.send_with_response( + [self._oid], response='stepper_position', response_oid=self._oid) + mcu_pos_dist = params['pos'] * self._step_dist + if self._invert_dir: + mcu_pos_dist = -mcu_pos_dist + self._ffi_lib.itersolve_set_commanded_pos( + self._stepper_kinematics, mcu_pos_dist - self._mcu_position_offset) + def set_trapq(self, tq): + if tq is None: + ffi_main, self._ffi_lib = chelper.get_ffi() + tq = ffi_main.NULL + self._ffi_lib.itersolve_set_trapq(self._stepper_kinematics, tq) + old_tq = self._trapq + self._trapq = tq + return old_tq + def add_active_callback(self, cb): + self._active_callbacks.append(cb) + def generate_steps(self, flush_time): + # Check for activity if necessary + if self._active_callbacks: + ret = self._itersolve_check_active(self._stepper_kinematics, + flush_time) + if ret: + cbs = self._active_callbacks + self._active_callbacks = [] + for cb in cbs: + cb(ret) + # Generate steps + ret = self._itersolve_generate_steps(self._stepper_kinematics, + flush_time) + if ret: + raise error("Internal error in stepcompress") + # Code storing the definitions for a stepper motor class PrinterStepper: def __init__(self, config): @@ -19,7 +155,8 @@ class PrinterStepper: # Stepper definition ppins = printer.lookup_object('pins') step_pin = config.get('step_pin') - self.mcu_stepper = mcu_stepper = ppins.setup_pin('stepper', step_pin) + step_pin_params = ppins.lookup_pin(step_pin, can_invert=True) + self.mcu_stepper = mcu_stepper = MCU_stepper(step_pin_params) dir_pin = config.get('dir_pin') dir_pin_params = ppins.lookup_pin(dir_pin, can_invert=True) mcu_stepper.setup_dir_pin(dir_pin_params)