X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Ftests%2Fstatic_beacon%2Fcoding.py;h=a41c69233f09c5b9dbdddbfe587e0dd058236d54;hp=440fd3bffdb27e26e5b03a61bbb76e1d391b9c94;hb=7bca487a2198825b6d7caabcd1143e2d5e19dd43;hpb=aa0ebd8b43a0e48d791864ae3268e78d980a70e9 diff --git a/projects/microb2010/tests/static_beacon/coding.py b/projects/microb2010/tests/static_beacon/coding.py index 440fd3b..a41c692 100644 --- a/projects/microb2010/tests/static_beacon/coding.py +++ b/projects/microb2010/tests/static_beacon/coding.py @@ -2,7 +2,13 @@ import sys, math -RPS = 10. +if 1: + RPS = 10. + TIMER_FREQ = 2000000. +else: + RPS = 40. + TIMER_FREQ = 16000000. + LASER_RADIUS = 25. # mm MIN = 200. @@ -11,6 +17,19 @@ NBITS = 9 STEPS = (1 << 9) k = math.pow(MAX/MIN, 1./STEPS) +def mm_to_framedist(mm): + d = mm + d -= MIN + d /= (MAX-MIN) + d *= 512 + return d + +def framedist_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): # process angle from t @@ -18,20 +37,65 @@ def time_to_frame(t): # 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_framedist(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 +def frame_to_time(frame): + d = framedist_to_mm(frame) + a = 2 * math.asin(LASER_RADIUS/d) + t = (a * (TIMER_FREQ/RPS)) / (2. * math.pi) + return t + +def sample_to_offset(samples, table): + offsets = samples[:] + for i in range(len(offsets)): + o = offsets[i] + framedist = mm_to_framedist(o[0]) + off = o[1] - table[int(framedist)] + offsets[i] = framedist, off + return offsets + +def linear_interpolation(offsets, framedist, time): + if framedist <= offsets[0][0]: + return time + offsets[0][1] + if framedist >= offsets[-1][0]: + return time + offsets[-1][1] + + #print (offsets, framedist, time) + o_prev = offsets[0] + for o in offsets[1:]: + if framedist > o[0]: + o_prev = o + continue + x = (framedist - o_prev[0]) / (o[0] - o_prev[0]) + return time + o_prev[1] + (x * (o[1] - o_prev[1])) + return None + +#x = time_to_frame(float(sys.argv[1])) +#frame_to_distance(x) +#frame_to_time(int(sys.argv[1])) + + +table = [0] * 512 +for i in range(512): + table[i] = frame_to_time(i) + +# linear correction: distance_mm, time +samples = [ + (250., 7600.), + (500., 3000.), + (3000., 400.), + ] -x = time_to_frame(float(sys.argv[1])) -frame_to_distance(x) +offsets = sample_to_offset(samples, table) +print "#include " +print "#include " +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]))), + if (i % 8 == 7): + print +print "};"