X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Ftests%2Fstatic_beacon%2Fcoding.py;fp=projects%2Fmicrob2010%2Ftests%2Fstatic_beacon%2Fcoding.py;h=440fd3bffdb27e26e5b03a61bbb76e1d391b9c94;hp=0000000000000000000000000000000000000000;hb=d992c7dd579170e226cfceabb652cdda6edb97b4;hpb=844fa3427676e08d7e9f92279788def635377714

diff --git a/projects/microb2010/tests/static_beacon/coding.py b/projects/microb2010/tests/static_beacon/coding.py
new file mode 100644
index 0000000..440fd3b
--- /dev/null
+++ b/projects/microb2010/tests/static_beacon/coding.py
@@ -0,0 +1,37 @@
+#!/usr/bin/python
+
+import sys, math
+
+RPS = 10.
+LASER_RADIUS = 25. # mm
+
+MIN = 200.
+MAX = 3500.
+NBITS = 9
+STEPS = (1 << 9)
+k = math.pow(MAX/MIN, 1./STEPS)
+
+# t is in us, result is 9 bits
+def time_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
+    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
+
+x = time_to_frame(float(sys.argv[1]))
+frame_to_distance(x)