beacon

[aversive.git] / projects / microb2010 / tests / static_beacon / coding.py

diff --git a/projects/microb2010/tests/static_beacon/coding.py b/projects/microb2010/tests/static_beacon/coding.py
index 440fd3b..9ef4c5c 100644 (file)
--- a/projects/microb2010/tests/static_beacon/coding.py
+++ b/projects/microb2010/tests/static_beacon/coding.py
@@ -1,8 +1,15 @@
 #!/usr/bin/python
 
 import sys, math
+import matplotlib.pyplot as plt
+
+
+#RPS = 10.
+RPS = 20.
+#RPS = 40.
+#TIMER_FREQ = 2000000.
+TIMER_FREQ = 16000000.
 
-RPS = 10.
 LASER_RADIUS = 25. # mm
 
 MIN = 200.
@@ -11,27 +18,122 @@ NBITS = 9
 STEPS = (1 << 9)
 k = math.pow(MAX/MIN, 1./STEPS)
 
+def mm_to_frame(mm):
+    d = mm
+    d -= MIN
+    d /= (MAX-MIN)
+    d *= 512
+    return d
+
+def frame_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):
+def us_to_frame(t):
     # process angle from t
     a = (t / (1000000./RPS)) * 2. * math.pi
 
     # 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_frame(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 in us
+def frame_to_us(frame):
+    d = frame_to_mm(frame)
+    a = 2 * math.asin(LASER_RADIUS/d)
+    t = (a * (1000000./RPS)) / (2. * math.pi)
+    return t
+
+# theorical: laser timediff to robot distance
+def us_to_mm(us):
+    return frame_to_mm(us_to_frame(us))
+
+# theorical: robot distance to laserdiff
+def mm_to_us(mm):
+    return frame_to_us(mm_to_frame(mm))
+
+def time_us_to_tick(us):
+    return (us / 1000000.) * TIMER_FREQ
+
+def time_tick_to_us(t):
+    return (t * 1000000.) / TIMER_FREQ
+
+
+##################
+
+# linear correction: distance_mm, time_us
+# must be ordered
+samples = [
+    (250., 2201.),
+    (450., 701.),
+    (1200., 231.),
+    (3000., 50.),
+    ]
+
+dist_mm = map(frame_to_mm, range(512))
+
+# theorical curve
+theorical = [0] * 512
+for i in range(512):
+    theorical[i] = frame_to_us(i)
+
+# find offset and update theorical curve
+off = samples[-1][1] - mm_to_us(3000.)
+print "offset=%f"%(off)
+theo_off = [0] * 512
+for i in range(512):
+    mm = frame_to_mm(i)
+    theo_off[i] = mm_to_us(mm) + off
+
+final = [0] * 512
+for i in range(512):
+    mm = frame_to_mm(i)
+
+    # find between which samples we are
+    smp = 0
+    while smp < (len(samples) - 2):
+        if samples[smp+1][0] >= mm:
+            break
+        smp += 1
+
+    mm_start = us_to_mm(samples[smp][1] - off)
+    mm_end = us_to_mm(samples[smp+1][1] - off)
+
+    # interpolation
+    ratio = (mm - samples[smp][0]) / (samples[smp+1][0] - samples[smp][0])
+    mm_new = mm_start + ratio * (mm_end - mm_start)
+
+    final[i] = mm_to_us(mm_new) + off
+
+sample_idx = 0
+while sample_idx < len(samples):
+    print samples[sample_idx][1],
+    print us_to_mm(samples[sample_idx][1] - off),
+    print mm_to_us(samples[sample_idx][0])
+    sample_idx += 1
+
+
+plt.plot(
+#    dist_mm, theorical, "r-",
+#    dist_mm, theo_off, "b-",
+    dist_mm, final, "g-",
+    map(lambda x:x[0], samples), map(lambda x:x[1], samples), "g^",
+    )
+plt.show()
+
 
-x = time_to_frame(float(sys.argv[1]))
-frame_to_distance(x)
+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]))),
+    print "%d,"%(int(table[i])),
+    if (i % 8 == 7):
+        print
+print "};"