X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fmainboard%2Fstrat_utils.c;h=708f08a27310fd9c18ff01b4436fdd8e0110417a;hp=81a12d643e571350f6a3890228cb0126ab6d0b81;hb=fa8546ea39c7442ad3bf5a822a72a2b50a41045d;hpb=5918edd6f4f713ef3c8b0b0020dd30a4fb8222ae diff --git a/projects/microb2010/mainboard/strat_utils.c b/projects/microb2010/mainboard/strat_utils.c index 81a12d6..708f08a 100644 --- a/projects/microb2010/mainboard/strat_utils.c +++ b/projects/microb2010/mainboard/strat_utils.c @@ -30,13 +30,14 @@ #include #include #include -#include +#include #include #include #include #include #include +#include #include #include #include @@ -86,18 +87,6 @@ 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) { @@ -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,7 +193,7 @@ 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; @@ -276,19 +257,20 @@ 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) { uint8_t flags; + return -1; // XXX IRQ_LOCK(flags); - *x = sensorboard.opponent_x; - *y = sensorboard.opponent_y; +/* *x = ballboard.opponent_x; */ +/* *y = ballboard.opponent_y; */ IRQ_UNLOCK(flags); if (*x == I2C_OPPONENT_NOT_THERE) return -1; @@ -300,10 +282,11 @@ int8_t get_opponent_da(int16_t *d, int16_t *a) { uint8_t flags; int16_t x_tmp; + return -1; // XXX IRQ_LOCK(flags); - x_tmp = sensorboard.opponent_x; - *d = sensorboard.opponent_d; - *a = sensorboard.opponent_a; +/* x_tmp = ballboard.opponent_x; */ +/* *d = ballboard.opponent_d; */ +/* *a = ballboard.opponent_a; */ IRQ_UNLOCK(flags); if (x_tmp == I2C_OPPONENT_NOT_THERE) return -1; @@ -314,100 +297,24 @@ 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 = sensorboard.opponent_x; - *y = sensorboard.opponent_y; - *d = sensorboard.opponent_d; - *a = sensorboard.opponent_a; +/* *x = ballboard.opponent_x; */ +/* *y = ballboard.opponent_y; */ +/* *d = ballboard.opponent_d; */ +/* *a = ballboard.opponent_a; */ 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; -} - -uint8_t get_scanner_status(void) -{ - return sensorboard.scan_status; -} - -/* return 0 if timeout, or 1 if cond is true */ -uint8_t wait_scan_done(uint16_t timeout) -{ - uint8_t err; - err = WAIT_COND_OR_TIMEOUT(get_scanner_status() & I2C_SCAN_DONE, timeout); - return err; -} - uint8_t opponent_is_behind(void) { - int8_t opp_there; - int16_t opp_d, opp_a; +/* 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; +/* 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; }