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 2
72 #define BEACON_ANGLE_OFFSET 449
74 static volatile uint8_t opp_age = 0;
75 static volatile int16_t opp_a = I2C_OPPONENT_NOT_THERE;
76 static volatile int16_t opp_d = I2C_OPPONENT_NOT_THERE;
78 static volatile uint8_t pos_age = 0;
79 static volatile int16_t pos_x = I2C_BEACON_NOT_FOUND;
80 static volatile int16_t pos_y = I2C_BEACON_NOT_FOUND;
81 static volatile int16_t pos_a = I2C_BEACON_NOT_FOUND;
83 #define BEACON_POS_OFFSET (-50.)
84 int8_t beacon_get_pos_double(double *x, double *y, double *a_rad)
87 int16_t tmpx, tmpy, tmpa;
88 double dtmpx, dtmpy, dtmpa;
91 tmpx = beaconboard.posx;
92 tmpy = beaconboard.posy;
93 tmpa = beaconboard.posa;
96 if (tmpx == I2C_BEACON_NOT_FOUND)
101 dtmpa = RAD((double)tmpa / 10.);
103 dtmpx += cos(dtmpa) * BEACON_POS_OFFSET;
104 dtmpx += sin(dtmpa) * BEACON_POS_OFFSET;
113 static void beacon_uart_cb(char c)
115 static uint8_t state;
116 static uint16_t tmp_opp_d, tmp_opp_a;
117 static uint16_t x, y, a;
140 tmp_opp_d = ((uint16_t)c) & 0x7F;
144 tmp_opp_d |= (((uint16_t)c << 7) & 0x3F80);
148 tmp_opp_a = ((uint16_t)c) & 0x7F;
152 tmp_opp_a |= (((uint16_t)c << 7) & 0x3F80);
159 x = ((uint16_t)c) & 0x7F;
163 x |= (((uint16_t)c << 7) & 0x3F80);
167 y = ((uint16_t)c) & 0x7F;
171 y |= (((uint16_t)c << 7) & 0x3F80);
175 a = ((uint16_t)c) & 0x7F;
179 a |= (((uint16_t)c << 7) & 0x3F80);
193 static void beacon_opponent_event(void)
201 if (beaconboard.oppx == I2C_OPPONENT_NOT_THERE) {
205 oppx = beaconboard.oppx;
206 oppy = beaconboard.oppy;
207 abs_xy_to_rel_da(oppx, oppy, &oppd, &oppa);
208 beaconboard.oppa = DEG(oppa);
209 if (beaconboard.oppa < 0)
210 beaconboard.oppa += 360;
211 beaconboard.oppd = oppd;
215 double fd, fa, fx, fy;
216 int16_t id, ia, ix, iy;
218 /* if beacon is too old, remove it */
223 beaconboard.oppx = I2C_OPPONENT_NOT_THERE;
232 ia = (ia + BEACON_ANGLE_OFFSET);
238 rel_da_to_abs_xy(fd, fa, &fx, &fy);
244 beaconboard.oppx = ix;
245 beaconboard.oppy = iy;
246 beaconboard.oppa = ia / 10;
247 beaconboard.oppd = id;
252 static void beacon_static_event(void)
256 /* if beacon is too old, remove it */
261 beaconboard.posx = I2C_BEACON_NOT_FOUND;
266 beaconboard.posx = pos_x;
267 beaconboard.posy = pos_y;
268 beaconboard.posa = pos_a;
273 static void beacon_event(void *dummy)
275 beacon_opponent_event();
276 beacon_static_event();
279 void beacon_set_color(uint8_t color)
281 uart_send(BEACON_UART_NUM, color);
284 void beacon_init(void)
287 uart_register_rx_event(BEACON_UART_NUM, beacon_uart_cb);
289 scheduler_add_periodical_event_priority(beacon_event, NULL,
290 100000L / SCHEDULER_UNIT,