vt100: include pgmspace.h as we use PROGMEM macro

[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 33c1474..1b98929 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)
 {
@@ -173,11 +184,8 @@ uint8_t robot_is_in_area(int16_t margin)
 
 /* return 1 or 0 depending on which side of a line (y=cste) is the
  * robot. works in yellow or blue color. */
-uint8_t y_is_more_than(int16_t y)
+uint8_t __y_is_more_than(int16_t posy, int16_t y)
 {
-       int16_t posy;
-
-       posy = position_get_y_s16(&mainboard.pos);
        if (mainboard.our_color == I2C_COLOR_YELLOW) {
                if (posy > y)
                        return 1;
@@ -194,17 +202,32 @@ 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 yellow or blue color. */
-uint8_t x_is_more_than(int16_t x)
+uint8_t __x_is_more_than(int16_t posx, int16_t x)
 {
-       int16_t posx;
-
-       posx = position_get_x_s16(&mainboard.pos);
        if (posx > x)
                return 1;
        else
                return 0;
 }
 
+/* return 1 or 0 depending on which side of a line (y=cste) is the
+ * 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);
+       return __y_is_more_than(posy, y);
+}
+
+/* return 1 or 0 depending on which side of a line (x=cste) is the
+ * 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);
+       return __x_is_more_than(posx, x);
+}
+
 int16_t sin_table[] = {
        0,
        3211,
@@ -267,10 +290,9 @@ uint8_t get_opponent_color(void)
 int8_t get_opponent_xy(int16_t *x, int16_t *y)
 {
        uint8_t flags;
-       return -1; // XXX
        IRQ_LOCK(flags);
-/*     *x = ballboard.opponent_x; */
-/*     *y = ballboard.opponent_y; */
+       *x = ballboard.opponent_x;
+       *y = ballboard.opponent_y;
        IRQ_UNLOCK(flags);
        if (*x == I2C_OPPONENT_NOT_THERE)
                return -1;
@@ -282,11 +304,10 @@ 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 = ballboard.opponent_x; */
-/*     *d = ballboard.opponent_d; */
-/*     *a = ballboard.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;
@@ -297,27 +318,16 @@ 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 = 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 opponent_is_behind(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;
-}
 
 int16_t distance_from_opponent(int16_t x, int16_t y)
 {