}
/** 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);
}
#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);
#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
#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)
/* 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;
}
#endif
-static void beacon_event(void *dummy)
+static void beacon_opponent_event(void)
{
#ifdef HOST_VERSION
uint8_t flags;
#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
*/
void beacon_init(void);
+int8_t beacon_get_pos(int16_t *x, int16_t *y, double *a);
+void beacon_set_color(uint8_t color);
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;
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;
(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,
#include "strat_corn.h"
#include "i2c_protocol.h"
#include "actuator.h"
+#include "beacon.h"
struct cmd_event_result {
fixed_string_t arg0;
},
};
+/**********************************************************/
+/* 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 */
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();
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
#include "strat_db.h"
#include "../common/i2c_commands.h"
#include "i2c_protocol.h"
+#include "beacon.h"
/**********************************************************/
/* Traj_Speeds for trajectory_manager */
#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();
}
#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();
}
ballboard.rcob = I2C_COB_NONE;
beaconboard.oppx = I2C_OPPONENT_NOT_THERE;
+ beaconboard.posx = I2C_BEACON_NOT_FOUND;
/* UART */
uart_init();
int16_t oppd;
uint16_t posx;
uint16_t posy;
+ uint16_t posa; /* between 0 and 3600 */
};
extern struct genboard gen;
AVRDUDE_DELAY=15
-CFLAGS += -W -Wall -Werror
+CFLAGS += -W -Wall -Werror -I../../common
########################################
#include "uart_proto.h"
#include "trigo.h"
#include "main.h"
+#include "i2c_commands.h"
#define BOARD2010
//#define BOARD2006
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)
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;
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();
#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)
{
* 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);
#include "cmdline.h"
#include "main.h"
-#include "board2006.h"
-//#include "board2010.h"
#define UART_NUM 0
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)
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);