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 = 25. # 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     (250., 2201.),
  72     (450., 701.),
  73     (1200., 231.),
  74     (3000., 50.),
  75     ]
  76
  77 dist_mm = map(frame_to_mm, range(512))
  78
  79 # theorical curve
  80 theorical = [0] * 512
  81 for i in range(512):
  82     theorical[i] = frame_to_us(i)
  83
  84 # find offset and update theorical curve
  85 off = samples[-1][1] - mm_to_us(3000.)
  86 print "offset=%f"%(off)
  87 theo_off = [0] * 512
  88 for i in range(512):
  89     mm = frame_to_mm(i)
  90     theo_off[i] = mm_to_us(mm) + off
  91
  92 final = [0] * 512
  93 for i in range(512):
  94     mm = frame_to_mm(i)
  95
  96     # find between which samples we are
  97     smp = 0
  98     while smp < (len(samples) - 2):
  99         if samples[smp+1][0] >= mm:
 100             break
 101         smp += 1
 102
 103     mm_start = us_to_mm(samples[smp][1] - off)
 104     mm_end = us_to_mm(samples[smp+1][1] - off)
 105
 106     # interpolation
 107     ratio = (mm - samples[smp][0]) / (samples[smp+1][0] - samples[smp][0])
 108     mm_new = mm_start + ratio * (mm_end - mm_start)
 109
 110     final[i] = mm_to_us(mm_new) + off
 111
 112 sample_idx = 0
 113 while sample_idx < len(samples):
 114     print samples[sample_idx][1],
 115     print us_to_mm(samples[sample_idx][1] - off),
 116     print mm_to_us(samples[sample_idx][0])
 117     sample_idx += 1
 118
 119
 120 plt.plot(
 121 #    dist_mm, theorical, "r-",
 122 #    dist_mm, theo_off, "b-",
 123     dist_mm, final, "g-",
 124     map(lambda x:x[0], samples), map(lambda x:x[1], samples), "g^",
 125     )
 126 plt.show()
 127
 128
 129 print "#include <aversive.h>"
 130 print "#include <aversive/pgmspace.h>"
 131 print "prog_uint16_t framedist_table[] = {"
 132 for i in range(512):
 133     if (i % 8) == 0:
 134         print " ",
 135 #    print "%d,"%(int(linear_interpolation(offsets, i, table[i]))),
 136     print "%d,"%(int(table[i])),
 137     if (i % 8 == 7):
 138         print
 139 print "};"