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
70 static volatile uint8_t opp_age = 0;
71 static volatile int16_t opp_a = I2C_OPPONENT_NOT_THERE;
72 static volatile int16_t opp_d = I2C_OPPONENT_NOT_THERE;
75 static void beacon_uart_cb(char c)
78 static uint16_t d, a, x, y;
100 d = ((uint16_t)c) & 0x7F;
103 d |= (((uint16_t)c << 7) & 0x3F80);
106 a = ((uint16_t)c) & 0x7F;
109 a |= (((uint16_t)c << 7) & 0x3F80);
115 x = ((uint16_t)c) & 0x7F;
118 x |= (((uint16_t)c << 7) & 0x3F80);
121 y = ((uint16_t)c) & 0x7F;
124 y |= (((uint16_t)c << 7) & 0x3F80);
125 beaconboard.posx = x;
126 beaconboard.posy = y;
128 /* XXX STA4 with angle */
136 static void beacon_event(void *dummy)
144 if (beaconboard.oppx == I2C_OPPONENT_NOT_THERE) {
148 oppx = beaconboard.oppx;
149 oppy = beaconboard.oppy;
150 abs_xy_to_rel_da(oppx, oppy, &oppd, &oppa);
151 beaconboard.oppa = DEG(oppa);
152 if (beaconboard.oppa < 0)
153 beaconboard.oppa += 360;
154 beaconboard.oppd = oppd;
158 double fd, fa, fx, fy;
159 int16_t id, ia, ix, iy;
161 /* if beacon is too old, remove it */
166 beaconboard.oppx = I2C_OPPONENT_NOT_THERE;
175 rel_da_to_abs_xy(fd, fa, &fx, &fy);
181 beaconboard.oppx = ix;
182 beaconboard.oppy = iy;
183 beaconboard.oppa = ia;
184 beaconboard.oppd = id;
189 void beacon_init(void)
192 uart_register_rx_event(BEACON_UART_NUM, beacon_uart_cb);
194 scheduler_add_periodical_event_priority(beacon_event, NULL,
195 100000L / SCHEDULER_UNIT,