1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
|
#!/usr/bin/env python3
import linuxcnc
import linuxcnc_util
import hal
import time
import sys
import os
#
# connect to LinuxCNC
#
c = linuxcnc.command()
s = linuxcnc.stat()
e = linuxcnc.error_channel()
l = linuxcnc_util.LinuxCNC(command=c, status=s, error=e)
#
# Create and connect test feedback comp
#
h = hal.component("test-ui")
h.newpin("Xpos", hal.HAL_FLOAT, hal.HAL_IN)
h.newpin("Ypos", hal.HAL_FLOAT, hal.HAL_IN)
h.newpin("Zpos", hal.HAL_FLOAT, hal.HAL_IN)
h.newpin("counter", hal.HAL_FLOAT, hal.HAL_IN)
h.ready()
os.system("halcmd source ./linuxcnc-test.hal")
def print_state():
sys.stderr.write(
"line=%d; Xpos=%.2f; Ypos=%.2f; Zpos=%.2f; counter=%d\n" %
(s.current_line, h["Xpos"], h["Ypos"], h["Zpos"], h["counter"]))
#
# Come out of E-stop, turn the machine on, home, and switch to Auto mode.
#
c.state(linuxcnc.STATE_ESTOP_RESET)
c.state(linuxcnc.STATE_ON)
c.home(0)
c.home(1)
c.home(2)
l.wait_for_home([1, 1, 1, 0, 0, 0, 0, 0, 0])
c.mode(linuxcnc.MODE_AUTO)
#
# run the .ngc test file, starting from the special line
#
c.program_open('linuxcnc-test.ngc')
epsilon = 0.000001
def mod_5_is_0(x):
return abs((x+epsilon) % 5) < 2*epsilon
# Take first step
c.auto(linuxcnc.AUTO_STEP)
count = 0
while True:
s.poll()
(x, y) = (h["Xpos"], h["Ypos"])
if mod_5_is_0(x) and mod_5_is_0(y):
# Both axes on goal; make next step and let motion start
sys.stderr.write("Taking step from X%.2f Y%.2f\n" % (x, y))
c.auto(linuxcnc.AUTO_STEP)
time.sleep(0.1)
print_state()
count += 1
if count >= 1000: # Shouldn't happen, but prevent runaways
sys.stderr.write("Finished: Exceeded max cycles\n")
sys.exit(1)
if s.interp_state == linuxcnc.INTERP_IDLE:
sys.stderr.write("Finished: Detected program finish\n")
break
time.sleep(0.1)
if h["counter"] != 25:
sys.stderr.write("End counter incorrect: %d != 25\n" % h["counter"])
sys.exit(1)
sys.exit(0)
|
