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
|
#!/usr/bin/env python3
import sys, os, re, math
def near(a, b):
fudge_factor = 0.001
error = abs(a-b)
allowance = abs(a * fudge_factor)
if error <= allowance:
return True
return False
def must_decel_to_avoid_pos_violation(sample, out, vel):
"""Returns True if maximum deceleration is needed NOW to avoid
violating position constraints."""
# Keep going one cycle without any acceleration.
next_out = out + (vel * 0.001) # Assume a 1 ms period.
# v = v0 + at
# 0 = v0 + at
# t = v0/a
seconds_to_stop = abs(vel) / params['maxa']
# figure out which direction we need to accelerate in
if vel > 0:
accel_sign = -1
else:
accel_sign = 1
# r = r0 + v0t + 1/2 at^2
pos_at_stop = next_out + (vel * seconds_to_stop) + (accel_sign * 0.5 * params['maxa'] * math.pow(seconds_to_stop, 2))
print("sample=%d, next_out=%.6f, vel=%.6f, seconds_to_stop=%.6f, pos_at_stop=%.6f, min=%.6f, max=%.6f" % \
(sample, next_out, vel, seconds_to_stop, pos_at_stop, params['min'], params['max']))
if pos_at_stop > (params['max'] * 1.01) or pos_at_stop < (params['min'] * 1.01):
return True
return False
halfile = os.path.join(os.path.dirname(__file__),"test.hal")
param_re = re.compile(r'^setp\s+limit3.0.([^\s]+)\s+([-0-9]+)$')
params = {}
params['min'] = -1e20
params['max'] = 1e20
params['maxv'] = 1e20
params['maxa'] = 1e20
with open(halfile, 'r') as f:
for line in f:
m = param_re.match(line)
if m is None: continue
params[m.group(1)] = float(m.group(2))
result_filename = sys.argv[1]
result_file = open(result_filename, 'r')
retval = 0
for line in result_file.readlines():
(sample, in_val, out, vel, acc) = line.split()[:5]
sample = int(sample)
in_val = float(in_val)
out = float(out)
vel = float(vel)
acc = float(acc)
#
# See if we've violated any constraints.
#
if float(out) > params['max']:
print("max=%.3f, in=%.3f, out=%.3f, max violation on sample %s" % \
(params['max'], float(in_val), out, sample))
retval = 1
if float(out) < params['min']:
print("min=%.3f, in=%.3f, out=%.3f, min violation on sample %s" % \
(params['min'], float(in_val), out, sample))
retval = 1
if abs(vel) > params['maxv']:
print("maxv=%.3f, vel=%.3f, velocity violation on sample %s" % \
(params['maxv'], vel, sample))
retval = 1
if abs(acc) > params['maxa']:
print("maxa=%.3f, acc=%.3f, acceleration violation on sample %s" % \
(params['maxa'], acc, sample))
retval = 1
#
# See if we're accelerating too sluggishly.
#
if abs(acc) < params['maxa']:
# Why aren't we accelerating at the max rate?
if abs(vel) == params['maxv']:
# We've already maxed out vel.
pass
elif near(out, params['min']) or near(out, params['max']):
# We're at our position limit.
pass
elif near(out, in_val):
# We're already near our target.
pass
else:
# We have no good reason for not accelerating hard, so fail this test.
print("maxa=%.3f, acc=%.3f, sluggish acceleration on sample %s" % \
(params['maxa'], acc, sample))
retval = 1
#
# See if our velocity is too sluggish.
#
if abs(vel) < params['maxv']:
# Why aren't we moving at the max rate?
if abs(acc) == params['maxa']:
# We're still accelerating.
pass
elif near(out, params['min']) or near(out, params['max']):
# We're at our position limit.
pass
elif near(out, in_val):
# We've already reached our target.
pass
elif must_decel_to_avoid_pos_violation(sample, out, vel):
# We're slowing down to avoid violating position constraints.
pass
else:
# We have no good reason for not going fast, so fail this test.
print("maxv=%.3f, vel=%.3f, sluggish velocity on sample %s" % \
(params['maxv'], vel, sample))
retval = 1
sys.exit(retval)
|
