1 #!/usr/bin/python
3 import sys, math
5 if 1:
6     RPS = 10.
7     TIMER_FREQ = 2000000.
8 else:
9     RPS = 40.
10     TIMER_FREQ = 16000000.
12 LASER_RADIUS = 25. # mm
14 MIN = 200.
15 MAX = 3500.
16 NBITS = 9
17 STEPS = (1 << 9)
18 k = math.pow(MAX/MIN, 1./STEPS)
20 def mm_to_framedist(mm):
21     d = mm
22     d -= MIN
23     d /= (MAX-MIN)
24     d *= 512
25     return d
27 def framedist_to_mm(d):
28     d /= 512.
29     d *= (MAX-MIN)
30     d += MIN
31     return d
33 # t is in us, result is 9 bits
34 def time_to_frame(t):
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))
41     return frame
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)
48     return t
50 def sample_to_offset(samples, table):
51     offsets = samples[:]
52     for i in range(len(offsets)):
53         o = offsets[i]
54         framedist = mm_to_framedist(o)
55         off = o - table[int(framedist)]
56         offsets[i] = framedist, off
57     return offsets
59 def linear_interpolation(offsets, framedist, time):
60     if framedist <= offsets:
61         return time + offsets
62     if framedist >= offsets[-1]:
63         return time + offsets[-1]
65     #print (offsets, framedist, time)
66     o_prev = offsets
67     for o in offsets[1:]:
68         if framedist > o:
69             o_prev = o
70             continue
71         x = (framedist - o_prev) / (o - o_prev)
72         return time + o_prev + (x * (o - o_prev))
73     return None
75 #x = time_to_frame(float(sys.argv))
76 #frame_to_distance(x)
77 #frame_to_time(int(sys.argv))
80 table =  * 512
81 for i in range(512):
82     table[i] = frame_to_time(i)
84 # linear correction: distance_mm, time
85 samples = [
86     (250., 7600.),
87     (500., 3000.),
88     (3000., 400.),
89     ]
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[] = {"
95 for i in range(512):
96     if (i % 8) == 0:
97         print " ",
98     print "%d,"%(int(linear_interpolation(offsets, i, table[i]))),
99     if (i % 8 == 7):
100         print
101 print "};"