From d62a41b930904f04c282e76de829f2bb21b5cd59 Mon Sep 17 00:00:00 2001 From: Kevin O'Connor Date: Sun, 3 Mar 2019 13:23:45 -0500 Subject: [PATCH] manual_stepper: Add support for moves with acceleration Signed-off-by: Kevin O'Connor --- config/example-extras.cfg | 9 ++++++++ docs/G-Codes.md | 41 ++++++++++++++++++--------------- klippy/extras/force_move.py | 32 +++++++++++++++++-------- klippy/extras/manual_stepper.py | 29 +++++++++++++---------- test/klippy/manual_stepper.cfg | 2 ++ test/klippy/manual_stepper.test | 6 ++--- 6 files changed, 77 insertions(+), 42 deletions(-) diff --git a/config/example-extras.cfg b/config/example-extras.cfg index 134ac66b..ed574b6d 100644 --- a/config/example-extras.cfg +++ b/config/example-extras.cfg @@ -657,6 +657,15 @@ #step_distance: # See the "[stepper_x]" section in example.cfg for a description of # these parameters. +#velocity: +# Set the default velocity (in mm/s) for the stepper. This value +# will be used if a MANUAL_STEPPER command does not specify a SPEED +# parameter. The default is 5mm/s. +#accel: +# Set the default acceleration (in mm/s^2) for the stepper. An +# acceleration of zero will result in no acceleration. This value +# will be used if a MANUAL_STEPPER command does not specify an ACCEL +# parameter. The default is zero. #endstop_pin: # Endstop switch detection pin. If specified, then one may perform # "homing moves" by adding a STOP_ON_ENDSTOP parameter to diff --git a/docs/G-Codes.md b/docs/G-Codes.md index 197ec3e2..6a004cf2 100644 --- a/docs/G-Codes.md +++ b/docs/G-Codes.md @@ -170,15 +170,18 @@ enabled: The following command is available when a "manual_stepper" config section is enabled: - `MANUAL_STEPPER STEPPER=config_name [ENABLE=[0|1]] - [SET_POSITION=] - [MOVE= SPEED= [STOP_ON_ENDSTOP=1]]`: This command will - alter the state of the stepper. Use the ENABLE parameter to - enable/disable the stepper. Use the SET_POSITION parameter to force - the stepper to think it is at the given position. Use the MOVE - parameter to request a movement to the given position at the given - SPEED. If STOP_ON_ENDSTOP is specified then the move will end early - should the endstop report as triggered (use STOP_ON_ENDSTOP=-1 to - stop early should the endstop report not triggered). + [SET_POSITION=] [SPEED=] [ACCEL=] + [MOVE= [STOP_ON_ENDSTOP=1]]`: This command will alter the state + of the stepper. Use the ENABLE parameter to enable/disable the + stepper. Use the SET_POSITION parameter to force the stepper to + think it is at the given position. Use the MOVE parameter to request + a movement to the given position. If SPEED and/or ACCEL is specified + then the given values will be used instead of the defaults specified + in the config file. If an ACCEL of zero is specified then no + acceleration will be preformed. If STOP_ON_ENDSTOP is specified then + the move will end early should the endstop report as triggered (use + STOP_ON_ENDSTOP=-1 to stop early should the endstop report not + triggered). ## Probe @@ -312,16 +315,18 @@ section is enabled: The following commands are available when the "force_move" config section is enabled: -- `FORCE_MOVE STEPPER= DISTANCE= - VELOCITY=`: This command will forcibly move the given stepper +- `FORCE_MOVE STEPPER= DISTANCE= VELOCITY= + [ACCEL=]`: This command will forcibly move the given stepper the given distance (in mm) at the given constant velocity (in - mm/s). No acceleration is performed; no boundary checks are - performed; no kinematic updates are made; other parallel steppers on - an axis will not be moved. Use caution as an incorrect command could - cause damage! Using this command will almost certainly place the - low-level kinematics in an incorrect state; issue a G28 afterwards - to reset the kinematics. This command is intended for low-level - diagnostics and debugging. + mm/s). If ACCEL is specified and is greater than zero, then the + given acceleration (in mm/s^2) will be used; otherwise no + acceleration is performed. No boundary checks are performed; no + kinematic updates are made; other parallel steppers on an axis will + not be moved. Use caution as an incorrect command could cause + damage! Using this command will almost certainly place the low-level + kinematics in an incorrect state; issue a G28 afterwards to reset + the kinematics. This command is intended for low-level diagnostics + and debugging. - `SET_KINEMATIC_POSITION [X=] [Y=] [Z=]`: Force the low-level kinematic code to believe the toolhead is at the given cartesian position. This is a diagnostic and debugging command; use diff --git a/klippy/extras/force_move.py b/klippy/extras/force_move.py index a6d7adc2..0b0ed902 100644 --- a/klippy/extras/force_move.py +++ b/klippy/extras/force_move.py @@ -3,12 +3,25 @@ # Copyright (C) 2018 Kevin O'Connor # # This file may be distributed under the terms of the GNU GPLv3 license. -import logging +import math, logging import chelper BUZZ_VELOCITY = 4. STALL_TIME = 0.100 +# Calculate a move's accel_t, cruise_t, and cruise_v +def calc_move_time(dist, speed, accel): + dist = abs(dist) + if not accel: + return 0., dist / speed, speed + max_cruise_v2 = dist * accel + if max_cruise_v2 < speed**2: + speed = math.sqrt(max_cruise_v2) + accel_t = speed / accel + accel_decel_d = accel_t * speed + cruise_t = (dist - accel_decel_d) / speed + return accel_t, cruise_t, speed + class ForceMove: def __init__(self, config): self.printer = config.get_printer() @@ -49,17 +62,17 @@ class ForceMove: print_time = toolhead.get_last_move_time() stepper.motor_enable(print_time, 0) toolhead.dwell(STALL_TIME) - def manual_move(self, stepper, dist, speed): + def manual_move(self, stepper, dist, speed, accel=0.): toolhead = self.printer.lookup_object('toolhead') print_time = toolhead.get_last_move_time() - move_t = abs(dist / speed) prev_sk = stepper.set_stepper_kinematics(self.stepper_kinematics) stepper.set_position((0., 0., 0.)) - self.move_fill(self.cmove, print_time, 0., move_t, 0., - 0., 0., 0., dist, 0., 0., 0., speed, 0.) + accel_t, cruise_t, cruise_v = calc_move_time(dist, speed, accel) + self.move_fill(self.cmove, print_time, accel_t, cruise_t, accel_t, + 0., 0., 0., dist, 0., 0., 0., cruise_v, accel) stepper.step_itersolve(self.cmove) stepper.set_stepper_kinematics(prev_sk) - toolhead.dwell(move_t) + toolhead.dwell(accel_t + cruise_t + accel_t) def _lookup_stepper(self, params): name = self.gcode.get_str('STEPPER', params) if name not in self.steppers: @@ -82,10 +95,11 @@ class ForceMove: stepper = self._lookup_stepper(params) distance = self.gcode.get_float('DISTANCE', params) speed = self.gcode.get_float('VELOCITY', params, above=0.) - logging.info("FORCE_MOVE %s distance=%.3f velocity=%.3f", - stepper.get_name(), distance, speed) + accel = self.gcode.get_float('ACCEL', params, 0., minval=0.) + logging.info("FORCE_MOVE %s distance=%.3f velocity=%.3f accel=%.3f", + stepper.get_name(), distance, speed, accel) was_enable, was_ignore = self.force_enable(stepper) - self.manual_move(stepper, distance, speed) + self.manual_move(stepper, distance, speed, accel) self.restore_enable(stepper, True, was_ignore) cmd_SET_KINEMATIC_POSITION_help = "Force a low-level kinematic position" def cmd_SET_KINEMATIC_POSITION(self, params): diff --git a/klippy/extras/manual_stepper.py b/klippy/extras/manual_stepper.py index fdb9fc10..01758d99 100644 --- a/klippy/extras/manual_stepper.py +++ b/klippy/extras/manual_stepper.py @@ -3,7 +3,7 @@ # Copyright (C) 2019 Kevin O'Connor # # This file may be distributed under the terms of the GNU GPLv3 license. -import stepper, homing, chelper +import stepper, homing, force_move, chelper ENDSTOP_SAMPLE_TIME = .000015 ENDSTOP_SAMPLE_COUNT = 4 @@ -18,6 +18,8 @@ class ManualStepper: else: self.can_home = False self.stepper = stepper.PrinterStepper(config) + self.velocity = config.getfloat('velocity', 5., above=0.) + self.accel = config.getfloat('accel', 0., minval=0.) self.next_cmd_time = 0. # Setup iterative solver ffi_main, ffi_lib = chelper.get_ffi() @@ -46,17 +48,20 @@ class ManualStepper: self.sync_print_time() def do_set_position(self, setpos): self.stepper.set_position([setpos, 0., 0.]) - def do_move(self, movepos, speed): + def do_move(self, movepos, speed, accel): self.sync_print_time() cp = self.stepper.get_commanded_position() dist = movepos - cp - move_t = abs(dist / speed) - self.move_fill(self.cmove, self.next_cmd_time, 0., move_t, 0., - cp, 0., 0., dist, 0., 0., 0., speed, 0.) + accel_t, cruise_t, cruise_v = force_move.calc_move_time( + dist, speed, accel) + self.move_fill(self.cmove, self.next_cmd_time, + accel_t, cruise_t, accel_t, + cp, 0., 0., dist, 0., 0., + 0., cruise_v, accel) self.stepper.step_itersolve(self.cmove) - self.next_cmd_time += move_t + self.next_cmd_time += accel_t + cruise_t + accel_t self.sync_print_time() - def do_homing_move(self, movepos, speed, triggered): + def do_homing_move(self, movepos, speed, accel, triggered): if not self.can_home: raise self.gcode.error("No endstop for this manual stepper") # Notify endstops of upcoming home @@ -72,7 +77,7 @@ class ManualStepper: self.next_cmd_time, ENDSTOP_SAMPLE_TIME, ENDSTOP_SAMPLE_COUNT, min_step_dist / speed, triggered=triggered) # Issue move - self.do_move(movepos, speed) + self.do_move(movepos, speed, accel) # Wait for endstops to trigger error = None for mcu_endstop, name in endstops: @@ -98,18 +103,18 @@ class ManualStepper: setpos = self.gcode.get_float('SET_POSITION', params) self.do_set_position(setpos) homing_move = self.gcode.get_int('STOP_ON_ENDSTOP', params, 0) + speed = self.gcode.get_float('SPEED', params, self.velocity, above=0.) + accel = self.gcode.get_float('ACCEL', params, self.accel, minval=0.) if homing_move: movepos = self.gcode.get_float('MOVE', params) - speed = self.gcode.get_float('SPEED', params, above=0.) if 'ENABLE' not in params and not self.stepper.is_motor_enabled(): self.do_enable(True) - self.do_homing_move(movepos, speed, homing_move > 0) + self.do_homing_move(movepos, speed, accel, homing_move > 0) elif 'MOVE' in params: movepos = self.gcode.get_float('MOVE', params) - speed = self.gcode.get_float('SPEED', params, above=0.) if 'ENABLE' not in params and not self.stepper.is_motor_enabled(): self.do_enable(True) - self.do_move(movepos, speed) + self.do_move(movepos, speed, accel) def handle_motor_off(self, print_time): self.do_enable(0) diff --git a/test/klippy/manual_stepper.cfg b/test/klippy/manual_stepper.cfg index 4ca734a3..2c65b33d 100644 --- a/test/klippy/manual_stepper.cfg +++ b/test/klippy/manual_stepper.cfg @@ -4,6 +4,8 @@ step_pin: ar54 dir_pin: ar55 enable_pin: !ar38 step_distance: .0125 +velocity: 7 +accel: 500 [manual_stepper homing_stepper] step_pin: ar60 diff --git a/test/klippy/manual_stepper.test b/test/klippy/manual_stepper.test index 4f682331..0d11970b 100644 --- a/test/klippy/manual_stepper.test +++ b/test/klippy/manual_stepper.test @@ -6,14 +6,14 @@ DICTIONARY atmega2560-16mhz.dict MANUAL_STEPPER STEPPER=basic_stepper ENABLE=1 MANUAL_STEPPER STEPPER=basic_stepper SET_POSITION=0 MANUAL_STEPPER STEPPER=basic_stepper MOVE=10 SPEED=10 -MANUAL_STEPPER STEPPER=basic_stepper MOVE=5 SPEED=5 -MANUAL_STEPPER STEPPER=basic_stepper MOVE=12 SPEED=12 +MANUAL_STEPPER STEPPER=basic_stepper MOVE=5 +MANUAL_STEPPER STEPPER=basic_stepper MOVE=12 SPEED=12 ACCEL=9000.2 MANUAL_STEPPER STEPPER=basic_stepper ENABLE=0 # Test homing move MANUAL_STEPPER STEPPER=homing_stepper ENABLE=1 MANUAL_STEPPER STEPPER=homing_stepper SET_POSITION=0 -MANUAL_STEPPER STEPPER=homing_stepper MOVE=10 SPEED=10 +MANUAL_STEPPER STEPPER=homing_stepper MOVE=10 SPEED=100 ACCEL=1 MANUAL_STEPPER STEPPER=homing_stepper ENABLE=0 # Test motor off