force_move: Add support for FORCE_MOVE command

Add initial support for commands that will forcibly move a stepper
(without updating the kinematic classes with the new position).

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

config/example-extras.cfg

@@ -660,6 +660,15 @@
 #   name in the above list.
 
 
+# Support manually moving stepper motors for diagnostic purposes.
+# Note, using this feature may place the printer in an invalid state -
+# see docs/G-Codes.md for important details.
+#[force_move]
+#enable_force_move: False
+#   Set to true to enable FORCE_MOVE and SET_KINEMATIC_POSITION
+#   extended G-Code commands. The default is false.
+
+
 # G-Code macros (one may define any number of sections with a
 # "gcode_macro" prefix).
 #[gcode_macro my_cmd]

docs/G-Codes.md

@@ -180,3 +180,25 @@ The following command is available when the "tmc2130" config section
 is enabled:
 - `DUMP_TMC STEPPER=<name>`: This command will read the TMC2130 driver
   registers and report their values.
+
+## Force movement
+
+The following commands are available when the "force_move" config
+section is enabled:
+- `FORCE_MOVE STEPPER=<config_name> DISTANCE=<value>
+  VELOCITY=<value>`: 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.
+- `SET_KINEMATIC_POSITION X=<value> Y=<value> Z=<value>`: Force the
+  low-level kinematic code to believe the toolhead is at the given
+  position. This is a diagnostic and debugging command; use
+  SET_GCODE_OFFSET and/or G92 for regular axis transformations.
+  Setting an incorrect or invalid position may lead to internal
+  software errors. This command may invalidate future boundary checks;
+  issue a G28 afterwards to reset the kinematics.

klippy/extras/force_move.py

@@ -19,10 +19,16 @@ class ForceMove:
         self.move_fill = ffi_lib.move_fill
         self.stepper_kinematics = ffi_main.gc(
             ffi_lib.cartesian_stepper_alloc('x'), ffi_lib.free)
-        # Register STEPPER_BUZZ command
+        # Register commands
         self.gcode = self.printer.lookup_object('gcode')
         self.gcode.register_command('STEPPER_BUZZ', self.cmd_STEPPER_BUZZ,
                                     desc=self.cmd_STEPPER_BUZZ_help)
+        if config.getboolean("enable_force_move", False):
+            self.gcode.register_command('FORCE_MOVE', self.cmd_FORCE_MOVE,
+                                        desc=self.cmd_FORCE_MOVE_help)
+            self.gcode.register_command(
+                'SET_KINEMATIC_POSITION', self.cmd_SET_KINEMATIC_POSITION,
+                desc=self.cmd_SET_KINEMATIC_POSITION_help)
     def register_stepper(self, stepper):
         name = stepper.get_name()
         self.steppers[name] = stepper
@@ -54,13 +60,15 @@ class ForceMove:
         stepper.step_itersolve(self.cmove)
         stepper.set_stepper_kinematics(prev_sk)
         toolhead.dwell(move_t)
-    cmd_STEPPER_BUZZ_help = "Oscillate a given stepper to help id it"
-    def cmd_STEPPER_BUZZ(self, params):
+    def _lookup_stepper(self, params):
         name = self.gcode.get_str('STEPPER', params)
         if name not in self.steppers:
             raise self.gcode.error("Unknown stepper %s" % (name,))
-        stepper = self.steppers[name]
-        logging.info("Stepper buzz %s", name)
+        return self.steppers[name]
+    cmd_STEPPER_BUZZ_help = "Oscillate a given stepper to help id it"
+    def cmd_STEPPER_BUZZ(self, params):
+        stepper = self._lookup_stepper(params)
+        logging.info("Stepper buzz %s", stepper.get_name())
         was_enable, was_ignore = self.force_enable(stepper)
         toolhead = self.printer.lookup_object('toolhead')
         for i in range(10):
@@ -69,6 +77,27 @@ class ForceMove:
             self.manual_move(stepper, -1., BUZZ_VELOCITY)
             toolhead.dwell(.450)
         self.restore_enable(stepper, was_enable, was_ignore)
+    cmd_FORCE_MOVE_help = "Manually move a stepper; invalidates kinematics"
+    def cmd_FORCE_MOVE(self, params):
+        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)
+        was_enable, was_ignore = self.force_enable(stepper)
+        self.manual_move(stepper, distance, speed)
+        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):
+        x = self.gcode.get_float('X', params)
+        y = self.gcode.get_float('Y', params)
+        z = self.gcode.get_float('Z', params)
+        logging.info("SET_KINEMATIC_POSITION pos=%.3f,%.3f,%.3f", x, y, z)
+        toolhead = self.printer.lookup_object('toolhead')
+        toolhead.get_last_move_time()
+        curpos = toolhead.get_position()
+        toolhead.set_position([x, y, z, curpos[3]], homing_axes=(0, 1, 2))
+        self.gcode.reset_last_position()
 
 def load_config(config):
     return ForceMove(config)