irrational unblocking, low speed obstacle bug and spicke pack when y too big

diff --git a/projects/microb2010/mainboard/display.py b/projects/microb2010/mainboard/display.py
index 583f9a8..ff5bb02 100644 (file)
--- a/projects/microb2010/mainboard/display.py
+++ b/projects/microb2010/mainboard/display.py
@@ -418,6 +418,8 @@ while True:
                 fw.write("l")
             elif k == "r":
                 fw.write("r")
                 fw.write("l")
             elif k == "r":
                 fw.write("r")

+            elif k == "b":
+                fw.write("b")

             elif k == "c":
                 robot_trail_list = []
             elif k == "x":
             elif k == "c":
                 robot_trail_list = []
             elif k == "x":

diff --git a/projects/microb2010/mainboard/robotsim.c b/projects/microb2010/mainboard/robotsim.c
index bf4b731..6971155 100644 (file)
--- a/projects/microb2010/mainboard/robotsim.c
+++ b/projects/microb2010/mainboard/robotsim.c
@@ -54,6 +54,8 @@
 #include "strat_utils.h"
 #include "main.h"
 
 #include "strat_utils.h"
 #include "main.h"
 

+uint8_t robotsim_blocking = 0;
+

 static int32_t l_pwm, r_pwm;
 static int32_t l_enc, r_enc;
 
 static int32_t l_pwm, r_pwm;
 static int32_t l_enc, r_enc;
 


@@ -264,6 +266,8 @@ void robotsim_update(void)
                pertl = 1;
        else if (cmd[0] == 'r')
                pertr = 1;
                pertl = 1;
        else if (cmd[0] == 'r')
                pertr = 1;

+       else if (cmd[0] == 'b')
+               robotsim_blocking = 1;

        if (cmd[0] == 'o') {
                if (sscanf(cmd, "opp %d %d", &oppx, &oppy) == 2) {
                        abs_xy_to_rel_da(oppx, oppy, &oppd, &oppa);
        if (cmd[0] == 'o') {
                if (sscanf(cmd, "opp %d %d", &oppx, &oppy) == 2) {
                        abs_xy_to_rel_da(oppx, oppy, &oppd, &oppa);

diff --git a/projects/microb2010/mainboard/robotsim.h b/projects/microb2010/mainboard/robotsim.h
index 7c1eccf..491f2b3 100644 (file)
--- a/projects/microb2010/mainboard/robotsim.h
+++ b/projects/microb2010/mainboard/robotsim.h
@@ -19,6 +19,8 @@
  *
  */
 
  *
  */
 

+extern uint8_t robotsim_blocking;
+

 int8_t robotsim_i2c(uint8_t addr, uint8_t *buf, uint8_t size);
 void robotsim_update(void);
 void robotsim_pwm(void *arg, int32_t val);
 int8_t robotsim_i2c(uint8_t addr, uint8_t *buf, uint8_t size);
 void robotsim_update(void);
 void robotsim_pwm(void *arg, int32_t val);

diff --git a/projects/microb2010/mainboard/strat.c b/projects/microb2010/mainboard/strat.c
index 7f7b9d9..41e7e8b 100644 (file)
--- a/projects/microb2010/mainboard/strat.c
+++ b/projects/microb2010/mainboard/strat.c
@@ -234,6 +234,9 @@ static void check_corn(void)
        static uint8_t prev_check_time;
        uint8_t cur_time;
        uint8_t need_lpack, need_rpack;
        static uint8_t prev_check_time;
        uint8_t cur_time;
        uint8_t need_lpack, need_rpack;

+       int16_t l_xspickle, l_yspickle;
+       int16_t r_xspickle, r_yspickle;
+       uint8_t l_y_too_high_pack = 0, r_y_too_high_pack = 0;

 
        /* read sensors from ballboard */
        IRQ_LOCK(flags);
 
        /* read sensors from ballboard */
        IRQ_LOCK(flags);


@@ -263,26 +266,33 @@ static void check_corn(void)
        }
 
        /* detect cob on left side */
        }
 
        /* detect cob on left side */

-       lcob_near = corn_is_near(&lidx, I2C_LEFT_SIDE);
+       lcob_near = corn_is_near(&lidx, I2C_LEFT_SIDE,
+                                &l_xspickle, &l_yspickle);

        if (lcob_near && lcob != I2C_COB_NONE) {
                if (strat_db.corn_table[lidx]->corn.color == I2C_COB_UNKNOWN)
                        DEBUG(E_USER_STRAT, "lcob %s %d",
                              lcob == I2C_COB_WHITE ? "white" : "black", lidx);
                corn_set_color(strat_db.corn_table[lidx], lcob);
        }
        if (lcob_near && lcob != I2C_COB_NONE) {
                if (strat_db.corn_table[lidx]->corn.color == I2C_COB_UNKNOWN)
                        DEBUG(E_USER_STRAT, "lcob %s %d",
                              lcob == I2C_COB_WHITE ? "white" : "black", lidx);
                corn_set_color(strat_db.corn_table[lidx], lcob);
        }

+       if (!__y_is_more_than(l_yspickle, 600))
+               l_y_too_high_pack = 1;

 
        /* detect cob on right side */
 
        /* detect cob on right side */

-       rcob_near = corn_is_near(&ridx, I2C_RIGHT_SIDE);
+       rcob_near = corn_is_near(&ridx, I2C_RIGHT_SIDE,
+                                &r_xspickle, &r_yspickle);

        if (rcob_near && rcob != I2C_COB_NONE) {
                if (strat_db.corn_table[ridx]->corn.color == I2C_COB_UNKNOWN)
                        DEBUG(E_USER_STRAT, "rcob %s %d",
                              rcob == I2C_COB_WHITE ? "white" : "black", ridx);
                corn_set_color(strat_db.corn_table[ridx], rcob);
        }
        if (rcob_near && rcob != I2C_COB_NONE) {
                if (strat_db.corn_table[ridx]->corn.color == I2C_COB_UNKNOWN)
                        DEBUG(E_USER_STRAT, "rcob %s %d",
                              rcob == I2C_COB_WHITE ? "white" : "black", ridx);
                corn_set_color(strat_db.corn_table[ridx], rcob);
        }

+       if (!__y_is_more_than(r_yspickle, 600))
+               r_y_too_high_pack = 1;

 
        /* control the cobboard mode for left spickle */
        need_lpack = get_cob_count() >= 5 || strat_want_pack ||
 
        /* control the cobboard mode for left spickle */
        need_lpack = get_cob_count() >= 5 || strat_want_pack ||

-               strat_lpack60 || strat_opponent_lpack;
+               strat_lpack60 || strat_opponent_lpack || l_y_too_high_pack;
+

        if (lcob_near && strat_db.corn_table[lidx]->present) {
                if (need_lpack) {
                        /* nothing  */
        if (lcob_near && strat_db.corn_table[lidx]->present) {
                if (need_lpack) {
                        /* nothing  */


@@ -319,7 +329,7 @@ static void check_corn(void)
 
        /* control the cobboard mode for right spickle */
        need_rpack = get_cob_count() >= 5 || strat_want_pack ||
 
        /* control the cobboard mode for right spickle */
        need_rpack = get_cob_count() >= 5 || strat_want_pack ||

-               strat_rpack60 || strat_opponent_rpack;
+               strat_rpack60 || strat_opponent_rpack || r_y_too_high_pack;

        if (rcob_near && strat_db.corn_table[ridx]->present) {
                if (need_rpack) {
                        /* nothing */
        if (rcob_near && strat_db.corn_table[ridx]->present) {
                if (need_rpack) {
                        /* nothing */

diff --git a/projects/microb2010/mainboard/strat_avoid.c b/projects/microb2010/mainboard/strat_avoid.c
index 73f97f7..37483ce 100644 (file)
--- a/projects/microb2010/mainboard/strat_avoid.c
+++ b/projects/microb2010/mainboard/strat_avoid.c
@@ -552,7 +552,7 @@ int16_t linedir2angle(uint8_t dir)
        case LINE_R_UP:
                return COLOR_A(30);
        case LINE_R_DOWN:
        case LINE_R_UP:
                return COLOR_A(30);
        case LINE_R_DOWN:

-               return COLOR_A(-90);
+               return COLOR_A(-30);

        case LINE_L_UP:
                return COLOR_A(150);
        case LINE_L_DOWN:
        case LINE_L_UP:
                return COLOR_A(150);
        case LINE_L_DOWN:


@@ -562,6 +562,25 @@ int16_t linedir2angle(uint8_t dir)
        }
 }
 
        }
 }
 

+int16_t get_nearest_dir_angle(int16_t a)
+{
+       uint8_t dir, min_dir = 0;
+       int16_t min_diff = 0x7FFF, diff;
+
+       for (dir = LINE_UP; dir <= LINE_R_UP; dir++) {
+               diff = abs(linedir2angle(dir) - a);
+               if (diff > 360)
+                       diff -= 360;
+               if (diff > 360)
+                       diff -= 360;
+               if (diff < min_diff) {
+                       min_diff = diff;
+                       min_dir = dir;
+               }
+       }
+       return linedir2angle(min_dir);
+}
+

 /* return true if a waypoint belongs to a line */
 uint8_t wp_belongs_to_line(uint8_t i, uint8_t j, uint8_t linenum, uint8_t dir)
 {
 /* return true if a waypoint belongs to a line */
 uint8_t wp_belongs_to_line(uint8_t i, uint8_t j, uint8_t linenum, uint8_t dir)
 {


@@ -1107,8 +1126,8 @@ const struct xy_point unblock_pts[] = {
 /* try to unblock in any situation */
 uint8_t strat_unblock(void)
 {
 /* try to unblock in any situation */
 uint8_t strat_unblock(void)
 {

-       int16_t x, y, posx, posy;
-       uint8_t i, j, k;
+       int16_t x, y, posx, posy, posa;
+       uint8_t i, j, k, cpt;

        uint16_t old_dspeed, old_aspeed;
        uint8_t err;
        uint16_t d_min = 0x7FFF, d;
        uint16_t old_dspeed, old_aspeed;
        uint8_t err;
        uint16_t d_min = 0x7FFF, d;


@@ -1120,6 +1139,8 @@ uint8_t strat_unblock(void)
        strat_get_speed(&old_dspeed, &old_aspeed);
 
        strat_hardstop();
        strat_get_speed(&old_dspeed, &old_aspeed);
 
        strat_hardstop();

+       posa = position_get_a_deg_s16(&mainboard.pos);
+

        strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW);
        posx = position_get_x_s16(&mainboard.pos);
        posy = position_get_y_s16(&mainboard.pos);
        strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW);
        posx = position_get_x_s16(&mainboard.pos);
        posy = position_get_y_s16(&mainboard.pos);


@@ -1148,10 +1169,32 @@ uint8_t strat_unblock(void)
        DEBUG(E_USER_STRAT, "%s() unblock point is %d,%d",
              __FUNCTION__, x, y);
 
        DEBUG(E_USER_STRAT, "%s() unblock point is %d,%d",
              __FUNCTION__, x, y);
 

-       /* XXX if opponent is too close, go back, or wait ? */
+       for (cpt = 0; cpt < 2; cpt++) {
+
+               /* go to nearest waypoint */
+               trajectory_goto_xy_abs(&mainboard.traj, x, y);
+               err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+               if (err == END_TIMER)
+                       return err;
+
+               if (TRAJ_SUCCESS(err))
+                       break;
+
+               if (cpt == 1)
+                       break;
+
+               /* aie... do a S */
+               trajectory_d_a_rel(&mainboard.traj, 100, 20);
+               err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+               trajectory_d_a_rel(&mainboard.traj, 100, -20);
+               err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+               trajectory_d_a_rel(&mainboard.traj, -100, -20);
+               err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+               trajectory_d_a_rel(&mainboard.traj, -100, 20);
+               err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+       }

 
 

-       /* go to nearest waypoint */
-       trajectory_goto_xy_abs(&mainboard.traj, x, y);
+       trajectory_a_abs(&mainboard.traj, get_nearest_dir_angle(posa));

        err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
        if (err == END_TIMER)
                return err;
        err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
        if (err == END_TIMER)
                return err;

diff --git a/projects/microb2010/mainboard/strat_base.c b/projects/microb2010/mainboard/strat_base.c
index 0b14384..5a819c9 100644 (file)
--- a/projects/microb2010/mainboard/strat_base.c
+++ b/projects/microb2010/mainboard/strat_base.c
@@ -59,10 +59,14 @@
 #include "strat.h"
 #include "sensor.h"
 #include "i2c_protocol.h"
 #include "strat.h"
 #include "sensor.h"
 #include "i2c_protocol.h"

+#include "robotsim.h"

 
 /* true if we want to interrupt a trajectory */
 static uint8_t traj_intr=0;
 
 
 /* true if we want to interrupt a trajectory */
 static uint8_t traj_intr=0;
 

+/* to debug state of strat_obstacle() */
+static uint8_t strat_obstacle_debug = 0;
+

 /* filled when a END_OBSTACLE is returned */
 struct opponent_obstacle opponent_obstacle;
 
 /* filled when a END_OBSTACLE is returned */
 struct opponent_obstacle opponent_obstacle;
 


@@ -382,7 +386,6 @@ void strat_start(void)
        strat_exit();
 }
 
        strat_exit();
 }
 

-uint8_t xxxdebug = 0;

 
 /* return true if we have to brake due to an obstacle */
 uint8_t strat_obstacle(void)
 
 /* return true if we have to brake due to an obstacle */
 uint8_t strat_obstacle(void)


@@ -391,22 +394,19 @@ uint8_t strat_obstacle(void)
        int16_t opp_x, opp_y, opp_d, opp_a;
 
        /* too slow */
        int16_t opp_x, opp_y, opp_d, opp_a;
 
        /* too slow */

-       if (ABS(mainboard.speed_d) < 0) {
-               if (xxxdebug != 1) {
-                       DEBUG(E_USER_STRAT, "XXX too slow");
-                       xxxdebug = 1;
+       if (ABS(mainboard.speed_d) < 100) {
+               if (strat_obstacle_debug != 1) {
+                       DEBUG(E_USER_STRAT, "disable opp, too slow");
+                       strat_obstacle_debug = 1;

                }
                return 0;
        }
 
        /* no opponent detected */
                }
                return 0;
        }
 
        /* no opponent detected */

-       if (get_opponent_xyda(&opp_x, &opp_y, &opp_d, &opp_a)) {
-               if (xxxdebug != 2) {
-                       DEBUG(E_USER_STRAT, "XXX no opp");
-                       DEBUG(E_USER_STRAT, "opponent d=%d, a=%d "
-                             "x=%d y=%d (speed_d=%d)",
-                             opp_d, opp_a, opp_x, opp_y, mainboard.speed_d);
-                       xxxdebug = 2;
+       if (get_opponent_xyda(&opp_x, &opp_y, &opp_d, &opp_a) < 0) {
+               if (strat_obstacle_debug != 2) {
+                       DEBUG(E_USER_STRAT, "no opponent found");
+                       strat_obstacle_debug = 2;

                }
                return 0;
        }
                }
                return 0;
        }


@@ -418,24 +418,24 @@ uint8_t strat_obstacle(void)
        opponent_obstacle.a = opp_a;
 
        if (!is_in_area(opp_x, opp_y, 250)) {
        opponent_obstacle.a = opp_a;
 
        if (!is_in_area(opp_x, opp_y, 250)) {

-               if (xxxdebug != 3) {
-                       DEBUG(E_USER_STRAT, "XXX not in area");
-                       DEBUG(E_USER_STRAT, "opponent d=%d, a=%d "
+               if (strat_obstacle_debug != 3) {
+                       DEBUG(E_USER_STRAT,
+                             "opponent not in area : d=%d, a=%d "

                              "x=%d y=%d (speed_d=%d)",
                              opp_d, opp_a, opp_x, opp_y, mainboard.speed_d);
                              "x=%d y=%d (speed_d=%d)",
                              opp_d, opp_a, opp_x, opp_y, mainboard.speed_d);

-                       xxxdebug = 3;
+                       strat_obstacle_debug = 3;

                }
                return 0;
        }
 
        /* sensor are temporarily disabled */
        if (sensor_obstacle_is_disabled()) {
                }
                return 0;
        }
 
        /* sensor are temporarily disabled */
        if (sensor_obstacle_is_disabled()) {

-               if (xxxdebug != 4) {
-                       DEBUG(E_USER_STRAT, "XXX disabled");
-                       DEBUG(E_USER_STRAT, "opponent d=%d, a=%d "
+               if (strat_obstacle_debug != 4) {
+                       DEBUG(E_USER_STRAT,
+                             "sensor are disabled: opponent d=%d, a=%d "

                              "x=%d y=%d (speed_d=%d)",
                              opp_d, opp_a, opp_x, opp_y, mainboard.speed_d);
                              "x=%d y=%d (speed_d=%d)",
                              opp_d, opp_a, opp_x, opp_y, mainboard.speed_d);

-                       xxxdebug = 4;
+                       strat_obstacle_debug = 4;

                }
                return 0;
        }
                }
                return 0;
        }


@@ -446,12 +446,11 @@ uint8_t strat_obstacle(void)
 
        /* opponent too far */
        if (opp_d > 650) {
 
        /* opponent too far */
        if (opp_d > 650) {

-               if (xxxdebug != 5) {
-                       DEBUG(E_USER_STRAT, "XXX too far");
-                       DEBUG(E_USER_STRAT, "opponent d=%d, a=%d "
+               if (strat_obstacle_debug != 5) {
+                       DEBUG(E_USER_STRAT, "opponent too far d=%d, a=%d "

                              "x=%d y=%d (speed_d=%d)",
                              opp_d, opp_a, opp_x, opp_y, mainboard.speed_d);
                              "x=%d y=%d (speed_d=%d)",
                              opp_d, opp_a, opp_x, opp_y, mainboard.speed_d);

-                       xxxdebug = 5;
+                       strat_obstacle_debug = 5;

                }
                return 0;
        }
                }
                return 0;
        }


@@ -474,12 +473,11 @@ uint8_t strat_obstacle(void)
                return 1;
        }
 
                return 1;
        }
 

-       if (xxxdebug != 6) {
-               DEBUG(E_USER_STRAT, "XXX not in cone");
-               DEBUG(E_USER_STRAT, "opponent d=%d, a=%d "
+       if (strat_obstacle_debug != 6) {
+               DEBUG(E_USER_STRAT, "opponent not in cone d=%d, a=%d "

                      "x=%d y=%d (speed_d=%d)",
                      opp_d, opp_a, opp_x, opp_y, mainboard.speed_d);
                      "x=%d y=%d (speed_d=%d)",
                      opp_d, opp_a, opp_x, opp_y, mainboard.speed_d);

-               xxxdebug = 6;
+               strat_obstacle_debug = 6;

        }
 
        return 0;
        }
 
        return 0;


@@ -549,6 +547,12 @@ uint8_t test_traj_end(uint8_t why)
                return END_OBSTACLE;
        }
 
                return END_OBSTACLE;
        }
 

+#ifdef HOST_VERSION
+       if (robotsim_blocking) {
+               robotsim_blocking = 0;
+               return END_BLOCKING;
+       }
+#endif

        return 0;
 }
 
        return 0;
 }
 

diff --git a/projects/microb2010/mainboard/strat_corn.c b/projects/microb2010/mainboard/strat_corn.c
index db672bf..8abef5b 100644 (file)
--- a/projects/microb2010/mainboard/strat_corn.c
+++ b/projects/microb2010/mainboard/strat_corn.c
@@ -70,7 +70,8 @@
 static volatile uint8_t clitoid_slow = 0;
 
 /* return 1 if there is a corn near, and fill the index ptr */
 static volatile uint8_t clitoid_slow = 0;
 
 /* return 1 if there is a corn near, and fill the index ptr */

-int8_t corn_is_near(uint8_t *corn_idx, uint8_t side)
+int8_t corn_is_near(uint8_t *corn_idx, uint8_t side,
+                   int16_t *xspickle, int16_t *yspickle)

 {
        /* XXX to be checked */
 #define SENSOR_CORN_DIST  225
 {
        /* XXX to be checked */
 #define SENSOR_CORN_DIST  225


@@ -92,6 +93,8 @@ int8_t corn_is_near(uint8_t *corn_idx, uint8_t side)
        }
        x_corn_int = x_corn;
        y_corn_int = y_corn;
        }
        x_corn_int = x_corn;
        y_corn_int = y_corn;

+       *xspickle = x_corn_int;
+       *yspickle = y_corn_int;

 
        wp = xycoord_to_corn_idx(&x_corn_int, &y_corn_int);
        if (wp == NULL)
 
        wp = xycoord_to_corn_idx(&x_corn_int, &y_corn_int);
        if (wp == NULL)


@@ -300,7 +303,7 @@ static int8_t strat_calc_clitoid(uint8_t num1, uint8_t dir1,
                }
                else {
                        beta_deg = 0;
                }
                else {
                        beta_deg = 0;

-                       *pack_spickles = I2C_RIGHT_SIDE;
+                       *pack_spickles = I2C_LEFT_SIDE;

                }
        }
        /* double 90 deg for half turn -- not used */
                }
        }
        /* double 90 deg for half turn -- not used */

diff --git a/projects/microb2010/mainboard/strat_corn.h b/projects/microb2010/mainboard/strat_corn.h
index 6f597f1..4f50cb6 100644 (file)
--- a/projects/microb2010/mainboard/strat_corn.h
+++ b/projects/microb2010/mainboard/strat_corn.h
@@ -33,7 +33,8 @@ struct line_2pts {
 };
 
 /* there is a corn near */
 };
 
 /* there is a corn near */

-int8_t corn_is_near(uint8_t *corn_idx, uint8_t side);
+int8_t corn_is_near(uint8_t *corn_idx, uint8_t side,
+                   int16_t *xspickle, int16_t *yspickle);

 
 /* go from line num1,dir1 to line num2,dir2. Uses trjectory flags
  * specified as argument and return END_xxx condition */
 
 /* go from line num1,dir1 to line num2,dir2. Uses trjectory flags
  * specified as argument and return END_xxx condition */

diff --git a/projects/microb2010/mainboard/strat_utils.c b/projects/microb2010/mainboard/strat_utils.c
index 9deba04..1b98929 100644 (file)
--- a/projects/microb2010/mainboard/strat_utils.c
+++ b/projects/microb2010/mainboard/strat_utils.c
@@ -184,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. */
 
 /* 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;
        if (mainboard.our_color == I2C_COLOR_YELLOW) {
                if (posy > y)
                        return 1;


@@ -205,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. */
 
 /* 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;
 }
 
        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,
 int16_t sin_table[] = {
        0,
        3211,

diff --git a/projects/microb2010/mainboard/strat_utils.h b/projects/microb2010/mainboard/strat_utils.h
index 2414c04..5c7b8e9 100644 (file)
--- a/projects/microb2010/mainboard/strat_utils.h
+++ b/projects/microb2010/mainboard/strat_utils.h
@@ -78,6 +78,8 @@ uint8_t robot_is_in_area(int16_t margin);
 uint8_t robot_is_near_disc(void);
 uint8_t y_is_more_than(int16_t y);
 uint8_t x_is_more_than(int16_t x);
 uint8_t robot_is_near_disc(void);
 uint8_t y_is_more_than(int16_t y);
 uint8_t x_is_more_than(int16_t x);

+uint8_t __y_is_more_than(int16_t posy, int16_t y);
+uint8_t __x_is_more_than(int16_t posx, int16_t x);

 int16_t fast_sin(int16_t deg);
 int16_t fast_cos(int16_t deg);
 uint8_t get_color(void);
 int16_t fast_sin(int16_t deg);
 int16_t fast_cos(int16_t deg);
 uint8_t get_color(void);