From: Olivier MATZ Date: Sun, 7 Mar 2010 19:57:58 +0000 (+0100) Subject: merge X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=commitdiff_plain;h=844fa3427676e08d7e9f92279788def635377714;hp=3e23629120eb6feb1f7ee8fcdc59ee6e680d76a5 merge --- diff --git a/projects/microb2010/tests/beacon_tsop/main.c b/projects/microb2010/tests/beacon_tsop/main.c index ce287cc..939719f 100755 --- a/projects/microb2010/tests/beacon_tsop/main.c +++ b/projects/microb2010/tests/beacon_tsop/main.c @@ -32,6 +32,12 @@ #include "cmdline.h" #include "main.h" +/* beacon identifier: must be odd, 3 bits */ +#define BEACON_ID_MASK 0x7 +#define BEACON_ID_SHIFT 0 +#define FRAME_DATA_MASK 0xFFF8 +#define FRAME_DATA_SHIFT 3 + /******************* TSOP */ #define EICRx_TSOP EICRB /* EICRA is not ok, cannot do intr on any edge */ @@ -40,18 +46,27 @@ #define ISCx0_TSOP ISC60 #define ISCx1_TSOP ISC61 #define SIG_TSOP SIG_INTERRUPT6 -#define TSOP_READ() (PINE & 0x40) +#define TSOP_READ() (!(PINE & 0x40)) #else #define INTx_TSOP INT4 #define ISCx0_TSOP ISC40 #define ISCx1_TSOP ISC41 #define SIG_TSOP SIG_INTERRUPT4 -#define TSOP_READ() (PINE & 0x10) +#define TSOP_READ() (!(PINE & 0x10)) #endif +//#define MODUL_455KHZ +#define MODUL_38KHZ + +#if (defined MODUL_455KHZ) #define TSOP_FREQ_MHZ 0.455 -#define TSOP_PERIOD_US (1./TSOP_FREQ_MHZ) #define N_PERIODS 10. +#else +#define TSOP_FREQ_MHZ 0.038 +#define N_PERIODS 15. +#endif + +#define TSOP_PERIOD_US (1./TSOP_FREQ_MHZ) #define TSOP_TIME_SHORT_US (1.5 * N_PERIODS * TSOP_PERIOD_US) #define TSOP_TIME_LONG_US (2.5 * N_PERIODS * TSOP_PERIOD_US) @@ -60,6 +75,7 @@ #define TSOP_TIME_LONG ((uint16_t)(TSOP_TIME_LONG_US*2)) #define FRAME_LEN 16 +#define FRAME_MASK ((1UL << FRAME_LEN) - 1) /* frame */ static uint16_t start_angle_time; @@ -149,8 +165,13 @@ SIGNAL(SIG_TSOP) { cur_tsop = TSOP_READ(); diff_time = cur_time - prev_time; + if (cur_tsop) + LED2_ON(); + else + LED2_OFF(); + /* first rising edge */ - if (cur_tsop && diff_time > TSOP_TIME_LONG) { + if (len == 0 && cur_tsop && diff_time > TSOP_TIME_LONG) { len = 1; val = 1; frame = 0; @@ -158,7 +179,7 @@ SIGNAL(SIG_TSOP) { mask = 1; } /* any short edge */ - else if (diff_time < TSOP_TIME_SHORT) { + else if (len != 0 && diff_time < TSOP_TIME_SHORT) { if (len & 1) { if (val) frame |= mask; @@ -167,23 +188,27 @@ SIGNAL(SIG_TSOP) { len ++; } /* any long edge */ - else if (diff_time < TSOP_TIME_LONG) { + else if (len != 0 && diff_time < TSOP_TIME_LONG) { val = !val; if (val) frame |= mask; mask <<= 1; len += 2; } + /* error case, reset */ + else { + len = 0; + } /* end of frame */ if (len == FRAME_LEN*2) { uint8_t tail_next = (frame_ring_tail+1) & FRAME_RING_MASK; if (tail_next != frame_ring_head) { - frame_ring[frame_ring_tail].frame = frame; + frame_ring[frame_ring_tail].frame = (frame & FRAME_MASK); frame_ring[frame_ring_tail].time = start_angle_time; frame_ring_tail = tail_next; } - if (led_cpt & 0x8) + if ((led_cpt & 0x7) == 0) LED3_TOGGLE(); led_cpt ++; } @@ -314,7 +339,7 @@ int main(void) sei(); ETIFR = _BV(ICF3); - LED2_TOGGLE(); + //LED2_TOGGLE(); diff_icr = (icr - prev_icr); cpt_icr = cpt; prev_icr = icr; @@ -373,10 +398,21 @@ int main(void) /* after CS, check if we have a new frame in ring */ if (frame_ring_head != frame_ring_tail) { uint8_t head_next; + uint32_t frame; head_next = (frame_ring_head+1) & FRAME_RING_MASK; - if (beacon_tsop.debug_frame) - printf("%x %d\n", frame_ring[frame_ring_head].frame, + frame = frame_ring[frame_ring_head].frame; + + /* display if needed */ + if (beacon_tsop.debug_frame) { + uint8_t beacon_id; + uint16_t data; + + beacon_id = (frame >> BEACON_ID_SHIFT) & BEACON_ID_MASK; + data = (frame >> FRAME_DATA_SHIFT) & FRAME_DATA_MASK; + printf("ID=%d data=%d time=%d\r\n", + beacon_id, data, frame_ring[frame_ring_head].time); + } frame_ring_head = head_next; } diff --git a/projects/microb2010/tests/static_beacon/static_beacon.c b/projects/microb2010/tests/static_beacon/static_beacon.c index c5572b3..08b4251 100755 --- a/projects/microb2010/tests/static_beacon/static_beacon.c +++ b/projects/microb2010/tests/static_beacon/static_beacon.c @@ -1,7 +1,7 @@ -/* +/* * Copyright Droids Corporation (2009) * 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 @@ -33,12 +33,42 @@ /* * hfuse: RSTDISBL=1 WTDON=1 SPIEN=0 CKOPT=0 EESAVE=1 BOOTSZ1=0 BOOTSZ0=0 BOOTRST=1 - * lfuse: BODLEVEL=1 BODEN=1 SUT1=1 SUT0=1 CKSEL3=1 CKSEL2=1 CKSEL1=1 CKSEL0=1 + * lfuse: BODLEVEL=1 BODEN=1 SUT1=1 SUT0=1 CKSEL3=1 CKSEL2=1 CKSEL1=1 CKSEL0=1 */ +//#define NO_MODULATION +#define WAIT_LASER +//#define MODUL_455KHZ +//#define MODUL_56KHZ +#define MODUL_38KHZ +//#define SPEED_40RPS +#define SPEED_10RPS + +/* beacon identifier: must be odd, 3 bits */ +#define BEACON_ID 0x1 +#define BEACON_ID_MASK 0x7 +#define BEACON_ID_SHIFT 0 + +/* */ +#define FRAME_DATA_MASK 0xFFF8 +#define FRAME_DATA_SHIFT 3 + +#if (defined MODUL_455KHZ) #define N_PERIODS 10 #define N_CYCLES_0 17 #define N_CYCLES_1 17 +#elif (defined MODUL_56KHZ) +#define N_PERIODS 15 +#define N_CYCLES_0 143 +#define N_CYCLES_1 143 +#elif (defined MODUL_38KHZ) +#define N_PERIODS 15 +#define N_CYCLES_0 210 +#define N_CYCLES_1 210 +#else +#error "no freq defined" +#endif + #define N_CYCLES_PERIOD (N_CYCLES_0 + N_CYCLES_1) #define LED_PORT PORTD @@ -48,11 +78,11 @@ #define LED3_BIT 7 #define LED_TOGGLE(port, bit) do { \ - if (port & _BV(bit)) \ - port &= ~_BV(bit); \ - else \ - port |= _BV(bit); \ - } while(0) + if (port & _BV(bit)) \ + port &= ~_BV(bit); \ + else \ + port |= _BV(bit); \ + } while(0) #define LED1_ON() sbi(LED_PORT, LED1_BIT) #define LED1_OFF() cbi(LED_PORT, LED1_BIT) @@ -74,58 +104,53 @@ #define FRAME 0xAA5B /* in little endian */ #define FRAME_LEN 16 -/* pin returns 1 when nothing, and 0 when laser is on photodiode */ -#define PHOTO_PIN PINC -#define PHOTO1_BIT 0 -#define PHOTO2_BIT 1 -#define READ_PHOTOS() (PHOTO_PIN & (_BV(PHOTO1_BIT) | _BV(PHOTO2_BIT))) -#define READ_PHOTO1() (PHOTO_PIN & _BV(PHOTO1_BIT)) -#define READ_PHOTO2() (PHOTO_PIN & _BV(PHOTO2_BIT)) - -#define PHOTOS_ALL_OFF (_BV(PHOTO1_BIT) | _BV(PHOTO2_BIT)) -#define PHOTOS_ALL_ON (0) -#define PHOTOS_1ON_2OFF (_BV(PHOTO2_BIT)) -#define PHOTOS_1OFF_2ON (_BV(PHOTO1_BIT)) - - -/* in cycles/64 (unit is 4 us at 16Mhz) */ -#define MAX_PHOTO_TIME ((uint8_t)25) /* t=100us len=5mm rps=40Hz dist=20cm */ - -#define MIN_INTER_TIME ((uint8_t)12) /* t=50us len=50mm rps=40Hz dist=350cm */ -#define MAX_INTER_TIME ((uint8_t)250) /* t=1000us len=50mm rps=40Hz dist=20cm */ - -/* in ms */ -#define INTER_LASER_TIME 10 - -#define NO_MODULATION -//#define WAIT_LASER +/* pin returns !0 when nothing, and 0 when laser is on photodiode */ +#define PHOTO_PIN PINB +#define PHOTO_BIT 0 +#define READ_PHOTO() (!!(PHOTO_PIN & (_BV(PHOTO_BIT)))) + +/* IR_DELAY **must** be < 32768 */ +#if (defined SPEED_10RPS) +#define MIN_INTER_TIME ((uint16_t)(160*2)) /* t~=160us dist=350cm */ +#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_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 */ +#define IR_DELAY ((uint16_t)(2000*16)) +#define INTER_LASER_TIME 10 /* in ms */ +#else +#error "speed not defined" +#endif /* basic functions to transmit on IR */ static inline void xmit_0(void) { - uint8_t t = ((N_CYCLES_PERIOD * N_PERIODS) / 3); + uint16_t t = ((N_CYCLES_PERIOD * N_PERIODS) / 4); #ifdef NO_MODULATION cbi(IR_PORT, IR_BIT); #else TCCR1B = 0; TCCR1A = 0; #endif - _delay_loop_1(t); /* 3 cycles per loop */ + _delay_loop_2(t); /* 4 cycles per loop */ } static inline void xmit_1(void) { - uint8_t t = ((N_CYCLES_PERIOD * N_PERIODS) / 3); + uint16_t t = ((N_CYCLES_PERIOD * N_PERIODS) / 4); #ifdef NO_MODULATION sbi(IR_PORT, IR_BIT); #else TCCR1B = _BV(WGM13) | _BV(WGM12); TCNT1 = N_CYCLES_PERIOD-1; TCCR1A = _BV(COM1A1) | _BV(WGM11); + ICR1 = N_CYCLES_PERIOD; TCCR1B = _BV(WGM13) | _BV(WGM12) | _BV(CS10); #endif - _delay_loop_1(t); /* 3 cycles per loop */ + _delay_loop_2(t); /* 4 cycles per loop */ } /* transmit in manchester code */ @@ -160,64 +185,74 @@ static void xmit_bits(uint32_t frame, uint8_t nbit) xmit_0(); } -/* */ -static inline int8_t wait_laser(void) +#ifdef WAIT_LASER +/* get the frame from laser time difference */ +static uint32_t get_frame(uint16_t laserdiff) +{ + uint32_t frame = 0; + frame |= ((uint32_t)BEACON_ID << BEACON_ID_SHIFT); + frame |= ((uint32_t)(laserdiff & FRAME_DATA_MASK) << FRAME_DATA_SHIFT); + return frame; +} + +/* Wait 2 consecutive rising edges on photodiode. Return 0 on success, + * in this case, the 'when' pointed area is assigned to the time when + * IR signal should be sent. The 'laserdiff' pointer is the time + * between the 2 lasers, in timer unit. */ +static inline int8_t wait_laser(uint16_t *when, uint16_t *laserdiff) { - uint8_t photos; - uint8_t time; - uint8_t diff; + uint16_t time1, time2; + uint16_t diff; + +#ifdef SPEED_40RPS + /* set timer to 16Mhz, we will use ICP */ + TCCR1A = 0; + TCCR1B = _BV(CS10); +#else /* 10 RPS */ + /* set timer to 2Mhz, we will use ICP */ + TCCR1A = 0; + TCCR1B = _BV(CS11); +#endif - /* wait until all is off */ - while (READ_PHOTOS() != PHOTOS_ALL_OFF); + /* wait until all is off (inverted logic) */ + while (READ_PHOTO() == 0); + TIFR = _BV(ICF1); - /* wait an event, it must be photo1 on */ - while ((photos = READ_PHOTOS()) == PHOTOS_ALL_OFF); + /* wait falling edge */ + while ((TIFR & _BV(ICF1)) == 0); + time1 = ICR1; - if (photos != PHOTOS_1ON_2OFF) - return -1; - time = TCNT0; LED1_ON(); - /* wait an event, it must be photo1 off */ - while ((photos = READ_PHOTOS()) == PHOTOS_1ON_2OFF) { - diff = TCNT0 - time; - if (diff > MAX_PHOTO_TIME) - return -1; - } - if (photos != PHOTOS_ALL_OFF) - return -1; - //time = TCNT0; + /* wait a bit to avoid oscillations */ + while ((uint16_t)(TCNT1-time1) < MIN_INTER_TIME); + TIFR = _BV(ICF1); - /* wait an event, it must be photo2 on */ - while ((photos = READ_PHOTOS()) == PHOTOS_ALL_OFF) { - diff = TCNT0 - time; + /* wait next falling edge + timeout */ + while ((TIFR & _BV(ICF1)) == 0) { + diff = TCNT1 - time1; if (diff > MAX_INTER_TIME) return -1; } + LED2_ON(); - - diff = TCNT0 - time; - if (diff < MIN_INTER_TIME) - return -1; - if (photos != PHOTOS_1OFF_2ON) - return -1; -#if 0 - time = TCNT0; + /* get time of 2nd laser */ + time2 = ICR1; + TIFR = _BV(ICF1); - /* wait an event, it must be photo2 off */ - while ((photos = READ_PHOTOS()) == PHOTOS_1OFF_2ON) { - diff = TCNT0 - time; - if (diff > MAX_PHOTO_TIME) - return -1; - } - if (photos != PHOTOS_ALL_OFF) + /* process time difference */ + diff = time2 - time1; + if (diff < MIN_INTER_TIME) return -1; -#endif + + *when = time1 + (diff/2) + IR_DELAY; + *laserdiff = diff; /* laser ok */ return 0; } +#endif /* */ @@ -225,9 +260,11 @@ int main(void) { /* must be odd */ uint32_t frame = FRAME; +#ifdef WAIT_LASER int8_t ret; - int16_t c; - + uint16_t when = 0; + uint16_t diff = 0; +#endif /* LEDS */ LED_DDR = _BV(LED1_BIT) | _BV(LED2_BIT) | _BV(LED3_BIT); IR_DDR |= _BV(IR_BIT); @@ -238,8 +275,9 @@ int main(void) #if 0 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); @@ -250,7 +288,7 @@ int main(void) #if 0 while (1) { - if (READ_PHOTO() & _BV(PHOTO1_BIT)) + if (READ_PHOTO()) LED1_ON(); else LED1_OFF(); @@ -265,34 +303,46 @@ int main(void) TCCR1B = _BV(WGM13) | _BV(WGM12); #endif +#if 0 + /* test freq */ + ICR1 = N_CYCLES_PERIOD; + OCR1A = N_CYCLES_1; + TCCR1B = _BV(WGM13) | _BV(WGM12); + TCNT1 = N_CYCLES_PERIOD-1; + TCCR1A = _BV(COM1A1) | _BV(WGM11); + TCCR1B = _BV(WGM13) | _BV(WGM12) | _BV(CS10); + while (1); +#endif + + /* configure timer 0, prescaler = 64 */ TCCR0 = _BV(CS01) | _BV(CS00); while (1) { #ifdef WAIT_LASER - ret = wait_laser(); - + ret = wait_laser(&when, &diff); + LED1_OFF(); LED2_OFF(); if (ret) continue; + + frame = get_frame(diff); + + /* wait before IR xmit */ + while ((int16_t)(when-TCNT1) > 0); #endif - -#if 1 + LED3_ON(); /* ok, transmit frame */ xmit_bits(frame, FRAME_LEN); - + + LED3_OFF(); + /* don't watch a laser during this time */ wait_ms(INTER_LASER_TIME); - LED3_OFF(); -#else - LED1_ON(); - wait_ms(1); - LED1_OFF(); -#endif } return 0; } diff --git a/projects/microb2010/tests/test_board2008/.config b/projects/microb2010/tests/test_board2008/.config index f1528d4..9ce152f 100644 --- a/projects/microb2010/tests/test_board2008/.config +++ b/projects/microb2010/tests/test_board2008/.config @@ -83,6 +83,7 @@ CONFIG_MODULE_CIRBUF=y CONFIG_MODULE_FIXED_POINT=y CONFIG_MODULE_VECT2=y CONFIG_MODULE_GEOMETRY=y +# CONFIG_MODULE_HOSTSIM is not set CONFIG_MODULE_SCHEDULER=y CONFIG_MODULE_SCHEDULER_STATS=y # CONFIG_MODULE_SCHEDULER_CREATE_CONFIG is not set @@ -169,9 +170,10 @@ CONFIG_MODULE_ENCODERS_MICROB_CREATE_CONFIG=y # CONFIG_MODULE_ENCODERS_SPI_CREATE_CONFIG is not set # -# Robot specific modules +# Robot specific modules (fixed point lib may be needed) # CONFIG_MODULE_ROBOT_SYSTEM=y +# CONFIG_MODULE_ROBOT_SYSTEM_USE_F64 is not set # CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT is not set CONFIG_MODULE_POSITION_MANAGER=y CONFIG_MODULE_COMPENSATE_CENTRIFUGAL_FORCE=y diff --git a/projects/microb2010/tests/test_board2008/actuator.c b/projects/microb2010/tests/test_board2008/actuator.c index 5e2f39a..8c09231 100644 --- a/projects/microb2010/tests/test_board2008/actuator.c +++ b/projects/microb2010/tests/test_board2008/actuator.c @@ -29,7 +29,7 @@ #include #include #include -#include +#include #include #include @@ -51,7 +51,7 @@ static volatile uint8_t fessor_state = OFF; static volatile uint32_t fessor_pos_up = 35000; -static volatile uint32_t fessor_pos_down = 63500; +static volatile uint32_t fessor_pos_down = 0; static volatile uint32_t fessor_delay_up = 500; static volatile uint32_t fessor_delay_down = 2000; static volatile uint32_t delay = 0; diff --git a/projects/microb2010/tests/test_board2008/cmdline.c b/projects/microb2010/tests/test_board2008/cmdline.c index c584222..483d4b8 100644 --- a/projects/microb2010/tests/test_board2008/cmdline.c +++ b/projects/microb2010/tests/test_board2008/cmdline.c @@ -30,7 +30,7 @@ #include #include #include -#include +#include #include #include diff --git a/projects/microb2010/tests/test_board2008/commands_ax12.c b/projects/microb2010/tests/test_board2008/commands_ax12.c index 804e084..75616f7 100644 --- a/projects/microb2010/tests/test_board2008/commands_ax12.c +++ b/projects/microb2010/tests/test_board2008/commands_ax12.c @@ -30,7 +30,7 @@ #include #include #include -#include +#include #include #include diff --git a/projects/microb2010/tests/test_board2008/commands_cs.c b/projects/microb2010/tests/test_board2008/commands_cs.c index 7af6205..5bd97d3 100644 --- a/projects/microb2010/tests/test_board2008/commands_cs.c +++ b/projects/microb2010/tests/test_board2008/commands_cs.c @@ -29,7 +29,7 @@ #include #include -#include +#include #include #include diff --git a/projects/microb2010/tests/test_board2008/commands_gen.c b/projects/microb2010/tests/test_board2008/commands_gen.c index 084d2cb..8438f38 100644 --- a/projects/microb2010/tests/test_board2008/commands_gen.c +++ b/projects/microb2010/tests/test_board2008/commands_gen.c @@ -29,7 +29,7 @@ #include #include -#include +#include #include #include diff --git a/projects/microb2010/tests/test_board2008/commands_mainboard.c b/projects/microb2010/tests/test_board2008/commands_mainboard.c index de3e936..1fb35a7 100644 --- a/projects/microb2010/tests/test_board2008/commands_mainboard.c +++ b/projects/microb2010/tests/test_board2008/commands_mainboard.c @@ -30,7 +30,7 @@ #include #include -#include +#include #include #include diff --git a/projects/microb2010/tests/test_board2008/commands_traj.c b/projects/microb2010/tests/test_board2008/commands_traj.c index 691b8c0..74d116d 100644 --- a/projects/microb2010/tests/test_board2008/commands_traj.c +++ b/projects/microb2010/tests/test_board2008/commands_traj.c @@ -29,7 +29,7 @@ #include #include -#include +#include #include #include diff --git a/projects/microb2010/tests/test_board2008/cs.c b/projects/microb2010/tests/test_board2008/cs.c index 9736646..c2409a1 100644 --- a/projects/microb2010/tests/test_board2008/cs.c +++ b/projects/microb2010/tests/test_board2008/cs.c @@ -31,7 +31,7 @@ #include #include #include -#include +#include #include #include @@ -99,15 +99,16 @@ static void do_cs(void *dummy) cs_manage(&mainboard.angle.cs); if (mainboard.distance.on) cs_manage(&mainboard.distance.cs); - if (mainboard.fessor.on) + if (mainboard.fessor.on) { cs_manage(&mainboard.fessor.cs); + fessor_manage(); + } if (mainboard.wheel.on) cs_manage(&mainboard.wheel.cs); - if (mainboard.elevator.on) + if (mainboard.elevator.on) { cs_manage(&mainboard.elevator.cs); - - fessor_manage(); - elevator_manage(); + elevator_manage(); + } } if ((cpt & 1) && (mainboard.flags & DO_POS)) { /* about 1.5ms (worst case without centrifugal force @@ -305,7 +306,7 @@ void microb_cs_init(void) mainboard.angle.on = 0; mainboard.distance.on = 0; mainboard.fessor.on = 1; - mainboard.elevator.on = 1; + mainboard.elevator.on = 0; mainboard.wheel.on = 1; mainboard.flags |= DO_CS; diff --git a/projects/microb2010/tests/test_board2008/main.bin b/projects/microb2010/tests/test_board2008/main.bin index 2538980..22a4acc 100755 Binary files a/projects/microb2010/tests/test_board2008/main.bin and b/projects/microb2010/tests/test_board2008/main.bin differ diff --git a/projects/microb2010/tests/test_board2008/main.c b/projects/microb2010/tests/test_board2008/main.c index 732bd73..c9a3293 100755 --- a/projects/microb2010/tests/test_board2008/main.c +++ b/projects/microb2010/tests/test_board2008/main.c @@ -33,7 +33,7 @@ #include #include #include -#include +#include #include #include diff --git a/projects/microb2010/tests/test_board2008/strat_base.c b/projects/microb2010/tests/test_board2008/strat_base.c index 9945e2c..ffd801e 100644 --- a/projects/microb2010/tests/test_board2008/strat_base.c +++ b/projects/microb2010/tests/test_board2008/strat_base.c @@ -30,7 +30,7 @@ #include #include -#include +#include #include #include diff --git a/projects/microb2010/tests/test_board2008/strat_utils.c b/projects/microb2010/tests/test_board2008/strat_utils.c index edd795a..2816744 100644 --- a/projects/microb2010/tests/test_board2008/strat_utils.c +++ b/projects/microb2010/tests/test_board2008/strat_utils.c @@ -29,7 +29,7 @@ #include #include -#include +#include #include #include @@ -77,17 +77,17 @@ int16_t simple_modulo_360(int16_t a) return a; } -/** do a modulo 2.pi -> [-Pi,+Pi], knowing that 'a' is in [-3Pi,+3Pi] */ -double simple_modulo_2pi(double a) -{ - if (a < -M_PI) { - a += M_2PI; - } - else if (a > M_PI) { - a -= M_2PI; - } - return a; -} +/* /\** do a modulo 2.pi -> [-Pi,+Pi], knowing that 'a' is in [-3Pi,+3Pi] *\/ */ +/* double simple_modulo_2pi(double a) */ +/* { */ +/* if (a < -M_PI) { */ +/* a += M_2PI; */ +/* } */ +/* else if (a > M_PI) { */ +/* a -= M_2PI; */ +/* } */ +/* return a; */ +/* } */ /* return the distance to a point in the area */ int16_t angle_abs_to_rel(int16_t a_abs) diff --git a/projects/microb2010/tests/tourel_beacon/graph.py b/projects/microb2010/tests/tourel_beacon/graph.py index af8edf8..fa87953 100644 --- a/projects/microb2010/tests/tourel_beacon/graph.py +++ b/projects/microb2010/tests/tourel_beacon/graph.py @@ -76,7 +76,8 @@ def graph_da(filename, real_x, real_y, real_a): a1,ea1 = get_angle((real_x, real_y), beacons[1]) a0 -= real_a a1 -= real_a - text = "a0 = %2.2f (%+2.2f deg)\n"%(a0, ea0*(180./math.pi)) + text = "real_pt = %2.2f, %2.2f, %2.2f\n"%(real_x, real_y, real_a) + text += "a0 = %2.2f (%+2.2f deg)\n"%(a0, ea0*(180./math.pi)) text += "a1 = %2.2f (%+2.2f deg)\n"%(a1, ea1*(180./math.pi)) d0,ed0 = get_distance((real_x, real_y), beacons[0]) d1,ed1 = get_distance((real_x, real_y), beacons[1]) @@ -108,6 +109,7 @@ def graph_da(filename, real_x, real_y, real_a): ax.plot(x, y, 'g-') result_pt = (-1, -1) + result_x, result_y, result_a = -1, -1, -1 patches = [] for l in s.split("\n"): m = re.match("circle: x=%s y=%s r=%s"%(FLOAT, FLOAT, FLOAT), l) @@ -121,7 +123,9 @@ def graph_da(filename, real_x, real_y, real_a): float(m.groups()[2]), float(m.groups()[3])) if (n == 0): patches += [ Circle((x, y), 20, alpha=0.4, facecolor="yellow") ] - result_pt = (x, y) + result_x, result_y = (x, y) + result_pt = (x,y) + result_a = a text += l + "\n" pcol.append(PatchCollection(patches, facecolor="none", alpha = 0.6)) @@ -135,8 +139,14 @@ def graph_da(filename, real_x, real_y, real_a): (2200., 1800.), (2200., 500.)] l.sort(cmp=lambda p1,p2: (dist(p1,real_pt) math.pi: + error_a -= 2*math.pi + if error_a < -math.pi: + error_a += 2*math.pi + text += "error = %2.2f mm, %2.2f deg"%(error_dist, error_a * 180. / math.pi) matplotlib.pyplot.text(x, y, text, size=8, ha="center", va="center", bbox = dict(boxstyle="round", @@ -257,7 +267,7 @@ def do_random_test(): print "---- random %d"%i x = random.randint(0, 3000) y = random.randint(0, 2100) - a = random.random()*2*math.pi + a = random.random()*2*math.pi - math.pi graph("angle/test%d.png"%i, x, y, a) graph_da("da/test_da%d.png"%i, x, y, a) @@ -275,7 +285,7 @@ def do_graph_2d(data, filename, title): fig.savefig(filename) def get_data(cmd, sat=0): - data = np.array([[0.]*210]*300) + data = np.array([[50.]*210]*300) oo,ii = popen2.popen2(cmd) ii.close() while True: @@ -313,15 +323,24 @@ def do_graph_2d_simple_error(): do_graph_2d(data, "simple_error/error_a%d_%s.png"%(i,j), title) def do_graph_2d_ad_error(): - for d in ["0.0", "0.1", "0.5"]: - for a in ["0.0", "0.1", "0.5"]: + for d in ["0.0", "0.1", "0.5", "1.0"]: + for a in ["0.0", "0.1", "0.5", "1.0"]: for i in ["0", "1", "2"]: print "do_graph_2d_ad_error %s %s %s"%(i, d, a) - data = get_data("./main da_error %s %s %s"%(i, d, a)) + data = get_data("./main da_error %s %s -%s"%(i, d, a)) title = 'Erreur de position en mm, pour une erreur\n' title += "d'angle de %s deg et dist de %s %% (algo %s)"%(a, d, i) do_graph_2d(data, "da_error/error_da_%s_%s_%s.png"%(i, d, a), title) +def do_graph_2d_ad_error_mm(): + for d in ["5", "10", "20"]: + for a in ["0.0", "0.1", "0.5", "1.0"]: + print "do_graph_2d_ad_error_mm 0 %s %s"%(d, a) + data = get_data("./main da_error_mm 0 %s -%s"%(d, a)) + title = 'Erreur de position en mm, pour une erreur\n' + title += "d'angle de %s deg et dist de %s mm"%(a, d) + do_graph_2d(data, "da_error_mm/error_da_%smm_%s.png"%(d, a), title) + def do_graph_2d_move_error(): i = 0 for period in [ 20, 40 ]: @@ -345,7 +364,8 @@ def do_graph_2d_move_error(): "En rouge, l'erreur de mesure est > 2cm (pour un deplacement\n" 'a %2.2f m/s vers %d deg et une periode tourelle = %d ms)'%(speed, angle_deg, period)) -do_random_test() -do_graph_2d_simple_error() -do_graph_2d_move_error() -do_graph_2d_ad_error() +#do_random_test() +#do_graph_2d_simple_error() +#do_graph_2d_move_error() +#do_graph_2d_ad_error() +do_graph_2d_ad_error_mm() diff --git a/projects/microb2010/tests/tourel_beacon/main.c b/projects/microb2010/tests/tourel_beacon/main.c index 97937d4..90fa109 100644 --- a/projects/microb2010/tests/tourel_beacon/main.c +++ b/projects/microb2010/tests/tourel_beacon/main.c @@ -204,10 +204,18 @@ ad_to_posxy_one(point_t *pos, tmp_a01_p1 = atan2(b1->y-p1.y, b1->x-p1.x) - atan2(b0->y-p1.y, b0->x-p1.x); + if (tmp_a01_p1 > M_PI) + tmp_a01_p1 -= 2*M_PI; + if (tmp_a01_p1 < -M_PI) + tmp_a01_p1 += 2*M_PI; dprintf("a01-1: %2.2f\n", tmp_a01_p1); tmp_a01_p2 = atan2(b1->y-p2.y, b1->x-p2.x) - atan2(b0->y-p2.y, b0->x-p2.x); + if (tmp_a01_p2 > M_PI) + tmp_a01_p2 -= 2*M_PI; + if (tmp_a01_p2 < -M_PI) + tmp_a01_p2 += 2*M_PI; dprintf("a01-2: %2.2f\n", tmp_a01_p2); /* in some conditions, we already know the position of the @@ -228,64 +236,41 @@ ad_to_posxy_one(point_t *pos, return -1; } +static int8_t +ad_to_posxy_algo(point_t *pos, int algo, + const point_t *b0, const point_t *b1, /* beacon position */ + const circle_t *cd0, const circle_t *cd1, const circle_t *c01, /* circles to intersect */ + double a01, /* seen angle of beacons */ + double d0, double d1 /* distance to beacons */ ) -/* get the position and angle of the robot from the angle of the 2 - * beacons, and the distance of 2 beacons */ -static int8_t ad_to_posxya(point_t *pos, double *a, int algo, - const point_t *b0, const point_t *b1, /* beacon position */ - double a0, double a1, /* seen angle of beacons */ - double d0, double d1 /* distance to beacons */ ) { - circle_t cd0, cd1, c01; - double a01; - - dprintf("algo=%d a0=%2.2f a1=%2.2f d0=%2.2f d1=%2.2f\n", - algo, a0, a1, d0, d1); - - /* the first 2 circles depends on distance between robot and - * beacons */ - cd0.x = b0->x; - cd0.y = b0->y; - cd0.r = d0; - dprintf("circle: x=%2.2f y=%2.2f r=%2.2f\n", cd0.x, cd0.y, cd0.r); - cd1.x = b1->x; - cd1.y = b1->y; - cd1.r = d1; - dprintf("circle: x=%2.2f y=%2.2f r=%2.2f\n", cd1.x, cd1.y, cd1.r); - - /* the third circle depends on angle between b0 and b1 */ - a01 = a1-a0; - if (angle_to_circles(&c01, NULL, b0, b1, a01)) - return -1; - dprintf("circle: x=%2.2f y=%2.2f r=%2.2f\n", c01.x, c01.y, c01.r); - switch (algo) { /* take the closer beacon first */ case 0: if (d0 < d1) { - if (ad_to_posxy_one(pos, b0, b1, &cd0, &c01, a01) == 0) + if (ad_to_posxy_one(pos, b0, b1, cd0, c01, a01) == 0) return 0; - if (ad_to_posxy_one(pos, b0, b1, &cd1, &c01, a01) == 0) + if (ad_to_posxy_one(pos, b0, b1, cd1, c01, a01) == 0) return 0; return -1; } else { - if (ad_to_posxy_one(pos, b0, b1, &cd1, &c01, a01) == 0) + if (ad_to_posxy_one(pos, b0, b1, cd1, c01, a01) == 0) return 0; - if (ad_to_posxy_one(pos, b0, b1, &cd0, &c01, a01) == 0) + if (ad_to_posxy_one(pos, b0, b1, cd0, c01, a01) == 0) return 0; return -1; } break; case 1: /* b0 only */ - if (ad_to_posxy_one(pos, b0, b1, &cd0, &c01, a01) == 0) + if (ad_to_posxy_one(pos, b0, b1, cd0, c01, a01) == 0) return 0; break; case 2: /* b0 only */ - if (ad_to_posxy_one(pos, b0, b1, &cd1, &c01, a01) == 0) + if (ad_to_posxy_one(pos, b0, b1, cd1, c01, a01) == 0) return 0; break; default: @@ -295,6 +280,59 @@ static int8_t ad_to_posxya(point_t *pos, double *a, int algo, return -1; } +/* get the position and angle of the robot from the angle of the 2 + * beacons, and the distance of 2 beacons */ +static int8_t +ad_to_posxya(point_t *pos, double *a, int algo, + const point_t *b0, const point_t *b1, /* beacon position */ + double a0, double a1, /* seen angle of beacons */ + double d0, double d1 /* distance to beacons */ ) +{ + circle_t cd0, cd1, c01; + double a01; + + dprintf("algo=%d a0=%2.2f a1=%2.2f d0=%2.2f d1=%2.2f\n", + algo, a0, a1, d0, d1); + + /* the first 2 circles depends on distance between robot and + * beacons */ + cd0.x = b0->x; + cd0.y = b0->y; + cd0.r = d0; + dprintf("circle: x=%2.2f y=%2.2f r=%2.2f\n", cd0.x, cd0.y, cd0.r); + cd1.x = b1->x; + cd1.y = b1->y; + cd1.r = d1; + dprintf("circle: x=%2.2f y=%2.2f r=%2.2f\n", cd1.x, cd1.y, cd1.r); + + /* the third circle depends on angle between b0 and b1 */ + a01 = a1-a0; + if (a01 < -M_PI) + a01 += 2*M_PI; + if (a01 > M_PI) + a01 -= 2*M_PI; + + if (angle_to_circles(&c01, NULL, b0, b1, a01)) + return -1; + dprintf("circle: x=%2.2f y=%2.2f r=%2.2f\n", c01.x, c01.y, c01.r); + + /* get the xy position, depending on algo */ + if (ad_to_posxy_algo(pos, algo, b0, b1, &cd0, &cd1, &c01, a01, d0, d1) < 0) + return -1; + + /* now, we have the xy position, we can get angle thanks to + * the angles a0 and a1. Take the far beacon. */ + if (d0 < d1) + *a = atan2(beacon1.y - pos->y, beacon1.x - pos->x) - a1; + else + *a = atan2(beacon0.y - pos->y, beacon0.x - pos->x) - a0; + if (*a < -M_PI) + *a += 2*M_PI; + if (*a > M_PI) + *a -= 2*M_PI; + return 0; +} + /* get the angles of beacons from xy pos */ int8_t posxy_to_abs_angles(point_t pos, double *a0, double *a1, double *a2, int err_num, float err_val) @@ -474,6 +512,7 @@ int main(int argc, char **argv) for (x=0; x<300; x++) { for (y=0; y<210; y++) { + pos.x = x*10; pos.y = y*10; posxy_to_angles(pos, &a01, &a12, &a20, @@ -490,14 +529,17 @@ int main(int argc, char **argv) } /* da_error algo errpercent errdeg */ - if (argc == 5 && strcmp(mode, "da_error") == 0) { + if ((argc == 5 && strcmp(mode, "da_error") == 0) || + (argc == 5 && strcmp(mode, "da_error_mm") == 0)) { int x, y, algo; double err_val_deg; double err_val_percent; + double err_val_mm; double err, d0, d1, a; algo = atoi(argv[2]); err_val_percent = atof(argv[3]); /* how many % of error for dist */ + err_val_mm = atof(argv[3]); /* how many mm of error for dist */ err_val_deg = atof(argv[4]); /* how many degrees of error */ for (x=0; x<300; x++) { @@ -508,9 +550,15 @@ int main(int argc, char **argv) posxy_to_abs_angles(pos, &a0, &a1, &a2, 0, err_val_deg); d0 = pt_norm(&pos, &beacon0); - d0 += d0 * err_val_percent / 100.; d1 = pt_norm(&pos, &beacon1); - d1 += d1 * err_val_percent / 100.; + if (strcmp(mode, "da_error") == 0) { + d0 += d0 * err_val_percent / 100.; + d1 += d1 * err_val_percent / 100.; + } + else { + d0 += err_val_mm; + d1 += err_val_mm; + } if (ad_to_posxya(&tmp, &a, algo, &beacon0, &beacon1, a0, a1, d0, d1) < 0)