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>
This commit is contained in:
Kevin O'Connor 2016-09-30 16:00:32 -04:00
parent 275b386856
commit b53da365a1
4 changed files with 32 additions and 3 deletions

View File

@ -59,12 +59,12 @@ homing_endstop_accuracy: 0.200
# phase will be used on all subsequent homes. # phase will be used on all subsequent homes.
position_min: -0.25 position_min: -0.25
# Minimum valid distance (in mm) the user may command the stepper to # Minimum valid distance (in mm) the user may command the stepper to
# move to (not currently enforced) # move to
position_endstop: 0 position_endstop: 0
# Location of the endstop (in mm) # Location of the endstop (in mm)
position_max: 200 position_max: 200
# Maximum valid distance (in mm) the user may command the stepper to # 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 stepper_y section is used to describe the stepper controlling
# the Y axis in a cartesian robot. It has the same settings as the # the Y axis in a cartesian robot. It has the same settings as the

View File

@ -13,6 +13,7 @@ class CartKinematics:
steppers = ['stepper_x', 'stepper_y', 'stepper_z'] steppers = ['stepper_x', 'stepper_y', 'stepper_z']
self.steppers = [stepper.PrinterStepper(printer, config.getsection(n)) self.steppers = [stepper.PrinterStepper(printer, config.getsection(n))
for n in steppers] for n in steppers]
self.limits = [(1.0, -1.0)] * 3
self.stepper_pos = [0, 0, 0] self.stepper_pos = [0, 0, 0]
def build_config(self): def build_config(self):
for stepper in self.steppers[:2]: for stepper in self.steppers[:2]:
@ -33,6 +34,7 @@ class CartKinematics:
homing_state = homing.Homing(toolhead, axes) homing_state = homing.Homing(toolhead, axes)
for axis in axes: for axis in axes:
s = self.steppers[axis] s = self.steppers[axis]
self.limits[axis] = (s.position_min, s.position_max)
# Determine moves # Determine moves
if s.homing_positive_dir: if s.homing_positive_dir:
pos = s.position_endstop - 1.5*( 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) homing_state.plan_home(list(coord), homepos, [s], s.homing_speed/2.0)
return homing_state return homing_state
def motor_off(self, move_time): def motor_off(self, move_time):
self.limits = [(1.0, -1.0)] * 3
for stepper in self.steppers: for stepper in self.steppers:
stepper.motor_enable(move_time, 0) stepper.motor_enable(move_time, 0)
def query_endstops(self, move_time): def query_endstops(self, move_time):
return homing.QueryEndstops(["x", "y", "z"], self.steppers) 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): 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]: if not move.axes_d[2]:
# Normal XY move - use defaults # Normal XY move - use defaults
return return
# Move with Z - update velocity and accel for slower Z axis # Move with Z - update velocity and accel for slower Z axis
self.check_endstops(move)
axes_d = move.axes_d axes_d = move.axes_d
move_d = move.move_d move_d = move.move_d
velocity_factor = min([self.steppers[i].max_velocity / abs(axes_d[i]) velocity_factor = min([self.steppers[i].max_velocity / abs(axes_d[i])

View File

@ -4,6 +4,7 @@
# #
# This file may be distributed under the terms of the GNU GPLv3 license. # This file may be distributed under the terms of the GNU GPLv3 license.
import os, re, logging, collections import os, re, logging, collections
import homing
# Parse out incoming GCode and find and translate head movements # Parse out incoming GCode and find and translate head movements
class GCodeParser: class GCodeParser:
@ -223,7 +224,10 @@ class GCodeParser:
self.last_position[p] = v + self.base_position[p] self.last_position[p] = v + self.base_position[p]
if 'F' in params: if 'F' in params:
self.speed = float(params['F']) / 60. self.speed = float(params['F']) / 60.
try:
self.toolhead.move(self.last_position, self.speed, sloppy) self.toolhead.move(self.last_position, self.speed, sloppy)
except homing.EndstopError, e:
self.respond("Error: %s" % (e,))
def cmd_G4(self, params): def cmd_G4(self, params):
# Dwell # Dwell
if 'S' in params: if 'S' in params:

View File

@ -90,3 +90,11 @@ class QueryEndstops:
msg = "TRIGGERED" msg = "TRIGGERED"
msgs.append("%s:%s" % (name, msg)) msgs.append("%s:%s" % (name, msg))
return " ".join(msgs) 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])