From: Olivier Matz Date: Sun, 28 Mar 2010 00:45:57 +0000 (+0100) Subject: beacon X-Git-Url: http://git.droids-corp.org/?a=commitdiff_plain;h=82d59983ef31ce70582b602600b4ee221f5bed03;p=aversive.git beacon --- diff --git a/projects/microb2010/microb_cmd/microbcmd.py b/projects/microb2010/microb_cmd/microbcmd.py index ff99e81..4e923c6 100755 --- a/projects/microb2010/microb_cmd/microbcmd.py +++ b/projects/microb2010/microb_cmd/microbcmd.py @@ -862,7 +862,7 @@ class Interp(cmd.Cmd): buf = f.read() addr = 0 while addr < len(buf): - #time.sleep(0.1) + time.sleep(0.1) if check_crc(self.ser, buf, addr, SPM_PAGE_SIZE) == 0: sys.stdout.write("*") sys.stdout.flush() @@ -884,12 +884,12 @@ class Interp(cmd.Cmd): filename = os.path.join(MICROB_PATH, "../mainboard/main.bin") self.bootloader(filename, 1) - def do_mechboard(self, args): - filename = os.path.join(MICROB_PATH, "../mechboard/main.bin") + def do_cobboard(self, args): + filename = os.path.join(MICROB_PATH, "../cobboard/main.bin") self.bootloader(filename, 2) - def do_sensorboard(self, args): - filename = os.path.join(MICROB_PATH, "../sensorboard/main.bin") + def do_ballboard(self, args): + filename = os.path.join(MICROB_PATH, "../ballboard/main.bin") self.bootloader(filename, 3) def do_toto(self, args): diff --git a/projects/microb2010/tests/beacon_tsop/.config b/projects/microb2010/tests/beacon_tsop/.config index f3cfa6e..adc106d 100644 --- a/projects/microb2010/tests/beacon_tsop/.config +++ b/projects/microb2010/tests/beacon_tsop/.config @@ -1,5 +1,5 @@ # -# Automatically generated by make menuconfig: don't edit +# Automatically generated make config: don't edit # # @@ -74,6 +74,10 @@ CONFIG_FORMAT_BINARY=y # # Base modules # + +# +# Enable math library in generation options to see all modules +# CONFIG_MODULE_CIRBUF=y # CONFIG_MODULE_CIRBUF_LARGE is not set # CONFIG_MODULE_FIXED_POINT is not set @@ -94,6 +98,10 @@ CONFIG_MODULE_SCHEDULER_TIMER0=y # # Communication modules # + +# +# uart needs circular buffer, mf2 client may need scheduler +# CONFIG_MODULE_UART=y # CONFIG_MODULE_UART_9BITS is not set CONFIG_MODULE_UART_CREATE_CONFIG=y @@ -178,6 +186,10 @@ CONFIG_MODULE_PARSE=y # Control system modules # # CONFIG_MODULE_CONTROL_SYSTEM_MANAGER is not set + +# +# Filters +# CONFIG_MODULE_PID=y CONFIG_MODULE_PID_CREATE_CONFIG=y # CONFIG_MODULE_RAMP is not set @@ -188,12 +200,20 @@ CONFIG_MODULE_PID_CREATE_CONFIG=y # # Radio devices # + +# +# Some radio devices require SPI to be activated +# # CONFIG_MODULE_CC2420 is not set # CONFIG_MODULE_CC2420_CREATE_CONFIG is not set # # Crypto modules # + +# +# Crypto modules depend on utils module +# # CONFIG_MODULE_AES is not set # CONFIG_MODULE_AES_CTR is not set # CONFIG_MODULE_MD5 is not set @@ -203,12 +223,20 @@ CONFIG_MODULE_PID_CREATE_CONFIG=y # # Encodings modules # + +# +# Encoding modules depend on utils module +# # CONFIG_MODULE_BASE64 is not set # CONFIG_MODULE_HAMMING is not set # # Debug modules # + +# +# Debug modules depend on utils module +# # CONFIG_MODULE_DIAGNOSTIC is not set # CONFIG_MODULE_DIAGNOSTIC_CREATE_CONFIG is not set # CONFIG_MODULE_ERROR is not set diff --git a/projects/microb2010/tests/beacon_tsop/board2006.h b/projects/microb2010/tests/beacon_tsop/board2006.h index 2e926bb..1b3f67c 100644 --- a/projects/microb2010/tests/beacon_tsop/board2006.h +++ b/projects/microb2010/tests/beacon_tsop/board2006.h @@ -86,8 +86,8 @@ #define TSOP_STA_TIME_SHORT_US (1.5 * TSOP_STA_N_PERIODS * TSOP_STA_PERIOD_US) #define TSOP_STA_TIME_LONG_US (2.5 * TSOP_STA_N_PERIODS * TSOP_STA_PERIOD_US) -#define TSOP_STA_TIME_SHORT ((uint16_t)(TSOP_STA_TIME_SHORT_US*2)) -#define TSOP_STA_TIME_LONG ((uint16_t)(TSOP_STA_TIME_LONG_US*2)) +#define TSOP_STA_TIME_SHORT ((uint16_t)(TSOP_STA_TIME_SHORT_US/4)) +#define TSOP_STA_TIME_LONG ((uint16_t)(TSOP_STA_TIME_LONG_US/4)) #define TSOP_STA_FRAME_LEN 16 #define TSOP_STA_FRAME_MASK ((1UL << TSOP_STA_FRAME_LEN) - 1) @@ -108,8 +108,8 @@ #define TSOP_OPP_TIME_SHORT_US (1.5 * TSOP_OPP_N_PERIODS * TSOP_OPP_PERIOD_US) #define TSOP_OPP_TIME_LONG_US (2.5 * TSOP_OPP_N_PERIODS * TSOP_OPP_PERIOD_US) -#define TSOP_OPP_TIME_SHORT ((uint16_t)(TSOP_OPP_TIME_SHORT_US*2)) -#define TSOP_OPP_TIME_LONG ((uint16_t)(TSOP_OPP_TIME_LONG_US*2)) +#define TSOP_OPP_TIME_SHORT ((uint16_t)(TSOP_OPP_TIME_SHORT_US/4)) +#define TSOP_OPP_TIME_LONG ((uint16_t)(TSOP_OPP_TIME_LONG_US/4)) #define TSOP_OPP_FRAME_LEN 16 #define TSOP_OPP_FRAME_MASK ((1UL << TSOP_OPP_FRAME_LEN) - 1) diff --git a/projects/microb2010/tests/beacon_tsop/board2010.h b/projects/microb2010/tests/beacon_tsop/board2010.h index 669d590..f66faa1 100644 --- a/projects/microb2010/tests/beacon_tsop/board2010.h +++ b/projects/microb2010/tests/beacon_tsop/board2010.h @@ -64,20 +64,20 @@ #define EICRx_TSOP EICRB /* EICRA is not ok, cannot do intr on any edge */ -#define INTx_TSOP_OPP INT5 -#define ISCx0_TSOP_OPP ISC50 -#define ISCx1_TSOP_OPP ISC(1 -#define SIG_TSOP_OPP SIG_INTERRUPT5 -#define TSOP_OPP_READ() (!(PINE & 0x20)) - -#define INTx_TSOP_STA INT6 -#define ISCx0_TSOP_STA ISC60 -#define ISCx1_TSOP_STA ISC61 -#define SIG_TSOP_STA SIG_INTERRUPT6 -#define TSOP_STA_READ() (!(PINE & 0x40)) +#define INTx_TSOP_OPP INT6 +#define ISCx0_TSOP_OPP ISC60 +#define ISCx1_TSOP_OPP ISC61 +#define SIG_TSOP_OPP SIG_INTERRUPT6 +#define TSOP_OPP_READ() (!(PINE & 0x40)) + +#define INTx_TSOP_STA INT5 +#define ISCx0_TSOP_STA ISC50 +#define ISCx1_TSOP_STA ISC51 +#define SIG_TSOP_STA SIG_INTERRUPT5 +#define TSOP_STA_READ() (!(PINE & 0x20)) #define TSOP_FREQ_455_MHZ 0.455 -#define N_PERIODS_455 10. +#define N_PERIODS_455 15. #define TSOP_FREQ_38_MHZ 0.038 #define N_PERIODS_38 15. #define TSOP_FREQ_30_MHZ 0.030 @@ -87,14 +87,14 @@ /* TSOP STATIC */ -#define TSOP_STA_PERIOD_US (1./TSOP_FREQ_38_MHZ) -#define TSOP_STA_N_PERIODS (N_PERIODS_38) +#define TSOP_STA_PERIOD_US (1./TSOP_FREQ_455_MHZ) +#define TSOP_STA_N_PERIODS (N_PERIODS_455) #define TSOP_STA_TIME_SHORT_US (1.5 * TSOP_STA_N_PERIODS * TSOP_STA_PERIOD_US) #define TSOP_STA_TIME_LONG_US (2.5 * TSOP_STA_N_PERIODS * TSOP_STA_PERIOD_US) -#define TSOP_STA_TIME_SHORT ((uint16_t)(TSOP_STA_TIME_SHORT_US*2)) -#define TSOP_STA_TIME_LONG ((uint16_t)(TSOP_STA_TIME_LONG_US*2)) +#define TSOP_STA_TIME_SHORT ((uint16_t)(TSOP_STA_TIME_SHORT_US/4)) +#define TSOP_STA_TIME_LONG ((uint16_t)(TSOP_STA_TIME_LONG_US/4)) #define TSOP_STA_FRAME_LEN 16 #define TSOP_STA_FRAME_MASK ((1UL << TSOP_STA_FRAME_LEN) - 1) @@ -115,8 +115,8 @@ #define TSOP_OPP_TIME_SHORT_US (1.5 * TSOP_OPP_N_PERIODS * TSOP_OPP_PERIOD_US) #define TSOP_OPP_TIME_LONG_US (2.5 * TSOP_OPP_N_PERIODS * TSOP_OPP_PERIOD_US) -#define TSOP_OPP_TIME_SHORT ((uint16_t)(TSOP_OPP_TIME_SHORT_US*2)) -#define TSOP_OPP_TIME_LONG ((uint16_t)(TSOP_OPP_TIME_LONG_US*2)) +#define TSOP_OPP_TIME_SHORT ((uint16_t)(TSOP_OPP_TIME_SHORT_US/4)) +#define TSOP_OPP_TIME_LONG ((uint16_t)(TSOP_OPP_TIME_LONG_US/4)) #define TSOP_OPP_FRAME_LEN 16 #define TSOP_OPP_FRAME_MASK ((1UL << TSOP_OPP_FRAME_LEN) - 1) diff --git a/projects/microb2010/tests/beacon_tsop/main.c b/projects/microb2010/tests/beacon_tsop/main.c index b1d0db7..7d819bb 100755 --- a/projects/microb2010/tests/beacon_tsop/main.c +++ b/projects/microb2010/tests/beacon_tsop/main.c @@ -177,6 +177,7 @@ static inline void decode_frame(struct frame_status *status, /* first rising edge */ if (status->len == 0 && cur_tsop && diff_time > status->time_long) { + LED6_ON(); status->len = 1; status->val = 1; status->frame = 0; @@ -184,7 +185,7 @@ static inline void decode_frame(struct frame_status *status, status->ref_time = ref_time; status->mask = 1; } - /* any short edge */ + /* any short pulse */ else if (status->len != 0 && diff_time < status->time_short) { if (status->len & 1) { if (status->val) @@ -193,7 +194,7 @@ static inline void decode_frame(struct frame_status *status, } status->len ++; } - /* any long edge */ + /* any long pulse */ else if (status->len != 0 && diff_time < status->time_long) { status->val = !status->val; if (status->val) @@ -214,13 +215,14 @@ static inline void decode_frame(struct frame_status *status, frame_mask = (1 << status->frame_len) - 1; if (tail_next != status->head) { + LED5_ON(); status->ring[status->tail].frame = (status->frame & frame_mask); status->ring[status->tail].ref_time = status->ref_time; status->ring[status->tail].time = status->start_time; status->ring[status->tail].tick = tick; status->tail = tail_next; - if ((status->led_cpt & 0x7) == 0) - LED3_TOGGLE(); +/* if ((status->led_cpt & 0x7) == 0) */ +/* LED3_TOGGLE(); */ status->led_cpt ++; } status->len = 0; @@ -228,6 +230,10 @@ static inline void decode_frame(struct frame_status *status, status->prev_time = cur_time; status->prev_tsop = cur_tsop; + LED3_OFF(); + LED4_OFF(); + LED5_OFF(); + LED6_OFF(); } /* decode frame */ @@ -249,10 +255,10 @@ SIGNAL(SIG_TSOP_STA) { running = 1; sei(); - if (cur_tsop) - LED5_ON(); - else - LED5_OFF(); +/* if (cur_tsop) */ +/* LED5_ON(); */ +/* else */ +/* LED5_OFF(); */ decode_frame(&static_beacon, ref_time, cur_time, cur_tsop); @@ -278,12 +284,12 @@ SIGNAL(SIG_TSOP_OPP) { running = 1; sei(); - if (cur_tsop) - LED6_ON(); - else - LED6_OFF(); +/* if (cur_tsop) */ +/* LED6_ON(); */ +/* else */ +/* LED6_OFF(); */ - decode_frame(&opp_beacon, ref_time, cur_time, cur_tsop); + //decode_frame(&opp_beacon, ref_time, cur_time, cur_tsop); running = 0; } @@ -353,13 +359,45 @@ static inline int32_t get_speed(uint8_t icr_cpt, uint16_t icr_diff) return TIM3_UNIT/icr_diff; } +static int8_t check_sta_frame(uint16_t frame, uint16_t time) +{ + int8_t beacon_id; + + /* ignore bad cksum */ + if (verify_cksum(frame) == 0xFFFF) + goto fail; + + beacon_id = (frame >> TSOP_STA_BEACON_ID_SHIFT) & TSOP_STA_BEACON_ID_MASK; + + if (beacon_id != TSOP_STA_BEACON_ID0 && + beacon_id != TSOP_STA_BEACON_ID1) + goto fail; + + /* if motor speed is not good, skip values */ + if (current_motor_period < MOTOR_PERIOD_MIN) + goto fail; + if (current_motor_period > MOTOR_PERIOD_MAX) + goto fail; + + return beacon_id; + + fail: + /* display if needed */ + if (beacon_tsop.debug_frame) { + printf("STA ID=%d frame=%x time=%d\r\n", + beacon_id, frame, time); + } + return -1; +} + + /* process the received frame ring */ static void process_sta_ring(struct frame_status *status) { uint8_t head, head_next; uint16_t frame, frametick; uint8_t found = 0; - uint8_t beacon_id; + int8_t beacon_id; /* beacon 0 */ uint16_t data0, time0, ref_time0; @@ -389,25 +427,10 @@ static void process_sta_ring(struct frame_status *status) head_next = (head+1) & FRAME_RING_MASK; frame = status->ring[head].frame; - /* ignore bad cksum */ - if (verify_cksum(frame) == 0xFFFF) - continue; - - beacon_id = (frame >> TSOP_STA_BEACON_ID_SHIFT) & TSOP_STA_BEACON_ID_MASK; - if (beacon_id != TSOP_STA_BEACON_ID0 && - beacon_id != TSOP_STA_BEACON_ID1) + beacon_id = check_sta_frame(frame, status->ring[head].time); + if (beacon_id < 0) { + head = head_next; continue; - - /* if motor speed is not good, skip values */ - if (current_motor_period < MOTOR_PERIOD_MIN) - continue; - if (current_motor_period > MOTOR_PERIOD_MAX) - continue; - - /* display if needed */ - if (beacon_tsop.debug_frame) { - printf("STA ID=%d time=%d\r\n", - beacon_id, status->ring[head].time); } if (beacon_id == TSOP_STA_BEACON_ID0) { @@ -465,6 +488,12 @@ static void process_sta_ring(struct frame_status *status) if (angle0 > M_PI) angle0 -= M_PI; + /* display if needed */ + if (beacon_tsop.debug_frame) { + printf("STA ID=%d dist0=%2.2f angle0=%2.2f dist1=%2.2f angle1=%2.2f\r\n", + beacon_id, dist0, angle0 * 180. / M_PI, dist1, angle1 * 180. / M_PI); + } + if (ad_to_posxya(&pos, &a, 0, &beacon0, &beacon1, angle0, dist0, angle1, dist1) < 0) return; @@ -472,13 +501,40 @@ static void process_sta_ring(struct frame_status *status) xmit_static((uint16_t)pos.x, (uint16_t)pos.y, (uint16_t)a); } +static int8_t check_opp_frame(uint16_t frame, uint16_t time) +{ + int8_t beacon_id = -1; + + /* ignore bad cksum */ + if (verify_cksum(frame) == 0xFFFF) + goto fail; + + beacon_id = (frame >> TSOP_OPP_BEACON_ID_SHIFT) & TSOP_OPP_BEACON_ID_MASK; + if (beacon_id != TSOP_OPP_BEACON_ID) + goto fail; + + /* if motor speed is not good, skip values */ + if (current_motor_period < MOTOR_PERIOD_MIN) + goto fail; + if (current_motor_period > MOTOR_PERIOD_MAX) + goto fail; + + return beacon_id; + fail: + /* display if needed */ + if (beacon_tsop.debug_frame) { + printf("OPP ID=%d frame=%x time=%d\r\n", + beacon_id, frame, time); + } + return -1; +} + /* process the received frame ring */ static void process_opp_ring(struct frame_status *status) { uint8_t head_next; uint16_t frame; uint8_t found = 0; - uint8_t beacon_id; uint16_t data, time, ref_time; double angle; double dist; @@ -488,31 +544,16 @@ static void process_opp_ring(struct frame_status *status) head_next = (status->head+1) & FRAME_RING_MASK; frame = status->ring[status->head].frame; - /* ignore bad cksum */ - if (verify_cksum(frame) == 0xFFFF) - continue; - - beacon_id = (frame >> TSOP_OPP_BEACON_ID_SHIFT) & TSOP_OPP_BEACON_ID_MASK; - if (beacon_id != TSOP_OPP_BEACON_ID) - continue; - - /* if motor speed is not good, skip values */ - if (current_motor_period < MOTOR_PERIOD_MIN) - continue; - if (current_motor_period > MOTOR_PERIOD_MAX) + if (check_opp_frame(frame, status->ring[status->head].time) < 0) { + status->head = head_next; continue; + } found = 1; data = (frame >> TSOP_OPP_FRAME_DATA_SHIFT) & TSOP_OPP_FRAME_DATA_MASK; time = status->ring[status->head].time; ref_time = status->ring[status->head].ref_time; - /* display if needed */ - if (beacon_tsop.debug_frame) { - printf("OPP ID=%d data=%d time=%d\r\n", - beacon_id, data, - status->ring[status->head].time); - } status->head = head_next; } @@ -532,6 +573,10 @@ static void process_opp_ring(struct frame_status *status) return; /* fail */ angle *= 3600; /* angle in 1/10 deg */ + /* display if needed */ + if (beacon_tsop.debug_frame) { + printf("OPP dist=%2.2f angle=%2.2f\r\n", dist, angle/10); + } xmit_opp((uint16_t)dist, (uint16_t)angle); } @@ -653,7 +698,7 @@ int main(void) speed = 0; /* enabled laser when rotation speed if at least 5tr/s */ - if (speed > 5000) + if (1 || speed > 5000) /* XXX */ LASER_ON(); else LASER_OFF(); @@ -662,12 +707,11 @@ int main(void) out = pid_do_filter(&beacon_tsop.pid, err); if (out < 0) out = 0; - /* XXX */ if (out > 3000) out = 3000; if (x == 0 && beacon_tsop.debug_speed) - printf("%ld %ld %u %u / %u\n", + printf("%ld %ld %u %u / %u\r\n", speed, out, diff_icr, cpt_icr, cpt); pwm_ng_set(&beacon_tsop.pwm_motor, out); diff --git a/projects/microb2010/tests/static_beacon/.config b/projects/microb2010/tests/static_beacon/.config index 8c61239..1d00663 100644 --- a/projects/microb2010/tests/static_beacon/.config +++ b/projects/microb2010/tests/static_beacon/.config @@ -1,5 +1,5 @@ # -# Automatically generated by make menuconfig: don't edit +# Automatically generated make config: don't edit # # @@ -74,6 +74,10 @@ CONFIG_FORMAT_BINARY=y # # Base modules # + +# +# Enable math library in generation options to see all modules +# CONFIG_MODULE_CIRBUF=y # CONFIG_MODULE_CIRBUF_LARGE is not set # CONFIG_MODULE_FIXED_POINT is not set @@ -94,6 +98,10 @@ CONFIG_MODULE_SCHEDULER_TIMER0=y # # Communication modules # + +# +# uart needs circular buffer, mf2 client may need scheduler +# CONFIG_MODULE_UART=y # CONFIG_MODULE_UART_9BITS is not set CONFIG_MODULE_UART_CREATE_CONFIG=y @@ -178,6 +186,10 @@ CONFIG_MODULE_UART_CREATE_CONFIG=y # Control system modules # # CONFIG_MODULE_CONTROL_SYSTEM_MANAGER is not set + +# +# Filters +# # CONFIG_MODULE_PID is not set # CONFIG_MODULE_PID_CREATE_CONFIG is not set # CONFIG_MODULE_RAMP is not set @@ -188,12 +200,20 @@ CONFIG_MODULE_UART_CREATE_CONFIG=y # # Radio devices # + +# +# Some radio devices require SPI to be activated +# # CONFIG_MODULE_CC2420 is not set # CONFIG_MODULE_CC2420_CREATE_CONFIG is not set # # Crypto modules # + +# +# Crypto modules depend on utils module +# # CONFIG_MODULE_AES is not set # CONFIG_MODULE_AES_CTR is not set # CONFIG_MODULE_MD5 is not set @@ -203,12 +223,20 @@ CONFIG_MODULE_UART_CREATE_CONFIG=y # # Encodings modules # + +# +# Encoding modules depend on utils module +# # CONFIG_MODULE_BASE64 is not set # CONFIG_MODULE_HAMMING is not set # # Debug modules # + +# +# Debug modules depend on utils module +# # CONFIG_MODULE_DIAGNOSTIC is not set # CONFIG_MODULE_DIAGNOSTIC_CREATE_CONFIG is not set # CONFIG_MODULE_ERROR is not set diff --git a/projects/microb2010/tests/static_beacon/cksum.py b/projects/microb2010/tests/static_beacon/cksum.py index 38d6634..6c27b4c 100644 --- a/projects/microb2010/tests/static_beacon/cksum.py +++ b/projects/microb2010/tests/static_beacon/cksum.py @@ -37,7 +37,6 @@ def verify_cksum(val): return 0xffff # wrong value - err = 0 total = 0 for i in range(200): diff --git a/projects/microb2010/tests/static_beacon/coding.py b/projects/microb2010/tests/static_beacon/coding.py index a41c692..9ef4c5c 100644 --- a/projects/microb2010/tests/static_beacon/coding.py +++ b/projects/microb2010/tests/static_beacon/coding.py @@ -1,13 +1,14 @@ #!/usr/bin/python import sys, math +import matplotlib.pyplot as plt -if 1: - RPS = 10. - TIMER_FREQ = 2000000. -else: - RPS = 40. - TIMER_FREQ = 16000000. + +#RPS = 10. +RPS = 20. +#RPS = 40. +#TIMER_FREQ = 2000000. +TIMER_FREQ = 16000000. LASER_RADIUS = 25. # mm @@ -17,85 +18,122 @@ NBITS = 9 STEPS = (1 << 9) k = math.pow(MAX/MIN, 1./STEPS) -def mm_to_framedist(mm): +def mm_to_frame(mm): d = mm d -= MIN d /= (MAX-MIN) d *= 512 return d -def framedist_to_mm(d): +def frame_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): +def us_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 = int(mm_to_framedist(d)) + frame = int(mm_to_frame(d)) return frame -# frame is integer 9 bits, result is laserdiff time -def frame_to_time(frame): - d = framedist_to_mm(frame) +# frame is integer 9 bits, result is laserdiff time in us +def frame_to_us(frame): + d = frame_to_mm(frame) a = 2 * math.asin(LASER_RADIUS/d) - t = (a * (TIMER_FREQ/RPS)) / (2. * math.pi) + t = (a * (1000000./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) +# theorical: laser timediff to robot distance +def us_to_mm(us): + return frame_to_mm(us_to_frame(us)) + +# theorical: robot distance to laserdiff +def mm_to_us(mm): + return frame_to_us(mm_to_frame(mm)) + +def time_us_to_tick(us): + return (us / 1000000.) * TIMER_FREQ + +def time_tick_to_us(t): + return (t * 1000000.) / TIMER_FREQ + -# linear correction: distance_mm, time +################## + +# linear correction: distance_mm, time_us +# must be ordered samples = [ - (250., 7600.), - (500., 3000.), - (3000., 400.), + (250., 2201.), + (450., 701.), + (1200., 231.), + (3000., 50.), ] -offsets = sample_to_offset(samples, table) +dist_mm = map(frame_to_mm, range(512)) + +# theorical curve +theorical = [0] * 512 +for i in range(512): + theorical[i] = frame_to_us(i) + +# find offset and update theorical curve +off = samples[-1][1] - mm_to_us(3000.) +print "offset=%f"%(off) +theo_off = [0] * 512 +for i in range(512): + mm = frame_to_mm(i) + theo_off[i] = mm_to_us(mm) + off + +final = [0] * 512 +for i in range(512): + mm = frame_to_mm(i) + + # find between which samples we are + smp = 0 + while smp < (len(samples) - 2): + if samples[smp+1][0] >= mm: + break + smp += 1 + + mm_start = us_to_mm(samples[smp][1] - off) + mm_end = us_to_mm(samples[smp+1][1] - off) + + # interpolation + ratio = (mm - samples[smp][0]) / (samples[smp+1][0] - samples[smp][0]) + mm_new = mm_start + ratio * (mm_end - mm_start) + + final[i] = mm_to_us(mm_new) + off + +sample_idx = 0 +while sample_idx < len(samples): + print samples[sample_idx][1], + print us_to_mm(samples[sample_idx][1] - off), + print mm_to_us(samples[sample_idx][0]) + sample_idx += 1 + + +plt.plot( +# dist_mm, theorical, "r-", +# dist_mm, theo_off, "b-", + dist_mm, final, "g-", + map(lambda x:x[0], samples), map(lambda x:x[1], samples), "g^", + ) +plt.show() + + 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]))), +# print "%d,"%(int(linear_interpolation(offsets, i, table[i]))), + print "%d,"%(int(table[i])), if (i % 8 == 7): print print "};" diff --git a/projects/microb2010/tests/static_beacon/static_beacon.c b/projects/microb2010/tests/static_beacon/static_beacon.c index caf0934..ef6e1aa 100755 --- a/projects/microb2010/tests/static_beacon/static_beacon.c +++ b/projects/microb2010/tests/static_beacon/static_beacon.c @@ -41,11 +41,12 @@ //#define NO_MODULATION #define WAIT_LASER -//#define MODUL_455KHZ +#define MODUL_455KHZ //#define MODUL_56KHZ -#define MODUL_38KHZ +//#define MODUL_38KHZ //#define SPEED_40RPS -#define SPEED_10RPS +#define SPEED_20RPS +//#define SPEED_10RPS /* beacon identifier: must be odd, 3 bits */ #define BEACON_ID 0x1 @@ -57,7 +58,7 @@ #define FRAME_DATA_SHIFT 3 #if (defined MODUL_455KHZ) -#define N_PERIODS 10 +#define N_PERIODS 15 #define N_CYCLES_0 17 #define N_CYCLES_1 17 #elif (defined MODUL_56KHZ) @@ -108,7 +109,7 @@ /* FRAME must be odd */ /* #define FRAME 0x0B /\* in little endian 1-1-0-1 *\/ */ /* #define FRAME_LEN 4 */ -#define FRAME 0xAA5B /* in little endian */ +#define FRAME 0x4A5B /* in little endian */ #define FRAME_LEN 16 /* pin returns !0 when nothing, and 0 when laser is on photodiode */ @@ -122,6 +123,11 @@ #define MAX_INTER_TIME ((uint16_t)(8000*2)) /* t=8ms dist=10cm */ #define IR_DELAY ((uint16_t)(8000*2)) #define INTER_LASER_TIME 30 /* in ms */ +#elif (defined SPEED_20RPS) +#define MIN_INTER_TIME ((uint16_t)(40*16)) /* t~=80us dist=350cm */ +#define MAX_INTER_TIME ((uint16_t)(2000*16)) /* t=2ms dist=? >10cm */ +#define IR_DELAY ((uint16_t)(2000*16)) +#define INTER_LASER_TIME 10 /* in ms */ #elif (defined SPEED_40RPS) #define MIN_INTER_TIME ((uint16_t)(40*16)) /* t~=40us dist=350cm */ #define MAX_INTER_TIME ((uint16_t)(2000*16)) /* t=2ms dist=10cm */ @@ -248,7 +254,7 @@ static inline int8_t wait_laser(uint16_t *when, uint16_t *laserdiff) uint16_t time1, time2; uint16_t diff; -#ifdef SPEED_40RPS +#if (defined SPEED_40RPS) || (defined SPEED_20RPS) /* set timer to 16Mhz, we will use ICP */ TCCR1A = 0; TCCR1B = _BV(CS10); @@ -358,7 +364,7 @@ int main(void) while (1); #endif -#if 1 +#if 0 /* test freq ~ 400khz */ ICR1 = 38; OCR1A = 19;