10 TIMER_FREQ = 16000000.
12 LASER_RADIUS = 25. # mm
18 k = math.pow(MAX/MIN, 1./STEPS)
20 def mm_to_framedist(mm):
27 def framedist_to_mm(d):
33 # t is in us, result is 9 bits
35 # process angle from t
36 a = (t / (1000000./RPS)) * 2. * math.pi
38 # process d from a (between 20cm and 350cm)
39 d = LASER_RADIUS / math.sin(a/2)
40 frame = int(mm_to_framedist(d))
43 # frame is integer 9 bits, result is laserdiff time
44 def frame_to_time(frame):
45 d = framedist_to_mm(frame)
46 a = 2 * math.asin(LASER_RADIUS/d)
47 t = (a * (TIMER_FREQ/RPS)) / (2. * math.pi)
50 def sample_to_offset(samples, table):
52 for i in range(len(offsets)):
54 framedist = mm_to_framedist(o[0])
55 off = o[1] - table[int(framedist)]
56 offsets[i] = framedist, off
59 def linear_interpolation(offsets, framedist, time):
60 if framedist <= offsets[0][0]:
61 return time + offsets[0][1]
62 if framedist >= offsets[-1][0]:
63 return time + offsets[-1][1]
65 #print (offsets, framedist, time)
71 x = (framedist - o_prev[0]) / (o[0] - o_prev[0])
72 return time + o_prev[1] + (x * (o[1] - o_prev[1]))
75 #x = time_to_frame(float(sys.argv[1]))
77 #frame_to_time(int(sys.argv[1]))
82 table[i] = frame_to_time(i)
84 # linear correction: distance_mm, time
91 offsets = sample_to_offset(samples, table)
92 print "#include <aversive.h>"
93 print "#include <aversive/pgmspace.h>"
94 print "prog_uint16_t framedist_table[] = {"
98 print "%d,"%(int(linear_interpolation(offsets, i, table[i]))),