From: zer0 Date: Mon, 10 May 2010 21:29:27 +0000 (+0200) Subject: beacon, prepare integration X-Git-Url: http://git.droids-corp.org/?a=commitdiff_plain;h=b6b61358268cccf0fe5cd3f808ceab6b8402b74c;p=aversive.git beacon, prepare integration --- diff --git a/modules/devices/robot/position_manager/position_manager.c b/modules/devices/robot/position_manager/position_manager.c index 60ed260..a1573a5 100644 --- a/modules/devices/robot/position_manager/position_manager.c +++ b/modules/devices/robot/position_manager/position_manager.c @@ -35,16 +35,16 @@ void position_init(struct robot_position *pos) } /** Set a new robot position */ -void position_set(struct robot_position *pos, int16_t x, int16_t y, int16_t a) +void position_set(struct robot_position *pos, int16_t x, int16_t y, double a_deg) { uint8_t flags; IRQ_LOCK(flags); - pos->pos_d.a = ((double)a * M_PI)/ 180.0; + pos->pos_d.a = (a_deg * M_PI)/ 180.0; pos->pos_d.x = x; pos->pos_d.y = y; pos->pos_s16.x = x; pos->pos_s16.y = y; - pos->pos_s16.a = a; + pos->pos_s16.a = a_deg; IRQ_UNLOCK(flags); } diff --git a/modules/devices/robot/position_manager/position_manager.h b/modules/devices/robot/position_manager/position_manager.h index 24c2447..a3d3d86 100644 --- a/modules/devices/robot/position_manager/position_manager.h +++ b/modules/devices/robot/position_manager/position_manager.h @@ -86,7 +86,7 @@ void position_set_centrifugal_coef(struct robot_position *pos, double coef); #endif /** Set a new robot position */ -void position_set(struct robot_position *pos, int16_t x, int16_t y, int16_t a); +void position_set(struct robot_position *pos, int16_t x, int16_t y, double a_deg); void position_use_ext(struct robot_position *pos); void position_use_mot(struct robot_position *pos); diff --git a/projects/microb2010/common/i2c_commands.h b/projects/microb2010/common/i2c_commands.h index 312a18e..ea0b05b 100644 --- a/projects/microb2010/common/i2c_commands.h +++ b/projects/microb2010/common/i2c_commands.h @@ -23,6 +23,7 @@ #define _I2C_COMMANDS_H_ #define I2C_OPPONENT_NOT_THERE -1000 +#define I2C_BEACON_NOT_FOUND -1000 #define I2C_MAINBOARD_ADDR 1 #define I2C_COBBOARD_ADDR 2 diff --git a/projects/microb2010/mainboard/beacon.c b/projects/microb2010/mainboard/beacon.c index ea26951..e560b6c 100644 --- a/projects/microb2010/mainboard/beacon.c +++ b/projects/microb2010/mainboard/beacon.c @@ -66,16 +66,44 @@ #define STA1 6 #define STA2 7 #define STA3 8 +#define STA4 9 +#define STA5 10 static volatile uint8_t opp_age = 0; static volatile int16_t opp_a = I2C_OPPONENT_NOT_THERE; static volatile int16_t opp_d = I2C_OPPONENT_NOT_THERE; +static volatile uint8_t pos_age = 0; +static volatile int16_t pos_x = I2C_BEACON_NOT_FOUND; +static volatile int16_t pos_y = I2C_BEACON_NOT_FOUND; +static volatile int16_t pos_a = I2C_BEACON_NOT_FOUND; + +int8_t beacon_get_pos(int16_t *x, int16_t *y, double *a) +{ + uint8_t flags; + int16_t tmpx, tmpy, tmpa; + + IRQ_LOCK(flags); + tmpx = beaconboard.posx; + tmpy = beaconboard.posy; + tmpa = beaconboard.posa; + IRQ_UNLOCK(flags); + + if (tmpx == I2C_BEACON_NOT_FOUND) + return -1; + + *x = tmpx; + *y = tmpy; + *a = ((double)tmpa / 10.); + return 0; +} + #ifndef HOST_VERSION static void beacon_uart_cb(char c) { static uint8_t state; - static uint16_t d, a, x, y; + static uint16_t tmp_opp_d, tmp_opp_a; + static uint16_t x, y, a; /* init command */ if ((c & 0x80) == 0) @@ -86,46 +114,64 @@ static void beacon_uart_cb(char c) /* recv opp */ if (c == 0) { state = OPP0; - d = 0; - a = 0; + tmp_opp_d = 0; + tmp_opp_a = 0; } /* recv opp */ - else if (c == 0) { + else if (c == 1) { state = STA0; x = 0; y = 0; + a = 0; } break; case OPP0: - d = ((uint16_t)c) & 0x7F; + tmp_opp_d = ((uint16_t)c) & 0x7F; + state = OPP1; break; case OPP1: - d |= (((uint16_t)c << 7) & 0x3F80); + tmp_opp_d |= (((uint16_t)c << 7) & 0x3F80); + state = OPP2; break; case OPP2: - a = ((uint16_t)c) & 0x7F; + tmp_opp_a = ((uint16_t)c) & 0x7F; + state = OPP3; break; case OPP3: - a |= (((uint16_t)c << 7) & 0x3F80); - opp_a = a; - opp_d = d; + tmp_opp_a |= (((uint16_t)c << 7) & 0x3F80); + opp_a = tmp_opp_a; + opp_d = tmp_opp_d; opp_age = 0; + state = INIT; break; case STA0: x = ((uint16_t)c) & 0x7F; + state = STA1; break; case STA1: x |= (((uint16_t)c << 7) & 0x3F80); + state = STA2; break; case STA2: y = ((uint16_t)c) & 0x7F; + state = STA3; break; case STA3: y |= (((uint16_t)c << 7) & 0x3F80); - beaconboard.posx = x; - beaconboard.posy = y; + state = STA4; + break; + case STA4: + a = ((uint16_t)c) & 0x7F; + state = STA5; + break; + case STA5: + a |= (((uint16_t)c << 7) & 0x3F80); + pos_x = x; + pos_y = y; + pos_a = a; + pos_age = 0; + state = INIT; break; - /* XXX STA4 with angle */ default: state = INIT; break; @@ -133,7 +179,7 @@ static void beacon_uart_cb(char c) } #endif -static void beacon_event(void *dummy) +static void beacon_opponent_event(void) { #ifdef HOST_VERSION uint8_t flags; @@ -189,6 +235,38 @@ static void beacon_event(void *dummy) #endif } +static void beacon_static_event(void) +{ + uint8_t flags; + + /* if beacon is too old, remove it */ + IRQ_LOCK(flags); + if (pos_age < 3) + pos_age ++; + else { + beaconboard.posx = I2C_BEACON_NOT_FOUND; + IRQ_UNLOCK(flags); + return; + } + + beaconboard.posx = pos_x; + beaconboard.posy = pos_y; + beaconboard.posa = pos_a; + IRQ_UNLOCK(flags); +} + + +static void beacon_event(void *dummy) +{ + beacon_opponent_event(); + beacon_static_event(); +} + +void beacon_set_color(uint8_t color) +{ + uart_send(BEACON_UART_NUM, color); +} + void beacon_init(void) { #ifndef HOST_VERSION diff --git a/projects/microb2010/mainboard/beacon.h b/projects/microb2010/mainboard/beacon.h index 7138ebc..1d08efe 100644 --- a/projects/microb2010/mainboard/beacon.h +++ b/projects/microb2010/mainboard/beacon.h @@ -20,3 +20,5 @@ */ void beacon_init(void); +int8_t beacon_get_pos(int16_t *x, int16_t *y, double *a); +void beacon_set_color(uint8_t color); diff --git a/projects/microb2010/mainboard/commands.c b/projects/microb2010/mainboard/commands.c index a49cb89..db6c19d 100644 --- a/projects/microb2010/mainboard/commands.c +++ b/projects/microb2010/mainboard/commands.c @@ -65,6 +65,7 @@ extern parse_pgm_inst_t cmd_event; extern parse_pgm_inst_t cmd_spi_test; extern parse_pgm_inst_t cmd_opponent; extern parse_pgm_inst_t cmd_opponent_set; +extern parse_pgm_inst_t cmd_beacon; extern parse_pgm_inst_t cmd_start; extern parse_pgm_inst_t cmd_interact; extern parse_pgm_inst_t cmd_color; @@ -78,7 +79,6 @@ extern parse_pgm_inst_t cmd_ballboard_show; extern parse_pgm_inst_t cmd_ballboard_setmode1; extern parse_pgm_inst_t cmd_ballboard_setmode2; extern parse_pgm_inst_t cmd_ballboard_setmode3; -extern parse_pgm_inst_t cmd_beacon_start; extern parse_pgm_inst_t cmd_servo_balls; extern parse_pgm_inst_t cmd_clitoid; extern parse_pgm_inst_t cmd_time_monitor; @@ -161,6 +161,7 @@ parse_pgm_ctx_t main_ctx[] = { (parse_pgm_inst_t *)&cmd_spi_test, (parse_pgm_inst_t *)&cmd_opponent, (parse_pgm_inst_t *)&cmd_opponent_set, + (parse_pgm_inst_t *)&cmd_beacon, (parse_pgm_inst_t *)&cmd_start, (parse_pgm_inst_t *)&cmd_interact, (parse_pgm_inst_t *)&cmd_color, diff --git a/projects/microb2010/mainboard/commands_mainboard.c b/projects/microb2010/mainboard/commands_mainboard.c index 8a97000..aae4b61 100644 --- a/projects/microb2010/mainboard/commands_mainboard.c +++ b/projects/microb2010/mainboard/commands_mainboard.c @@ -70,6 +70,7 @@ #include "strat_corn.h" #include "i2c_protocol.h" #include "actuator.h" +#include "beacon.h" struct cmd_event_result { fixed_string_t arg0; @@ -283,6 +284,43 @@ parse_pgm_inst_t cmd_opponent_set = { }, }; +/**********************************************************/ +/* Beacon tests */ + +/* this structure is filled when cmd_beacon is parsed successfully */ +struct cmd_beacon_result { + fixed_string_t arg0; + fixed_string_t arg1; +}; + +/* function called when cmd_beacon is parsed successfully */ +static void cmd_beacon_parsed(void *parsed_result, void *data) +{ + int16_t x, y; + double a; + + if (beacon_get_pos(&x, &y, &a) < 0) + printf_P(PSTR("No position from beacon\r\n")); + else + printf_P(PSTR("x=%d y=%d a=%2.2f\r\n"), x, y, a); +} + +prog_char str_beacon_arg0[] = "beacon"; +parse_pgm_token_string_t cmd_beacon_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_beacon_result, arg0, str_beacon_arg0); +prog_char str_beacon_arg1[] = "show"; +parse_pgm_token_string_t cmd_beacon_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_beacon_result, arg1, str_beacon_arg1); + +prog_char help_beacon[] = "Show (x,y) beacon"; +parse_pgm_inst_t cmd_beacon = { + .f = cmd_beacon_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_beacon, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_beacon_arg0, + (prog_void *)&cmd_beacon_arg1, + NULL, + }, +}; /**********************************************************/ /* Start */ @@ -314,11 +352,13 @@ static void cmd_start_parsed(void *parsed_result, void *data) mainboard.our_color = I2C_COLOR_YELLOW; i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_YELLOW); i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_YELLOW); + beacon_set_color(I2C_COLOR_YELLOW); } else if (!strcmp_P(res->color, PSTR("blue"))) { mainboard.our_color = I2C_COLOR_BLUE; i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_BLUE); i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_BLUE); + beacon_set_color(I2C_COLOR_BLUE); } strat_start(); @@ -574,11 +614,13 @@ static void cmd_color_parsed(void *parsed_result, void *data) mainboard.our_color = I2C_COLOR_YELLOW; i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_YELLOW); i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_YELLOW); + beacon_set_color(I2C_COLOR_YELLOW); } else if (!strcmp_P(res->color, PSTR("blue"))) { mainboard.our_color = I2C_COLOR_BLUE; i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_BLUE); i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_BLUE); + beacon_set_color(I2C_COLOR_BLUE); } printf_P(PSTR("Done\r\n")); #endif diff --git a/projects/microb2010/mainboard/commands_traj.c b/projects/microb2010/mainboard/commands_traj.c index 09a59b7..4da3b47 100644 --- a/projects/microb2010/mainboard/commands_traj.c +++ b/projects/microb2010/mainboard/commands_traj.c @@ -62,6 +62,7 @@ #include "strat_db.h" #include "../common/i2c_commands.h" #include "i2c_protocol.h" +#include "beacon.h" /**********************************************************/ /* Traj_Speeds for trajectory_manager */ @@ -891,6 +892,7 @@ static void cmd_position_parsed(void * parsed_result, void * data) #ifndef HOST_VERSION i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_BLUE); i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_BLUE); + beacon_set_color(I2C_COLOR_YELLOW); #endif auto_position(); } @@ -900,6 +902,7 @@ static void cmd_position_parsed(void * parsed_result, void * data) #ifndef HOST_VERSION i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_YELLOW); i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_YELLOW); + beacon_set_color(I2C_COLOR_BLUE); #endif auto_position(); } diff --git a/projects/microb2010/mainboard/main.c b/projects/microb2010/mainboard/main.c index 7690838..ea9d240 100755 --- a/projects/microb2010/mainboard/main.c +++ b/projects/microb2010/mainboard/main.c @@ -193,6 +193,7 @@ int main(void) ballboard.rcob = I2C_COB_NONE; beaconboard.oppx = I2C_OPPONENT_NOT_THERE; + beaconboard.posx = I2C_BEACON_NOT_FOUND; /* UART */ uart_init(); diff --git a/projects/microb2010/mainboard/main.h b/projects/microb2010/mainboard/main.h index 1c41d89..4a9503d 100755 --- a/projects/microb2010/mainboard/main.h +++ b/projects/microb2010/mainboard/main.h @@ -221,6 +221,7 @@ struct beaconboard { int16_t oppd; uint16_t posx; uint16_t posy; + uint16_t posa; /* between 0 and 3600 */ }; extern struct genboard gen; diff --git a/projects/microb2010/tests/beacon_tsop/Makefile b/projects/microb2010/tests/beacon_tsop/Makefile index 1596ca3..9eaf66f 100755 --- a/projects/microb2010/tests/beacon_tsop/Makefile +++ b/projects/microb2010/tests/beacon_tsop/Makefile @@ -16,7 +16,7 @@ ASRC = AVRDUDE_DELAY=15 -CFLAGS += -W -Wall -Werror +CFLAGS += -W -Wall -Werror -I../../common ######################################## diff --git a/projects/microb2010/tests/beacon_tsop/main.c b/projects/microb2010/tests/beacon_tsop/main.c index 2edd572..fc58a23 100755 --- a/projects/microb2010/tests/beacon_tsop/main.c +++ b/projects/microb2010/tests/beacon_tsop/main.c @@ -38,6 +38,7 @@ #include "uart_proto.h" #include "trigo.h" #include "main.h" +#include "i2c_commands.h" #define BOARD2010 //#define BOARD2006 @@ -454,11 +455,12 @@ static void process_sta_ring(struct frame_status *status) beacon_id, dist0, angle0 * 180. / M_PI, dist1, angle1 * 180. / M_PI); } - if (ad_to_posxya(&pos, &a, 0, &beacon0, &beacon1, angle0, dist0, + if (ad_to_posxya(&pos, &a, 0, &beacon0, &beacon2, angle0, dist0, angle1, dist1) < 0) return; - xmit_static((uint16_t)pos.x, (uint16_t)pos.y, (uint16_t)a); + /* /!\ angle is between 0 and 3600 */ + xmit_static((uint16_t)pos.x, (uint16_t)pos.y, (uint16_t)(a*10)); } static int8_t check_opp_frame(uint16_t frame, uint16_t time) @@ -554,6 +556,7 @@ int main(void) uint8_t cpt = 0; int32_t speed = 0, out, err; uint16_t tcnt3; + int16_t c; uint8_t x = 0; /* debug display counter */ opp_beacon.frame_len = TSOP_OPP_FRAME_LEN; @@ -689,6 +692,25 @@ int main(void) process_sta_ring(&static_beacon); process_opp_ring(&opp_beacon); + + c = uart_proto_recv(); + if (c == I2C_COLOR_YELLOW) { + beacon0.x = -70; + beacon0.y = 1050; + beacon1.x = 3065; + beacon1.y = -65; + beacon2.x = 3065; + beacon2.y = 2165; + } + else { + beacon0.x = -70; + beacon0.y = 1050; + beacon1.x = 3065; + beacon1.y = 2165; + beacon2.x = 3065; + beacon2.y = -65; + } + cli(); tick ++; /* global imprecise time reference */ sei(); diff --git a/projects/microb2010/tests/beacon_tsop/trigo.c b/projects/microb2010/tests/beacon_tsop/trigo.c index df45edf..90fb799 100644 --- a/projects/microb2010/tests/beacon_tsop/trigo.c +++ b/projects/microb2010/tests/beacon_tsop/trigo.c @@ -43,9 +43,10 @@ static int dprint = 0; #define ANGLE_EPSILON 0.005 -const point_t beacon0 = { 0, 1050 }; -const point_t beacon1 = { 3000, 0 }; -const point_t beacon2 = { 3000, 2100 }; +/* valid for yellow */ +point_t beacon0 = { -70, 1050 }; +point_t beacon1 = { 3065, -65 }; +point_t beacon2 = { 3065, 2165 }; static inline double abs_dbl(double x) { diff --git a/projects/microb2010/tests/beacon_tsop/trigo.h b/projects/microb2010/tests/beacon_tsop/trigo.h index 8ac82f8..15b90dc 100644 --- a/projects/microb2010/tests/beacon_tsop/trigo.h +++ b/projects/microb2010/tests/beacon_tsop/trigo.h @@ -18,9 +18,9 @@ * Olivier MATZ */ -extern const point_t beacon0; -extern const point_t beacon1; -extern const point_t beacon2; +extern point_t beacon0; +extern point_t beacon1; +extern point_t beacon2; /* get the position of the robot from the angle of the 3 beacons */ int8_t angles_to_posxy(point_t *pos, double a01, double a12, double a20); diff --git a/projects/microb2010/tests/beacon_tsop/uart_proto.c b/projects/microb2010/tests/beacon_tsop/uart_proto.c index 81c315c..940cd38 100644 --- a/projects/microb2010/tests/beacon_tsop/uart_proto.c +++ b/projects/microb2010/tests/beacon_tsop/uart_proto.c @@ -31,8 +31,6 @@ #include "cmdline.h" #include "main.h" -#include "board2006.h" -//#include "board2010.h" #define UART_NUM 0 @@ -79,12 +77,18 @@ void uart_proto_init(void) UBRRxH = 0; } -static void uart_proto_send(char c) -{ +static void uart_proto_send(char c) +{ while ( !( UCSRxA & (1<