projects/microb2010/tests/static_beacon/coding.py

   1 #!/usr/bin/python
   2
   3 import sys, math
   4 import matplotlib.pyplot as plt
   5
   6
   7 #RPS = 10.
   8 RPS = 20.
   9 #RPS = 40.
  10 #TIMER_FREQ = 2000000.
  11 TIMER_FREQ = 16000000.
  12
  13 LASER_RADIUS = 20. # mm
  14
  15 MIN = 200.
  16 MAX = 3500.
  17 NBITS = 9
  18 STEPS = (1 << 9)
  19 k = math.pow(MAX/MIN, 1./STEPS)
  20
  21 def mm_to_frame(mm):
  22     d = mm
  23     d -= MIN
  24     d /= (MAX-MIN)
  25     d *= 512
  26     return d
  27
  28 def frame_to_mm(d):
  29     d /= 512.
  30     d *= (MAX-MIN)
  31     d += MIN
  32     return d
  33
  34 # t is in us, result is 9 bits
  35 def us_to_frame(t):
  36     # process angle from t
  37     a = (t / (1000000./RPS)) * 2. * math.pi
  38
  39     # process d from a (between 20cm and 350cm)
  40     d = LASER_RADIUS / math.sin(a/2)
  41     frame = int(mm_to_frame(d))
  42     return frame
  43
  44 # frame is integer 9 bits, result is laserdiff time in us
  45 def frame_to_us(frame):
  46     d = frame_to_mm(frame)
  47     a = 2 * math.asin(LASER_RADIUS/d)
  48     t = (a * (1000000./RPS)) / (2. * math.pi)
  49     return t
  50
  51 # theorical: laser timediff to robot distance
  52 def us_to_mm(us):
  53     return frame_to_mm(us_to_frame(us))
  54
  55 # theorical: robot distance to laserdiff
  56 def mm_to_us(mm):
  57     return frame_to_us(mm_to_frame(mm))
  58
  59 def time_us_to_tick(us):
  60     return (us / 1000000.) * TIMER_FREQ
  61
  62 def time_tick_to_us(t):
  63     return (t * 1000000.) / TIMER_FREQ
  64
  65
  66 ##################
  67
  68 # linear correction: distance_mm, time_us
  69 # must be ordered
  70 samples = [
  71     (330.,  15681./16),
  72     (778.,  6437./16),
  73     (1180., 4351./16),
  74     (1608., 3221./16),
  75     (2045., 2583./16),
  76     (2487., 2167./16),
  77     ]
  78
  79 dist_mm = map(frame_to_mm, range(512))
  80
  81 # theorical curve
  82 theorical = [0] * 512
  83 for i in range(512):
  84     theorical[i] = frame_to_us(i)
  85
  86 # find offset and update theorical curve
  87 off = samples[-1][1] - mm_to_us(3000.)
  88 #print "offset=%f"%(off)
  89 theo_off = [0] * 512
  90 for i in range(512):
  91     mm = frame_to_mm(i)
  92     theo_off[i] = mm_to_us(mm) + off
  93
  94 final = [0] * 512
  95 for i in range(512):
  96     mm = frame_to_mm(i)
  97
  98     # find between which samples we are
  99     smp = 0
 100     while smp < (len(samples) - 2):
 101         if samples[smp+1][0] >= mm:
 102             break
 103         smp += 1
 104
 105     mm_start = us_to_mm(samples[smp][1] - off)
 106     mm_end = us_to_mm(samples[smp+1][1] - off)
 107
 108     # interpolation
 109     ratio = (mm - samples[smp][0]) / (samples[smp+1][0] - samples[smp][0])
 110     mm_new = mm_start + ratio * (mm_end - mm_start)
 111
 112     if mm_new < 0:
 113         mm_new = 1.
 114     final[i] = mm_to_us(mm_new) + off
 115
 116
 117 plt.plot(
 118     dist_mm, theorical, "r-",
 119     dist_mm, theo_off, "b-",
 120     dist_mm, final, "g-",
 121     map(lambda x:x[0], samples), map(lambda x:x[1], samples), "g^",
 122     )
 123 plt.show()
 124
 125 print "#include <aversive.h>"
 126 print "#include <aversive/pgmspace.h>"
 127 print "prog_uint16_t framedist_table[] = {"
 128 for i in range(512):
 129     if (i % 8) == 0:
 130         print " ",
 131     print "%d,"%(int(time_us_to_tick(final[i]))),
 132     if (i % 8 == 7):
 133         print
 134 print "};"