2 * Copyright Droids Corporation, Microb Technology (2009)
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_utils.c,v 1.7 2009-11-08 17:24:33 zer0 Exp $
26 #include <aversive/pgmspace.h>
27 #include <aversive/wait.h>
28 #include <aversive/error.h>
33 #include <clock_time.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>
52 #include "../common/i2c_commands.h"
55 #include "strat_utils.h"
58 #include "i2c_protocol.h"
60 /* return the distance between two points */
61 int16_t distance_between(int16_t x1, int16_t y1, int16_t x2, int16_t y2)
71 /* return the distance between two points */
72 int32_t quad_distance_between(int16_t x1, int16_t y1, int16_t x2, int16_t y2)
82 /* return the distance to a point in the area */
83 int16_t distance_from_robot(int16_t x, int16_t y)
85 return distance_between(position_get_x_s16(&mainboard.pos),
86 position_get_y_s16(&mainboard.pos), x, y);
89 /** do a modulo 360 -> [-180,+180], knowing that 'a' is in [-3*180,+3*180] */
90 int16_t simple_modulo_360(int16_t a)
101 /* return the distance to a point in the area */
102 int16_t angle_abs_to_rel(int16_t a_abs)
104 return simple_modulo_360(a_abs - position_get_a_deg_s16(&mainboard.pos));
107 void rel_da_to_abs_xy(double d_rel, double a_rel_rad,
108 double *x_abs, double *y_abs)
110 double x = position_get_x_double(&mainboard.pos);
111 double y = position_get_y_double(&mainboard.pos);
112 double a = position_get_a_rad_double(&mainboard.pos);
114 *x_abs = x + d_rel*cos(a+a_rel_rad);
115 *y_abs = y + d_rel*sin(a+a_rel_rad);
118 double norm(double x, double y)
120 return sqrt(x*x + y*y);
123 void rel_xy_to_abs_xy(double x_rel, double y_rel,
124 double *x_abs, double *y_abs)
127 d_rel = norm(x_rel, y_rel);
128 a_rel = atan2(y_rel, x_rel);
129 rel_da_to_abs_xy(d_rel, a_rel, x_abs, y_abs);
132 /* return an angle between -pi and pi */
133 void abs_xy_to_rel_da(double x_abs, double y_abs,
134 double *d_rel, double *a_rel_rad)
136 double x = position_get_x_double(&mainboard.pos);
137 double y = position_get_y_double(&mainboard.pos);
138 double a = position_get_a_rad_double(&mainboard.pos);
140 *a_rel_rad = atan2(y_abs - y, x_abs - x) - a;
141 if (*a_rel_rad < -M_PI) {
144 else if (*a_rel_rad > M_PI) {
147 *d_rel = norm(x_abs-x, y_abs-y);
150 void rotate(double *x, double *y, double rot)
162 /* return true if the point is in area */
163 uint8_t is_in_area(int16_t x, int16_t y, int16_t margin)
167 if (x > (AREA_X - margin))
171 if (y > (AREA_Y - margin))
177 /* return true if the point is in area */
178 uint8_t robot_is_in_area(int16_t margin)
180 return is_in_area(position_get_x_s16(&mainboard.pos),
181 position_get_y_s16(&mainboard.pos),
185 /* return 1 or 0 depending on which side of a line (y=cste) is the
186 * robot. works in yellow or blue color. */
187 uint8_t y_is_more_than(int16_t y)
191 posy = position_get_y_s16(&mainboard.pos);
192 if (mainboard.our_color == I2C_COLOR_YELLOW) {
199 if (posy < (AREA_Y-y))
206 /* return 1 or 0 depending on which side of a line (x=cste) is the
207 * robot. works in yellow or blue color. */
208 uint8_t x_is_more_than(int16_t x)
212 posx = position_get_x_s16(&mainboard.pos);
219 int16_t sin_table[] = {
239 int16_t fast_sin(int16_t deg)
247 return sin_table[(deg*16)/90];
249 return sin_table[((180-deg)*16)/90];
251 return -sin_table[((deg-180)*16)/90];
253 return -sin_table[((360-deg)*16)/90];
256 int16_t fast_cos(int16_t deg)
258 return fast_sin(90+deg);
262 /* get the color of our robot */
263 uint8_t get_color(void)
265 return mainboard.our_color;
268 /* get the color of the opponent robot */
269 uint8_t get_opponent_color(void)
271 if (mainboard.our_color == I2C_COLOR_YELLOW)
272 return I2C_COLOR_BLUE;
274 return I2C_COLOR_YELLOW;
277 /* get the da pos of the opponent robot */
278 int8_t get_opponent_da(int16_t *d, int16_t *a)
284 *d = beaconboard.oppd;
285 *a = beaconboard.oppa;
286 x = beaconboard.oppx;
288 if (x == I2C_OPPONENT_NOT_THERE)
293 /* get the xy pos of the opponent robot */
294 int8_t get_opponent_xy(int16_t *x, int16_t *y)
299 *x = beaconboard.oppx;
300 *y = beaconboard.oppy;
302 if (*x == I2C_OPPONENT_NOT_THERE)
308 /* get the da pos of the opponent robot */
309 int8_t get_opponent_xyda(int16_t *x, int16_t *y, int16_t *d, int16_t *a)
314 *x = beaconboard.oppx;
315 *y = beaconboard.oppy;
316 *d = beaconboard.oppd;
317 *a = beaconboard.oppa;
319 if (*x == I2C_OPPONENT_NOT_THERE)
325 int16_t distance_from_opponent(int16_t x, int16_t y)
328 if (get_opponent_xy(&oppx, &oppy) < 0)
330 return distance_between(x, y, oppx, oppy);
333 uint8_t get_ball_count(void)
335 return ballboard.ball_count;
338 uint8_t get_cob_count(void)
340 return cobboard.cob_count;