X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fmainboard%2Fstrat_utils.c;h=1f19904a6f3322d41735c74c25722b9ac1cdbb51;hp=81a12d643e571350f6a3890228cb0126ab6d0b81;hb=51418f44261edc59d818ca990456726027e366ad;hpb=5918edd6f4f713ef3c8b0b0020dd30a4fb8222ae diff --git a/projects/microb2010/mainboard/strat_utils.c b/projects/microb2010/mainboard/strat_utils.c index 81a12d6..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 @@ -30,13 +30,14 @@ #include #include #include -#include +#include #include #include #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) { @@ -86,28 +87,16 @@ int16_t simple_modulo_360(int16_t a) return a; } -/** do a modulo 2.pi -> [-Pi,+Pi], knowing that 'a' is in [-3Pi,+3Pi] */ -double simple_modulo_2pi(double a) -{ - if (a < -M_PI) { - a += M_2PI; - } - else if (a > M_PI) { - a -= M_2PI; - } - return a; -} - /* return the distance to a point in the area */ 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); @@ -120,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; @@ -130,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; @@ -150,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); @@ -182,22 +171,14 @@ uint8_t robot_is_in_area(int16_t margin) margin); } -/* return true if we are near the disc */ -uint8_t robot_is_near_disc(void) -{ - if (distance_from_robot(CENTER_X, CENTER_Y) < DISC_PENTA_DIAG) - return 1; - return 0; -} - /* return 1 or 0 depending on which side of a line (y=cste) is the - * robot. works in red or green color. */ + * robot. works in yellow or blue color. */ 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_RED) { + if (mainboard.our_color == I2C_COLOR_YELLOW) { if (posy > y) return 1; else @@ -212,11 +193,11 @@ uint8_t y_is_more_than(int16_t y) } /* return 1 or 0 depending on which side of a line (x=cste) is the - * robot. works in red or green color. */ + * robot. works in yellow or blue color. */ uint8_t x_is_more_than(int16_t x) { int16_t posx; - + posx = position_get_x_s16(&mainboard.pos); if (posx > x) return 1; @@ -247,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]; @@ -276,37 +257,40 @@ uint8_t get_color(void) /* get the color of the opponent robot */ uint8_t get_opponent_color(void) { - if (mainboard.our_color == I2C_COLOR_RED) - return I2C_COLOR_GREEN; + if (mainboard.our_color == I2C_COLOR_YELLOW) + return I2C_COLOR_BLUE; else - return I2C_COLOR_RED; + 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; + int16_t x; + IRQ_LOCK(flags); - *x = sensorboard.opponent_x; - *y = sensorboard.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; + IRQ_LOCK(flags); - x_tmp = sensorboard.opponent_x; - *d = sensorboard.opponent_d; - *a = sensorboard.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; } @@ -314,100 +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; + IRQ_LOCK(flags); - *x = sensorboard.opponent_x; - *y = sensorboard.opponent_y; - *d = sensorboard.opponent_d; - *a = sensorboard.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 pump_left1_is_full(void) -{ - return !!( (mechboard.column_flags & I2C_MECHBOARD_COLUMN_L1) && - (sensor_get_adc(ADC_CSENSE3) > I2C_MECHBOARD_CURRENT_COLUMN)); -} - -uint8_t pump_left2_is_full(void) -{ - return !!( (mechboard.column_flags & I2C_MECHBOARD_COLUMN_L2) && - (sensor_get_adc(ADC_CSENSE4) > I2C_MECHBOARD_CURRENT_COLUMN)); -} - -uint8_t pump_right1_is_full(void) -{ - return !!( (mechboard.column_flags & I2C_MECHBOARD_COLUMN_R1) && - (mechboard.pump_right1_current > I2C_MECHBOARD_CURRENT_COLUMN)); -} - -uint8_t pump_right2_is_full(void) -{ - return !!( (mechboard.column_flags & I2C_MECHBOARD_COLUMN_R2) && - (mechboard.pump_right2_current > I2C_MECHBOARD_CURRENT_COLUMN)); -} - -/* number of column owned by the robot */ -uint8_t get_column_count_left(void) -{ - uint8_t ret = 0; - ret += pump_left1_is_full(); - ret += pump_left2_is_full(); - return ret; -} -/* number of column owned by the robot */ -uint8_t get_column_count_right(void) -{ - uint8_t ret = 0; - ret += pump_right1_is_full(); - ret += pump_right2_is_full(); - return ret; -} - -/* number of column owned by the robot */ -uint8_t get_column_count(void) -{ - uint8_t ret = 0; - ret += pump_left1_is_full(); - ret += pump_left2_is_full(); - ret += pump_right1_is_full(); - ret += pump_right2_is_full(); - return ret; -} - -uint8_t get_lintel_count(void) -{ - return mechboard.lintel_count; -} - -uint8_t get_mechboard_mode(void) -{ - return mechboard.mode; + return 0; } -uint8_t get_scanner_status(void) +int16_t distance_from_opponent(int16_t x, int16_t y) { - return sensorboard.scan_status; + int16_t oppx, oppy; + if (get_opponent_xy(&oppx, &oppy) < 0) + return -1; + return distance_between(x, y, oppx, oppy); } -/* return 0 if timeout, or 1 if cond is true */ -uint8_t wait_scan_done(uint16_t timeout) +uint8_t get_ball_count(void) { - uint8_t err; - err = WAIT_COND_OR_TIMEOUT(get_scanner_status() & I2C_SCAN_DONE, timeout); - return err; + return ballboard.ball_count; } -uint8_t opponent_is_behind(void) +uint8_t get_cob_count(void) { - int8_t opp_there; - int16_t opp_d, opp_a; - - 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; + return cobboard.cob_count; }