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 (file)
--- 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 (file)
--- 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 (file)
--- 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 (file)
--- 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 (file)
--- 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 (file)
--- 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 (file)
--- 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 (file)
--- 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 (executable)
--- 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 (executable)
--- 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 (executable)
--- 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 (executable)
--- 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 (file)
--- 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 (file)
--- a/projects/microb2010/tests/beacon_tsop/trigo.h
+++ b/projects/microb2010/tests/beacon_tsop/trigo.h
@@ -18,9 +18,9 @@
  *  Olivier MATZ <zer0@droids-corp.org>
  */
 
-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 (file)
--- 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<<UDREx)) ) ;
-       UDRx = c; 
+       UDRx = c;
 }
 
+int16_t uart_proto_recv(void)
+{
+       if ( !(UCSRxA & (1<<RXCx)) )
+               return -1;
+       return UDRx;
+}
 
 /* transmit an integer between 0 and 16384 */
 static void xmit_int14(uint16_t x)

diff --git a/projects/microb2010/tests/beacon_tsop/uart_proto.h b/projects/microb2010/tests/beacon_tsop/uart_proto.h
index bd24c7d..78cb825 100644 (file)
--- a/projects/microb2010/tests/beacon_tsop/uart_proto.h
+++ b/projects/microb2010/tests/beacon_tsop/uart_proto.h
@@ -23,3 +23,4 @@
 void uart_proto_init(void);
 void xmit_opp(uint16_t d, uint16_t a);
 void xmit_static(uint16_t x, uint16_t y, uint16_t a);
+int16_t uart_proto_recv(void);