4 import matplotlib.pyplot as plt
10 #TIMER_FREQ = 2000000.
11 TIMER_FREQ = 16000000.
13 LASER_RADIUS = 25. # mm
19 k = math.pow(MAX/MIN, 1./STEPS)
34 # t is in us, result is 9 bits
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))
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)
51 # theorical: laser timediff to robot distance
53 return frame_to_mm(us_to_frame(us))
55 # theorical: robot distance to laserdiff
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
68 # linear correction: distance_mm, time_us
77 dist_mm = map(frame_to_mm, range(512))
82 theorical[i] = frame_to_us(i)
84 # find offset and update theorical curve
85 off = samples[-1][1] - mm_to_us(3000.)
86 print "offset=%f"%(off)
90 theo_off[i] = mm_to_us(mm) + off
96 # find between which samples we are
98 while smp < (len(samples) - 2):
99 if samples[smp+1][0] >= mm:
103 mm_start = us_to_mm(samples[smp][1] - off)
104 mm_end = us_to_mm(samples[smp+1][1] - off)
107 ratio = (mm - samples[smp][0]) / (samples[smp+1][0] - samples[smp][0])
108 mm_new = mm_start + ratio * (mm_end - mm_start)
110 final[i] = mm_to_us(mm_new) + off
113 while sample_idx < len(samples):
114 print samples[sample_idx][1],
115 print us_to_mm(samples[sample_idx][1] - off),
116 print mm_to_us(samples[sample_idx][0])
121 # dist_mm, theorical, "r-",
122 # dist_mm, theo_off, "b-",
123 dist_mm, final, "g-",
124 map(lambda x:x[0], samples), map(lambda x:x[1], samples), "g^",
129 print "#include <aversive.h>"
130 print "#include <aversive/pgmspace.h>"
131 print "prog_uint16_t framedist_table[] = {"
135 # print "%d,"%(int(linear_interpolation(offsets, i, table[i]))),
136 print "%d,"%(int(table[i])),