X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Ftests%2Fstatic_beacon%2Fcoding.py;h=a41c69233f09c5b9dbdddbfe587e0dd058236d54;hp=440fd3bffdb27e26e5b03a61bbb76e1d391b9c94;hb=7bca487a2198825b6d7caabcd1143e2d5e19dd43;hpb=aa0ebd8b43a0e48d791864ae3268e78d980a70e9

diff --git a/projects/microb2010/tests/static_beacon/coding.py b/projects/microb2010/tests/static_beacon/coding.py
index 440fd3b..a41c692 100644
--- a/projects/microb2010/tests/static_beacon/coding.py
+++ b/projects/microb2010/tests/static_beacon/coding.py
@@ -2,7 +2,13 @@
 
 import sys, math
 
-RPS = 10.
+if 1:
+    RPS = 10.
+    TIMER_FREQ = 2000000.
+else:
+    RPS = 40.
+    TIMER_FREQ = 16000000.
+
 LASER_RADIUS = 25. # mm
 
 MIN = 200.
@@ -11,6 +17,19 @@ NBITS = 9
 STEPS = (1 << 9)
 k = math.pow(MAX/MIN, 1./STEPS)
 
+def mm_to_framedist(mm):
+    d = mm
+    d -= MIN
+    d /= (MAX-MIN)
+    d *= 512
+    return d
+
+def framedist_to_mm(d):
+    d /= 512.
+    d *= (MAX-MIN)
+    d += MIN
+    return d
+
 # t is in us, result is 9 bits
 def time_to_frame(t):
     # process angle from t
@@ -18,20 +37,65 @@ def time_to_frame(t):
 
     # process d from a (between 20cm and 350cm)
     d = LASER_RADIUS / math.sin(a/2)
-
-    frame = math.log(d/MIN)/math.log(k)
-    if frame >= 512:
-        frame = 511
-    else:
-        frame = int(frame)
-    print frame
+    frame = int(mm_to_framedist(d))
     return frame
 
-# frame is integer 9 bits, result is distance
-def frame_to_distance(frame):
-    d = MIN*(math.pow(k, frame))
-    print d
-    return d
+# frame is integer 9 bits, result is laserdiff time
+def frame_to_time(frame):
+    d = framedist_to_mm(frame)
+    a = 2 * math.asin(LASER_RADIUS/d)
+    t = (a * (TIMER_FREQ/RPS)) / (2. * math.pi)
+    return t
+
+def sample_to_offset(samples, table):
+    offsets = samples[:]
+    for i in range(len(offsets)):
+        o = offsets[i]
+        framedist = mm_to_framedist(o[0])
+        off = o[1] - table[int(framedist)]
+        offsets[i] = framedist, off
+    return offsets
+
+def linear_interpolation(offsets, framedist, time):
+    if framedist <= offsets[0][0]:
+        return time + offsets[0][1]
+    if framedist >= offsets[-1][0]:
+        return time + offsets[-1][1]
+
+    #print (offsets, framedist, time)
+    o_prev = offsets[0]
+    for o in offsets[1:]:
+        if framedist > o[0]:
+            o_prev = o
+            continue
+        x = (framedist - o_prev[0]) / (o[0] - o_prev[0])
+        return time + o_prev[1] + (x * (o[1] - o_prev[1]))
+    return None
+
+#x = time_to_frame(float(sys.argv[1]))
+#frame_to_distance(x)
+#frame_to_time(int(sys.argv[1]))
+
+
+table = [0] * 512
+for i in range(512):
+    table[i] = frame_to_time(i)
+
+# linear correction: distance_mm, time
+samples = [
+    (250., 7600.),
+    (500., 3000.),
+    (3000., 400.),
+    ]
 
-x = time_to_frame(float(sys.argv[1]))
-frame_to_distance(x)
+offsets = sample_to_offset(samples, table)
+print "#include <aversive.h>"
+print "#include <aversive/pgmspace.h>"
+print "prog_uint16_t framedist_table[] = {"
+for i in range(512):
+    if (i % 8) == 0:
+        print "	",
+    print "%d,"%(int(linear_interpolation(offsets, i, table[i]))),
+    if (i % 8 == 7):
+        print
+print "};"