9ef4c5caac918e18fc3050e60db9b29d07d67a4d
1 #!/usr/bin/python
3 import sys, math
4 import matplotlib.pyplot as plt
7 #RPS = 10.
8 RPS = 20.
9 #RPS = 40.
10 #TIMER_FREQ = 2000000.
11 TIMER_FREQ = 16000000.
13 LASER_RADIUS = 25. # mm
15 MIN = 200.
16 MAX = 3500.
17 NBITS = 9
18 STEPS = (1 << 9)
19 k = math.pow(MAX/MIN, 1./STEPS)
21 def mm_to_frame(mm):
22     d = mm
23     d -= MIN
24     d /= (MAX-MIN)
25     d *= 512
26     return d
28 def frame_to_mm(d):
29     d /= 512.
30     d *= (MAX-MIN)
31     d += MIN
32     return d
34 # t is in us, result is 9 bits
35 def us_to_frame(t):
36     # process angle from t
37     a = (t / (1000000./RPS)) * 2. * math.pi
39     # process d from a (between 20cm and 350cm)
40     d = LASER_RADIUS / math.sin(a/2)
41     frame = int(mm_to_frame(d))
42     return frame
44 # frame is integer 9 bits, result is laserdiff time in us
45 def frame_to_us(frame):
46     d = frame_to_mm(frame)
47     a = 2 * math.asin(LASER_RADIUS/d)
48     t = (a * (1000000./RPS)) / (2. * math.pi)
49     return t
51 # theorical: laser timediff to robot distance
52 def us_to_mm(us):
53     return frame_to_mm(us_to_frame(us))
55 # theorical: robot distance to laserdiff
56 def mm_to_us(mm):
57     return frame_to_us(mm_to_frame(mm))
59 def time_us_to_tick(us):
60     return (us / 1000000.) * TIMER_FREQ
62 def time_tick_to_us(t):
63     return (t * 1000000.) / TIMER_FREQ
66 ##################
68 # linear correction: distance_mm, time_us
69 # must be ordered
70 samples = [
71     (250., 2201.),
72     (450., 701.),
73     (1200., 231.),
74     (3000., 50.),
75     ]
77 dist_mm = map(frame_to_mm, range(512))
79 # theorical curve
80 theorical =  * 512
81 for i in range(512):
82     theorical[i] = frame_to_us(i)
84 # find offset and update theorical curve
85 off = samples[-1] - mm_to_us(3000.)
86 print "offset=%f"%(off)
87 theo_off =  * 512
88 for i in range(512):
89     mm = frame_to_mm(i)
90     theo_off[i] = mm_to_us(mm) + off
92 final =  * 512
93 for i in range(512):
94     mm = frame_to_mm(i)
96     # find between which samples we are
97     smp = 0
98     while smp < (len(samples) - 2):
99         if samples[smp+1] >= mm:
100             break
101         smp += 1
103     mm_start = us_to_mm(samples[smp] - off)
104     mm_end = us_to_mm(samples[smp+1] - off)
106     # interpolation
107     ratio = (mm - samples[smp]) / (samples[smp+1] - samples[smp])
108     mm_new = mm_start + ratio * (mm_end - mm_start)
110     final[i] = mm_to_us(mm_new) + off
112 sample_idx = 0
113 while sample_idx < len(samples):
114     print samples[sample_idx],
115     print us_to_mm(samples[sample_idx] - off),
116     print mm_to_us(samples[sample_idx])
117     sample_idx += 1
120 plt.plot(
121 #    dist_mm, theorical, "r-",
122 #    dist_mm, theo_off, "b-",
123     dist_mm, final, "g-",
124     map(lambda x:x, samples), map(lambda x:x, samples), "g^",
125     )
126 plt.show()
129 print "#include <aversive.h>"
130 print "#include <aversive/pgmspace.h>"
131 print "prog_uint16_t framedist_table[] = {"
132 for i in range(512):
133     if (i % 8) == 0:
134         print " ",
135 #    print "%d,"%(int(linear_interpolation(offsets, i, table[i]))),
136     print "%d,"%(int(table[i])),
137     if (i % 8 == 7):
138         print
139 print "};"