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
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
|
#!/usr/bin/python
import sys
import os
from subprocess import *
#
# read some stuff out of the ini file
#
# servo period in nanoseconds
servo_period_ns = float(Popen(
[
"inivar",
"-var",
"SERVO_PERIOD",
"-sec",
"EMCMOT",
"-ini",
"%s/tests/motion/g0/motion-test.ini" % os.getenv("EMC2_HOME")
],
stdout=PIPE
).communicate()[0])
cycles_per_second = 1000000000.0 / servo_period_ns
# max acceleration in inches per second^2
max_acceleration_ips2 = float(Popen(
[
"inivar",
"-var",
"MAX_ACCELERATION",
"-sec",
"AXIS_0",
"-ini",
"%s/tests/motion/g0/motion-test.ini" % os.getenv("EMC2_HOME")
],
stdout=PIPE
).communicate()[0])
# max acceleration in inches per second per servo-thread cycle
# the factor of half at the end is because emc2 reserves half the acceleration for blending
max_acceleration_ipspc = (max_acceleration_ips2 / cycles_per_second) * 0.5
# max velocity in inches per second
max_velocity_ips = float(Popen(
[
"inivar",
"-var",
"MAX_VELOCITY",
"-sec",
"AXIS_0",
"-ini",
"%s/tests/motion/g0/motion-test.ini" % os.getenv("EMC2_HOME")
],
stdout=PIPE
).communicate()[0])
max_velocity_ipc = max_velocity_ips / cycles_per_second
# read the samples file and turn it into an array, where index i is an
# array of the floats found on line i of the samples file
samples_filename = sys.argv[1] + ".halsamples"
lines = file(samples_filename).readlines()
samples = []
for line in lines:
s = [ float(x) for x in line.split() ]
samples.append(s)
#
# here comes the test proper
#
# verify that X starts at 0.000000
if samples[0][1] != 0.000000:
print "sample 0: X starts at %.6f, not at 0.000000" % samples[0][1]
sys.exit(1)
print "line 0: X starts at 0.000000"
# find where X starts moving (X is the second column, column 1)
i = 0
while samples[i][1] == samples[i+1][1]:
i += 1
print "line %d: accel phase starts" % i
# now i is the sample before it starts moving
# verify accel does not exceed maxaccel
# verify vel does not exceed maxvel
# verify accel phase stops when vel == maxvel
highest_seen_accel_ipc2 = 0
old_accel_ipc2 = 0
old_v_ipc = 0
for i in range(i, len(samples)):
new_v_ipc = samples[i+1][1] - samples[i][1]
accel_ipc2 = new_v_ipc - old_v_ipc
highest_seen_accel_ipc2 = max(accel_ipc2, highest_seen_accel_ipc2)
#print "line %d: X=%.6f, v=%.6f i/c (%.6f i/s), a=%.6f i/c^2 (%.6f i/s^2)" % (i, samples[i][1], new_v_ipc, (new_v_ipc * cycles_per_second), accel_ipc2, (accel_ipc2 * cycles_per_second * cycles_per_second))
# i hate floating point
if ((accel_ipc2 * cycles_per_second) - max_acceleration_ipspc) > 0.0000001:
print "line %d: detected accel constraint violation!" % i
print "detected accel %.6f i/c^2 (%.6f i/s^2)" % (accel_ipc2, (accel_ipc2 * cycles_per_second * cycles_per_second))
print "max accel %.6f i/s^2)" % max_acceleration_ips2
sys.exit(1)
if (new_v_ipc - max_velocity_ipc) > 0.0000001:
print "line %d: detected vel constraint violation!" % i
print "detected vel %.6f i/c (%.6f i/s)" % (new_v_ipc, new_v_ipc * cycles_per_second)
print "max vel %.6f i/c (%.6f i/s)" % (max_velocity_ipc, max_velocity_ips)
sys.exit(1)
if accel_ipc2 == 0:
print "line %d: accel phase over" % i
break
old_v_ipc = new_v_ipc
# verify highest seen accel is very close to max accel
if abs(max_acceleration_ipspc - (highest_seen_accel_ipc2 * cycles_per_second)) > 0.0000001:
print "accel only reached %.6f i/c^2 (%.6f i/s^2)" % (highest_seen_accel_ipc2, (highest_seen_accel_ipc2 * cycles_per_second * cycles_per_second))
print "max accel is %.6f i/s^2" % max_acceleration_ips2
sys.exit(1)
print " accel reached but did not exceed 1/2 of max accel of %.6f i/s^2" % max_acceleration_ips2
print "line %d: entering cruise phase, vel=%.6f i/s, max_accel = %.6f i/s^2" % (i, new_v_ipc * cycles_per_second, highest_seen_accel_ipc2 * cycles_per_second * cycles_per_second)
# now i is the first sample of the cruise phase
# wait for vel to drop again
for i in range(i, len(samples)):
new_v_ipc = samples[i+1][1] - samples[i][1]
if abs(new_v_ipc - old_v_ipc) > 0.0000001:
# decel phase starting
break
old_v_ipc = new_v_ipc
print "line %d: decel phase starting, old_v=%.6f i/s, new_v=%.6f i/s" % (i, old_v_ipc * cycles_per_second, new_v_ipc * cycles_per_second)
# now i is the sample before it starts decel
# verify accel does not exceed maxaccel
# verify vel does not exceed maxvel
# verify decel phase stops when vel == 0
highest_seen_accel_ipc2 = 0
old_accel_ipc2 = 0
for i in range(i, len(samples)):
new_v_ipc = samples[i+1][1] - samples[i][1]
accel_ipc2 = new_v_ipc - old_v_ipc
highest_seen_accel_ipc2 = min(accel_ipc2, highest_seen_accel_ipc2)
#print "line %d: X=%.6f, v=%.6f i/c (%.6f i/s), a=%.6f i/c^2 (%.6f i/s^2)" % (i, samples[i][1], new_v_ipc, (new_v_ipc * cycles_per_second), accel_ipc2, (accel_ipc2 * cycles_per_second * cycles_per_second))
# i hate floating point
if ((accel_ipc2 * cycles_per_second) - max_acceleration_ipspc) > 0.0000001:
print "line %d: detected accel constraint violation!" % i
print "detected accel %.6f i/c^2 (%.6f i/s^2)" % (accel_ipc2, accel_ipc2 * cycles_per_second * cycles_per_second)
print "max accel %.6f i/s^2)" % max_acceleration_ips2
sys.exit(1)
if (new_v_ipc - max_velocity_ipc) > 0.0000001:
print "line %d: detected vel constraint violation!" % i
print "detected vel %.6f i/c (%.6f i/s)" % (new_v_ipc, new_v_ipc * cycles_per_second)
print "max vel %.6f i/c (%.6f i/s)" % (max_velocity_ipc, max_velocity_ips)
sys.exit(1)
if new_v_ipc < -0.0000001:
print "line %d: detected vel undershoot!" % i
print "detected vel %.6f i/c (%.6f i/s)" % (new_v_ipc, new_v_ipc * cycles_per_second)
sys.exit(1)
if new_v_ipc == 0:
print "line %d: decel phase over" % i
break
old_v_ipc = new_v_ipc
# verify highest seen accel is very close to max accel
if abs(max_acceleration_ipspc + (highest_seen_accel_ipc2 * cycles_per_second)) > 0.0000001:
print "accel only reached %.6f i/c^2 (%.6f i/s^2)" % (highest_seen_accel_ipc2, (highest_seen_accel_ipc2 * cycles_per_second * cycles_per_second))
print "max accel is %.6f i/s^2" % max_acceleration_ips2
sys.exit(1)
print " decel reached but did not exceed 1/2 of max accel of %.6f i/s^2" % max_acceleration_ips2
# verify X stopped at 1.000000
if samples[i][1] != 1.000000:
print "line %d: X stopped at %.6f, not at 1.000000!" % (i, samples[i][1])
sys.exit(1)
print " X reached target of 1.0000"
# verify X doesn't move from now on
for i in range(i, len(samples) - 1):
if samples[i][1] != samples[i+1][1]:
print "line %d: X moved!" % i
sys.exit(1)
print "line %d: X did not move after reaching target" % i
print "success!\n"
sys.exit(0)
|
