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.5 2009-05-02 10:08:09 zer0 Exp $
26 #include <aversive/pgmspace.h>
27 #include <aversive/wait.h>
28 #include <aversive/error.h>
32 #include <clock_time.h>
36 #include <control_system_manager.h>
37 #include <trajectory_manager.h>
38 #include <blocking_detection_manager.h>
39 #include <robot_system.h>
40 #include <position_manager.h>
46 #include "strat_base.h"
47 #include "strat_utils.h"
50 /* return the distance between two points */
51 int16_t distance_between(int16_t x1, int16_t y1, int16_t x2, int16_t y2)
61 /* return the distance to a point in the area */
62 int16_t distance_from_robot(int16_t x, int16_t y)
64 return distance_between(position_get_x_s16(&mainboard.pos),
65 position_get_y_s16(&mainboard.pos), x, y);
68 /** do a modulo 360 -> [-180,+180], knowing that 'a' is in [-3*180,+3*180] */
69 int16_t simple_modulo_360(int16_t a)
80 /* /\** do a modulo 2.pi -> [-Pi,+Pi], knowing that 'a' is in [-3Pi,+3Pi] *\/ */
81 /* double simple_modulo_2pi(double a) */
83 /* if (a < -M_PI) { */
86 /* else if (a > M_PI) { */
92 /* return the distance to a point in the area */
93 int16_t angle_abs_to_rel(int16_t a_abs)
95 return simple_modulo_360(a_abs - position_get_a_deg_s16(&mainboard.pos));
98 void rel_da_to_abs_xy(double d_rel, double a_rel_rad,
99 double *x_abs, double *y_abs)
101 double x = position_get_x_double(&mainboard.pos);
102 double y = position_get_y_double(&mainboard.pos);
103 double a = position_get_a_rad_double(&mainboard.pos);
105 *x_abs = x + d_rel*cos(a+a_rel_rad);
106 *y_abs = y + d_rel*sin(a+a_rel_rad);
109 double norm(double x, double y)
111 return sqrt(x*x + y*y);
114 void rel_xy_to_abs_xy(double x_rel, double y_rel,
115 double *x_abs, double *y_abs)
118 d_rel = norm(x_rel, y_rel);
119 a_rel = atan2(y_rel, x_rel);
120 rel_da_to_abs_xy(d_rel, a_rel, x_abs, y_abs);
123 /* return an angle between -pi and pi */
124 void abs_xy_to_rel_da(double x_abs, double y_abs,
125 double *d_rel, double *a_rel_rad)
127 double x = position_get_x_double(&mainboard.pos);
128 double y = position_get_y_double(&mainboard.pos);
129 double a = position_get_a_rad_double(&mainboard.pos);
131 *a_rel_rad = atan2(y_abs - y, x_abs - x) - a;
132 if (*a_rel_rad < -M_PI) {
135 else if (*a_rel_rad > M_PI) {
138 *d_rel = norm(x_abs-x, y_abs-y);
141 void rotate(double *x, double *y, double rot)
153 /* return true if the point is in area */
154 uint8_t is_in_area(int16_t x, int16_t y, int16_t margin)
158 if (x > (AREA_X - margin))
162 if (y > (AREA_Y - margin))
168 /* return true if the point is in area */
169 uint8_t robot_is_in_area(int16_t margin)
171 return is_in_area(position_get_x_s16(&mainboard.pos),
172 position_get_y_s16(&mainboard.pos),
176 /* /\* return true if we are near the disc *\/ */
177 /* uint8_t robot_is_near_disc(void) */
179 /* if (distance_from_robot(CENTER_X, CENTER_Y) < DISC_PENTA_DIAG) */
184 /* /\* return 1 or 0 depending on which side of a line (y=cste) is the */
185 /* * robot. works in yellow or blue color. *\/ */
186 /* uint8_t y_is_more_than(int16_t y) */
190 /* posy = position_get_y_s16(&mainboard.pos); */
191 /* if (mainboard.our_color == I2C_COLOR_YELLOW) { */
198 /* if (posy < (AREA_Y-y)) */
205 /* return 1 or 0 depending on which side of a line (x=cste) is the
206 * robot. works in yellow or blue color. */
207 uint8_t x_is_more_than(int16_t x)
211 posx = position_get_x_s16(&mainboard.pos);
218 int16_t sin_table[] = {
238 int16_t fast_sin(int16_t deg)
246 return sin_table[(deg*16)/90];
248 return sin_table[((180-deg)*16)/90];
250 return -sin_table[((deg-180)*16)/90];
252 return -sin_table[((360-deg)*16)/90];
255 int16_t fast_cos(int16_t deg)
257 return fast_sin(90+deg);
261 /* get the color of our robot */
262 uint8_t get_color(void)
264 return mainboard.our_color;
267 /* /\* get the color of the opponent robot *\/ */
268 /* uint8_t get_opponent_color(void) */
270 /* if (mainboard.our_color == I2C_COLOR_YELLOW) */
271 /* return I2C_COLOR_BLUE; */
273 /* return I2C_COLOR_YELLOW; */
276 /* /\* get the xy pos of the opponent robot *\/ */
277 /* int8_t get_opponent_xy(int16_t *x, int16_t *y) */
280 /* IRQ_LOCK(flags); */
281 /* *x = sensorboard.opponent_x; */
282 /* *y = sensorboard.opponent_y; */
283 /* IRQ_UNLOCK(flags); */
284 /* if (*x == I2C_OPPONENT_NOT_THERE) */
289 /* /\* get the da pos of the opponent robot *\/ */
290 /* int8_t get_opponent_da(int16_t *d, int16_t *a) */
294 /* IRQ_LOCK(flags); */
295 /* x_tmp = sensorboard.opponent_x; */
296 /* *d = sensorboard.opponent_d; */
297 /* *a = sensorboard.opponent_a; */
298 /* IRQ_UNLOCK(flags); */
299 /* 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 /* IRQ_LOCK(flags); */
309 /* *x = sensorboard.opponent_x; */
310 /* *y = sensorboard.opponent_y; */
311 /* *d = sensorboard.opponent_d; */
312 /* *a = sensorboard.opponent_a; */
313 /* IRQ_UNLOCK(flags); */
314 /* if (*x == I2C_OPPONENT_NOT_THERE) */
319 /* uint8_t pump_left1_is_full(void) */
321 /* return !!( (mechboard.column_flags & I2C_MECHBOARD_COLUMN_L1) && */
322 /* (sensor_get_adc(ADC_CSENSE3) > I2C_MECHBOARD_CURRENT_COLUMN)); */
325 /* uint8_t pump_left2_is_full(void) */
327 /* return !!( (mechboard.column_flags & I2C_MECHBOARD_COLUMN_L2) && */
328 /* (sensor_get_adc(ADC_CSENSE4) > I2C_MECHBOARD_CURRENT_COLUMN)); */
331 /* uint8_t pump_right1_is_full(void) */
333 /* return !!( (mechboard.column_flags & I2C_MECHBOARD_COLUMN_R1) && */
334 /* (mechboard.pump_right1_current > I2C_MECHBOARD_CURRENT_COLUMN)); */
337 /* uint8_t pump_right2_is_full(void) */
339 /* return !!( (mechboard.column_flags & I2C_MECHBOARD_COLUMN_R2) && */
340 /* (mechboard.pump_right2_current > I2C_MECHBOARD_CURRENT_COLUMN)); */
343 /* /\* number of column owned by the robot *\/ */
344 /* uint8_t get_column_count_left(void) */
346 /* uint8_t ret = 0; */
347 /* ret += pump_left1_is_full(); */
348 /* ret += pump_left2_is_full(); */
352 /* /\* number of column owned by the robot *\/ */
353 /* uint8_t get_column_count_right(void) */
355 /* uint8_t ret = 0; */
356 /* ret += pump_right1_is_full(); */
357 /* ret += pump_right2_is_full(); */
361 /* /\* number of column owned by the robot *\/ */
362 /* uint8_t get_column_count(void) */
364 /* uint8_t ret = 0; */
365 /* ret += pump_left1_is_full(); */
366 /* ret += pump_left2_is_full(); */
367 /* ret += pump_right1_is_full(); */
368 /* ret += pump_right2_is_full(); */
372 /* uint8_t get_lintel_count(void) */
374 /* return mechboard.lintel_count; */
377 /* uint8_t get_mechboard_mode(void) */
379 /* return mechboard.mode; */
382 /* uint8_t get_scanner_status(void) */
384 /* return sensorboard.scan_status; */
387 /* /\* return 0 if timeout, or 1 if cond is true *\/ */
388 /* uint8_t wait_scan_done(uint16_t timeout) */
391 /* err = WAIT_COND_OR_TIMEOUT(get_scanner_status() & I2C_SCAN_DONE, timeout); */
395 /* uint8_t opponent_is_behind(void) */
397 /* int8_t opp_there; */
398 /* int16_t opp_d, opp_a; */
400 /* opp_there = get_opponent_da(&opp_d, &opp_a); */
401 /* if (opp_there && (opp_a < 215 && opp_a > 145) && opp_d < 600) */