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 = 20. # 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     (330.,  15681./16),
72     (778.,  6437./16),
73     (1180., 4351./16),
74     (1608., 3221./16),
75     (2045., 2583./16),
76     (2487., 2167./16),
77     ]
79 dist_mm = map(frame_to_mm, range(512))
81 # theorical curve
82 theorical =  * 512
83 for i in range(512):
84     theorical[i] = frame_to_us(i)
86 # find offset and update theorical curve
87 off = samples[-1] - mm_to_us(3000.)
88 #print "offset=%f"%(off)
89 theo_off =  * 512
90 for i in range(512):
91     mm = frame_to_mm(i)
92     theo_off[i] = mm_to_us(mm) + off
94 final =  * 512
95 for i in range(512):
96     mm = frame_to_mm(i)
98     # find between which samples we are
99     smp = 0
100     while smp < (len(samples) - 2):
101         if samples[smp+1] >= mm:
102             break
103         smp += 1
105     mm_start = us_to_mm(samples[smp] - off)
106     mm_end = us_to_mm(samples[smp+1] - off)
108     # interpolation
109     ratio = (mm - samples[smp]) / (samples[smp+1] - samples[smp])
110     mm_new = mm_start + ratio * (mm_end - mm_start)
112     if mm_new < 0:
113         mm_new = 1.
114     final[i] = mm_to_us(mm_new) + off
117 plt.plot(
118     dist_mm, theorical, "r-",
119     dist_mm, theo_off, "b-",
120     dist_mm, final, "g-",
121     map(lambda x:x, samples), map(lambda x:x, samples), "g^",
122     )
123 plt.show()
125 print "#include <aversive.h>"
126 print "#include <aversive/pgmspace.h>"
127 print "prog_uint16_t framedist_table[] = {"
128 for i in range(512):
129     if (i % 8) == 0:
130         print " ",
131     print "%d,"%(int(time_us_to_tick(final[i]))),
132     if (i % 8 == 7):
133         print
134 print "};"