coefs

[aversive.git] / projects / microb2010 / mainboard / strat_utils.c

diff --git a/projects/microb2010/mainboard/strat_utils.c b/projects/microb2010/mainboard/strat_utils.c
index 1f19904..9deba04 100644 (file)
--- a/projects/microb2010/mainboard/strat_utils.c
+++ b/projects/microb2010/mainboard/strat_utils.c
@@ -68,6 +68,17 @@ int16_t distance_between(int16_t x1, int16_t y1, int16_t x2, int16_t y2)
        return sqrt(x+y);
 }
 
+/* return the distance between two points */
+int32_t quad_distance_between(int16_t x1, int16_t y1, int16_t x2, int16_t y2)
+{
+       int32_t x,y;
+       x = (x2-x1);
+       x = x*x;
+       y = (y2-y1);
+       y = y*y;
+       return x+y;
+}
+
 /* return the distance to a point in the area */
 int16_t distance_from_robot(int16_t x, int16_t y)
 {
@@ -263,34 +274,31 @@ uint8_t get_opponent_color(void)
                return I2C_COLOR_YELLOW;
 }
 
-/* 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;
-
        IRQ_LOCK(flags);
-       *d = beaconboard.oppd;
-       *a = beaconboard.oppa;
-       x = beaconboard.oppx;
+       *x = ballboard.opponent_x;
+       *y = ballboard.opponent_y;
        IRQ_UNLOCK(flags);
-       if (x == I2C_OPPONENT_NOT_THERE)
+       if (*x == I2C_OPPONENT_NOT_THERE)
                return -1;
        return 0;
 }
 
-/* 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_tmp;
        IRQ_LOCK(flags);
-       *x = beaconboard.oppx;
-       *y = beaconboard.oppy;
+       x_tmp = ballboard.opponent_x;
+       *d = ballboard.opponent_d;
+       *a = ballboard.opponent_a;
        IRQ_UNLOCK(flags);
-       if (*x == I2C_OPPONENT_NOT_THERE)
+       if (x_tmp == I2C_OPPONENT_NOT_THERE)
                return -1;
-
        return 0;
 }
 
@@ -298,16 +306,14 @@ int8_t get_opponent_xy(int16_t *x, int16_t *y)
 int8_t get_opponent_xyda(int16_t *x, int16_t *y, int16_t *d, int16_t *a)
 {
        uint8_t flags;
-
        IRQ_LOCK(flags);
-       *x = beaconboard.oppx;
-       *y = beaconboard.oppy;
-       *d = beaconboard.oppd;
-       *a = beaconboard.oppa;
+       *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;
 }