X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fmainboard%2Fstrat_utils.c;h=1b98929ddcf7e926693e4447504a13b53e7da1fb;hp=1f19904a6f3322d41735c74c25722b9ac1cdbb51;hb=HEAD;hpb=51418f44261edc59d818ca990456726027e366ad

diff --git a/projects/microb2010/mainboard/strat_utils.c b/projects/microb2010/mainboard/strat_utils.c
index 1f19904..1b98929 100644
--- 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,
@@ -263,34 +286,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 +318,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;
 }