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

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

diff --git a/projects/microb2010/mainboard/strat.c b/projects/microb2010/mainboard/strat.c
index 64562f5..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;
+       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);
@@ -263,26 +266,33 @@ static void check_corn(void)
        }
 
        /* 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 (!__y_is_more_than(l_yspickle, 600))
+               l_y_too_high_pack = 1;
 
        /* 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 (!__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 ||
-               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  */
@@ -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 ||
-               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 */
@@ -501,6 +511,7 @@ static uint8_t strat_eject(void)
        /* ball ejection */
        if (get_ball_count() > 0) {
                i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_EJECT);
+               time_wait_ms(300);
                trajectory_a_abs(&mainboard.traj, COLOR_A(70));
                err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
                if (!TRAJ_SUCCESS(err))
@@ -517,6 +528,10 @@ static uint8_t strat_eject(void)
                                     2000);
 #endif
        }
+       else {
+               /* to pack spickles */
+               time_wait_ms(300);
+       }
 
        if (get_cob_count() > 0) {
                /* half turn */
@@ -904,7 +919,12 @@ uint8_t run_to_the_hills(uint8_t orange_color)
        strat_hardstop();
        i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_TAKE_FORK);
 
-       time_wait_ms(1800);
+       time_wait_ms(1100);
+
+       trajectory_d_rel(&mainboard.traj, 15);
+       time_wait_ms(400);
+       strat_hardstop();
+       time_wait_ms(200);
 
        /* reach top, go down */
        trajectory_d_rel(&mainboard.traj, -HILL_LEN);