mirror of https://github.com/Desuuuu/klipper.git
homing: Support querying the current status of endstops
Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
This commit is contained in:
parent
c8ff439722
commit
0685802cb8
|
@ -67,6 +67,8 @@ class CartKinematics:
|
||||||
def motor_off(self, move_time):
|
def motor_off(self, move_time):
|
||||||
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):
|
||||||
|
return homing.QueryEndstops(["x", "y", "z"], self.steppers)
|
||||||
def move(self, move_time, move):
|
def move(self, move_time, move):
|
||||||
inv_accel = 1. / move.accel
|
inv_accel = 1. / move.accel
|
||||||
inv_cruise_v = 1. / move.cruise_v
|
inv_cruise_v = 1. / move.cruise_v
|
||||||
|
|
|
@ -44,7 +44,7 @@ class GCodeParser:
|
||||||
def build_handlers(self):
|
def build_handlers(self):
|
||||||
shutdown_handlers = ['M105', 'M110', 'M114']
|
shutdown_handlers = ['M105', 'M110', 'M114']
|
||||||
handlers = ['G0', 'G1', 'G4', 'G20', 'G21', 'G28', 'G90', 'G91', 'G92',
|
handlers = ['G0', 'G1', 'G4', 'G20', 'G21', 'G28', 'G90', 'G91', 'G92',
|
||||||
'M18', 'M82', 'M83', 'M84', 'M110', 'M114', 'M206']
|
'M18', 'M82', 'M83', 'M84', 'M110', 'M114', 'M119', 'M206']
|
||||||
if self.heater_nozzle is not None:
|
if self.heater_nozzle is not None:
|
||||||
handlers.extend(['M104', 'M105', 'M109', 'M303'])
|
handlers.extend(['M104', 'M105', 'M109', 'M303'])
|
||||||
if self.heater_bed is not None:
|
if self.heater_bed is not None:
|
||||||
|
@ -294,6 +294,20 @@ class GCodeParser:
|
||||||
self.last_position[0], self.last_position[1],
|
self.last_position[0], self.last_position[1],
|
||||||
self.last_position[2], self.last_position[3],
|
self.last_position[2], self.last_position[3],
|
||||||
kinpos[0], kinpos[1], kinpos[2]))
|
kinpos[0], kinpos[1], kinpos[2]))
|
||||||
|
def cmd_M119(self, params):
|
||||||
|
# Get Endstop Status
|
||||||
|
if self.inputfile:
|
||||||
|
return
|
||||||
|
# Wrapper class for check_busy() that responds with final status
|
||||||
|
class endstop_handler_wrapper:
|
||||||
|
gcode = self
|
||||||
|
state = self.toolhead.query_endstops()
|
||||||
|
def check_busy(self, eventtime):
|
||||||
|
busy = self.state.check_busy(eventtime)
|
||||||
|
if not busy:
|
||||||
|
self.gcode.respond(self.state.get_msg())
|
||||||
|
return busy
|
||||||
|
self.set_busy(endstop_handler_wrapper())
|
||||||
def cmd_M140(self, params):
|
def cmd_M140(self, params):
|
||||||
# Set Bed Temperature
|
# Set Bed Temperature
|
||||||
self.set_temp(self.heater_bed, params)
|
self.set_temp(self.heater_bed, params)
|
||||||
|
|
|
@ -11,6 +11,7 @@ class Homing:
|
||||||
self.toolhead = toolhead
|
self.toolhead = toolhead
|
||||||
self.changed_axes = changed_axes
|
self.changed_axes = changed_axes
|
||||||
|
|
||||||
|
self.eventtime = 0.
|
||||||
self.states = []
|
self.states = []
|
||||||
self.endstops = []
|
self.endstops = []
|
||||||
def plan_home(self, forcepos, movepos, steppers, speed):
|
def plan_home(self, forcepos, movepos, steppers, speed):
|
||||||
|
@ -21,6 +22,7 @@ class Homing:
|
||||||
def plan_axes_update(self, callback):
|
def plan_axes_update(self, callback):
|
||||||
self.states.append((callback, (self.changed_axes,)))
|
self.states.append((callback, (self.changed_axes,)))
|
||||||
def check_busy(self, eventtime):
|
def check_busy(self, eventtime):
|
||||||
|
self.eventtime = eventtime
|
||||||
while self.states:
|
while self.states:
|
||||||
first = self.states[0]
|
first = self.states[0]
|
||||||
ret = first[0](*first[1])
|
ret = first[0](*first[1])
|
||||||
|
@ -57,8 +59,34 @@ class Homing:
|
||||||
def do_wait_endstop(self):
|
def do_wait_endstop(self):
|
||||||
# Check if endstops have triggered
|
# Check if endstops have triggered
|
||||||
for es in self.endstops:
|
for es in self.endstops:
|
||||||
if es.is_homing():
|
if es.check_busy(self.eventtime):
|
||||||
return True
|
return True
|
||||||
# Finished
|
# Finished
|
||||||
del self.endstops[:]
|
del self.endstops[:]
|
||||||
return False
|
return False
|
||||||
|
|
||||||
|
class QueryEndstops:
|
||||||
|
def __init__(self, names, steppers):
|
||||||
|
self.endstops = []
|
||||||
|
self.busy = []
|
||||||
|
for name, stepper in zip(names, steppers):
|
||||||
|
es = stepper.query_endstop()
|
||||||
|
if es is None:
|
||||||
|
continue
|
||||||
|
self.endstops.append((name, es))
|
||||||
|
self.busy.append(es)
|
||||||
|
def check_busy(self, eventtime):
|
||||||
|
while self.busy:
|
||||||
|
busy = self.busy[0].check_busy(eventtime)
|
||||||
|
if busy:
|
||||||
|
return True
|
||||||
|
self.busy.pop(0)
|
||||||
|
return False
|
||||||
|
def get_msg(self):
|
||||||
|
msgs = []
|
||||||
|
for name, es in self.endstops:
|
||||||
|
msg = "open"
|
||||||
|
if es.get_last_triggered():
|
||||||
|
msg = "TRIGGERED"
|
||||||
|
msgs.append("%s:%s" % (name, msg))
|
||||||
|
return " ".join(msgs)
|
||||||
|
|
|
@ -97,16 +97,15 @@ class MCU_endstop:
|
||||||
, self._oid)
|
, self._oid)
|
||||||
self._query_cmd = mcu.lookup_command("end_stop_query oid=%c")
|
self._query_cmd = mcu.lookup_command("end_stop_query oid=%c")
|
||||||
self._homing = False
|
self._homing = False
|
||||||
self._last_position = 0
|
self._min_query_time = self._last_query_time = 0.
|
||||||
self._next_query_clock = 0
|
self._last_state = {}
|
||||||
self._mcu_freq = mcu.get_mcu_freq()
|
self._mcu_freq = mcu.get_mcu_freq()
|
||||||
self._retry_query_ticks = int(self._mcu_freq * self.RETRY_QUERY)
|
|
||||||
self.print_to_mcu_time = mcu.print_to_mcu_time
|
self.print_to_mcu_time = mcu.print_to_mcu_time
|
||||||
def home(self, mcu_time, rest_time):
|
def home(self, mcu_time, rest_time):
|
||||||
clock = int(mcu_time * self._mcu_freq)
|
clock = int(mcu_time * self._mcu_freq)
|
||||||
rest_ticks = int(rest_time * self._mcu_freq)
|
rest_ticks = int(rest_time * self._mcu_freq)
|
||||||
self._homing = True
|
self._homing = True
|
||||||
self._next_query_clock = clock + self._retry_query_ticks
|
self._min_query_time = self._last_query_time = time.time()
|
||||||
msg = self._home_cmd.encode(
|
msg = self._home_cmd.encode(
|
||||||
self._oid, clock, rest_ticks, 1 ^ self._invert)
|
self._oid, clock, rest_ticks, 1 ^ self._invert)
|
||||||
self._mcu.send(msg, reqclock=clock, cq=self._cmd_queue)
|
self._mcu.send(msg, reqclock=clock, cq=self._cmd_queue)
|
||||||
|
@ -117,23 +116,30 @@ class MCU_endstop:
|
||||||
self._stepper.note_stepper_stop()
|
self._stepper.note_stepper_stop()
|
||||||
def _handle_end_stop_state(self, params):
|
def _handle_end_stop_state(self, params):
|
||||||
logging.debug("end_stop_state %s" % (params,))
|
logging.debug("end_stop_state %s" % (params,))
|
||||||
self._last_position = params['pos']
|
self._last_state = params
|
||||||
self._homing = params['homing'] != 0
|
def check_busy(self, eventtime):
|
||||||
def is_homing(self):
|
# Check if need to send an end_stop_query command
|
||||||
if not self._homing:
|
|
||||||
return self._homing
|
|
||||||
if self._mcu.output_file_mode:
|
if self._mcu.output_file_mode:
|
||||||
return False
|
return False
|
||||||
last_clock = self._mcu.get_last_clock()
|
if self._last_state.get('#sent_time', -1.) >= self._min_query_time:
|
||||||
if last_clock >= self._next_query_clock:
|
if not self._homing or not self._last_state.get('homing', 0):
|
||||||
self._next_query_clock = last_clock + self._retry_query_ticks
|
return False
|
||||||
|
if self._last_query_time + self.RETRY_QUERY <= eventtime:
|
||||||
|
self._last_query_time = eventtime
|
||||||
msg = self._query_cmd.encode(self._oid)
|
msg = self._query_cmd.encode(self._oid)
|
||||||
self._mcu.send(msg, cq=self._cmd_queue)
|
self._mcu.send(msg, cq=self._cmd_queue)
|
||||||
return self._homing
|
return True
|
||||||
def get_last_position(self):
|
def get_last_position(self):
|
||||||
|
pos = self._last_state.get('pos', 0)
|
||||||
if self._stepper.get_invert_dir():
|
if self._stepper.get_invert_dir():
|
||||||
return -self._last_position
|
return -pos
|
||||||
return self._last_position
|
return pos
|
||||||
|
def query_endstop(self):
|
||||||
|
self._homing = False
|
||||||
|
self._min_query_time = time.time()
|
||||||
|
self._last_query_time = 0.
|
||||||
|
def get_last_triggered(self):
|
||||||
|
return self._last_state.get('pin', self._invert) ^ self._invert
|
||||||
|
|
||||||
class MCU_digital_out:
|
class MCU_digital_out:
|
||||||
def __init__(self, mcu, pin, max_duration):
|
def __init__(self, mcu, pin, max_duration):
|
||||||
|
|
|
@ -80,6 +80,11 @@ class PrinterStepper:
|
||||||
mcu_time = self.mcu_endstop.print_to_mcu_time(move_time)
|
mcu_time = self.mcu_endstop.print_to_mcu_time(move_time)
|
||||||
self.mcu_endstop.home(mcu_time, step_time)
|
self.mcu_endstop.home(mcu_time, step_time)
|
||||||
return self.mcu_endstop
|
return self.mcu_endstop
|
||||||
|
def query_endstop(self):
|
||||||
|
if self.mcu_endstop is None:
|
||||||
|
return None
|
||||||
|
self.mcu_endstop.query_endstop()
|
||||||
|
return self.mcu_endstop
|
||||||
def get_homed_position(self):
|
def get_homed_position(self):
|
||||||
if not self.homing_stepper_phases:
|
if not self.homing_stepper_phases:
|
||||||
return self.position_endstop
|
return self.position_endstop
|
||||||
|
|
|
@ -280,6 +280,9 @@ class ToolHead:
|
||||||
self.extruder.motor_off(last_move_time)
|
self.extruder.motor_off(last_move_time)
|
||||||
self.dwell(STALL_TIME)
|
self.dwell(STALL_TIME)
|
||||||
logging.debug('; Max time of %f' % (last_move_time,))
|
logging.debug('; Max time of %f' % (last_move_time,))
|
||||||
|
def query_endstops(self):
|
||||||
|
last_move_time = self.get_last_move_time()
|
||||||
|
return self.kin.query_endstops(last_move_time)
|
||||||
def force_shutdown(self):
|
def force_shutdown(self):
|
||||||
self.printer.mcu.force_shutdown()
|
self.printer.mcu.force_shutdown()
|
||||||
self.move_queue.reset()
|
self.move_queue.reset()
|
||||||
|
|
Loading…
Reference in New Issue