From e52113a319d460489ebd26867dbe85446f0a4f15 Mon Sep 17 00:00:00 2001 From: Kevin O'Connor Date: Wed, 24 Aug 2016 16:42:25 -0400 Subject: [PATCH] mcu: convert stepper, endstop, and digital_out to take mcu_time Signed-off-by: Kevin O'Connor --- klippy/cartesian.py | 25 ++++++++-------- klippy/extruder.py | 37 ++++++++++++----------- klippy/homing.py | 3 +- klippy/mcu.py | 71 +++++++++++++++++++++++---------------------- klippy/stepper.py | 29 ++++++++---------- 5 files changed, 79 insertions(+), 86 deletions(-) diff --git a/klippy/cartesian.py b/klippy/cartesian.py index b904d73f..653b2276 100644 --- a/klippy/cartesian.py +++ b/klippy/cartesian.py @@ -80,34 +80,33 @@ class CartKinematics: if steps < 0: sdir = 1 steps = -steps - clock_offset, clock_freq, so = self.steppers[i].prep_move( - sdir, move_time) + mcu_time, so = self.steppers[i].prep_move(move_time, sdir) step_dist = move.move_d / steps step_offset = 0.5 # Acceleration steps #t = sqrt(2*pos/accel + (start_v/accel)**2) - start_v/accel - accel_clock_offset = move.start_v * inv_accel * clock_freq - accel_sqrt_offset = accel_clock_offset**2 - accel_multiplier = 2.0 * step_dist * inv_accel * clock_freq**2 + accel_time_offset = move.start_v * inv_accel + accel_sqrt_offset = accel_time_offset**2 + accel_multiplier = 2.0 * step_dist * inv_accel accel_steps = move.accel_r * steps step_offset = so.step_sqrt( - accel_steps, step_offset, clock_offset - accel_clock_offset + mcu_time - accel_time_offset, accel_steps, step_offset , accel_sqrt_offset, accel_multiplier) - clock_offset += move.accel_t * clock_freq + mcu_time += move.accel_t # Cruising steps #t = pos/cruise_v - cruise_multiplier = step_dist * inv_cruise_v * clock_freq + cruise_multiplier = step_dist * inv_cruise_v cruise_steps = move.cruise_r * steps step_offset = so.step_factor( - cruise_steps, step_offset, clock_offset, cruise_multiplier) - clock_offset += move.cruise_t * clock_freq + mcu_time, cruise_steps, step_offset, cruise_multiplier) + mcu_time += move.cruise_t # Deceleration steps #t = cruise_v/accel - sqrt((cruise_v/accel)**2 - 2*pos/accel) - decel_clock_offset = move.cruise_v * inv_accel * clock_freq - decel_sqrt_offset = decel_clock_offset**2 + decel_time_offset = move.cruise_v * inv_accel + decel_sqrt_offset = decel_time_offset**2 decel_steps = move.decel_r * steps so.step_sqrt( - decel_steps, step_offset, clock_offset + decel_clock_offset + mcu_time + decel_time_offset, decel_steps, step_offset , decel_sqrt_offset, -accel_multiplier) diff --git a/klippy/extruder.py b/klippy/extruder.py index ba12fd64..3aef5a19 100644 --- a/klippy/extruder.py +++ b/klippy/extruder.py @@ -77,8 +77,7 @@ class PrinterExtruder: if steps < 0: sdir = 1 steps = -steps - clock_offset, clock_freq, so = self.stepper.prep_move( - sdir, move_time) + mcu_time, so = self.stepper.prep_move(move_time, sdir) step_dist = forward_d / steps inv_step_dist = 1. / step_dist @@ -86,28 +85,28 @@ class PrinterExtruder: # Acceleration steps #t = sqrt(2*pos/accel + (start_v/accel)**2) - start_v/accel - accel_clock_offset = start_v * inv_accel * clock_freq - accel_sqrt_offset = accel_clock_offset**2 - accel_multiplier = 2.0 * step_dist * inv_accel * clock_freq**2 + accel_time_offset = start_v * inv_accel + accel_sqrt_offset = accel_time_offset**2 + accel_multiplier = 2.0 * step_dist * inv_accel accel_steps = accel_d * inv_step_dist step_offset = so.step_sqrt( - accel_steps, step_offset, clock_offset - accel_clock_offset + mcu_time - accel_time_offset, accel_steps, step_offset , accel_sqrt_offset, accel_multiplier) - clock_offset += accel_t * clock_freq + mcu_time += accel_t # Cruising steps #t = pos/cruise_v - cruise_multiplier = step_dist * clock_freq / cruise_v + cruise_multiplier = step_dist / cruise_v cruise_steps = cruise_d * inv_step_dist step_offset = so.step_factor( - cruise_steps, step_offset, clock_offset, cruise_multiplier) - clock_offset += cruise_t * clock_freq + mcu_time, cruise_steps, step_offset, cruise_multiplier) + mcu_time += cruise_t # Deceleration steps #t = cruise_v/accel - sqrt((cruise_v/accel)**2 - 2*pos/accel) - decel_clock_offset = decel_v * inv_accel * clock_freq - decel_sqrt_offset = decel_clock_offset**2 + decel_time_offset = decel_v * inv_accel + decel_sqrt_offset = decel_time_offset**2 decel_steps = decel_d * inv_step_dist so.step_sqrt( - decel_steps, step_offset, clock_offset + decel_clock_offset + mcu_time + decel_time_offset, decel_steps, step_offset , decel_sqrt_offset, -accel_multiplier) # Determine retract steps @@ -116,15 +115,15 @@ class PrinterExtruder: steps = self.stepper_pos - new_step_pos if steps: self.stepper_pos = new_step_pos - clock_offset, clock_freq, so = self.stepper.prep_move( - 1, move_time+accel_t+cruise_t+decel_t) + mcu_time, so = self.stepper.prep_move( + move_time+accel_t+cruise_t+decel_t, 1) step_dist = retract_d / steps # Acceleration steps #t = sqrt(2*pos/accel + (start_v/accel)**2) - start_v/accel - accel_clock_offset = retract_v * inv_accel * clock_freq - accel_sqrt_offset = accel_clock_offset**2 - accel_multiplier = 2.0 * step_dist * inv_accel * clock_freq**2 - so.step_sqrt(steps, 0.5, clock_offset - accel_clock_offset + accel_time_offset = retract_v * inv_accel + accel_sqrt_offset = accel_time_offset**2 + accel_multiplier = 2.0 * step_dist * inv_accel + so.step_sqrt(mcu_time - accel_time_offset, steps, 0.5 , accel_sqrt_offset, accel_multiplier) diff --git a/klippy/homing.py b/klippy/homing.py index 8bcf4a24..8670c45a 100644 --- a/klippy/homing.py +++ b/klippy/homing.py @@ -47,8 +47,7 @@ class Homing: # Start homing and issue move print_time = self.toolhead.get_last_move_time() for s in steppers: - hz = speed * s.inv_step_dist - es = s.enable_endstop_checking(print_time, hz) + es = s.enable_endstop_checking(print_time, s.step_dist / speed) self.endstops.append(es) self.toolhead.move(self.fill_coord(movepos), speed) self.toolhead.reset_print_time() diff --git a/klippy/mcu.py b/klippy/mcu.py index 2c7dcbd3..c5e4ee16 100644 --- a/klippy/mcu.py +++ b/klippy/mcu.py @@ -24,6 +24,9 @@ class MCU_stepper: dir_pin, pullup, self._invert_dir = parse_pin_extras(dir_pin) self._sdir = -1 self._last_move_clock = -2**29 + self._mcu_freq = mcu.get_mcu_freq() + min_stop_interval = int(min_stop_interval * self._mcu_freq) + max_error = int(max_error * self._mcu_freq) mcu.add_config_cmd( "config_stepper oid=%d step_pin=%s dir_pin=%s" " min_stop_interval=%d invert_step=%d" % ( @@ -38,6 +41,7 @@ class MCU_stepper: ffi_main, self.ffi_lib = chelper.get_ffi() self._stepqueue = self.ffi_lib.stepcompress_alloc( max_error, self._step_cmd.msgid, self._oid) + self.print_to_mcu_time = mcu.print_to_mcu_time def get_oid(self): return self._oid def get_invert_dir(self): @@ -45,32 +49,35 @@ class MCU_stepper: def note_stepper_stop(self): self._sdir = -1 self._last_move_clock = -2**29 - def reset_step_clock(self, clock): + def _reset_step_clock(self, clock): self.ffi_lib.stepcompress_reset(self._stepqueue, clock) data = (self._reset_cmd.msgid, self._oid, clock & 0xffffffff) self.ffi_lib.stepcompress_queue_msg(self._stepqueue, data, len(data)) - def set_next_step_dir(self, sdir, clock): + def set_next_step_dir(self, mcu_time, sdir): + clock = int(mcu_time * self._mcu_freq) if clock - self._last_move_clock >= 2**29: - self.reset_step_clock(clock) + self._reset_step_clock(clock) self._last_move_clock = clock if self._sdir == sdir: return self._sdir = sdir data = (self._dir_cmd.msgid, self._oid, sdir ^ self._invert_dir) self.ffi_lib.stepcompress_queue_msg(self._stepqueue, data, len(data)) - def step(self, steptime): - self.ffi_lib.stepcompress_push(self._stepqueue, steptime) - def step_sqrt(self, steps, step_offset, clock_offset, sqrt_offset, factor): + def step(self, mcu_time): + clock = mcu_time * self._mcu_freq + self.ffi_lib.stepcompress_push(self._stepqueue, clock) + def step_sqrt(self, mcu_time, steps, step_offset, sqrt_offset, factor): + clock = mcu_time * self._mcu_freq + mcu_freq2 = self._mcu_freq**2 return self.ffi_lib.stepcompress_push_sqrt( - self._stepqueue, steps, step_offset, clock_offset - , sqrt_offset, factor) - def step_factor(self, steps, step_offset, clock_offset, factor): + self._stepqueue, steps, step_offset, clock + , sqrt_offset * mcu_freq2, factor * mcu_freq2) + def step_factor(self, mcu_time, steps, step_offset, factor): + clock = mcu_time * self._mcu_freq return self.ffi_lib.stepcompress_push_factor( - self._stepqueue, steps, step_offset, clock_offset, factor) + self._stepqueue, steps, step_offset, clock, factor * self._mcu_freq) def get_errors(self): return self.ffi_lib.stepcompress_get_errors(self._stepqueue) - def get_print_clock(self, print_time): - return self._mcu.get_print_clock(print_time) class MCU_endstop: RETRY_QUERY = 1.000 @@ -92,9 +99,12 @@ class MCU_endstop: self._homing = False self._last_position = 0 self._next_query_clock = 0 - mcu_freq = self._mcu.get_mcu_freq() - self._retry_query_ticks = mcu_freq * self.RETRY_QUERY - def home(self, clock, rest_ticks): + self._mcu_freq = mcu.get_mcu_freq() + self._retry_query_ticks = int(self._mcu_freq * self.RETRY_QUERY) + self.print_to_mcu_time = mcu.print_to_mcu_time + def home(self, mcu_time, rest_time): + clock = int(mcu_time * self._mcu_freq) + rest_ticks = int(rest_time * self._mcu_freq) self._homing = True self._next_query_clock = clock + self._retry_query_ticks msg = self._home_cmd.encode( @@ -124,8 +134,6 @@ class MCU_endstop: if self._stepper.get_invert_dir(): return -self._last_position return self._last_position - def get_print_clock(self, print_time): - return self._mcu.get_print_clock(print_time) class MCU_digital_out: def __init__(self, mcu, pin, max_duration): @@ -141,7 +149,9 @@ class MCU_digital_out: " max_duration=%d" % (self._oid, pin, self._invert, max_duration)) self._set_cmd = mcu.lookup_command( "schedule_digital_out oid=%c clock=%u value=%c") - def set_digital(self, clock, value): + self.print_to_mcu_time = mcu.print_to_mcu_time + def set_digital(self, mcu_time, value): + clock = int(mcu_time * self._mcu_freq) msg = self._set_cmd.encode(self._oid, clock, value ^ self._invert) self._mcu.send(msg, minclock=self._last_clock, reqclock=clock , cq=self._cmd_queue) @@ -150,13 +160,10 @@ class MCU_digital_out: def get_last_setting(self): return self._last_value def set_pwm(self, mcu_time, value): - clock = int(mcu_time * self._mcu_freq) dval = 0 if value > 127: dval = 1 - self.set_digital(clock, dval) - def get_print_clock(self, print_time): - return self._mcu.get_print_clock(print_time) + self.set_digital(mcu_time, dval) class MCU_pwm: def __init__(self, mcu, pin, cycle_ticks, max_duration, hard_pwm=True): @@ -253,7 +260,7 @@ class MCU: self._steppers = [] self._steppersync = None # Print time to clock epoch calculations - self._print_start_clock = 0. + self._print_start_clock = 0 self._clock_freq = 0. # Stats self._mcu_tick_avg = 0. @@ -420,8 +427,6 @@ class MCU: return last_move_end - (float(clock_diff) / self._clock_freq) def print_to_mcu_time(self, print_time): return print_time + self._print_start_clock / self._clock_freq - def get_print_clock(self, print_time): - return print_time * self._clock_freq + self._print_start_clock def get_mcu_freq(self): return self._clock_freq def get_last_clock(self): @@ -430,8 +435,8 @@ class MCU: def send(self, cmd, minclock=0, reqclock=0, cq=None): self.serial.send(cmd, minclock, reqclock, cq=cq) def flush_moves(self, print_time): - move_clock = int(self.get_print_clock(print_time)) - self.ffi_lib.steppersync_flush(self._steppersync, move_clock) + clock = int(print_time * self._clock_freq) + self._print_start_clock + self.ffi_lib.steppersync_flush(self._steppersync, clock) ###################################################################### @@ -456,10 +461,10 @@ class Dummy_MCU_stepper: self._stepid, dirstr, countstr, addstr, interval)) def set_next_step_dir(self, dir): self._sdir = dir - def reset_step_clock(self, clock): + def _reset_step_clock(self, clock): self._mcu.outfile.write("G6S%dT%d\n" % (self._stepid, clock)) - def get_print_clock(self, print_time): - return self._mcu.get_print_clock(print_time) + def print_to_mcu_time(self, print_time): + return self._mcu.print_to_mcu_time(print_time) class Dummy_MCU_obj: def __init__(self, mcu): @@ -480,14 +485,12 @@ class Dummy_MCU_obj: pass def print_to_mcu_time(self, print_time): return self._mcu.print_to_mcu_time(print_time) - def get_print_clock(self, print_time): - return self._mcu.get_print_clock(print_time) class DummyMCU: def __init__(self, outfile): self.outfile = outfile self._stepid = -1 - self._print_start_clock = 0. + self._print_start_clock = 0 self._clock_freq = 16000000. logging.debug('Translated by klippy') def connect(self): @@ -515,8 +518,6 @@ class DummyMCU: return 0.250 def print_to_mcu_time(self, print_time): return print_time + self._print_start_clock / self._clock_freq - def get_print_clock(self, print_time): - return print_time * self._clock_freq + self._print_start_clock def get_mcu_freq(self): return self._clock_freq def flush_moves(self, print_time): diff --git a/klippy/stepper.py b/klippy/stepper.py index c15d194c..084f0131 100644 --- a/klippy/stepper.py +++ b/klippy/stepper.py @@ -42,23 +42,18 @@ class PrinterStepper: self.position_endstop = config.getfloat('position_endstop') self.position_max = config.getfloat('position_max') - self.clock_ticks = None self.need_motor_enable = True def set_max_jerk(self, max_jerk): self.max_jerk = max_jerk def build_config(self): - self.clock_ticks = self.printer.mcu.get_mcu_freq() max_error = self.config.getfloat('max_error', 0.000050) - max_error = int(max_error * self.clock_ticks) - step_pin = self.config.get('step_pin') dir_pin = self.config.get('dir_pin') jc = self.max_jerk / self.max_accel inv_max_step_accel = self.step_dist / self.max_accel - min_stop_interval = int((math.sqrt(3.*inv_max_step_accel + jc**2) - - math.sqrt(inv_max_step_accel + jc**2)) - * self.clock_ticks) - max_error - min_stop_interval = max(0, min_stop_interval) + min_stop_interval = (math.sqrt(3.*inv_max_step_accel + jc**2) + - math.sqrt(inv_max_step_accel + jc**2)) - max_error + min_stop_interval = max(0., min_stop_interval) mcu = self.printer.mcu self.mcu_stepper = mcu.create_stepper( step_pin, dir_pin, min_stop_interval, max_error) @@ -71,19 +66,19 @@ class PrinterStepper: def motor_enable(self, move_time, enable=0): if (self.mcu_enable is not None and self.mcu_enable.get_last_setting() != enable): - mc = int(self.mcu_enable.get_print_clock(move_time)) - self.mcu_enable.set_digital(mc + 1, enable) + mcu_time = self.mcu_enable.print_to_mcu_time(move_time) + self.mcu_enable.set_digital(mcu_time, enable) self.need_motor_enable = True - def prep_move(self, sdir, move_time): - move_clock = self.mcu_stepper.get_print_clock(move_time) - self.mcu_stepper.set_next_step_dir(sdir, int(move_clock)) + def prep_move(self, move_time, sdir): + mcu_time = self.mcu_stepper.print_to_mcu_time(move_time) + self.mcu_stepper.set_next_step_dir(mcu_time, sdir) if self.need_motor_enable: self.motor_enable(move_time, 1) self.need_motor_enable = False - return (move_clock, self.clock_ticks, self.mcu_stepper) - def enable_endstop_checking(self, move_time, hz): - move_clock = int(self.mcu_endstop.get_print_clock(move_time)) - self.mcu_endstop.home(move_clock, int(self.clock_ticks / hz)) + return (mcu_time, self.mcu_stepper) + 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) return self.mcu_endstop def get_homed_position(self): if not self.homing_stepper_phases: