121 lines
3.4 KiB
Python
Executable File
121 lines
3.4 KiB
Python
Executable File
#!/usr/bin/env python3
|
|
|
|
import linuxcnc
|
|
import hal
|
|
import time
|
|
import sys
|
|
import os
|
|
import math
|
|
|
|
|
|
def wait_for_joint_to_stop(joint_number):
|
|
pos_pin = 'joint-%d-position' % joint_number
|
|
start_time = time.time()
|
|
timeout = 2.0
|
|
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:
|
|
return
|
|
prev_pos = new_pos
|
|
print("Error: joint didn't stop jogging!")
|
|
print("joint %d is at %.6f %.6f seconds after reaching target (prev_pos=%.6f)" % (joint_number, h[pos_pin], timeout, prev_pos))
|
|
sys.exit(1)
|
|
|
|
|
|
def close_enough(a, b, epsilon=0.000001):
|
|
return math.fabs(a - b) < epsilon
|
|
|
|
|
|
def jog_joint(joint_number, counts=1, scale=0.001):
|
|
timeout = 5.0
|
|
|
|
start_pos = 3*[0]
|
|
for j in range(0,3):
|
|
start_pos[j] = h['joint-%d-position' % j]
|
|
|
|
target = h['joint-%d-position' % joint_number] + (counts * scale)
|
|
|
|
h['joint-%d-jog-scale' % joint_number] = scale
|
|
h['joint-%d-jog-enable' % joint_number] = 1
|
|
h['joint-%d-jog-counts' % joint_number] += counts
|
|
|
|
start_time = time.time()
|
|
while not close_enough(h['joint-%d-position' % joint_number], target) and (time.time() - start_time < timeout):
|
|
#print "joint %d is at %.9f" % (joint_number, h['joint-%d-position' % joint_number])
|
|
time.sleep(0.010)
|
|
|
|
h['joint-%d-jog-enable' % joint_number] = 0
|
|
|
|
print("joint jogged from %.6f to %.6f (%d counts at scale %.6f)" % (start_pos[joint_number], h['joint-%d-position' % joint_number], counts, scale))
|
|
|
|
success = True
|
|
for j in range(0,3):
|
|
pin_name = 'joint-%d-position' % j
|
|
if j == joint_number:
|
|
if not close_enough(h[pin_name], target):
|
|
print("joint %d didn't get to target (start=%.6f, target=%.6f, got to %.6f)" % (joint_number, start_pos[joint_number], target, h['joint-%d-position' % joint_number]))
|
|
success = False
|
|
else:
|
|
if h[pin_name] != start_pos[j]:
|
|
print("joint %d moved from %.6f to %.6f but should not have!" % (j, start_pos[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("test-ui")
|
|
|
|
h.newpin("joint-0-jog-enable", hal.HAL_BIT, hal.HAL_OUT)
|
|
h.newpin("joint-0-jog-counts", hal.HAL_S32, hal.HAL_OUT)
|
|
h.newpin("joint-0-jog-scale", hal.HAL_FLOAT, hal.HAL_OUT)
|
|
h.newpin("joint-0-position", hal.HAL_FLOAT, hal.HAL_IN)
|
|
|
|
h.newpin("joint-1-jog-enable", hal.HAL_BIT, hal.HAL_OUT)
|
|
h.newpin("joint-1-jog-counts", hal.HAL_S32, hal.HAL_OUT)
|
|
h.newpin("joint-1-jog-scale", hal.HAL_FLOAT, hal.HAL_OUT)
|
|
h.newpin("joint-1-position", hal.HAL_FLOAT, hal.HAL_IN)
|
|
|
|
h.newpin("joint-2-jog-enable", hal.HAL_BIT, hal.HAL_OUT)
|
|
h.newpin("joint-2-jog-counts", hal.HAL_S32, hal.HAL_OUT)
|
|
h.newpin("joint-2-jog-scale", hal.HAL_FLOAT, hal.HAL_OUT)
|
|
h.newpin("joint-2-position", hal.HAL_FLOAT, hal.HAL_IN)
|
|
|
|
h.ready() # mark the component as 'ready'
|
|
|
|
os.system("halcmd source ./postgui.hal")
|
|
|
|
|
|
#
|
|
# connect to LinuxCNC
|
|
#
|
|
|
|
c = linuxcnc.command()
|
|
c.state(linuxcnc.STATE_ESTOP_RESET)
|
|
c.state(linuxcnc.STATE_ON)
|
|
c.mode(linuxcnc.MODE_MANUAL)
|
|
|
|
|
|
#
|
|
# run the test
|
|
#
|
|
# These jog_joint() functions will exit with a return value of 1 if
|
|
# something goes wrong.
|
|
#
|
|
|
|
jog_joint(0, counts=1, scale=0.001)
|
|
jog_joint(1, counts=10, scale=-0.025)
|
|
jog_joint(2, counts=-100, scale=0.100)
|
|
|
|
sys.exit(0)
|
|
|