2 * Copyright Droids Corporation (2010)
4 * This program is free software; you can redistribute it and/or modify
5 * it under the terms of the GNU General Public License as published by
6 * the Free Software Foundation; either version 2 of the License, or
7 * (at your option) any later version.
9 * This program is distributed in the hope that it will be useful,
10 * but WITHOUT ANY WARRANTY; without even the implied warranty of
11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 * GNU General Public License for more details.
14 * You should have received a copy of the GNU General Public License
15 * along with this program; if not, write to the Free Software
16 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
18 * Revision : $Id: strat.h,v 1.7 2009-11-08 17:24:33 zer0 Exp $
25 #include <aversive/pgmspace.h>
26 #include <aversive/wait.h>
27 #include <aversive/error.h>
33 #include <clock_time.h>
35 #include <scheduler.h>
38 #include <control_system_manager.h>
39 #include <trajectory_manager.h>
40 #include <trajectory_manager_utils.h>
41 #include <vect_base.h>
44 #include <obstacle_avoidance.h>
45 #include <blocking_detection_manager.h>
46 #include <robot_system.h>
47 #include <position_manager.h>
51 #include <parse_string.h>
52 #include <parse_num.h>
54 #include "../common/i2c_commands.h"
56 #include "strat_utils.h"
58 #define BEACON_UART_NUM 0
72 static volatile uint8_t opp_age = 0;
73 static volatile int16_t opp_a = I2C_OPPONENT_NOT_THERE;
74 static volatile int16_t opp_d = I2C_OPPONENT_NOT_THERE;
76 static volatile uint8_t pos_age = 0;
77 static volatile int16_t pos_x = I2C_BEACON_NOT_FOUND;
78 static volatile int16_t pos_y = I2C_BEACON_NOT_FOUND;
79 static volatile int16_t pos_a = I2C_BEACON_NOT_FOUND;
81 int8_t beacon_get_pos(int16_t *x, int16_t *y, double *a)
84 int16_t tmpx, tmpy, tmpa;
87 tmpx = beaconboard.posx;
88 tmpy = beaconboard.posy;
89 tmpa = beaconboard.posa;
92 if (tmpx == I2C_BEACON_NOT_FOUND)
97 *a = ((double)tmpa / 10.);
102 static void beacon_uart_cb(char c)
104 static uint8_t state;
105 static uint16_t tmp_opp_d, tmp_opp_a;
106 static uint16_t x, y, a;
129 tmp_opp_d = ((uint16_t)c) & 0x7F;
133 tmp_opp_d |= (((uint16_t)c << 7) & 0x3F80);
137 tmp_opp_a = ((uint16_t)c) & 0x7F;
141 tmp_opp_a |= (((uint16_t)c << 7) & 0x3F80);
148 x = ((uint16_t)c) & 0x7F;
152 x |= (((uint16_t)c << 7) & 0x3F80);
156 y = ((uint16_t)c) & 0x7F;
160 y |= (((uint16_t)c << 7) & 0x3F80);
164 a = ((uint16_t)c) & 0x7F;
168 a |= (((uint16_t)c << 7) & 0x3F80);
182 static void beacon_opponent_event(void)
190 if (beaconboard.oppx == I2C_OPPONENT_NOT_THERE) {
194 oppx = beaconboard.oppx;
195 oppy = beaconboard.oppy;
196 abs_xy_to_rel_da(oppx, oppy, &oppd, &oppa);
197 beaconboard.oppa = DEG(oppa);
198 if (beaconboard.oppa < 0)
199 beaconboard.oppa += 360;
200 beaconboard.oppd = oppd;
204 double fd, fa, fx, fy;
205 int16_t id, ia, ix, iy;
207 /* if beacon is too old, remove it */
212 beaconboard.oppx = I2C_OPPONENT_NOT_THERE;
224 rel_da_to_abs_xy(fd, fa, &fx, &fy);
230 beaconboard.oppx = ix;
231 beaconboard.oppy = iy;
232 beaconboard.oppa = ia;
233 beaconboard.oppd = id;
238 static void beacon_static_event(void)
242 /* if beacon is too old, remove it */
247 beaconboard.posx = I2C_BEACON_NOT_FOUND;
252 beaconboard.posx = pos_x;
253 beaconboard.posy = pos_y;
254 beaconboard.posa = pos_a;
259 static void beacon_event(void *dummy)
261 beacon_opponent_event();
262 beacon_static_event();
265 void beacon_set_color(uint8_t color)
267 uart_send(BEACON_UART_NUM, color);
270 void beacon_init(void)
273 uart_register_rx_event(BEACON_UART_NUM, beacon_uart_cb);
275 scheduler_add_periodical_event_priority(beacon_event, NULL,
276 100000L / SCHEDULER_UNIT,