4 import matplotlib.pyplot as plt
10 #TIMER_FREQ = 2000000.
11 TIMER_FREQ = 16000000.
13 LASER_RADIUS = 20. # 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
79 dist_mm = map(frame_to_mm, range(512))
84 theorical[i] = frame_to_us(i)
86 # find offset and update theorical curve
87 off = samples[-1][1] - mm_to_us(3000.)
88 #print "offset=%f"%(off)
92 theo_off[i] = mm_to_us(mm) + off
98 # find between which samples we are
100 while smp < (len(samples) - 2):
101 if samples[smp+1][0] >= mm:
105 mm_start = us_to_mm(samples[smp][1] - off)
106 mm_end = us_to_mm(samples[smp+1][1] - off)
109 ratio = (mm - samples[smp][0]) / (samples[smp+1][0] - samples[smp][0])
110 mm_new = mm_start + ratio * (mm_end - mm_start)
114 final[i] = mm_to_us(mm_new) + off
118 dist_mm, theorical, "r-",
119 dist_mm, theo_off, "b-",
120 dist_mm, final, "g-",
121 map(lambda x:x[0], samples), map(lambda x:x[1], samples), "g^",
125 print "#include <aversive.h>"
126 print "#include <aversive/pgmspace.h>"
127 print "prog_uint16_t framedist_table[] = {"
131 print "%d,"%(int(time_us_to_tick(final[i]))),