From 51418f44261edc59d818ca990456726027e366ad Mon Sep 17 00:00:00 2001 From: zer0 Date: Sat, 8 May 2010 00:57:24 +0200 Subject: [PATCH 1/1] add support for beacon --- projects/microb2010/mainboard/Makefile | 2 +- projects/microb2010/mainboard/beacon.c | 197 ++++++++++++++++++++ projects/microb2010/mainboard/beacon.h | 22 +++ projects/microb2010/mainboard/main.c | 9 +- projects/microb2010/mainboard/main.h | 12 ++ projects/microb2010/mainboard/robotsim.c | 39 +++- projects/microb2010/mainboard/sensor.c | 11 +- projects/microb2010/mainboard/strat_utils.c | 48 ++--- projects/microb2010/mainboard/strat_utils.h | 1 - 9 files changed, 298 insertions(+), 43 deletions(-) create mode 100644 projects/microb2010/mainboard/beacon.c create mode 100644 projects/microb2010/mainboard/beacon.h diff --git a/projects/microb2010/mainboard/Makefile b/projects/microb2010/mainboard/Makefile index 7df1dc5..518eb19 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 +SRC += strat_db.c strat_avoid.c beacon.c ifeq ($(H),1) SRC += robotsim.c endif diff --git a/projects/microb2010/mainboard/beacon.c b/projects/microb2010/mainboard/beacon.c new file mode 100644 index 0000000..c661613 --- /dev/null +++ b/projects/microb2010/mainboard/beacon.c @@ -0,0 +1,197 @@ +/* + * 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 0 + +#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 + +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; + +#ifndef HOST_VERSION +static void beacon_uart_cb(char c) +{ + static uint8_t state; + static uint16_t d, a, x, y; + + /* init command */ + if ((c & 0x80) == 0) + state = INIT; + + switch (state) { + case INIT: + /* recv opp */ + if (c == 0) { + state = OPP0; + d = 0; + a = 0; + } + /* recv opp */ + else if (c == 0) { + state = STA0; + x = 0; + y = 0; + } + break; + case OPP0: + d = ((uint16_t)c) & 0x7F; + break; + case OPP1: + d |= (((uint16_t)c << 7) & 0x3F80); + break; + case OPP2: + a = ((uint16_t)c) & 0x7F; + break; + case OPP3: + a |= (((uint16_t)c << 7) & 0x3F80); + opp_a = a; + opp_d = d; + opp_age = 0; + break; + case STA0: + x = ((uint16_t)c) & 0x7F; + break; + case STA1: + x |= (((uint16_t)c << 7) & 0x3F80); + break; + case STA2: + y = ((uint16_t)c) & 0x7F; + break; + case STA3: + y |= (((uint16_t)c << 7) & 0x3F80); + beaconboard.posx = x; + beaconboard.posy = y; + break; + /* XXX STA4 with angle */ + default: + state = INIT; + break; + } +} +#endif + +static void beacon_event(void *dummy) +{ +#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 < 3) + opp_age ++; + else + beaconboard.oppx = I2C_OPPONENT_NOT_THERE; + + ia = opp_a; + id = opp_d; + IRQ_UNLOCK(flags); + + 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; + beaconboard.oppd = id; + IRQ_UNLOCK(flags); +#endif +} + +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 new file mode 100644 index 0000000..7138ebc --- /dev/null +++ b/projects/microb2010/mainboard/beacon.h @@ -0,0 +1,22 @@ +/* + * 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); diff --git a/projects/microb2010/mainboard/main.c b/projects/microb2010/mainboard/main.c index eb549c5..d7ac3d4 100755 --- a/projects/microb2010/mainboard/main.c +++ b/projects/microb2010/mainboard/main.c @@ -70,6 +70,7 @@ #include "strat_db.h" #include "strat_avoid.h" #include "i2c_protocol.h" +#include "beacon.h" /* 0 means "programmed" @@ -88,6 +89,7 @@ struct genboard gen; struct mainboard mainboard; volatile struct cobboard cobboard; volatile struct ballboard ballboard; +volatile struct beaconboard beaconboard; #ifndef HOST_VERSION /***********************/ @@ -190,6 +192,8 @@ int main(void) ballboard.lcob = I2C_COB_NONE; ballboard.rcob = I2C_COB_NONE; + beaconboard.oppx = I2C_OPPONENT_NOT_THERE; + /* UART */ uart_init(); uart_register_rx_event(CMDLINE_UART, emergency); @@ -286,10 +290,13 @@ int main(void) /* TIME */ time_init(TIME_PRIO); -#ifndef HOST_VERSION /* 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, 8000L / SCHEDULER_UNIT, I2C_POLL_PRIO); diff --git a/projects/microb2010/mainboard/main.h b/projects/microb2010/mainboard/main.h index 00b6e86..1c41d89 100755 --- a/projects/microb2010/mainboard/main.h +++ b/projects/microb2010/mainboard/main.h @@ -119,6 +119,7 @@ #define CS_PRIO 100 #define STRAT_PRIO 30 #define I2C_POLL_PRIO 20 +#define BEACON_PRIO 15 #define EEPROM_TIME_PRIO 10 #define CS_PERIOD 5000L /* in microsecond */ @@ -212,10 +213,21 @@ struct ballboard { 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; +}; + 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/robotsim.c b/projects/microb2010/mainboard/robotsim.c index 42a33d9..1b103d8 100644 --- a/projects/microb2010/mainboard/robotsim.c +++ b/projects/microb2010/mainboard/robotsim.c @@ -44,6 +44,7 @@ #include #include #include +#include #include #include @@ -193,9 +194,12 @@ void robotsim_update(void) static int32_t l_speed, r_speed; static unsigned i = 0; static unsigned cpt = 0; + + uint8_t flags; int32_t local_l_pwm, local_r_pwm; double x, y, a, a2, d; - char cmd = 0; + char cmd[BUFSIZ]; + int n, pertl = 0, pertr = 0; /* corners of the robot */ double xfl, yfl; /* front left */ @@ -203,6 +207,9 @@ void robotsim_update(void) double xrr, yrr; /* rear right */ double xfr, yfr; /* front right */ + int oppx, oppy; + double oppa, oppd; + /* time shift the command */ l_pwm_shift[i] = l_pwm; r_pwm_shift[i] = r_pwm; @@ -213,8 +220,29 @@ void robotsim_update(void) /* read command */ if (((cpt ++) & 0x7) == 0) { - if (read(fdr, &cmd, 1) != 1) - cmd = 0; + n = read(fdr, &cmd, BUFSIZ - 1); + if (n < 1) + n = 0; + cmd[n] = 0; + } + + /* perturbation */ + if (cmd[0] == 'l') + pertl = 1; + else if (cmd[0] == 'r') + pertr = 1; + if (cmd[0] == 'o') { + if (sscanf(cmd, "opp %d %d", &oppx, &oppy) == 2) { + abs_xy_to_rel_da(oppx, oppy, &oppd, &oppa); + IRQ_LOCK(flags); + beaconboard.oppx = oppx; + beaconboard.oppy = oppy; + beaconboard.oppa = DEG(oppa); + if (beaconboard.oppa < 0) + beaconboard.oppa += 360; + beaconboard.oppd = oppd; + IRQ_UNLOCK(flags); + } } x = position_get_x_double(&mainboard.pos); @@ -250,10 +278,9 @@ void robotsim_update(void) if (!is_in_area(xfr, yfr, 0) && r_speed > 0) r_speed = 0; - /* perturbation */ - if (cmd == 'l') + if (pertl) l_enc += 5000; /* push 1 cm */ - if (cmd == 'r') + if (pertr) r_enc += 5000; /* push 1 cm */ /* XXX should lock */ diff --git a/projects/microb2010/mainboard/sensor.c b/projects/microb2010/mainboard/sensor.c index bda3bd5..6648679 100644 --- a/projects/microb2010/mainboard/sensor.c +++ b/projects/microb2010/mainboard/sensor.c @@ -288,29 +288,28 @@ uint8_t sensor_obstacle_is_disabled(void) /************ global sensor init */ #define BACKGROUND_ADC 0 -#ifndef HOST_VERSION /* called every 10 ms, see init below */ static void do_sensors(void *dummy) { +#ifndef HOST_VERSION if (BACKGROUND_ADC) do_adc(NULL); do_boolean_sensors(NULL); +#endif sensor_obstacle_update(); } -#endif void sensor_init(void) { -#ifdef HOST_VERSION - return; -#else +#ifndef HOST_VERSION adc_init(); if (BACKGROUND_ADC) adc_register_event(adc_event); +#endif + /* CS EVENT */ scheduler_add_periodical_event_priority(do_sensors, NULL, 10000L / SCHEDULER_UNIT, ADC_PRIO); -#endif } diff --git a/projects/microb2010/mainboard/strat_utils.c b/projects/microb2010/mainboard/strat_utils.c index 33c1474..1f19904 100644 --- a/projects/microb2010/mainboard/strat_utils.c +++ b/projects/microb2010/mainboard/strat_utils.c @@ -263,33 +263,34 @@ uint8_t get_opponent_color(void) return I2C_COLOR_YELLOW; } -/* 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; - return -1; // XXX + int16_t x; + IRQ_LOCK(flags); -/* *x = ballboard.opponent_x; */ -/* *y = ballboard.opponent_y; */ + *d = beaconboard.oppd; + *a = beaconboard.oppa; + x = beaconboard.oppx; IRQ_UNLOCK(flags); - if (*x == I2C_OPPONENT_NOT_THERE) + if (x == I2C_OPPONENT_NOT_THERE) return -1; return 0; } -/* 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_tmp; - return -1; // XXX + IRQ_LOCK(flags); -/* x_tmp = ballboard.opponent_x; */ -/* *d = ballboard.opponent_d; */ -/* *a = ballboard.opponent_a; */ + *x = beaconboard.oppx; + *y = beaconboard.oppy; IRQ_UNLOCK(flags); - if (x_tmp == I2C_OPPONENT_NOT_THERE) + if (*x == I2C_OPPONENT_NOT_THERE) return -1; + return 0; } @@ -297,25 +298,16 @@ int8_t get_opponent_da(int16_t *d, int16_t *a) int8_t get_opponent_xyda(int16_t *x, int16_t *y, int16_t *d, int16_t *a) { uint8_t flags; - return -1; // XXX + IRQ_LOCK(flags); -/* *x = ballboard.opponent_x; */ -/* *y = ballboard.opponent_y; */ -/* *d = ballboard.opponent_d; */ -/* *a = ballboard.opponent_a; */ + *x = beaconboard.oppx; + *y = beaconboard.oppy; + *d = beaconboard.oppd; + *a = beaconboard.oppa; IRQ_UNLOCK(flags); if (*x == I2C_OPPONENT_NOT_THERE) return -1; - return 0; -} -uint8_t opponent_is_behind(void) -{ -/* int8_t opp_there; */ -/* int16_t opp_d, opp_a; */ -/* opp_there = get_opponent_da(&opp_d, &opp_a); */ -/* if (opp_there && (opp_a < 215 && opp_a > 145) && opp_d < 600) */ -/* return 1; */ return 0; } diff --git a/projects/microb2010/mainboard/strat_utils.h b/projects/microb2010/mainboard/strat_utils.h index 020b7f1..49b58b0 100644 --- a/projects/microb2010/mainboard/strat_utils.h +++ b/projects/microb2010/mainboard/strat_utils.h @@ -58,7 +58,6 @@ int8_t get_opponent_xy(int16_t *x, int16_t *y); int8_t get_opponent_da(int16_t *d, int16_t *a); int8_t get_opponent_xyda(int16_t *x, int16_t *y, int16_t *d, int16_t *a); int16_t distance_from_opponent(int16_t x, int16_t y); -uint8_t opponent_is_behind(void); uint8_t get_ball_count(void); uint8_t get_cob_count(void); -- 2.20.1