From: zer0 <zer0@carbon.local>
Date: Thu, 13 May 2010 06:41:51 +0000 (+0200)
Subject: beacon from 2009
X-Git-Url: http://git.droids-corp.org/?a=commitdiff_plain;h=ad466314d3aff8f661654d4701b9d12fdeb7811f;p=aversive.git

beacon from 2009
---

diff --git a/projects/microb2010/ballboard/Makefile b/projects/microb2010/ballboard/Makefile
index 9f197fd..9ce1a2a 100644
--- a/projects/microb2010/ballboard/Makefile
+++ b/projects/microb2010/ballboard/Makefile
@@ -11,7 +11,7 @@ LDFLAGS = -T ../common/avr6.x
 SRC  = $(TARGET).c cmdline.c commands_ax12.c commands_gen.c
 SRC += commands_cs.c commands_ballboard.c commands.c
 SRC += i2c_protocol.c sensor.c actuator.c cs.c ax12_user.c
-SRC += state.c
+SRC += state.c beacon.c
 
 # List Assembler source files here.
 # Make them always end in a capital .S.  Files ending in a lowercase .s
diff --git a/projects/microb2010/ballboard/beacon.c b/projects/microb2010/ballboard/beacon.c
new file mode 100644
index 0000000..147a2a9
--- /dev/null
+++ b/projects/microb2010/ballboard/beacon.c
@@ -0,0 +1,396 @@
+
+#include <stdio.h>
+#include <string.h>
+#include <math.h>
+
+#include <aversive.h>
+#include <aversive/pgmspace.h>
+#include <aversive/wait.h>
+#include <aversive/error.h>
+
+#include <uart.h>
+#include <i2c.h>
+#include <ax12.h>
+#include <parse.h>
+#include <rdline.h>
+#include <pwm_ng.h>
+#include <encoders_spi.h>
+#include <timer.h>
+#include <scheduler.h>
+#include <pid.h>
+#include <clock_time.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <adc.h>
+#include <spi.h>
+
+#include <blocking_detection_manager.h>
+
+#include "sensor.h"
+
+#include "../common/i2c_commands.h"
+#include "main.h"
+#include "beacon.h"
+
+struct beacon beacon;
+
+#define BEACON_PWM_VALUE 1000
+#define IR_SENSOR() (!!(PINK&(1<<5)))
+#define MODULO_TIMER (1023L)
+#define COEFF_TIMER (2)
+#define COEFF_MULT (100L)
+#define COEFF_MULT2 (1000L)
+#define BEACON_SIZE (9)
+#define BEACON_MAX_SAMPLE (3)
+
+#define OPPONENT_POS_X (11)
+#define OPPONENT_POS_Y (22)
+
+#define BEACON_DEBUG(args...) DEBUG(E_USER_BEACON, args)
+#define BEACON_NOTICE(args...) NOTICE(E_USER_BEACON, args)
+#define BEACON_ERROR(args...) ERROR(E_USER_BEACON, args)
+
+static volatile int32_t rising = -1;
+static volatile int32_t falling = -1;
+
+static int32_t get_dist(float size);
+static int32_t get_angle(int32_t middle, int32_t ref);
+
+static int32_t pos_ref = 0;
+static int32_t invalid_count = 0;
+
+static volatile int32_t beacon_speed;
+static volatile int32_t beacon_save_count = 0;
+static volatile int32_t beacon_prev_save_count = 0;
+static volatile int32_t count = 0;
+static volatile int32_t count_diff_rising  = 0;
+static volatile int32_t count_diff_falling = 0;
+static int32_t beacon_coeff = 0;
+
+static volatile int8_t valid_beacon = 0;
+
+static volatile int32_t beacon_pos;
+
+//static int32_t beacon_sample_size[BEACON_MAX_SAMPLE];
+
+int32_t encoders_spi_get_value_beacon(void *number)
+{
+	int32_t ret;
+
+	ret = encoders_spi_get_value(number);
+	return ret*4;
+}
+
+void encoders_spi_set_value_beacon(void * number, int32_t v)
+{
+	encoders_spi_set_value(number, v/4);
+}
+
+int32_t encoders_spi_update_beacon_speed(void * number)
+{
+	int32_t ret;
+	uint8_t flags;
+
+	IRQ_LOCK(flags);
+	ret = encoders_spi_get_value_beacon(number);
+	beacon_speed = ret - beacon_pos;
+	beacon_pos = ret;
+	beacon_prev_save_count = beacon_save_count;
+	beacon_save_count = TCNT3;
+	IRQ_UNLOCK(flags);
+
+	beacon_coeff = COEFF_TIMER  * COEFF_MULT;//beacon_speed * COEFF_MULT / ((beacon_prev_save_count - beacon_save_count + MODULO_TIMER + 1)&MODULO_TIMER);
+
+	return beacon_speed;
+}
+
+
+void beacon_init(void)
+{
+	//int8_t i;
+
+	beacon_reset_pos();
+	pos_ref = encoders_spi_get_value_beacon(BEACON_ENCODER);
+
+	memset(&beacon, 0, sizeof(struct beacon));
+	beacon.opponent_x = I2C_OPPONENT_NOT_THERE;
+
+	beacon_speed = 0;
+
+	/*for(i=0;i<BEACON_MAX_SAMPLE;i++)
+		beacon_sample_size[i] = 0;*/
+
+	/* set external interrupt (any edge) */
+	PCMSK2 = (1<<PCINT21);
+	PCICR  = (1<<PCIE2);
+
+
+}
+
+void beacon_calibre_pos(void)
+{
+	ballboard.flags &= ~DO_CS;
+
+	/* init beacon pos */
+	pwm_ng_set(BEACON_PWM, 100);
+
+	/* find rising edge of the mirror*/
+	wait_ms(100);
+	while (sensor_get(BEACON_POS_SENSOR));
+	wait_ms(100);
+	while (!sensor_get(BEACON_POS_SENSOR));
+
+	pwm_ng_set(BEACON_PWM, 0);
+
+
+	beacon_reset_pos();
+	pid_reset(&ballboard.beacon.pid);
+	encoders_spi_set_value_beacon(BEACON_ENCODER, BEACON_OFFSET_CALIBRE);
+
+	cs_set_consign(&ballboard.beacon.cs, 0);
+
+	ballboard.flags |= DO_CS;
+}
+
+void beacon_start(void)
+{
+	beacon_reset_pos();
+	ballboard.beacon.on = 1;
+	cs_set_consign(&ballboard.beacon.cs, 600);
+}
+
+void beacon_stop(void)
+{
+	ballboard.beacon.on = 0;
+	pwm_ng_set(BEACON_PWM, 0);
+}
+
+void beacon_reset_pos(void)
+{
+	pwm_ng_set(BEACON_PWM, 0);
+	encoders_spi_set_value(BEACON_ENCODER, 0);
+}
+
+
+
+int32_t encoders_spi_get_beacon_speed(void * dummy)
+{
+	return beacon_speed;
+}
+
+
+//port K bit 5
+/* motor speed (top tour) */
+SIGNAL(SIG_PIN_CHANGE2)
+{
+	uint8_t flags;
+
+	/* rising edge */
+	if ( IR_SENSOR()) {
+		IRQ_LOCK(flags);
+		count = TCNT3;
+		rising = beacon_pos;
+		count_diff_rising = (count - beacon_save_count + MODULO_TIMER + 1)&MODULO_TIMER;
+		valid_beacon = 0;
+		IRQ_UNLOCK(flags);
+
+	}
+	/* falling edge */
+	else {
+		IRQ_LOCK(flags);
+		count = TCNT3;
+		falling = beacon_pos;
+		count_diff_falling = (count - beacon_save_count + MODULO_TIMER + 1)&MODULO_TIMER;
+		valid_beacon = 1;
+		IRQ_UNLOCK(flags);
+	}
+}
+
+void beacon_calc(void *dummy)
+{
+	static uint8_t a=0;
+	static int32_t local_rising, local_falling;
+	static int32_t middle;
+	static float size = 0;
+	int32_t local_angle;
+	int32_t local_dist;
+
+	int32_t local_count_diff_rising ;
+	int32_t local_count_diff_falling ;
+	int32_t local_beacon_coeff;
+
+	int32_t result_x=0;
+	int32_t result_y=0;
+	int32_t temp=0;
+	int32_t edge=0;
+	//int32_t total_size=0;
+
+	uint8_t flags;
+	//uint8_t i;
+	int8_t local_valid;
+
+	if(a)
+		LED4_ON();
+	else
+		LED4_OFF();
+
+	a = !a;
+
+	if (falling == -1){
+		/* 0.5 second timeout */
+		if (invalid_count < 25)
+			invalid_count++;
+		else {
+			IRQ_LOCK(flags);
+			beacon.opponent_x = I2C_OPPONENT_NOT_THERE;
+			IRQ_UNLOCK(flags);
+		}
+		return;
+	}
+
+	invalid_count = 0;
+	IRQ_LOCK(flags);
+	local_valid = valid_beacon;
+	local_count_diff_rising  = count_diff_rising;
+	local_count_diff_falling = count_diff_falling ;
+	local_rising = rising;
+	local_falling = falling;
+	local_beacon_coeff = beacon_coeff;
+	IRQ_UNLOCK(flags);
+
+	if (local_valid){
+		invalid_count = 0;
+		//BEACON_DEBUG("rising= %ld\t",local_rising);
+		//BEACON_DEBUG("falling= %ld\r\n",local_falling);
+
+		/* recalculate number of pulse by adding the value of the counter, then put value back into motor's round range */
+		local_rising  = ((local_rising + (local_count_diff_rising * local_beacon_coeff) / COEFF_MULT)) %(BEACON_STEP_TOUR);
+		local_falling = ((local_falling + (local_count_diff_falling * local_beacon_coeff) / COEFF_MULT)) %(BEACON_STEP_TOUR);
+
+		//BEACON_DEBUG("rising1= %ld\t",local_rising);
+		//BEACON_DEBUG("falling1= %ld\r\n",local_falling);
+
+		//BEACON_DEBUG("count diff rising= %ld\t",local_count_diff_rising);
+		//BEACON_DEBUG("count diff falling= %ld\r\n",local_count_diff_falling);
+
+		/* if around 360 deg, rising > falling, so invert both and recalculate size and middle */
+		if(local_falling < local_rising){
+			temp          = local_rising;
+			local_rising  = local_falling;
+			local_falling = temp;
+			size          = BEACON_STEP_TOUR - local_falling + local_rising;
+			middle        = (local_falling + ((int32_t)(size)/2) + BEACON_STEP_TOUR) %(BEACON_STEP_TOUR);
+			edge = local_falling;
+		}
+		/* else rising > falling */
+		else{
+			size   = local_falling - local_rising;
+			middle = local_rising + (size / 2);
+			edge   = local_rising;
+		}
+
+		//for(i=BEACON_MAX_SAMPLE-1;i>0;i--){
+		//	beacon_sample_size[i] = beacon_sample_size[i-1];
+		//	total_size += beacon_sample_size[i];
+		//}
+		//beacon_sample_size[0] = size;
+		//total_size += size;
+		//total_size /= BEACON_MAX_SAMPLE;
+
+		//BEACON_DEBUG("rising2= %ld\t",local_rising);
+		//BEACON_DEBUG("falling2= %ld\r\n",local_falling);
+		/* 			BEACON_DEBUG("size= %ld %ld\t",size, total_size); */
+		BEACON_DEBUG("size= %f\r\n",size);
+		//BEACON_DEBUG("middle= %ld\r\n",middle);
+
+		local_angle = get_angle(middle,0);
+		BEACON_NOTICE("opponent angle= %ld\t",local_angle);
+
+		local_dist = get_dist(size);
+		BEACON_NOTICE("opponent dist= %ld\r\n",local_dist);
+
+		beacon_angle_dist_to_x_y(local_angle, local_dist, &result_x, &result_y);
+
+		IRQ_LOCK(flags);
+		beacon.opponent_x = result_x;
+		beacon.opponent_y = result_y;
+		beacon.opponent_angle = local_angle;
+		beacon.opponent_dist = local_dist;
+		/* for I2C test */
+		//beacon.opponent_x = OPPONENT_POS_X;
+		//beacon.opponent_y = OPPONENT_POS_Y;
+		IRQ_UNLOCK(flags);
+
+		BEACON_NOTICE("opponent x= %ld\t",beacon.opponent_x);
+		BEACON_NOTICE("opponent y= %ld\r\n\n",beacon.opponent_y);
+	}
+	else {
+		BEACON_NOTICE("non valid\r\n\n");
+	}
+
+	falling = -1;
+}
+
+static int32_t get_dist(float size)
+{
+	int32_t dist=0;
+	//int32_t alpha=0;
+
+	//alpha = (size*2*M_PI*COEFF_MULT2);
+	//dist = ((2*BEACON_SIZE*BEACON_STEP_TOUR*COEFF_MULT2)/alpha)/2;
+
+	/* function found by measuring points up to 80cm */
+	//dist = ((size - 600)*(size - 600)) / 2400 +28;
+
+	/* new function */
+	/* dist = a0 + a1*x + a2*x² + a3x³ */
+	dist = 1157.3 + (1.4146*size) + (-0.013508*size*size) + (0.00001488*size*size*size);
+
+	return dist;
+
+}
+
+static int32_t get_angle(int32_t middle, int32_t ref)
+{
+	int32_t ret_angle;
+
+	ret_angle = (middle - ref) * 360 / BEACON_STEP_TOUR;
+
+	if(ret_angle > 360)
+		ret_angle -= 360;
+
+	return ret_angle;
+}
+
+void beacon_angle_dist_to_x_y(int32_t angle, int32_t dist, int32_t *x, int32_t *y)
+{
+	uint8_t flags;
+
+	int32_t local_x;
+	int32_t local_y;
+	int32_t x_opponent;
+	int32_t y_opponent;
+	int32_t local_robot_angle;
+
+	IRQ_LOCK(flags);
+	local_x           = beacon.robot_x;
+	local_y           = beacon.robot_y;
+	local_robot_angle = beacon.robot_angle;
+	IRQ_UNLOCK(flags);
+
+	if (local_robot_angle < 0)
+		local_robot_angle += 360;
+
+	x_opponent = cos((local_robot_angle + angle)* 2 * M_PI / 360)* dist;
+	y_opponent = sin((local_robot_angle + angle)* 2 * M_PI / 360)* dist;
+
+	//BEACON_DEBUG("x_op= %ld\t",x_opponent);
+	//BEACON_DEBUG("y_op= %ld\r\n",y_opponent);
+	//BEACON_NOTICE("robot_x= %ld\t",local_x);
+	//BEACON_NOTICE("robot_y= %ld\t",local_y);
+	//BEACON_NOTICE("robot_angle= %ld\r\n",local_robot_angle);
+
+	*x = local_x + x_opponent;
+	*y = local_y + y_opponent;
+
+}
diff --git a/projects/microb2010/ballboard/beacon.h b/projects/microb2010/ballboard/beacon.h
new file mode 100644
index 0000000..4780c5a
--- /dev/null
+++ b/projects/microb2010/ballboard/beacon.h
@@ -0,0 +1,57 @@
+/*
+ *  Copyright Droids Corporation (2008)
+ *
+ *  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
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ *  Revision : $Id: beacon.h,v 1.2 2009-05-27 20:04:07 zer0 Exp $
+ *
+ */
+
+struct beacon {
+	int32_t beacon_speed;
+
+	int32_t opponent_angle;
+	int32_t opponent_dist;
+	int32_t prev_opponent_angle;
+	int32_t prev_opponent_dist;
+	int32_t robot_x;
+	int32_t robot_y;
+	int32_t robot_angle;
+	int32_t opponent_x;
+	int32_t opponent_y;
+};
+
+/* real encoder value: 3531.75 so, multiple by 4 to have round
+ * value */
+#define BEACON_STEP_TOUR (14127L)
+#define BEACON_OFFSET_CALIBRE 40
+
+extern struct beacon beacon;
+
+void beacon_dump(void);
+void beacon_init(void);
+void beacon_calibre_pos(void);
+void beacon_start(void);
+void beacon_stop(void);
+void beacon_calc(void *dummy);
+void beacon_angle_dist_to_x_y(int32_t angle, int32_t dist, int32_t *x, int32_t *y);
+
+void beacon_reset_pos(void);
+void beacon_set_consign(int32_t val);
+
+int32_t encoders_spi_get_beacon_speed(void *dummy);
+int32_t encoders_spi_update_beacon_speed(void *number);
+int32_t encoders_spi_get_value_beacon(void *number);
+
diff --git a/projects/microb2010/ballboard/commands_ballboard.c b/projects/microb2010/ballboard/commands_ballboard.c
index 70ae084..bd7920a 100644
--- a/projects/microb2010/ballboard/commands_ballboard.c
+++ b/projects/microb2010/ballboard/commands_ballboard.c
@@ -105,6 +105,7 @@ static void cmd_event_parsed(void *parsed_result, void *data)
 			pwm_ng_set(ROLLER_PWM, 0);
 			pwm_ng_set(FORKTRANS_PWM, 0);
 			pwm_ng_set(FORKROT_PWM, 0);
+			pwm_ng_set(BEACON_PWM, 0);
 		}
 		ballboard.flags &= (~bit);
 	}
diff --git a/projects/microb2010/ballboard/commands_cs.c b/projects/microb2010/ballboard/commands_cs.c
index d0dd24e..deea9be 100644
--- a/projects/microb2010/ballboard/commands_cs.c
+++ b/projects/microb2010/ballboard/commands_cs.c
@@ -54,10 +54,12 @@ struct csb_list {
 prog_char csb_roller_str[] = "roller";
 prog_char csb_forktrans_str[] = "forktrans";
 prog_char csb_forkrot_str[] = "forkrot";
+prog_char csb_beacon_str[] = "beacon";
 struct csb_list csb_list[] = {
 	{ .name = csb_roller_str, .csb = &ballboard.roller },
 	{ .name = csb_forktrans_str, .csb = &ballboard.forktrans },
 	{ .name = csb_forkrot_str, .csb = &ballboard.forkrot },
+	{ .name = csb_beacon_str, .csb = &ballboard.beacon },
 };
 
 struct cmd_cs_result {
@@ -66,7 +68,7 @@ struct cmd_cs_result {
 };
 
 /* token to be used for all cs-related commands */
-prog_char str_csb_name[] = "roller#forktrans#forkrot";
+prog_char str_csb_name[] = "roller#forktrans#forkrot#beacon";
 parse_pgm_token_string_t cmd_csb_name_tok = TOKEN_STRING_INITIALIZER(struct cmd_cs_result, csname, str_csb_name);
 
 struct cs_block *cs_from_name(const char *name)
diff --git a/projects/microb2010/ballboard/commands_gen.c b/projects/microb2010/ballboard/commands_gen.c
index 66dccc1..b314938 100644
--- a/projects/microb2010/ballboard/commands_gen.c
+++ b/projects/microb2010/ballboard/commands_gen.c
@@ -358,6 +358,7 @@ static const prog_char i2cproto_log[] = "i2cproto";
 static const prog_char sensor_log[] = "sensor";
 static const prog_char block_log[] = "bd";
 static const prog_char state_log[] = "state";
+static const prog_char beacon_log[] = "beacon";
 
 struct log_name_and_num {
 	const prog_char * name;
@@ -371,6 +372,7 @@ static const struct log_name_and_num log_name_and_num[] = {
 	{ sensor_log, E_USER_SENSOR },
 	{ block_log, E_BLOCKING_DETECTION_MANAGER },
 	{ state_log, E_USER_ST_MACH },
+	{ beacon_log, E_USER_BEACON },
 };
 
 static uint8_t
@@ -519,7 +521,7 @@ static void cmd_log_type_parsed(void * parsed_result, void * data)
 prog_char str_log_arg1_type[] = "type";
 parse_pgm_token_string_t cmd_log_arg1_type = TOKEN_STRING_INITIALIZER(struct cmd_log_type_result, arg1, str_log_arg1_type);
 /* keep it sync with log_name_and_num above */
-prog_char str_log_arg2_type[] = "uart#i2c#i2cproto#sensor#bd#state";
+prog_char str_log_arg2_type[] = "uart#i2c#i2cproto#sensor#bd#state#beacon";
 parse_pgm_token_string_t cmd_log_arg2_type = TOKEN_STRING_INITIALIZER(struct cmd_log_type_result, arg2, str_log_arg2_type);
 prog_char str_log_arg3[] = "on#off";
 parse_pgm_token_string_t cmd_log_arg3 = TOKEN_STRING_INITIALIZER(struct cmd_log_type_result, arg3, str_log_arg3);
diff --git a/projects/microb2010/ballboard/cs.c b/projects/microb2010/ballboard/cs.c
index 262f73c..e1166da 100644
--- a/projects/microb2010/ballboard/cs.c
+++ b/projects/microb2010/ballboard/cs.c
@@ -45,6 +45,7 @@
 
 #include "main.h"
 #include "actuator.h"
+#include "beacon.h"
 
 int32_t encoders_spi_update_roller_speed(void *number)
 {
@@ -71,6 +72,8 @@ static void do_cs(void *dummy)
 			cs_manage(&ballboard.forktrans.cs);
 		if (ballboard.forkrot.on)
 			cs_manage(&ballboard.forkrot.cs);
+		if (ballboard.beacon.on)
+			cs_manage(&ballboard.beacon.cs);
 	}
 	if ((ballboard.flags & DO_BD) && (ballboard.flags & DO_POWER)) {
 		bd_manage_from_cs(&ballboard.forktrans.bd, &ballboard.forktrans.cs);
@@ -194,10 +197,27 @@ void microb_cs_init(void)
 	bd_set_speed_threshold(&ballboard.forkrot.bd, 150);
 	bd_set_current_thresholds(&ballboard.forkrot.bd, 500, 8000, 1000000, 200);
 
+	/* BEACON */
+
+	/* PID */
+	pid_init(&ballboard.beacon.pid);
+	pid_set_gains(&ballboard.beacon.pid, 80, 80, 250);
+	pid_set_maximums(&ballboard.beacon.pid, 0, 10000, 1500);
+	pid_set_out_shift(&ballboard.beacon.pid, 6);
+	pid_set_derivate_filter(&ballboard.beacon.pid, 6);
+
+	/* CS */
+	cs_init(&ballboard.beacon.cs);
+	cs_set_correct_filter(&ballboard.beacon.cs, pid_do_filter, &ballboard.beacon.pid);
+	cs_set_process_in(&ballboard.beacon.cs, pwm_ng_set, BEACON_PWM);
+	cs_set_process_out(&ballboard.beacon.cs, encoders_spi_update_beacon_speed, BEACON_ENCODER);
+	cs_set_consign(&ballboard.beacon.cs, 0);
+
 	/* set them on !! */
 	ballboard.roller.on = 1;
 	ballboard.forktrans.on = 1;
 	ballboard.forkrot.on = 0;
+	ballboard.beacon.on = 0;
 
 
 	scheduler_add_periodical_event_priority(do_cs, NULL,
diff --git a/projects/microb2010/ballboard/i2c_protocol.c b/projects/microb2010/ballboard/i2c_protocol.c
index 36bbf96..8f0242d 100644
--- a/projects/microb2010/ballboard/i2c_protocol.c
+++ b/projects/microb2010/ballboard/i2c_protocol.c
@@ -48,6 +48,7 @@
 #include "main.h"
 #include "state.h"
 #include "sensor.h"
+#include "beacon.h"
 #include "actuator.h"
 
 void i2c_protocol_init(void)
@@ -78,6 +79,10 @@ void i2c_send_status(void)
 	ans.ball_count = state_get_ball_count();
 	ans.lcob = cob_detect_left();
 	ans.rcob = cob_detect_right();
+	ans.opponent_x = beacon.opponent_x;
+	ans.opponent_y = beacon.opponent_y;
+	ans.opponent_a = beacon.opponent_angle;
+	ans.opponent_d = beacon.opponent_dist;
 
 	i2c_send(I2C_ADD_MASTER, (uint8_t *) &ans,
 		 sizeof(ans), I2C_CTRL_GENERIC);
@@ -124,6 +129,19 @@ void i2c_recvevent(uint8_t * buf, int8_t size)
 			break;
 		}
 
+	case I2C_CMD_BALLBOARD_SET_BEACON:
+		{
+			struct i2c_cmd_ballboard_start_beacon *cmd = void_cmd;
+			if (size != sizeof (*cmd))
+				goto error;
+
+			if (cmd->enable)
+				beacon_start();
+			else
+				beacon_stop();
+			break;
+		}
+
 	case I2C_CMD_BALLBOARD_SET_MODE:
 		{
 			struct i2c_cmd_ballboard_set_mode *cmd = void_cmd;
diff --git a/projects/microb2010/ballboard/main.c b/projects/microb2010/ballboard/main.c
index e737ba3..7a3cee5 100755
--- a/projects/microb2010/ballboard/main.c
+++ b/projects/microb2010/ballboard/main.c
@@ -59,6 +59,7 @@
 #include "cs.h"
 #include "i2c_protocol.h"
 #include "state.h"
+#include "beacon.h"
 
 /* 0 means "programmed"
  * ---- with 16 Mhz quartz
@@ -214,7 +215,8 @@ int main(void)
 		      &PORTD, 5);
 	PWM_NG_INIT16(&gen.pwm3_1A, 1, A, 10, PWM_NG_MODE_SIGNED |
 		      PWM_NG_MODE_SIGN_INVERTED, &PORTD, 6);
-	PWM_NG_INIT16(&gen.pwm4_1B, 1, B, 10, PWM_NG_MODE_SIGNED,
+	PWM_NG_INIT16(&gen.pwm4_1B, 1, B, 10, PWM_NG_MODE_SIGNED |
+		      PWM_NG_MODE_SIGN_INVERTED,
 		      &PORTD, 7);
 
 
@@ -244,6 +246,11 @@ int main(void)
 	/* sensors, will also init hardware adc */
 	sensor_init();
 
+	/* beacon */
+	beacon_init();
+	scheduler_add_periodical_event_priority(beacon_calc, NULL, 
+						20000L / SCHEDULER_UNIT, 
+						BEACON_PRIO);
 	/* TIME */
 	time_init(TIME_PRIO);
 
@@ -256,6 +263,7 @@ int main(void)
 	sei();
 
 	actuator_init();
+	beacon_calibre_pos();
 
 	printf_P(PSTR("\r\n"));
 	printf_P(PSTR("Dass das Gluck deinen Haus setzt.\r\n"));
diff --git a/projects/microb2010/ballboard/main.h b/projects/microb2010/ballboard/main.h
index 68ecf97..adfa627 100755
--- a/projects/microb2010/ballboard/main.h
+++ b/projects/microb2010/ballboard/main.h
@@ -51,23 +51,26 @@
 #define ROLLER_ENCODER    ((void *)0)
 #define FORKTRANS_ENCODER ((void *)1)
 #define FORKROT_ENCODER   ((void *)2)
+#define BEACON_ENCODER    ((void *)3)
 
 #define ROLLER_PWM     ((void *)&gen.pwm1_4A)
 #define FORKTRANS_PWM  ((void *)&gen.pwm2_4B)
 #define FORKROT_PWM    ((void *)&gen.pwm3_1A)
-#define XXX_PWM        ((void *)&gen.pwm4_1B)
+#define BEACON_PWM     ((void *)&gen.pwm4_1B)
 
-#define BALL_PRESENT_SENSOR  2 /* XXX dummy example */
+#define BEACON_POS_SENSOR  2
 
 /** ERROR NUMS */
 #define E_USER_I2C_PROTO       195
 #define E_USER_SENSOR          196
 #define E_USER_ST_MACH         197
+#define E_USER_BEACON         198
 
 #define LED_PRIO           170
 #define TIME_PRIO          160
 #define ADC_PRIO           120
 #define CS_PRIO            100
+#define BEACON_PRIO	    80
 #define I2C_POLL_PRIO       20
 
 #define CS_PERIOD 5000L
@@ -122,6 +125,7 @@ struct ballboard {
         struct cs_block roller;
         struct cs_block forktrans;
         struct cs_block forkrot;
+        struct cs_block beacon;
 
 	/* robot status */
 	uint8_t our_color;
diff --git a/projects/microb2010/ballboard/sensor.h b/projects/microb2010/ballboard/sensor.h
index cb09f30..067310d 100644
--- a/projects/microb2010/ballboard/sensor.h
+++ b/projects/microb2010/ballboard/sensor.h
@@ -30,8 +30,8 @@
 /* synchronize with sensor.c */
 #define S_HIGH_BARRIER 0
 #define S_LOW_BARRIER  1
-#define S_CAP3         2
-#define S_CAP4         3
+#define S_CAP3         2 /* TT balise */
+#define S_CAP4         3 /* INTR balise */
 #define S_R_IR         4
 #define S_R_US         5
 #define S_L_US         6
diff --git a/projects/microb2010/common/i2c_commands.h b/projects/microb2010/common/i2c_commands.h
index ea0b05b..9882ba6 100644
--- a/projects/microb2010/common/i2c_commands.h
+++ b/projects/microb2010/common/i2c_commands.h
@@ -98,6 +98,13 @@ struct i2c_cmd_ballboard_set_mode {
 	uint8_t mode;
 };
 
+#define I2C_CMD_BALLBOARD_SET_BEACON 0x04
+
+struct i2c_cmd_ballboard_start_beacon {
+	struct i2c_cmd_hdr hdr;
+	uint8_t enable;
+};
+
 /****/
 /* requests and their answers */
 /****/
@@ -157,6 +164,11 @@ struct i2c_ans_ballboard_status {
 #define I2C_BALLBOARD_STATUS_F_EXCPT         0x02
 	uint8_t status;
 
+	int16_t opponent_x;
+	int16_t opponent_y;
+	int16_t opponent_a;
+	int16_t opponent_d;
+
 	uint8_t ball_count;
 
 	/* detection of cobs */
diff --git a/projects/microb2010/mainboard/Makefile b/projects/microb2010/mainboard/Makefile
index 518eb19..7df1dc5 100755
--- a/projects/microb2010/mainboard/Makefile
+++ b/projects/microb2010/mainboard/Makefile
@@ -7,7 +7,7 @@ SRC  = $(TARGET).c cmdline.c commands_ax12.c commands_gen.c
 SRC += commands_cs.c commands_mainboard.c commands_traj.c commands.c
 SRC += i2c_protocol.c sensor.c actuator.c cs.c ax12_user.c
 SRC += strat_utils.c strat_base.c strat.c strat_corn.c
-SRC += strat_db.c strat_avoid.c beacon.c
+SRC += strat_db.c strat_avoid.c
 ifeq ($(H),1)
 SRC += robotsim.c
 endif
diff --git a/projects/microb2010/mainboard/beacon.c b/projects/microb2010/mainboard/beacon.c
deleted file mode 100644
index cc78ca7..0000000
--- a/projects/microb2010/mainboard/beacon.c
+++ /dev/null
@@ -1,292 +0,0 @@
-/*
- *  Copyright Droids Corporation (2010)
- *
- *  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
- *  (at your option) any later version.
- *
- *  This program is distributed in the hope that it will be useful,
- *  but WITHOUT ANY WARRANTY; without even the implied warranty of
- *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- *  GNU General Public License for more details.
- *
- *  You should have received a copy of the GNU General Public License
- *  along with this program; if not, write to the Free Software
- *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
- *
- *  Revision : $Id: strat.h,v 1.7 2009-11-08 17:24:33 zer0 Exp $
- *
- */
-
-#include <stdio.h>
-#include <string.h>
-
-#include <aversive/pgmspace.h>
-#include <aversive/wait.h>
-#include <aversive/error.h>
-
-#include <ax12.h>
-#include <uart.h>
-#include <pwm_ng.h>
-#include <i2c.h>
-#include <clock_time.h>
-
-#include <scheduler.h>
-#include <pid.h>
-#include <quadramp.h>
-#include <control_system_manager.h>
-#include <trajectory_manager.h>
-#include <trajectory_manager_utils.h>
-#include <vect_base.h>
-#include <lines.h>
-#include <polygon.h>
-#include <obstacle_avoidance.h>
-#include <blocking_detection_manager.h>
-#include <robot_system.h>
-#include <position_manager.h>
-
-#include <rdline.h>
-#include <parse.h>
-#include <parse_string.h>
-#include <parse_num.h>
-
-#include "../common/i2c_commands.h"
-#include "main.h"
-#include "strat_utils.h"
-
-#define BEACON_UART_NUM 2
-
-#define INIT 0
-#define OPP0 1
-#define OPP1 2
-#define OPP2 3
-#define OPP3 4
-#define STA0 5
-#define STA1 6
-#define STA2 7
-#define STA3 8
-#define STA4 9
-#define STA5 10
-
-#define BEACON_ANGLE_OFFSET 449
-
-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;
-
-#define BEACON_POS_OFFSET (-50.)
-int8_t beacon_get_pos_double(double *x, double *y, double *a_rad)
-{
-	uint8_t flags;
-	int16_t tmpx, tmpy, tmpa;
-	double dtmpx, dtmpy, dtmpa;
-
-	IRQ_LOCK(flags);
-	tmpx = beaconboard.posx;
-	tmpy = beaconboard.posy;
-	tmpa = beaconboard.posa;
-	IRQ_UNLOCK(flags);
-
-	if (tmpx == I2C_BEACON_NOT_FOUND)
-		return -1;
-
-	dtmpx = tmpx;
-	dtmpy = tmpy;
-	dtmpa = RAD((double)tmpa / 10.);
-
-	dtmpx += cos(dtmpa) * BEACON_POS_OFFSET;
-	dtmpx += sin(dtmpa) * BEACON_POS_OFFSET;
-
-	*x = dtmpx;
-	*y = dtmpy;
-	*a_rad = dtmpa;
-	return 0;
-}
-
-#ifndef HOST_VERSION
-static void beacon_uart_cb(char c)
-{
-	static uint8_t state;
-	static uint16_t tmp_opp_d, tmp_opp_a;
-	static uint16_t x, y, a;
-
-	/* init command */
-	if ((c & 0x80) == 0)
-		state = INIT;
-
-	switch (state) {
-	case INIT:
-		/* recv opp */
-		if (c == 0) {
-			state = OPP0;
-			tmp_opp_d = 0;
-			tmp_opp_a = 0;
-		}
-		/* recv opp */
-		else if (c == 1) {
-			state = STA0;
-			x = 0;
-			y = 0;
-			a = 0;
-		}
-		break;
-	case OPP0:
-		tmp_opp_d = ((uint16_t)c) & 0x7F;
-		state = OPP1;
-		break;
-	case OPP1:
-		tmp_opp_d |= (((uint16_t)c << 7) & 0x3F80);
-		state = OPP2;
-		break;
-	case OPP2:
-		tmp_opp_a = ((uint16_t)c) & 0x7F;
-		state = OPP3;
-		break;
-	case OPP3:
-		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);
-		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;
-	default:
-		state = INIT;
-		break;
-	}
-}
-#endif
-
-static void beacon_opponent_event(void)
-{
-#ifdef HOST_VERSION
-	uint8_t flags;
-	int16_t oppx, oppy;
-	double oppa, oppd;
-
-	IRQ_LOCK(flags);
-	if (beaconboard.oppx == I2C_OPPONENT_NOT_THERE) {
-		IRQ_UNLOCK(flags);
-		return;
-	}
-	oppx = beaconboard.oppx;
-	oppy = beaconboard.oppy;
-	abs_xy_to_rel_da(oppx, oppy, &oppd, &oppa);
-	beaconboard.oppa = DEG(oppa);
-	if (beaconboard.oppa < 0)
-		beaconboard.oppa += 360;
-	beaconboard.oppd = oppd;
-	IRQ_UNLOCK(flags);
-#else
-	uint8_t flags;
-	double fd, fa, fx, fy;
-	int16_t id, ia, ix, iy;
-
-	/* if beacon is too old, remove it */
-	IRQ_LOCK(flags);
-	if (opp_age < 50)
-		opp_age ++;
-	else {
-		beaconboard.oppx = I2C_OPPONENT_NOT_THERE;
-		IRQ_UNLOCK(flags);
-		return;
-	}
-
-	ia = opp_a;
-	id = opp_d;
-	IRQ_UNLOCK(flags);
-
-	ia = (ia + BEACON_ANGLE_OFFSET);
-	if (ia > 3600)
-		ia -= 3600;
-	fa = ia;
-	fa = RAD(fa);
-	fd = id;
-	rel_da_to_abs_xy(fd, fa, &fx, &fy);
-
-	ix = fx;
-	iy = fy;
-
-	IRQ_LOCK(flags);
-	beaconboard.oppx = ix;
-	beaconboard.oppy = iy;
-	beaconboard.oppa = ia / 10;
-	beaconboard.oppd = id;
-	IRQ_UNLOCK(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
-	uart_register_rx_event(BEACON_UART_NUM, beacon_uart_cb);
-#endif
-	scheduler_add_periodical_event_priority(beacon_event, NULL,
-						100000L / SCHEDULER_UNIT,
-						BEACON_PRIO);
-}
diff --git a/projects/microb2010/mainboard/beacon.h b/projects/microb2010/mainboard/beacon.h
deleted file mode 100644
index c5dd26a..0000000
--- a/projects/microb2010/mainboard/beacon.h
+++ /dev/null
@@ -1,24 +0,0 @@
-/*
- *  Copyright Droids Corporation (2010)
- *
- *  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
- *  (at your option) any later version.
- *
- *  This program is distributed in the hope that it will be useful,
- *  but WITHOUT ANY WARRANTY; without even the implied warranty of
- *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- *  GNU General Public License for more details.
- *
- *  You should have received a copy of the GNU General Public License
- *  along with this program; if not, write to the Free Software
- *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
- *
- *  Revision : $Id: strat.h,v 1.7 2009-11-08 17:24:33 zer0 Exp $
- *
- */
-
-void beacon_init(void);
-void beacon_set_color(uint8_t color);
-int8_t beacon_get_pos_double(double *x, double *y, double *a_rad);
diff --git a/projects/microb2010/mainboard/commands.c b/projects/microb2010/mainboard/commands.c
index db6c19d..b054a27 100644
--- a/projects/microb2010/mainboard/commands.c
+++ b/projects/microb2010/mainboard/commands.c
@@ -65,7 +65,6 @@ 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;
@@ -84,6 +83,7 @@ extern parse_pgm_inst_t cmd_clitoid;
 extern parse_pgm_inst_t cmd_time_monitor;
 extern parse_pgm_inst_t cmd_strat_event;
 extern parse_pgm_inst_t cmd_sleep;
+extern parse_pgm_inst_t cmd_beacon_start;
 extern parse_pgm_inst_t cmd_test;
 
 /* commands_traj.c */
@@ -161,7 +161,6 @@ 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,
@@ -180,6 +179,7 @@ parse_pgm_ctx_t main_ctx[] = {
 	(parse_pgm_inst_t *)&cmd_time_monitor,
 	(parse_pgm_inst_t *)&cmd_strat_event,
 	(parse_pgm_inst_t *)&cmd_sleep,
+	(parse_pgm_inst_t *)&cmd_beacon_start,
 	(parse_pgm_inst_t *)&cmd_test,
 
 	/* commands_traj.c */
diff --git a/projects/microb2010/mainboard/commands_mainboard.c b/projects/microb2010/mainboard/commands_mainboard.c
index fcc47e6..1875965 100644
--- a/projects/microb2010/mainboard/commands_mainboard.c
+++ b/projects/microb2010/mainboard/commands_mainboard.c
@@ -70,7 +70,6 @@
 #include "strat_corn.h"
 #include "i2c_protocol.h"
 #include "actuator.h"
-#include "beacon.h"
 
 struct cmd_event_result {
 	fixed_string_t arg0;
@@ -285,38 +284,38 @@ parse_pgm_inst_t cmd_opponent_set = {
 };
 
 /**********************************************************/
-/* Beacon tests */
+/* Beacon_Start */
 
-/* this structure is filled when cmd_beacon is parsed successfully */
-struct cmd_beacon_result {
+/* this structure is filled when cmd_beacon_start is parsed successfully */
+struct cmd_beacon_start_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)
+/* function called when cmd_beacon_start is parsed successfully */
+static void cmd_beacon_start_parsed(void *parsed_result, void *data)
 {
-	double x, y, a;
+	struct cmd_beacon_start_result *res = parsed_result;
 
-	if (beacon_get_pos_double(&x, &y, &a) < 0)
-		printf_P(PSTR("No position from beacon\r\n"));
+	if (!strcmp_P(res->arg1, PSTR("start")))
+		i2c_ballboard_set_beacon(1);
 	else
-		printf_P(PSTR("x=%2.2f y=%2.2f a=%2.2f\r\n"), x, y, DEG(a));
+		i2c_ballboard_set_beacon(0);
 }
 
-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 str_beacon_start_arg0[] = "beacon";
+parse_pgm_token_string_t cmd_beacon_start_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_beacon_start_result, arg0, str_beacon_start_arg0);
+prog_char str_beacon_start_arg1[] = "start#stop";
+parse_pgm_token_string_t cmd_beacon_start_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_beacon_start_result, arg1, str_beacon_start_arg1);
 
-prog_char help_beacon[] = "Show (x,y) beacon";
-parse_pgm_inst_t cmd_beacon = {
-	.f = cmd_beacon_parsed,  /* function to call */
+prog_char help_beacon_start[] = "Beacon enabled/disable";
+parse_pgm_inst_t cmd_beacon_start = {
+	.f = cmd_beacon_start_parsed,  /* function to call */
 	.data = NULL,      /* 2nd arg of func */
-	.help_str = help_beacon,
+	.help_str = help_beacon_start,
 	.tokens = {        /* token list, NULL terminated */
-		(prog_void *)&cmd_beacon_arg0,
-		(prog_void *)&cmd_beacon_arg1,
+		(prog_void *)&cmd_beacon_start_arg0, 
+		(prog_void *)&cmd_beacon_start_arg1, 
 		NULL,
 	},
 };
@@ -351,15 +350,18 @@ 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);
 	}
 
+	printf_P(PSTR("Press a key when beacon ready\r\n"));
+	i2c_ballboard_set_beacon(0);
+	while(!cmdline_keypressed());
+	i2c_ballboard_set_beacon(1);
+
 	strat_start();
 
 	gen.logs[NB_LOGS] = 0;
@@ -613,13 +615,11 @@ 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 e0e6a1b..d08b4e5 100644
--- a/projects/microb2010/mainboard/commands_traj.c
+++ b/projects/microb2010/mainboard/commands_traj.c
@@ -62,7 +62,6 @@
 #include "strat_db.h"
 #include "../common/i2c_commands.h"
 #include "i2c_protocol.h"
-#include "beacon.h"
 
 /**********************************************************/
 /* Traj_Speeds for trajectory_manager */
@@ -892,7 +891,6 @@ 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();
 	}
@@ -902,7 +900,6 @@ 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/i2c_protocol.c b/projects/microb2010/mainboard/i2c_protocol.c
index 442cf3a..2c27410 100644
--- a/projects/microb2010/mainboard/i2c_protocol.c
+++ b/projects/microb2010/mainboard/i2c_protocol.c
@@ -299,6 +299,10 @@ void i2c_recvevent(uint8_t * buf, int8_t size)
 		tmp = ans->rcob;
 		if (tmp != I2C_COB_NONE)
 			ballboard.rcob = tmp;
+		ballboard.opponent_x = ans->opponent_x;
+		ballboard.opponent_y = ans->opponent_y;
+		ballboard.opponent_a = ans->opponent_a;
+		ballboard.opponent_d = ans->opponent_d;
 		break;
 	}
 
@@ -468,3 +472,11 @@ int8_t i2c_ballboard_set_mode(uint8_t mode)
 	return i2c_send_command(I2C_BALLBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
 }
 
+int8_t i2c_ballboard_set_beacon(uint8_t enable)
+{
+	struct i2c_cmd_ballboard_start_beacon buf;
+	buf.hdr.cmd = I2C_CMD_BALLBOARD_SET_BEACON;
+	buf.enable = enable;
+	return i2c_send_command(I2C_BALLBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
+}
+
diff --git a/projects/microb2010/mainboard/i2c_protocol.h b/projects/microb2010/mainboard/i2c_protocol.h
index b5a8136..613a098 100644
--- a/projects/microb2010/mainboard/i2c_protocol.h
+++ b/projects/microb2010/mainboard/i2c_protocol.h
@@ -44,5 +44,6 @@ int8_t i2c_cobboard_autoharvest_nomove(uint8_t side);
 int8_t i2c_cobboard_deploy_nomove(uint8_t side);
 
 int8_t i2c_ballboard_set_mode(uint8_t mode);
+int8_t i2c_ballboard_set_beacon(uint8_t enable);
 
 #endif
diff --git a/projects/microb2010/mainboard/main.c b/projects/microb2010/mainboard/main.c
index ea9d240..f657ddf 100755
--- a/projects/microb2010/mainboard/main.c
+++ b/projects/microb2010/mainboard/main.c
@@ -70,7 +70,6 @@
 #include "strat_db.h"
 #include "strat_avoid.h"
 #include "i2c_protocol.h"
-#include "beacon.h"
 
 
 /* 0 means "programmed"
@@ -89,7 +88,6 @@ struct genboard gen;
 struct mainboard mainboard;
 volatile struct cobboard cobboard;
 volatile struct ballboard ballboard;
-volatile struct beaconboard beaconboard;
 
 #ifndef HOST_VERSION
 /***********************/
@@ -192,9 +190,6 @@ int main(void)
 	ballboard.lcob = I2C_COB_NONE;
 	ballboard.rcob = I2C_COB_NONE;
 
-	beaconboard.oppx = I2C_OPPONENT_NOT_THERE;
-	beaconboard.posx = I2C_BEACON_NOT_FOUND;
-
 	/* UART */
 	uart_init();
 	uart_register_rx_event(CMDLINE_UART, emergency);
@@ -294,9 +289,6 @@ int main(void)
 	/* sensors, will also init hardware adc */
 	sensor_init();
 
-	/* beacon */
-	beacon_init();
-
 #ifndef HOST_VERSION
 	/* start i2c slave polling */
 	scheduler_add_periodical_event_priority(i2c_poll_slaves, NULL,
diff --git a/projects/microb2010/mainboard/main.h b/projects/microb2010/mainboard/main.h
index f98bd99..2fe7199 100755
--- a/projects/microb2010/mainboard/main.h
+++ b/projects/microb2010/mainboard/main.h
@@ -217,24 +217,16 @@ struct ballboard {
 	uint8_t ball_count;
 	uint8_t lcob;
 	uint8_t rcob;
-};
-
-/* state of beaconboard, sync'd through uart */
-struct beaconboard {
-	int16_t oppx;
-	int16_t oppy;
-	int16_t oppa;
-	int16_t oppd;
-	uint16_t posx;
-	uint16_t posy;
-	uint16_t posa; /* between 0 and 3600 */
+	int16_t opponent_x;
+	int16_t opponent_y;
+	int16_t opponent_a;
+	int16_t opponent_d;
 };
 
 extern struct genboard gen;
 extern struct mainboard mainboard;
 extern volatile struct cobboard cobboard;
 extern volatile struct ballboard ballboard;
-extern volatile struct beaconboard beaconboard;
 
 /* start the bootloader */
 void bootloader(void);
diff --git a/projects/microb2010/mainboard/strat_utils.c b/projects/microb2010/mainboard/strat_utils.c
index 35803ac..9deba04 100644
--- a/projects/microb2010/mainboard/strat_utils.c
+++ b/projects/microb2010/mainboard/strat_utils.c
@@ -274,34 +274,31 @@ uint8_t get_opponent_color(void)
 		return I2C_COLOR_YELLOW;
 }
 
-/* get the da pos of the opponent robot */
-int8_t get_opponent_da(int16_t *d, int16_t *a)
+/* get the xy pos of the opponent robot */
+int8_t get_opponent_xy(int16_t *x, int16_t *y)
 {
 	uint8_t flags;
-	int16_t x;
-
 	IRQ_LOCK(flags);
-	*d = beaconboard.oppd;
-	*a = beaconboard.oppa;
-	x = beaconboard.oppx;
+	*x = ballboard.opponent_x;
+	*y = ballboard.opponent_y;
 	IRQ_UNLOCK(flags);
-	if (x == I2C_OPPONENT_NOT_THERE)
+	if (*x == I2C_OPPONENT_NOT_THERE)
 		return -1;
 	return 0;
 }
 
-/* get the xy pos of the opponent robot */
-int8_t get_opponent_xy(int16_t *x, int16_t *y)
+/* get the da pos of the opponent robot */
+int8_t get_opponent_da(int16_t *d, int16_t *a)
 {
 	uint8_t flags;
-
+	int16_t x_tmp;
 	IRQ_LOCK(flags);
-	*x = beaconboard.oppx;
-	*y = beaconboard.oppy;
+	x_tmp = ballboard.opponent_x;
+	*d = ballboard.opponent_d;
+	*a = ballboard.opponent_a;
 	IRQ_UNLOCK(flags);
-	if (*x == I2C_OPPONENT_NOT_THERE)
+	if (x_tmp == I2C_OPPONENT_NOT_THERE)
 		return -1;
-
 	return 0;
 }
 
@@ -309,16 +306,14 @@ int8_t get_opponent_xy(int16_t *x, int16_t *y)
 int8_t get_opponent_xyda(int16_t *x, int16_t *y, int16_t *d, int16_t *a)
 {
 	uint8_t flags;
-
 	IRQ_LOCK(flags);
-	*x = beaconboard.oppx;
-	*y = beaconboard.oppy;
-	*d = beaconboard.oppd;
-	*a = beaconboard.oppa;
+	*x = ballboard.opponent_x;
+	*y = ballboard.opponent_y;
+	*d = ballboard.opponent_d;
+	*a = ballboard.opponent_a;
 	IRQ_UNLOCK(flags);
 	if (*x == I2C_OPPONENT_NOT_THERE)
 		return -1;
-
 	return 0;
 }