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>
35 #include <control_system_manager.h>
36 #include <trajectory_manager.h>
37 #include <vect_base.h>
40 #include <obstacle_avoidance.h>
41 #include <blocking_detection_manager.h>
42 #include <robot_system.h>
43 #include <position_manager.h>
49 #include "i2c_commands.h"
50 #include "strat_utils.h"
53 /* return the distance between two points */
54 int16_t distance_between(int16_t x1, int16_t y1, int16_t x2, int16_t y2)
64 /* return the distance to a point in the area */
65 int16_t distance_from_robot(int16_t x, int16_t y)
67 return distance_between(position_get_x_s16(&mainboard.pos),
68 position_get_y_s16(&mainboard.pos), x, y);
71 /** do a modulo 360 -> [-180,+180], knowing that 'a' is in [-3*180,+3*180] */
72 int16_t simple_modulo_360(int16_t a)
83 /* return the distance to a point in the area */
84 int16_t angle_abs_to_rel(int16_t a_abs)
86 return simple_modulo_360(a_abs - position_get_a_deg_s16(&mainboard.pos));
89 void rel_da_to_abs_xy(double d_rel, double a_rel_rad,
90 double *x_abs, double *y_abs)
92 double x = position_get_x_double(&mainboard.pos);
93 double y = position_get_y_double(&mainboard.pos);
94 double a = position_get_a_rad_double(&mainboard.pos);
96 *x_abs = x + d_rel*cos(a+a_rel_rad);
97 *y_abs = y + d_rel*sin(a+a_rel_rad);
100 double norm(double x, double y)
102 return sqrt(x*x + y*y);
105 void rel_xy_to_abs_xy(double x_rel, double y_rel,
106 double *x_abs, double *y_abs)
109 d_rel = norm(x_rel, y_rel);
110 a_rel = atan2(y_rel, x_rel);
111 rel_da_to_abs_xy(d_rel, a_rel, x_abs, y_abs);
114 /* return an angle between -pi and pi */
115 void abs_xy_to_rel_da(double x_abs, double y_abs,
116 double *d_rel, double *a_rel_rad)
118 double x = position_get_x_double(&mainboard.pos);
119 double y = position_get_y_double(&mainboard.pos);
120 double a = position_get_a_rad_double(&mainboard.pos);
122 *a_rel_rad = atan2(y_abs - y, x_abs - x) - a;
123 if (*a_rel_rad < -M_PI) {
126 else if (*a_rel_rad > M_PI) {
129 *d_rel = norm(x_abs-x, y_abs-y);
132 void rotate(double *x, double *y, double rot)
144 /* return true if the point is in area */
145 uint8_t is_in_area(int16_t x, int16_t y, int16_t margin)
149 if (x > (AREA_X - margin))
153 if (y > (AREA_Y - margin))
159 /* return true if the point is in area */
160 uint8_t robot_is_in_area(int16_t margin)
162 return is_in_area(position_get_x_s16(&mainboard.pos),
163 position_get_y_s16(&mainboard.pos),
167 /* return true if we are near the disc */
168 uint8_t robot_is_near_disc(void)
170 if (distance_from_robot(CENTER_X, CENTER_Y) < DISC_PENTA_DIAG)
175 /* return 1 or 0 depending on which side of a line (y=cste) is the
176 * robot. works in red or green color. */
177 uint8_t y_is_more_than(int16_t y)
181 posy = position_get_y_s16(&mainboard.pos);
182 if (mainboard.our_color == I2C_COLOR_RED) {
189 if (posy < (AREA_Y-y))
196 /* return 1 or 0 depending on which side of a line (x=cste) is the
197 * robot. works in red or green color. */
198 uint8_t x_is_more_than(int16_t x)
202 posx = position_get_x_s16(&mainboard.pos);
209 int16_t sin_table[] = {
229 int16_t fast_sin(int16_t deg)
237 return sin_table[(deg*16)/90];
239 return sin_table[((180-deg)*16)/90];
241 return -sin_table[((deg-180)*16)/90];
243 return -sin_table[((360-deg)*16)/90];
246 int16_t fast_cos(int16_t deg)
248 return fast_sin(90+deg);
252 /* get the color of our robot */
253 uint8_t get_color(void)
255 return mainboard.our_color;
258 /* get the color of the opponent robot */
259 uint8_t get_opponent_color(void)
261 if (mainboard.our_color == I2C_COLOR_RED)
262 return I2C_COLOR_GREEN;
264 return I2C_COLOR_RED;
267 /* get the xy pos of the opponent robot */
268 int8_t get_opponent_xy(int16_t *x, int16_t *y)
271 *x = I2C_OPPONENT_NOT_THERE;
276 *x = sensorboard.opponent_x;
277 *y = sensorboard.opponent_y;
279 if (*x == I2C_OPPONENT_NOT_THERE)
285 /* get the da pos of the opponent robot */
286 int8_t get_opponent_da(int16_t *d, int16_t *a)
294 x_tmp = sensorboard.opponent_x;
295 *d = sensorboard.opponent_d;
296 *a = sensorboard.opponent_a;
298 if (x_tmp == I2C_OPPONENT_NOT_THERE)
304 /* get the da pos of the opponent robot */
305 int8_t get_opponent_xyda(int16_t *x, int16_t *y, int16_t *d, int16_t *a)
308 *x = I2C_OPPONENT_NOT_THERE;
313 *x = sensorboard.opponent_x;
314 *y = sensorboard.opponent_y;
315 *d = sensorboard.opponent_d;
316 *a = sensorboard.opponent_a;
318 if (*x == I2C_OPPONENT_NOT_THERE)
325 uint8_t pump_left1_is_full(void)
327 return !!( (mechboard.column_flags & I2C_MECHBOARD_COLUMN_L1) &&
328 (sensor_get_adc(ADC_CSENSE3) > I2C_MECHBOARD_CURRENT_COLUMN));
331 uint8_t pump_left2_is_full(void)
333 return !!( (mechboard.column_flags & I2C_MECHBOARD_COLUMN_L2) &&
334 (sensor_get_adc(ADC_CSENSE4) > I2C_MECHBOARD_CURRENT_COLUMN));
337 uint8_t pump_right1_is_full(void)
339 return !!( (mechboard.column_flags & I2C_MECHBOARD_COLUMN_R1) &&
340 (mechboard.pump_right1_current > I2C_MECHBOARD_CURRENT_COLUMN));
343 uint8_t pump_right2_is_full(void)
345 return !!( (mechboard.column_flags & I2C_MECHBOARD_COLUMN_R2) &&
346 (mechboard.pump_right2_current > I2C_MECHBOARD_CURRENT_COLUMN));
349 /* number of column owned by the robot */
350 uint8_t get_column_count_left(void)
353 ret += pump_left1_is_full();
354 ret += pump_left2_is_full();
358 /* number of column owned by the robot */
359 uint8_t get_column_count_right(void)
362 ret += pump_right1_is_full();
363 ret += pump_right2_is_full();
367 /* number of column owned by the robot */
368 uint8_t get_column_count(void)
371 ret += pump_left1_is_full();
372 ret += pump_left2_is_full();
373 ret += pump_right1_is_full();
374 ret += pump_right2_is_full();
378 uint8_t get_lintel_count(void)
380 return mechboard.lintel_count;
383 uint8_t get_mechboard_mode(void)
385 return mechboard.mode;
388 uint8_t get_scanner_status(void)
390 return sensorboard.scan_status;
393 /* return 0 if timeout, or 1 if cond is true */
394 uint8_t wait_scan_done(uint16_t timeout)
397 err = WAIT_COND_OR_TIMEOUT(get_scanner_status() & I2C_SCAN_DONE, timeout);
402 uint8_t opponent_is_behind(void)
405 int16_t opp_d = 0, opp_a = 0;
407 opp_there = get_opponent_da(&opp_d, &opp_a);
408 if (opp_there && (opp_a < 215 && opp_a > 145) && opp_d < 600)