X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Ftests%2Fbeacon_tsop%2Fmain.c;h=2edd5721ada0307edfe628505f4e33e6cda5dbb4;hp=855eb05f2b4046b32466bf415fde58332f59f6b1;hb=4e68c9b6f30c864b671b5c51affe61d7c9205465;hpb=8d6a47e9e21a9a31f4bc12d32fb3d11091a4b305 diff --git a/projects/microb2010/tests/beacon_tsop/main.c b/projects/microb2010/tests/beacon_tsop/main.c index 855eb05..2edd572 100755 --- a/projects/microb2010/tests/beacon_tsop/main.c +++ b/projects/microb2010/tests/beacon_tsop/main.c @@ -1,7 +1,7 @@ -/* +/* * Copyright Droids Corporation (2010) * Olivier Matz - * + * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation; either version 2 of the License, or @@ -38,8 +38,15 @@ #include "uart_proto.h" #include "trigo.h" #include "main.h" + +#define BOARD2010 +//#define BOARD2006 + +#ifdef BOARD2010 +#include "board2010.h" +#else #include "board2006.h" -//#include "board2010.h" +#endif /******************* TSOP */ @@ -86,12 +93,12 @@ static uint16_t tick = 0; /* 8ms, easier if it's a pow of 2 */ #define CS_PERIOD_US (8192) -#define CS_PERIOD ((uint16_t)(CS_PERIOD_US*2)) +#define CS_PERIOD ((uint16_t)(CS_PERIOD_US/4)) #define CPT_ICR_MAX (uint8_t)((1000000UL/(uint32_t)CS_PERIOD_US)) /* too slow = 1 tr/s */ #define CPT_ICR_MIN (uint8_t)((10000UL/(uint32_t)CS_PERIOD_US)) /* too fast = 100 tr/s */ /* in tr / 1000s */ -#define CS_CONSIGN (10 * 1000L) +#define CS_CONSIGN (20 * 1000L) /* 5% tolerance to validate captures, period is in */ #define TIM3_UNIT 250000000L @@ -106,6 +113,7 @@ static uint16_t tick = 0; #define LASER_OFF() do { TCCR0 = 0; } while (0) struct beacon_tsop beacon_tsop; +uint32_t cs_consign = CS_CONSIGN; static uint32_t current_motor_period; @@ -115,7 +123,7 @@ void debug_serial(void) while (1) { int16_t c; c = uart_recv_nowait(0); - if (c != -1) + if (c != -1) printf("%c", (char)(c+1)); LED1_ON(); wait_ms(500); @@ -124,7 +132,7 @@ void debug_serial(void) } #endif } - + void debug_tsop(void) { #if 0 @@ -176,7 +184,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) @@ -185,7 +193,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) @@ -211,8 +219,6 @@ static inline void decode_frame(struct frame_status *status, 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(); status->led_cpt ++; } status->len = 0; @@ -242,9 +248,9 @@ SIGNAL(SIG_TSOP_STA) { sei(); if (cur_tsop) - LED2_ON(); + LED5_ON(); else - LED2_OFF(); + LED5_OFF(); decode_frame(&static_beacon, ref_time, cur_time, cur_tsop); @@ -270,10 +276,10 @@ SIGNAL(SIG_TSOP_OPP) { running = 1; sei(); -/* if (cur_tsop) */ -/* LED2_ON(); */ -/* else */ -/* LED2_OFF(); */ + if (cur_tsop) + LED6_ON(); + else + LED6_OFF(); decode_frame(&opp_beacon, ref_time, cur_time, cur_tsop); @@ -295,48 +301,63 @@ static inline int32_t AbS(int32_t x) * (modulo 65536 obviously) */ static inline int32_t get_speed(uint8_t icr_cpt, uint16_t icr_diff) { - int32_t best_diff = 65536L; - int8_t best_cpt = -2; - int32_t diff; - int8_t i; - /* too slow (less than 1 tr/s) */ - if (icr_cpt > CPT_ICR_MAX) + if (icr_cpt >= CPT_ICR_MAX) { + current_motor_period = 250000; return 1000L; + } /* too fast (more than 100 tr/s) */ - if (icr_cpt < CPT_ICR_MIN) + if (icr_cpt <= CPT_ICR_MIN) { + current_motor_period = 2500; return 100000L; - - /* try to get the real time knowning icr_cpt and icr_diff */ - for (i=-1; i<2; i++) { - diff = ((icr_cpt+i)&3) * 16384L; - diff += (icr_diff & 0x3fff); - diff -= icr_diff; - if (diff > 32768L) - diff -= 65536L; - if (diff < -32768) - diff += 65536L; - - if (AbS(diff) < AbS(best_diff)) { - best_diff = diff; - best_cpt = icr_cpt + i; - } } - /* real time difference in timer unit (resolution 4us) */ - diff = (best_cpt * 16384L) + (icr_diff & 0x3fff); - current_motor_period = diff; /* save it in global var */ - return 250000000L/diff; + current_motor_period = icr_diff; + return TIM3_UNIT/icr_diff; +} + +static int8_t check_sta_frame(uint16_t frame, uint16_t time) +{ + int8_t beacon_id; + uint16_t cksum; + + /* ignore bad cksum */ + cksum = verify_cksum(frame); + if (cksum == 0xFFFF) + goto fail; + + beacon_id = frame & 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, cksum=%x\r\n", + beacon_id, frame, time, cksum); + } + 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; @@ -366,36 +387,21 @@ 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) - continue; - - /* if motor speed is not good, skip values */ - if (current_motor_period < MOTOR_PERIOD_MIN) + beacon_id = check_sta_frame(frame, status->ring[head].time); + if (beacon_id < 0) { + head = head_next; 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) { found |= 0x1; - data0 = (frame >> TSOP_STA_FRAME_DATA_SHIFT) & TSOP_STA_FRAME_DATA_MASK; + data0 = (frame & TSOP_STA_FRAME_DATA_MASK) >> TSOP_STA_FRAME_DATA_SHIFT; time0 = status->ring[head].time; ref_time0 = status->ring[head].ref_time; } else if (beacon_id == TSOP_STA_BEACON_ID1) { found |= 0x2; - data1 = (frame >> TSOP_STA_FRAME_DATA_SHIFT) & TSOP_STA_FRAME_DATA_MASK; + data1 = (frame & TSOP_STA_FRAME_DATA_MASK) >> TSOP_STA_FRAME_DATA_SHIFT; time1 = status->ring[head].time; ref_time1 = status->ring[head].ref_time; } @@ -442,6 +448,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; @@ -449,47 +461,63 @@ 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; + uint16_t cksum; + + /* ignore bad cksum */ + cksum = verify_cksum(frame); + if (cksum == 0xFFFF) + goto fail; + + beacon_id = frame & 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 cksum=%x d=%d\r\n", + beacon_id, frame, time, cksum, + (frame & TSOP_OPP_FRAME_DATA_MASK) >> + TSOP_OPP_FRAME_DATA_SHIFT); + } + 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; - + /* after CS, check if we have a new frame in ring */ while (status->head != status->tail) { 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; + data = (frame & TSOP_OPP_FRAME_DATA_MASK) >> TSOP_OPP_FRAME_DATA_SHIFT; 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; } @@ -509,6 +537,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); } @@ -520,9 +552,9 @@ int main(void) uint16_t diff_icr = 0; uint8_t cpt_icr = 0; uint8_t cpt = 0; - int32_t speed, out, err; + int32_t speed = 0, out, err; uint16_t tcnt3; - uint8_t x = 0; + uint8_t x = 0; /* debug display counter */ opp_beacon.frame_len = TSOP_OPP_FRAME_LEN; opp_beacon.time_long = TSOP_OPP_TIME_LONG; @@ -533,23 +565,25 @@ int main(void) static_beacon.time_short = TSOP_STA_TIME_SHORT; /* LEDS */ - LED1_DDR |= _BV(LED1_BIT); - LED2_DDR |= _BV(LED2_BIT); - LED3_DDR |= _BV(LED3_BIT); + LED_DDR_INIT(); DDRB |= 0x10; /* OC0 (laser pwm) */ /* PID init */ pid_init(&beacon_tsop.pid); - pid_set_gains(&beacon_tsop.pid, 500, 0, 0); - pid_set_maximums(&beacon_tsop.pid, 0, 20000, 4095); + pid_set_gains(&beacon_tsop.pid, 700, 10, 0); + pid_set_maximums(&beacon_tsop.pid, 0, 200000, 4095); pid_set_out_shift(&beacon_tsop.pid, 10); pid_set_derivate_filter(&beacon_tsop.pid, 4); uart_init(); +#if CMDLINE_UART == 0 fdevopen(uart0_dev_send, uart0_dev_recv); +#elif CMDLINE_UART == 1 + fdevopen(uart1_dev_send, uart1_dev_recv); +#endif rdline_init(&beacon_tsop.rdl, write_char, valid_buffer, complete_buffer); - snprintf(beacon_tsop.prompt, sizeof(beacon_tsop.prompt), "beacon > "); + snprintf(beacon_tsop.prompt, sizeof(beacon_tsop.prompt), "beacon > "); rdline_newline(&beacon_tsop.rdl, beacon_tsop.prompt); debug_tsop(); @@ -560,21 +594,25 @@ int main(void) EIMSK |= _BV(INTx_TSOP_STA) | _BV(INTx_TSOP_OPP); /* pwm for motor */ - PWM_NG_TIMER_16BITS_INIT(1, TIMER_16_MODE_PWM_10, + PWM_NG_TIMER_16BITS_INIT(1, TIMER_16_MODE_PWM_10, TIMER1_PRESCALER_DIV_1); +#ifdef BOARD2010 + PWM_NG_INIT16(&beacon_tsop.pwm_motor, 1, C, 10, 0, NULL, 0); +#else PWM_NG_INIT16(&beacon_tsop.pwm_motor, 1, A, 10, 0, NULL, 0); +#endif /* pwm for laser: * - clear on timer compare (CTC) * - Toggle OC0 on compare match * - prescaler = 1 */ TCCR0 = _BV(WGM01) | _BV(COM00) | _BV(CS00); - OCR0 = 80; /* f = 100 khz at 16 Mhz */ + OCR0 = 18; /* f ~= 420 khz at 16 Mhz */ /* configure timer 3: CLK/64 * it is used as a reference time * enable noise canceller for ICP3 */ - TCCR3B = _BV(ICNC3) | _BV(CS11) | _BV(CS10); + TCCR3B = _BV(CS11) | _BV(CS10); sei(); @@ -593,7 +631,7 @@ int main(void) sei(); ETIFR = _BV(ICF3); - //LED2_TOGGLE(); + LED2_TOGGLE(); diff_icr = (icr - prev_icr); cpt_icr = cpt; prev_icr = icr; @@ -619,26 +657,26 @@ int main(void) /* process CS... maybe we don't need to use * control_system_manager, just PID is enough */ + if (cpt == CPT_ICR_MAX) speed = 0; - else - speed = get_speed(cpt_icr, diff_icr); - + /* enabled laser when rotation speed if at least 5tr/s */ - if (speed > 5000) + if (1 || speed > 5000) /* XXX */ LASER_ON(); else LASER_OFF(); - err = CS_CONSIGN - speed; + err = cs_consign - speed; out = pid_do_filter(&beacon_tsop.pid, err); - if (x == 0 && beacon_tsop.debug_speed) - printf("%ld %ld\n", speed, out); if (out < 0) out = 0; - /* XXX */ - if (out > 2000) - out = 2000; + if (out > 3000) + out = 3000; + + if (x == 0 && beacon_tsop.debug_speed) + printf("%ld %ld %u %u / %u\r\n", + speed, out, diff_icr, cpt_icr, cpt); pwm_ng_set(&beacon_tsop.pwm_motor, out);