projects/microb2010/tests/static_beacon/coding.py

   1 #!/usr/bin/python
   2
   3 import sys, math
   4
   5 if 1:
   6     RPS = 10.
   7     TIMER_FREQ = 2000000.
   8 else:
   9     RPS = 40.
  10     TIMER_FREQ = 16000000.
  11
  12 LASER_RADIUS = 25. # mm
  13
  14 MIN = 200.
  15 MAX = 3500.
  16 NBITS = 9
  17 STEPS = (1 << 9)
  18 k = math.pow(MAX/MIN, 1./STEPS)
  19
  20 def mm_to_framedist(mm):
  21     d = mm
  22     d -= MIN
  23     d /= (MAX-MIN)
  24     d *= 512
  25     return d
  26
  27 def framedist_to_mm(d):
  28     d /= 512.
  29     d *= (MAX-MIN)
  30     d += MIN
  31     return d
  32
  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
  37
  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
  42
  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
  49
  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[0])
  55         off = o[1] - table[int(framedist)]
  56         offsets[i] = framedist, off
  57     return offsets
  58
  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]
  64
  65     #print (offsets, framedist, time)
  66     o_prev = offsets[0]
  67     for o in offsets[1:]:
  68         if framedist > o[0]:
  69             o_prev = o
  70             continue
  71         x = (framedist - o_prev[0]) / (o[0] - o_prev[0])
  72         return time + o_prev[1] + (x * (o[1] - o_prev[1]))
  73     return None
  74
  75 #x = time_to_frame(float(sys.argv[1]))
  76 #frame_to_distance(x)
  77 #frame_to_time(int(sys.argv[1]))
  78
  79
  80 table = [0] * 512
  81 for i in range(512):
  82     table[i] = frame_to_time(i)
  83
  84 # linear correction: distance_mm, time
  85 samples = [
  86     (250., 7600.),
  87     (500., 3000.),
  88     (3000., 400.),
  89     ]
  90
  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 "};"