From b53da365a1d9a3cb2cf28acd9f09f43fa15a81ca Mon Sep 17 00:00:00 2001
From: Kevin O'Connor <kevin@koconnor.net>
Date: Fri, 30 Sep 2016 16:00:32 -0400
Subject: [PATCH] cartesian: Enforce endstop min and max boundaries

Verify that each move command is within range of the configured
minimum and maximum for each axis.

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
---
 config/example.cfg  |  4 ++--
 klippy/cartesian.py | 17 +++++++++++++++++
 klippy/gcode.py     |  6 +++++-
 klippy/homing.py    |  8 ++++++++
 4 files changed, 32 insertions(+), 3 deletions(-)

diff --git a/config/example.cfg b/config/example.cfg
index 090f3653..860cbb62 100644
--- a/config/example.cfg
+++ b/config/example.cfg
@@ -59,12 +59,12 @@ homing_endstop_accuracy: 0.200
 #   phase will be used on all subsequent homes.
 position_min: -0.25
 #   Minimum valid distance (in mm) the user may command the stepper to
-#   move to (not currently enforced)
+#   move to
 position_endstop: 0
 #   Location of the endstop (in mm)
 position_max: 200
 #   Maximum valid distance (in mm) the user may command the stepper to
-#   move to (not currently enforced)
+#   move to
 
 # The stepper_y section is used to describe the stepper controlling
 # the Y axis in a cartesian robot. It has the same settings as the
diff --git a/klippy/cartesian.py b/klippy/cartesian.py
index 18a22382..de9d4fa4 100644
--- a/klippy/cartesian.py
+++ b/klippy/cartesian.py
@@ -13,6 +13,7 @@ class CartKinematics:
         steppers = ['stepper_x', 'stepper_y', 'stepper_z']
         self.steppers = [stepper.PrinterStepper(printer, config.getsection(n))
                          for n in steppers]
+        self.limits = [(1.0, -1.0)] * 3
         self.stepper_pos = [0, 0, 0]
     def build_config(self):
         for stepper in self.steppers[:2]:
@@ -33,6 +34,7 @@ class CartKinematics:
         homing_state = homing.Homing(toolhead, axes)
         for axis in axes:
             s = self.steppers[axis]
+            self.limits[axis] = (s.position_min, s.position_max)
             # Determine moves
             if s.homing_positive_dir:
                 pos = s.position_endstop - 1.5*(
@@ -58,15 +60,30 @@ class CartKinematics:
             homing_state.plan_home(list(coord), homepos, [s], s.homing_speed/2.0)
         return homing_state
     def motor_off(self, move_time):
+        self.limits = [(1.0, -1.0)] * 3
         for stepper in self.steppers:
             stepper.motor_enable(move_time, 0)
     def query_endstops(self, move_time):
         return homing.QueryEndstops(["x", "y", "z"], self.steppers)
+    def check_endstops(self, move):
+        for i in StepList:
+            if (move.axes_d[i]
+                and (move.pos[i] < self.limits[i][0]
+                     or move.pos[i] > self.limits[i][1])):
+                if self.limits[i][0] > self.limits[i][1]:
+                    raise homing.EndstopError(move.pos, "Must home axis first")
+                raise homing.EndstopError(move.pos)
     def check_move(self, move):
+        limits = self.limits
+        xpos, ypos = move.pos[:2]
+        if (xpos < limits[0][0] or xpos > limits[0][1]
+            or ypos < limits[1][0] or ypos > limits[1][1]):
+            self.check_endstops(move)
         if not move.axes_d[2]:
             # Normal XY move - use defaults
             return
         # Move with Z - update velocity and accel for slower Z axis
+        self.check_endstops(move)
         axes_d = move.axes_d
         move_d = move.move_d
         velocity_factor = min([self.steppers[i].max_velocity / abs(axes_d[i])
diff --git a/klippy/gcode.py b/klippy/gcode.py
index 37dd3b23..4e7c9af5 100644
--- a/klippy/gcode.py
+++ b/klippy/gcode.py
@@ -4,6 +4,7 @@
 #
 # This file may be distributed under the terms of the GNU GPLv3 license.
 import os, re, logging, collections
+import homing
 
 # Parse out incoming GCode and find and translate head movements
 class GCodeParser:
@@ -223,7 +224,10 @@ class GCodeParser:
                     self.last_position[p] = v + self.base_position[p]
         if 'F' in params:
             self.speed = float(params['F']) / 60.
-        self.toolhead.move(self.last_position, self.speed, sloppy)
+        try:
+            self.toolhead.move(self.last_position, self.speed, sloppy)
+        except homing.EndstopError, e:
+            self.respond("Error: %s" % (e,))
     def cmd_G4(self, params):
         # Dwell
         if 'S' in params:
diff --git a/klippy/homing.py b/klippy/homing.py
index 1bfe141e..9b405299 100644
--- a/klippy/homing.py
+++ b/klippy/homing.py
@@ -90,3 +90,11 @@ class QueryEndstops:
                 msg = "TRIGGERED"
             msgs.append("%s:%s" % (name, msg))
         return " ".join(msgs)
+
+class EndstopError(Exception):
+    def __init__(self, pos, msg="Move out of range"):
+        self.pos = pos
+        self.msg = msg
+    def __str__(self):
+        return "%s: %.3f %.3f %.3f [%.3f]" % (
+            self.msg, self.pos[0], self.pos[1], self.pos[2], self.pos[3])