388 lines
11 KiB
Python
Executable File
388 lines
11 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 = 10.0
|
|
|
|
|
|
# unbuffer stdout
|
|
sys.stdout = os.fdopen(sys.stdout.fileno(), 'w')
|
|
|
|
|
|
program_start = time.time()
|
|
|
|
def log(msg):
|
|
delta_t = time.time() - program_start;
|
|
print("%.3f: %s" % (delta_t, msg))
|
|
sys.stdout.flush()
|
|
|
|
|
|
def introspect(h):
|
|
os.system("halcmd show pin halui")
|
|
os.system("halcmd show pin python-ui")
|
|
os.system("halcmd show sig")
|
|
|
|
|
|
def home_joint(name):
|
|
log(" homing %s" % name)
|
|
|
|
h[name + '-home'] = 1
|
|
start = time.time()
|
|
while (h[name + '-homed'] == 0) and ((time.time() - start) < timeout):
|
|
time.sleep(0.1)
|
|
|
|
if h[name + '-homed'] == 0:
|
|
log("failed to home %s in halui" % name)
|
|
introspect(h)
|
|
sys.exit(1)
|
|
|
|
h[name + '-home'] = 0
|
|
|
|
|
|
def select_joint(name, deassert=True):
|
|
log(" selecting %s" % name)
|
|
|
|
h[name + '-select'] = 1
|
|
start = time.time()
|
|
while (h[name + '-selected'] == 0) and ((time.time() - start) < timeout):
|
|
time.sleep(0.1)
|
|
|
|
if h[name + '-selected'] == 0:
|
|
log("failed to select %s in halui" % name)
|
|
introspect(h)
|
|
sys.exit(1)
|
|
|
|
if deassert:
|
|
h[name + '-select'] = 0
|
|
|
|
|
|
def jog_minus(name, target):
|
|
start_position = h[name + '-position']
|
|
log(" jogging %s negative: to %.3f" % (name, target))
|
|
|
|
h['jog-selected-minus'] = 1
|
|
|
|
start = time.time()
|
|
while (h[name + '-position'] > target) and ((time.time() - start) < timeout):
|
|
time.sleep(0.1)
|
|
|
|
if h[name + '-position'] > target:
|
|
log("failed to jog %s to %.3f (timed out at %.3f after %.3f seconds)" % (name, target, h[name + '-position'], timeout))
|
|
introspect(h)
|
|
sys.exit(1)
|
|
|
|
h['jog-selected-minus'] = 0
|
|
|
|
log(" jogged %s negative past target %.3f" % (name, target))
|
|
|
|
return True
|
|
|
|
|
|
def jog_plus(name, target):
|
|
start_position = h[name + '-position']
|
|
log(" jogging %s positive: to %.3f" % (name, target))
|
|
|
|
h['jog-selected-plus'] = 1
|
|
|
|
start = time.time()
|
|
while (h[name + '-position'] < target) and ((time.time() - start) < timeout):
|
|
time.sleep(0.1)
|
|
|
|
if h[name + '-position'] < target:
|
|
log("failed to jog %s to %.3f (timed out at %.3f after %.3f seconds)" % (name, target, h[name + '-position'], timeout))
|
|
introspect(h)
|
|
sys.exit(1)
|
|
|
|
h['jog-selected-plus'] = 0
|
|
|
|
log(" jogged %s positive past target %.3f" % (name, target))
|
|
|
|
return True
|
|
|
|
|
|
def wait_for_pin_value(pin_name, target_value, timeout=2.0):
|
|
start_time = time.time()
|
|
while h[pin_name] != target_value:
|
|
if (time.time() - start_time) > timeout:
|
|
log("Error: pin %s didn't reach target value %s!" % (pin_name, target_value))
|
|
log("pin value is %s at timeout after %.3f seconds" % (h[pin_value], timeout))
|
|
sys.exit(1)
|
|
time.sleep(0.1)
|
|
|
|
|
|
def wait_for_joint_to_stop(joint_number):
|
|
pos_pin = 'joint-%d-position' % joint_number
|
|
vel_pin = 'joint-%d-velocity' % joint_number
|
|
start_time = time.time()
|
|
prev_pos = h[pos_pin]
|
|
while (time.time() - start_time) < timeout:
|
|
time.sleep(0.1)
|
|
new_pos = h[pos_pin]
|
|
if new_pos == prev_pos and h[vel_pin] == 0.0:
|
|
log("joint %d stopped jogging" % joint_number)
|
|
return
|
|
prev_pos = new_pos
|
|
log("Error: joint didn't stop jogging!")
|
|
log("joint %d is at %.3f %.3f seconds after reaching target (prev_pos=%.3f, val=%.3f)" % (joint_number, h[pos_pin], timeout, prev_pos, h[vel_pin]))
|
|
sys.exit(1)
|
|
|
|
|
|
def jog_joint(joint_number, target):
|
|
success = True
|
|
|
|
joint = []
|
|
for j in range(0,3):
|
|
joint.append(h['joint-%d-position' % j])
|
|
|
|
name = 'joint-%d' % joint_number
|
|
|
|
log("jogging %s to %.3f" % (name, target))
|
|
select_joint(name)
|
|
|
|
if h[name + '-position'] > target:
|
|
jog_minus(name, target)
|
|
else:
|
|
jog_plus(name, target)
|
|
|
|
for j in range(0,3):
|
|
pin_name = 'joint-%d-position' % j
|
|
if j == joint_number:
|
|
if joint[j] == h[pin_name]:
|
|
log("joint %d didn't move but should have!" % j)
|
|
success = False
|
|
else:
|
|
if joint[j] != h[pin_name]:
|
|
log("joint %d moved from %.3f to %.3f but should not have!" % (j, joint[j], h[pin_name]))
|
|
success = False
|
|
|
|
wait_for_joint_to_stop(joint_number)
|
|
|
|
if not success:
|
|
sys.exit(1)
|
|
|
|
|
|
#
|
|
# set up pins
|
|
# shell out to halcmd to make nets to halui and motion
|
|
#
|
|
|
|
h = hal.component("python-ui")
|
|
|
|
h.newpin("joint-0-home", hal.HAL_BIT, hal.HAL_OUT)
|
|
h.newpin("joint-0-homed", hal.HAL_BIT, hal.HAL_IN)
|
|
h.newpin("joint-0-select", hal.HAL_BIT, hal.HAL_OUT)
|
|
h.newpin("joint-0-selected", hal.HAL_BIT, hal.HAL_IN)
|
|
h.newpin("joint-0-position", hal.HAL_FLOAT, hal.HAL_IN)
|
|
h.newpin("joint-0-velocity", hal.HAL_FLOAT, hal.HAL_IN)
|
|
h.newpin("joint-0-jog-plus", hal.HAL_BIT, hal.HAL_OUT)
|
|
h.newpin("joint-0-jog-minus", hal.HAL_BIT, hal.HAL_OUT)
|
|
|
|
h.newpin("joint-1-home", hal.HAL_BIT, hal.HAL_OUT)
|
|
h.newpin("joint-1-homed", hal.HAL_BIT, hal.HAL_IN)
|
|
h.newpin("joint-1-select", hal.HAL_BIT, hal.HAL_OUT)
|
|
h.newpin("joint-1-selected", hal.HAL_BIT, hal.HAL_IN)
|
|
h.newpin("joint-1-position", hal.HAL_FLOAT, hal.HAL_IN)
|
|
h.newpin("joint-1-velocity", hal.HAL_FLOAT, hal.HAL_IN)
|
|
h.newpin("joint-1-jog-plus", hal.HAL_BIT, hal.HAL_OUT)
|
|
h.newpin("joint-1-jog-minus", hal.HAL_BIT, hal.HAL_OUT)
|
|
|
|
h.newpin("joint-2-home", hal.HAL_BIT, hal.HAL_OUT)
|
|
h.newpin("joint-2-homed", hal.HAL_BIT, hal.HAL_IN)
|
|
h.newpin("joint-2-select", hal.HAL_BIT, hal.HAL_OUT)
|
|
h.newpin("joint-2-selected", hal.HAL_BIT, hal.HAL_IN)
|
|
h.newpin("joint-2-position", hal.HAL_FLOAT, hal.HAL_IN)
|
|
h.newpin("joint-2-velocity", hal.HAL_FLOAT, hal.HAL_IN)
|
|
h.newpin("joint-2-jog-plus", hal.HAL_BIT, hal.HAL_OUT)
|
|
h.newpin("joint-2-jog-minus", hal.HAL_BIT, hal.HAL_OUT)
|
|
|
|
h.newpin("jog-selected-minus", hal.HAL_BIT, hal.HAL_OUT)
|
|
h.newpin("jog-selected-plus", hal.HAL_BIT, hal.HAL_OUT)
|
|
|
|
h.newpin("motion-mode-joint", hal.HAL_BIT, hal.HAL_OUT)
|
|
h.newpin("motion-mode-is-joint", hal.HAL_BIT, hal.HAL_IN)
|
|
|
|
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.wait_complete()
|
|
|
|
# Select joints 1 and 2, but do not de-assert the .select pins.
|
|
select_joint('joint-1', deassert=False)
|
|
select_joint('joint-2', deassert=False)
|
|
|
|
# Home all the joints. This should work fine.
|
|
home_joint('joint-0')
|
|
home_joint('joint-1')
|
|
home_joint('joint-2')
|
|
|
|
# Deassert the .select pins for joints 1 and 2.
|
|
h['joint-1-select'] = 0
|
|
h['joint-2-select'] = 0
|
|
|
|
# The machine is homed and all the .joint.N.select pins are deasserted.
|
|
c.mode(linuxcnc.MODE_MANUAL)
|
|
c.wait_complete()
|
|
|
|
h['motion-mode-joint'] = 1
|
|
wait_for_pin_value('motion-mode-is-joint', 1, 2.0)
|
|
h['motion-mode-joint'] = 0
|
|
|
|
|
|
#
|
|
# First some simple single-axis jog & stop. Test each axis jogging in each
|
|
# direction, one at a time.
|
|
#
|
|
# These jog_joint() functions will exit with a return value of 1 if
|
|
# something goes wrong, signalling test failure.
|
|
#
|
|
|
|
jog_joint(0, -0.5)
|
|
jog_joint(0, 0.0)
|
|
|
|
jog_joint(1, -0.5)
|
|
jog_joint(1, 0.0)
|
|
|
|
jog_joint(2, -0.5)
|
|
jog_joint(2, 0.0)
|
|
|
|
|
|
#
|
|
# Next try selecting a joint and jogging it with the "jog selected" pins,
|
|
# then changing which joint is selected. The expected behavior when
|
|
# changing which joint is selected is that the old joint stops jogging and
|
|
# the new one starts jogging.
|
|
#
|
|
# We do this while jogging yet a third joint using its private, per-joint
|
|
# jog pins, to verify that it is unaffected by all the drama.
|
|
#
|
|
|
|
name0 = 'joint-0'
|
|
name1 = 'joint-1'
|
|
name2 = 'joint-2'
|
|
|
|
start_position0 = h[name0 + '-position']
|
|
start_position1 = h[name1 + '-position']
|
|
start_position2 = h[name2 + '-position']
|
|
|
|
print("%s starting at %.3f" % (name0, start_position0))
|
|
print("%s starting at %.3f" % (name1, start_position1))
|
|
print("%s starting at %.3f" % (name2, start_position2))
|
|
|
|
|
|
#
|
|
# start joint0 jogging in the positive direction
|
|
#
|
|
|
|
print("jogging %s positive using the private per-joint jog pins" % name0)
|
|
h[name0 + '-jog-plus'] = 1
|
|
|
|
# wait for this joint to come up to speed
|
|
start = time.time()
|
|
while (h[name0 + '-velocity'] <= 0) and ((time.time() - start) < timeout):
|
|
time.sleep(0.1)
|
|
if h[name0 + '-velocity'] <= 0:
|
|
print("%s did not start jogging" % name0)
|
|
sys.exit(1)
|
|
|
|
|
|
#
|
|
# start the selected joint1 jogging in the negative direction
|
|
#
|
|
|
|
print("jogging selected joint (%s) negative" % (name1))
|
|
select_joint(name1)
|
|
h['jog-selected-minus'] = 1
|
|
|
|
# wait for this joint to start moving
|
|
start = time.time()
|
|
while (h[name1 + '-velocity'] >= 0) and ((time.time() - start) < timeout):
|
|
time.sleep(0.1)
|
|
if h[name1 + '-velocity'] >= 0:
|
|
print("%s did not start jogging" % name1)
|
|
sys.exit(1)
|
|
|
|
if h[name0 + '-velocity'] <= 0:
|
|
print("%s stopped jogging" % name0)
|
|
sys.exit(1)
|
|
|
|
if h[name1 + '-position'] >= start_position1:
|
|
print("%s was selected but did not jog negative (start=%.3f, end=%.3f)" % (name1, start_position1, h[name1 + '-position']))
|
|
sys.exit(1)
|
|
|
|
if h[name2 + '-position'] != start_position2:
|
|
print("%s was not selected but moved (start=%.3f, end=%.3f)" % (name2, start_position2, h[name2 + '-position']))
|
|
sys.exit(1)
|
|
|
|
print("%s was selected and jogged, %s was not selected and stayed still" % (name1, name2))
|
|
|
|
|
|
start_position1 = h[name1 + '-position']
|
|
start_position2 = h[name2 + '-position']
|
|
|
|
start_velocity1 = h[name1 + '-velocity']
|
|
start_velocity2 = h[name2 + '-velocity']
|
|
|
|
print("selecting %s" % name2)
|
|
select_joint(name2)
|
|
|
|
wait_for_joint_to_stop(1)
|
|
|
|
if h[name0 + '-velocity'] <= 0:
|
|
print("%s stopped jogging" % name0)
|
|
sys.exit(1)
|
|
|
|
if h[name1 + '-velocity'] != 0:
|
|
print("%s was deselected but did not stop (start_vel=%.3f, end_vel=%.3f)" % (name1, start_velocity1, h[name1 + '-velocity']))
|
|
sys.exit(1)
|
|
|
|
if h[name2 + '-velocity'] >= 0:
|
|
print("%s was selected but did not move (start_vel=%.3f, end_vel=%.3f)" % (name2, start_velocity2, h[name2 + '-velocity']))
|
|
sys.exit(1)
|
|
|
|
print("%s was deselected and stopped, %s was selected and jogged" % (name1, name2))
|
|
|
|
|
|
start_velocity1 = h[name1 + '-velocity']
|
|
start_velocity2 = h[name2 + '-velocity']
|
|
|
|
print("stopping jog")
|
|
h['jog-selected-minus'] = 0
|
|
|
|
wait_for_joint_to_stop(2)
|
|
|
|
if h[name0 + '-velocity'] <= 0:
|
|
print("%s stopped jogging" % name0)
|
|
sys.exit(1)
|
|
|
|
if h[name1 + '-velocity'] != 0:
|
|
print("%s started moving again (start_vel=%.3f, end_vel=%.3f)" % (name1, start_velocity1, h[name1 + '-velocity']))
|
|
sys.exit(1)
|
|
|
|
if h[name2 + '-velocity'] != 0:
|
|
print("%s did not stop (start_vel=%.3f, end_vel=%.3f)" % (name2, start_velocity2, h[name2 + '-velocity']))
|
|
sys.exit(1)
|
|
|
|
print("%s stopped" % name2)
|
|
|
|
|
|
sys.exit(0)
|
|
|