314 lines
8.2 KiB
Python
Executable File
314 lines
8.2 KiB
Python
Executable File
#!/usr/bin/env python3
|
|
|
|
import linuxcnc
|
|
import hal
|
|
|
|
import time
|
|
import sys
|
|
import os
|
|
|
|
|
|
# this is how long we wait for linuxcnc to do our bidding
|
|
timeout = 1.0
|
|
|
|
|
|
def introspect():
|
|
os.system("halcmd show pin python-ui")
|
|
|
|
|
|
def wait_for_pin_value(pin_name, value, timeout=1):
|
|
print("waiting for %s to go to %f (timeout=%f)" % (pin_name, value, timeout))
|
|
|
|
start = time.time()
|
|
while (h[pin_name] != value) and ((time.time() - start) < timeout):
|
|
time.sleep(0.1)
|
|
|
|
if h[pin_name] != value:
|
|
print("timeout! pin %s is %f, didn't get to %f" % (pin_name, h[pin_name], value))
|
|
introspect()
|
|
sys.exit(1)
|
|
|
|
print("pin %s went to %f!" % (pin_name, value))
|
|
|
|
|
|
def verify_pin_value(pin_name, value):
|
|
if (h[pin_name] != value):
|
|
print("pin %s is %f, not %f" % (pin_name, h[pin_name], value))
|
|
sys.exit(1);
|
|
|
|
print("pin %s is %f" % (pin_name, value))
|
|
|
|
|
|
def get_interp_param(param_number):
|
|
c.mdi("(debug, #%d)" % param_number)
|
|
c.wait_complete()
|
|
|
|
# wait up to 2 seconds for a reply
|
|
start = time.time()
|
|
while (time.time() - start) < 2:
|
|
error = e.poll()
|
|
if error == None:
|
|
time.sleep(0.010)
|
|
continue
|
|
|
|
kind, text = error
|
|
if kind == linuxcnc.OPERATOR_DISPLAY:
|
|
return float(text)
|
|
|
|
print(text)
|
|
|
|
print("error getting parameter %d" % param_number)
|
|
return None
|
|
|
|
|
|
def verify_interp_param(param_number, expected_value):
|
|
param_value = get_interp_param(param_number)
|
|
print("interp param #%d = %f (expecting %f)" % (param_number, param_value, expected_value))
|
|
if param_value != expected_value:
|
|
print("ERROR: interp param #%d = %f, expected %f" % (param_number, param_value, expected_value))
|
|
sys.exit(1)
|
|
|
|
|
|
def verify_stable_pin_values(pins, duration=1):
|
|
start = time.time()
|
|
while (time.time() - start) < duration:
|
|
for pin_name in list(pins.keys()):
|
|
val = h[pin_name]
|
|
if val != pins[pin_name]:
|
|
print("ERROR: pin %s = %f (expected %f)" % (pin_name, val, pins[pin_name]))
|
|
sys.exit(1)
|
|
time.sleep(0.010)
|
|
|
|
|
|
def verify_tool_number(tool_number):
|
|
verify_interp_param(5400, tool_number) # 5400 == tool in spindle
|
|
verify_pin_value('tool-number', tool_number) # pin from iocontrol
|
|
|
|
# verify stat buffer
|
|
s.poll()
|
|
if s.tool_in_spindle != tool_number:
|
|
print("ERROR: stat buffer .tool_in_spindle is %f, should be %f" % (s.tool_in_spindle, tool_number))
|
|
sys.exit(1)
|
|
print("stat buffer .tool_in_spindle is %f" % s.tool_in_spindle)
|
|
|
|
|
|
def do_tool_change_handshake(tool_number, pocket_number):
|
|
# prepare for tool change
|
|
wait_for_pin_value('tool-prepare', 1)
|
|
verify_pin_value('tool-prep-number', tool_number)
|
|
verify_pin_value('tool-prep-pocket', pocket_number)
|
|
|
|
h['tool-prepared'] = 1
|
|
wait_for_pin_value('tool-prepare', 0)
|
|
h['tool-prepared'] = 0
|
|
|
|
time.sleep(0.1)
|
|
s.poll()
|
|
print("tool prepare done, s.pocket_prepped = ", s.pocket_prepped)
|
|
if s.pocket_prepped != pocket_number:
|
|
print("ERROR: wrong pocket prepped in stat buffer (got %d, expected %d)" % (s.pocket_prepped, pocket_number))
|
|
sys.exit(1)
|
|
|
|
# change tool
|
|
wait_for_pin_value('tool-change', 1)
|
|
h['tool-changed'] = 1
|
|
wait_for_pin_value('tool-change', 0)
|
|
h['tool-changed'] = 0
|
|
|
|
|
|
|
|
|
|
#
|
|
# set up pins
|
|
# shell out to halcmd to net our pins to where they need to go
|
|
#
|
|
|
|
h = hal.component("python-ui")
|
|
|
|
h.newpin("tool-number", hal.HAL_S32, hal.HAL_IN)
|
|
h.newpin("tool-prep-number", hal.HAL_S32, hal.HAL_IN)
|
|
h.newpin("tool-prep-pocket", hal.HAL_S32, hal.HAL_IN)
|
|
h.newpin("tool-from-pocket", hal.HAL_S32, hal.HAL_IN)
|
|
|
|
h.newpin("tool-prepare", hal.HAL_BIT, hal.HAL_IN)
|
|
h.newpin("tool-prepared", hal.HAL_BIT, hal.HAL_OUT)
|
|
|
|
h.newpin("tool-change", hal.HAL_BIT, hal.HAL_IN)
|
|
h.newpin("tool-changed", hal.HAL_BIT, hal.HAL_OUT)
|
|
|
|
h.ready() # mark the component as 'ready'
|
|
|
|
os.system("halcmd source ./postgui.hal")
|
|
|
|
|
|
#
|
|
# connect to LinuxCNC
|
|
#
|
|
|
|
c = linuxcnc.command()
|
|
s = linuxcnc.stat()
|
|
e = linuxcnc.error_channel()
|
|
|
|
c.state(linuxcnc.STATE_ESTOP_RESET)
|
|
c.state(linuxcnc.STATE_ON)
|
|
c.mode(linuxcnc.MODE_MDI)
|
|
c.wait_complete()
|
|
|
|
|
|
# at startup, we should have the special tool 0 in the spindle, meaning
|
|
# "no tool" or "unknown tool"
|
|
|
|
verify_tool_number(0)
|
|
|
|
|
|
#
|
|
# test m6 to get a baseline
|
|
#
|
|
|
|
print("*** starting 'T1 M6' tool change")
|
|
|
|
c.mdi('t1 m6')
|
|
c.wait_complete()
|
|
|
|
do_tool_change_handshake(tool_number=1, pocket_number=1)
|
|
|
|
print("*** tool change complete")
|
|
verify_tool_number(1)
|
|
|
|
verify_interp_param(5401, 0) # tlo x
|
|
verify_interp_param(5402, 0) # tlo y
|
|
verify_interp_param(5403, 1) # tlo z
|
|
verify_interp_param(5404, 0) # tlo a
|
|
verify_interp_param(5405, 0) # tlo b
|
|
verify_interp_param(5406, 0) # tlo c
|
|
verify_interp_param(5407, 0) # tlo u
|
|
verify_interp_param(5408, 0) # tlo v
|
|
verify_interp_param(5409, 0) # tlo w
|
|
verify_interp_param(5410, 0.125) # tool diameter
|
|
verify_interp_param(5411, 0) # front angle
|
|
verify_interp_param(5412, 0) # back angle
|
|
verify_interp_param(5413, 0) # tool orientation
|
|
|
|
verify_interp_param(5420, 0) # current x
|
|
verify_interp_param(5421, 0) # current y
|
|
verify_interp_param(5422, 0) # current z
|
|
verify_interp_param(5423, 0) # current a
|
|
verify_interp_param(5424, 0) # current b
|
|
verify_interp_param(5425, 0) # current c
|
|
verify_interp_param(5426, 0) # current u
|
|
verify_interp_param(5427, 0) # current v
|
|
verify_interp_param(5428, 0) # current w
|
|
|
|
c.mdi('g43')
|
|
c.wait_complete()
|
|
|
|
verify_interp_param(5420, 0) # current x
|
|
verify_interp_param(5421, 0) # current y
|
|
verify_interp_param(5422, -1) # current z
|
|
verify_interp_param(5423, 0) # current a
|
|
verify_interp_param(5424, 0) # current b
|
|
verify_interp_param(5425, 0) # current c
|
|
verify_interp_param(5426, 0) # current u
|
|
verify_interp_param(5427, 0) # current v
|
|
verify_interp_param(5428, 0) # current w
|
|
|
|
introspect()
|
|
|
|
|
|
|
|
|
|
#
|
|
# now finally test m61
|
|
#
|
|
|
|
print("*** starting 'M61 Q10' tool change")
|
|
|
|
c.mdi('m61 q10')
|
|
c.wait_complete()
|
|
|
|
verify_stable_pin_values(
|
|
{
|
|
'tool-change': 0,
|
|
'tool-prep-number': 0,
|
|
'tool-prep-pocket': 0,
|
|
'tool-prepare': 0,
|
|
'tool-from-pocket': 1
|
|
},
|
|
duration=1
|
|
)
|
|
|
|
verify_tool_number(10)
|
|
|
|
verify_interp_param(5401, 0) # tlo x
|
|
verify_interp_param(5402, 0) # tlo y
|
|
verify_interp_param(5403, 3) # tlo z
|
|
verify_interp_param(5404, 0) # tlo a
|
|
verify_interp_param(5405, 0) # tlo b
|
|
verify_interp_param(5406, 0) # tlo c
|
|
verify_interp_param(5407, 0) # tlo u
|
|
verify_interp_param(5408, 0) # tlo v
|
|
verify_interp_param(5409, 0) # tlo w
|
|
verify_interp_param(5410, 0.500) # tool diameter
|
|
verify_interp_param(5411, 0) # front angle
|
|
verify_interp_param(5412, 0) # back angle
|
|
verify_interp_param(5413, 0) # tool orientation
|
|
|
|
verify_interp_param(5420, 0) # current x
|
|
verify_interp_param(5421, 0) # current y
|
|
verify_interp_param(5422, -1) # current z
|
|
verify_interp_param(5423, 0) # current a
|
|
verify_interp_param(5424, 0) # current b
|
|
verify_interp_param(5425, 0) # current c
|
|
verify_interp_param(5426, 0) # current u
|
|
verify_interp_param(5427, 0) # current v
|
|
verify_interp_param(5428, 0) # current w
|
|
|
|
c.mdi('g43')
|
|
c.wait_complete()
|
|
|
|
verify_interp_param(5420, 0) # current x
|
|
verify_interp_param(5421, 0) # current y
|
|
verify_interp_param(5422, -3) # current z
|
|
verify_interp_param(5423, 0) # current a
|
|
verify_interp_param(5424, 0) # current b
|
|
verify_interp_param(5425, 0) # current c
|
|
verify_interp_param(5426, 0) # current u
|
|
verify_interp_param(5427, 0) # current v
|
|
verify_interp_param(5428, 0) # current w
|
|
|
|
|
|
#
|
|
# use M6 to unload the spindle (T0)
|
|
#
|
|
|
|
print("*** using 'T0 M6' to unload the spindle")
|
|
c.mdi("t0 m6")
|
|
c.wait_complete()
|
|
do_tool_change_handshake(tool_number=0, pocket_number=0)
|
|
verify_tool_number(0)
|
|
|
|
|
|
#
|
|
# use M61 to load T1 in the spindle again
|
|
#
|
|
|
|
print("*** using 'M61 Q1' to load a tool again")
|
|
c.mdi("m61 q1")
|
|
c.wait_complete()
|
|
verify_tool_number(1)
|
|
|
|
|
|
#
|
|
# use M61 to unload the spindle (T0)
|
|
#
|
|
|
|
print("*** using 'M61 Q0' to unload the spindle again")
|
|
c.mdi("m61 q0")
|
|
c.wait_complete()
|
|
verify_tool_number(0)
|
|
|
|
|
|
# if we get here it all worked!
|
|
sys.exit(0)
|
|
|