X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fmainboard%2Fstrat_utils.c;h=1f19904a6f3322d41735c74c25722b9ac1cdbb51;hp=06df8c7bc29a599875f084e996a5d634796f5086;hb=51418f44261edc59d818ca990456726027e366ad;hpb=31bb619b1046f561cff432cb116735b1423a8d89 diff --git a/projects/microb2010/mainboard/strat_utils.c b/projects/microb2010/mainboard/strat_utils.c index 06df8c7..1f19904 100644 --- a/projects/microb2010/mainboard/strat_utils.c +++ b/projects/microb2010/mainboard/strat_utils.c @@ -1,6 +1,6 @@ -/* +/* * Copyright Droids Corporation, Microb Technology (2009) - * + * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation; either version 2 of the License, or @@ -37,6 +37,7 @@ #include #include #include +#include #include #include #include @@ -74,7 +75,7 @@ int16_t distance_from_robot(int16_t x, int16_t y) position_get_y_s16(&mainboard.pos), x, y); } -/** do a modulo 360 -> [-180,+180], knowing that 'a' is in [-3*180,+3*180] */ +/** do a modulo 360 -> [-180,+180], knowing that 'a' is in [-3*180,+3*180] */ int16_t simple_modulo_360(int16_t a) { if (a < -180) { @@ -92,10 +93,10 @@ int16_t angle_abs_to_rel(int16_t a_abs) return simple_modulo_360(a_abs - position_get_a_deg_s16(&mainboard.pos)); } -void rel_da_to_abs_xy(double d_rel, double a_rel_rad, +void rel_da_to_abs_xy(double d_rel, double a_rel_rad, double *x_abs, double *y_abs) { - double x = position_get_x_double(&mainboard.pos); + double x = position_get_x_double(&mainboard.pos); double y = position_get_y_double(&mainboard.pos); double a = position_get_a_rad_double(&mainboard.pos); @@ -108,7 +109,7 @@ double norm(double x, double y) return sqrt(x*x + y*y); } -void rel_xy_to_abs_xy(double x_rel, double y_rel, +void rel_xy_to_abs_xy(double x_rel, double y_rel, double *x_abs, double *y_abs) { double d_rel, a_rel; @@ -118,13 +119,13 @@ void rel_xy_to_abs_xy(double x_rel, double y_rel, } /* return an angle between -pi and pi */ -void abs_xy_to_rel_da(double x_abs, double y_abs, +void abs_xy_to_rel_da(double x_abs, double y_abs, double *d_rel, double *a_rel_rad) { - double x = position_get_x_double(&mainboard.pos); + double x = position_get_x_double(&mainboard.pos); double y = position_get_y_double(&mainboard.pos); double a = position_get_a_rad_double(&mainboard.pos); - + *a_rel_rad = atan2(y_abs - y, x_abs - x) - a; if (*a_rel_rad < -M_PI) { *a_rel_rad += M_2PI; @@ -138,7 +139,7 @@ void abs_xy_to_rel_da(double x_abs, double y_abs, void rotate(double *x, double *y, double rot) { double l, a; - + l = norm(*x, *y); a = atan2(*y, *x); @@ -175,7 +176,7 @@ uint8_t robot_is_in_area(int16_t margin) uint8_t y_is_more_than(int16_t y) { int16_t posy; - + posy = position_get_y_s16(&mainboard.pos); if (mainboard.our_color == I2C_COLOR_YELLOW) { if (posy > y) @@ -196,7 +197,7 @@ uint8_t y_is_more_than(int16_t y) uint8_t x_is_more_than(int16_t x) { int16_t posx; - + posx = position_get_x_s16(&mainboard.pos); if (posx > x) return 1; @@ -227,15 +228,15 @@ int16_t sin_table[] = { int16_t fast_sin(int16_t deg) { deg %= 360; - + if (deg < 0) deg += 360; - if (deg < 90) + if (deg < 90) return sin_table[(deg*16)/90]; - else if (deg < 180) + else if (deg < 180) return sin_table[((180-deg)*16)/90]; - else if (deg < 270) + else if (deg < 270) return -sin_table[((deg-180)*16)/90]; else return -sin_table[((360-deg)*16)/90]; @@ -262,33 +263,34 @@ uint8_t get_opponent_color(void) return I2C_COLOR_YELLOW; } -/* get the xy pos of the opponent robot */ -int8_t get_opponent_xy(int16_t *x, int16_t *y) +/* get the da pos of the opponent robot */ +int8_t get_opponent_da(int16_t *d, int16_t *a) { uint8_t flags; - return -1; // XXX + int16_t x; + IRQ_LOCK(flags); -/* *x = ballboard.opponent_x; */ -/* *y = ballboard.opponent_y; */ + *d = beaconboard.oppd; + *a = beaconboard.oppa; + x = beaconboard.oppx; IRQ_UNLOCK(flags); - if (*x == I2C_OPPONENT_NOT_THERE) + if (x == I2C_OPPONENT_NOT_THERE) return -1; return 0; } -/* get the da pos of the opponent robot */ -int8_t get_opponent_da(int16_t *d, int16_t *a) +/* get the xy pos of the opponent robot */ +int8_t get_opponent_xy(int16_t *x, int16_t *y) { uint8_t flags; - int16_t x_tmp; - return -1; // XXX + IRQ_LOCK(flags); -/* x_tmp = ballboard.opponent_x; */ -/* *d = ballboard.opponent_d; */ -/* *a = ballboard.opponent_a; */ + *x = beaconboard.oppx; + *y = beaconboard.oppy; IRQ_UNLOCK(flags); - if (x_tmp == I2C_OPPONENT_NOT_THERE) + if (*x == I2C_OPPONENT_NOT_THERE) return -1; + return 0; } @@ -296,24 +298,33 @@ int8_t get_opponent_da(int16_t *d, int16_t *a) int8_t get_opponent_xyda(int16_t *x, int16_t *y, int16_t *d, int16_t *a) { uint8_t flags; - return -1; // XXX + IRQ_LOCK(flags); -/* *x = ballboard.opponent_x; */ -/* *y = ballboard.opponent_y; */ -/* *d = ballboard.opponent_d; */ -/* *a = ballboard.opponent_a; */ + *x = beaconboard.oppx; + *y = beaconboard.oppy; + *d = beaconboard.oppd; + *a = beaconboard.oppa; IRQ_UNLOCK(flags); if (*x == I2C_OPPONENT_NOT_THERE) return -1; + return 0; } -uint8_t opponent_is_behind(void) + +int16_t distance_from_opponent(int16_t x, int16_t y) +{ + int16_t oppx, oppy; + if (get_opponent_xy(&oppx, &oppy) < 0) + return -1; + return distance_between(x, y, oppx, oppy); +} + +uint8_t get_ball_count(void) { -/* int8_t opp_there; */ -/* int16_t opp_d, opp_a; */ + return ballboard.ball_count; +} -/* opp_there = get_opponent_da(&opp_d, &opp_a); */ -/* if (opp_there && (opp_a < 215 && opp_a > 145) && opp_d < 600) */ -/* return 1; */ - return 0; +uint8_t get_cob_count(void) +{ + return cobboard.cob_count; }