From ad466314d3aff8f661654d4701b9d12fdeb7811f Mon Sep 17 00:00:00 2001 From: zer0 Date: Thu, 13 May 2010 08:41:51 +0200 Subject: [PATCH] beacon from 2009 --- projects/microb2010/ballboard/Makefile | 2 +- projects/microb2010/ballboard/beacon.c | 396 ++++++++++++++++++ projects/microb2010/ballboard/beacon.h | 57 +++ .../microb2010/ballboard/commands_ballboard.c | 1 + projects/microb2010/ballboard/commands_cs.c | 4 +- projects/microb2010/ballboard/commands_gen.c | 4 +- projects/microb2010/ballboard/cs.c | 20 + projects/microb2010/ballboard/i2c_protocol.c | 18 + projects/microb2010/ballboard/main.c | 10 +- projects/microb2010/ballboard/main.h | 8 +- projects/microb2010/ballboard/sensor.h | 4 +- projects/microb2010/common/i2c_commands.h | 12 + projects/microb2010/mainboard/Makefile | 2 +- projects/microb2010/mainboard/beacon.c | 292 ------------- projects/microb2010/mainboard/beacon.h | 24 -- projects/microb2010/mainboard/commands.c | 4 +- .../microb2010/mainboard/commands_mainboard.c | 48 +-- projects/microb2010/mainboard/commands_traj.c | 3 - projects/microb2010/mainboard/i2c_protocol.c | 12 + projects/microb2010/mainboard/i2c_protocol.h | 1 + projects/microb2010/mainboard/main.c | 8 - projects/microb2010/mainboard/main.h | 16 +- projects/microb2010/mainboard/strat_utils.c | 37 +- 23 files changed, 588 insertions(+), 395 deletions(-) create mode 100644 projects/microb2010/ballboard/beacon.c create mode 100644 projects/microb2010/ballboard/beacon.h delete mode 100644 projects/microb2010/mainboard/beacon.c delete mode 100644 projects/microb2010/mainboard/beacon.h 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 +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#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 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 -#include - -#include -#include -#include - -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#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; } -- 2.20.1