88 lines
3.0 KiB
Python
Executable File
88 lines
3.0 KiB
Python
Executable File
#!/usr/bin/env python3
|
|
|
|
import hal
|
|
import linuxcnc
|
|
|
|
h = hal.component("twp-helper-comp")
|
|
|
|
# this pin reflects the machine.analog pin used for the
|
|
# twp-status
|
|
h.newpin("twp-status", hal.HAL_FLOAT, hal.HAL_IN)
|
|
# these pins are created here from 'twp-status''
|
|
h.newpin("twp-is-redefined", hal.HAL_BIT, hal.HAL_OUT)
|
|
h.newpin("twp-is-defined", hal.HAL_BIT, hal.HAL_OUT)
|
|
h.newpin("twp-is-active", hal.HAL_BIT, hal.HAL_OUT)
|
|
|
|
# twp origin vector
|
|
h.newpin("twp-ox-in", hal.HAL_FLOAT, hal.HAL_IN)
|
|
h.newpin("twp-oy-in", hal.HAL_FLOAT, hal.HAL_IN)
|
|
h.newpin("twp-oz-in", hal.HAL_FLOAT, hal.HAL_IN)
|
|
h.newpin("twp-ox", hal.HAL_FLOAT, hal.HAL_OUT)
|
|
h.newpin("twp-oy", hal.HAL_FLOAT, hal.HAL_OUT)
|
|
h.newpin("twp-oz", hal.HAL_FLOAT, hal.HAL_OUT)
|
|
# twp x-orientation vector
|
|
h.newpin("twp-xx-in", hal.HAL_FLOAT, hal.HAL_IN)
|
|
h.newpin("twp-xy-in", hal.HAL_FLOAT, hal.HAL_IN)
|
|
h.newpin("twp-xz-in", hal.HAL_FLOAT, hal.HAL_IN)
|
|
h.newpin("twp-xx", hal.HAL_FLOAT, hal.HAL_OUT)
|
|
h.newpin("twp-xy", hal.HAL_FLOAT, hal.HAL_OUT)
|
|
h.newpin("twp-xz", hal.HAL_FLOAT, hal.HAL_OUT)
|
|
# twp z-orientation vector
|
|
h.newpin("twp-zx-in", hal.HAL_FLOAT, hal.HAL_IN)
|
|
h.newpin("twp-zy-in", hal.HAL_FLOAT, hal.HAL_IN)
|
|
h.newpin("twp-zz-in", hal.HAL_FLOAT, hal.HAL_IN)
|
|
h.newpin("twp-zx", hal.HAL_FLOAT, hal.HAL_OUT)
|
|
h.newpin("twp-zy", hal.HAL_FLOAT, hal.HAL_OUT)
|
|
h.newpin("twp-zz", hal.HAL_FLOAT, hal.HAL_OUT)
|
|
# twp origin vector in machine coordinate system
|
|
h.newpin("twp-ox-world-in", hal.HAL_FLOAT, hal.HAL_IN)
|
|
h.newpin("twp-oy-world-in", hal.HAL_FLOAT, hal.HAL_IN)
|
|
h.newpin("twp-oz-world-in", hal.HAL_FLOAT, hal.HAL_IN)
|
|
h.newpin("twp-ox-world", hal.HAL_FLOAT, hal.HAL_OUT)
|
|
h.newpin("twp-oy-world", hal.HAL_FLOAT, hal.HAL_OUT)
|
|
h.newpin("twp-oz-world", hal.HAL_FLOAT, hal.HAL_OUT)
|
|
|
|
h.ready()
|
|
|
|
# create a connection to the status channel
|
|
s = linuxcnc.stat()
|
|
|
|
try:
|
|
while 1:
|
|
# publish twp-status
|
|
if h['twp-status'] == 1:
|
|
h['twp-is-defined'] = 1
|
|
h['twp-is-active'] = 0
|
|
elif h['twp-status'] == 2:
|
|
h['twp-is-defined'] = 1
|
|
h['twp-is-active'] = 1
|
|
else:
|
|
h['twp-is-defined'] = 0
|
|
h['twp-is-active'] = 0
|
|
|
|
# passthrough the twp arguments
|
|
h['twp-ox'] = h['twp-ox-in']
|
|
h['twp-oy'] = h['twp-oy-in']
|
|
h['twp-oz'] = h['twp-oz-in']
|
|
h['twp-xx'] = h['twp-xx-in']
|
|
h['twp-xy'] = h['twp-xy-in']
|
|
h['twp-xz'] = h['twp-xz-in']
|
|
h['twp-zx'] = h['twp-zx-in']
|
|
h['twp-zy'] = h['twp-zy-in']
|
|
h['twp-zz'] = h['twp-zz-in']
|
|
|
|
# we only want to expose offsets when twp is not defined
|
|
if not h['twp-is-defined']:
|
|
s.poll() # get current values
|
|
g5x_offset = s.g5x_offset
|
|
h['twp-ox-world'] = g5x_offset[0]
|
|
h['twp-oy-world'] = g5x_offset[1]
|
|
h['twp-oz-world'] = g5x_offset[2]
|
|
else : # use the values from the remap
|
|
h['twp-ox-world'] = h['twp-ox-world-in']
|
|
h['twp-oy-world'] = h['twp-oy-world-in']
|
|
h['twp-oz-world'] = h['twp-oz-world-in']
|
|
|
|
except KeyboardInterrupt:
|
|
raise SystemExit
|