manual_stepper: Support INSTANTANEOUS_CORNER_VELOCITY on gcode axes

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>

2 files changed, 21 insertions, 12 deletions

diff --git a/docs/G-Codes.md b/docs/G-Codes.md
index 6d69dd9c..48aa0363 100644
--- a/docs/G-Codes.md
+++ b/docs/G-Codes.md
@@ -968,17 +968,20 @@ scheduled to run after the stepper move completes, however if a manual
 stepper move uses SYNC=0 then future G-Code movement commands may run
 in parallel with the stepper movement.
 
-`MANUAL_STEPPER STEPPER=config_name GCODE_AXIS=[A-Z]`: If the
-`GCODE_AXIS` parameter is specified then it configures the stepper
-motor as an extra axis on `G1` move commands.  For example, if one
-were to issue a `MANUAL_STEPPER ... GCODE_AXIS=R` command then one
-could issue commands like `G1 X10 Y20 R30` to move the stepper motor.
-The resulting moves will occur synchronously with the associated
-toolhead xyz movements.  If the motor is associated with a
-`GCODE_AXIS` then one may no longer issue movements using the above
-`MANUAL_STEPPER` command - one may unregister the stepper with a
-`MANUAL_STEPPER ... GCODE_AXIS=` command to resume manual control of
-the motor.
+`MANUAL_STEPPER STEPPER=config_name GCODE_AXIS=[A-Z]
+[INSTANTANEOUS_CORNER_VELOCITY=<velocity>]`: If the `GCODE_AXIS`
+parameter is specified then it configures the stepper motor as an
+extra axis on `G1` move commands.  For example, if one were to issue a
+`MANUAL_STEPPER ... GCODE_AXIS=R` command then one could issue
+commands like `G1 X10 Y20 R30` to move the stepper motor.  The
+resulting moves will occur synchronously with the associated toolhead
+xyz movements.  If the motor is associated with a `GCODE_AXIS` then
+one may no longer issue movements using the above `MANUAL_STEPPER`
+command - one may unregister the stepper with a `MANUAL_STEPPER
+... GCODE_AXIS=` command to resume manual control of the motor. The
+`INSTANTANEOUS_CORNER_VELOCITY` specifies the maximum instantaneous
+velocity change (in mm/s) of the motor during the junction of two
+moves (the default is 1mm/s).
 
 ### [mcp4018]
 
diff --git a/klippy/extras/manual_stepper.py b/klippy/extras/manual_stepper.py
index 4c24ff75..6e02ee86 100644
--- a/klippy/extras/manual_stepper.py
+++ b/klippy/extras/manual_stepper.py
@@ -31,6 +31,7 @@ class ManualStepper:
         self.rail.set_trapq(self.trapq)
         # Registered with toolhead as an axtra axis
         self.axis_gcode_id = None
+        self.instant_corner_v = 0.
         # Register commands
         stepper_name = config.get_name().split()[1]
         gcode = self.printer.lookup_object('gcode')
@@ -119,6 +120,8 @@ class ManualStepper:
         gcode_move = self.printer.lookup_object("gcode_move")
         toolhead = self.printer.lookup_object('toolhead')
         gcode_axis = gcmd.get('GCODE_AXIS').upper()
+        instant_corner_v = gcmd.get_float('INSTANTANEOUS_CORNER_VELOCITY', 1.,
+                                          minval=0.)
         if self.axis_gcode_id is not None:
             if gcode_axis:
                 raise gcmd.error("Must unregister axis first")
@@ -137,6 +140,7 @@ class ManualStepper:
             if ea is not None and ea.get_axis_gcode_id() == gcode_axis:
                 raise gcmd.error("Axis '%s' already registered" % (gcode_axis,))
         self.axis_gcode_id = gcode_axis
+        self.instant_corner_v = instant_corner_v
         toolhead.add_extra_axis(self, self.get_position()[0])
         toolhead.register_step_generator(self.rail.generate_steps)
     def process_move(self, print_time, move, ea_index):
@@ -156,7 +160,9 @@ class ManualStepper:
         # XXX - support non-kinematic max accel/velocity
         pass
     def calc_junction(self, prev_move, move, ea_index):
-        # XXX - support max instantaneous velocity change
+        diff_r = move.axes_r[ea_index] - prev_move.axes_r[ea_index]
+        if diff_r:
+            return (self.instant_corner_v / abs(diff_r))**2
         return move.max_cruise_v2
     def get_axis_gcode_id(self):
         return self.axis_gcode_id