lots of work in opponent avoidance and strats

diff --git a/projects/microb2010/mainboard/strat.h b/projects/microb2010/mainboard/strat.h
index 8a2de56..2be7651 100644 (file)
--- a/projects/microb2010/mainboard/strat.h
+++ b/projects/microb2010/mainboard/strat.h
@@ -135,15 +135,15 @@
 #define ACC_ANGLE 16.
 
 /* default speeds */
 #define ACC_ANGLE 16.
 
 /* default speeds */

-#define SPEED_DIST_FAST 2500.
-#define SPEED_ANGLE_FAST 2000.
-#define SPEED_DIST_SLOW 1000.
-#define SPEED_ANGLE_SLOW 1000.
-#define SPEED_DIST_VERY_SLOW 400.
-#define SPEED_ANGLE_VERY_SLOW 400.
+#define SPEED_DIST_FAST 1500.
+#define SPEED_ANGLE_FAST 1000.
+#define SPEED_DIST_SLOW 500.
+#define SPEED_ANGLE_SLOW 500.
+#define SPEED_DIST_VERY_SLOW 200.
+#define SPEED_ANGLE_VERY_SLOW 200.

 
 

-#define SPEED_CLITOID_SLOW 100. ///250.
-#define SPEED_CLITOID_FAST 100. ///500.
+#define SPEED_CLITOID_SLOW 250.
+#define SPEED_CLITOID_FAST 500.

 
 
 /* strat infos structures */
 
 
 /* strat infos structures */


@@ -157,6 +157,7 @@ struct strat_conf {
 extern struct strat_conf strat_conf;
 extern volatile uint8_t strat_lpack60;
 extern volatile uint8_t strat_rpack60;
 extern struct strat_conf strat_conf;
 extern volatile uint8_t strat_lpack60;
 extern volatile uint8_t strat_rpack60;

+extern volatile uint8_t strat_want_pack;

 
 /* in strat.c */
 void strat_conf_dump(const char *caller);
 
 /* in strat.c */
 void strat_conf_dump(const char *caller);

diff --git a/projects/microb2010/mainboard/strat_avoid.c b/projects/microb2010/mainboard/strat_avoid.c
index 4f497ea..5211c4f 100644 (file)
--- a/projects/microb2010/mainboard/strat_avoid.c
+++ b/projects/microb2010/mainboard/strat_avoid.c
@@ -89,7 +89,7 @@ const struct wp_coord butterfly_tab[] = {
        { .i = 7, .j = 4, },
        { .i = 6, .j = 4, },
        { .i = 5, .j = 3, },
        { .i = 7, .j = 4, },
        { .i = 6, .j = 4, },
        { .i = 5, .j = 3, },

-       { .i = 4, .j = 4, },
+       { .i = 4, .j = 3, },

        { .i = 3, .j = 2, },
        { .i = 2, .j = 2, },
        { .i = 1, .j = 1, },
        { .i = 3, .j = 2, },
        { .i = 2, .j = 2, },
        { .i = 1, .j = 1, },


@@ -281,6 +281,40 @@ const struct circuit big_h_lambda_circuit = {
        .path = big_h_lambda_tab,
 };
 
        .path = big_h_lambda_tab,
 };
 

+const struct wp_coord letter_v_tab[] = {
+       { .i = 11, .j = 6, },
+       { .i = 10, .j = 6, },
+       { .i = 9, .j = 5, },
+       { .i = 8, .j = 5, },
+       { .i = 7, .j = 4, },
+       { .i = 6, .j = 4, },
+       { .i = 5, .j = 4, },
+       { .i = 4, .j = 5, },
+       { .i = 3, .j = 5, },
+       { .i = 2, .j = 6, },
+       { .i = 1, .j = 6, },
+       { .i = 1, .j = 5, },
+       { .i = 1, .j = 4, },
+       { .i = 2, .j = 4, },
+       { .i = 3, .j = 3, },
+       { .i = 4, .j = 3, },
+       { .i = 5, .j = 2, },
+       { .i = 6, .j = 2, },
+       { .i = 7, .j = 2, },
+       { .i = 8, .j = 3, },
+       { .i = 9, .j = 3, },
+       { .i = 10, .j = 4, },
+       { .i = 11, .j = 4, },
+       { .i = 11, .j = 5, },
+       { .i = 11, .j = 6, },
+};
+
+const struct circuit letter_v_circuit = {
+       .name = "letter_v",
+       .len = sizeof(letter_v_tab)/sizeof(struct wp_coord),
+       .path = letter_v_tab,
+};
+

 /* list of all possible circuits */
 const struct circuit *circuits[] = {
        &butterfly_circuit,
 /* list of all possible circuits */
 const struct circuit *circuits[] = {
        &butterfly_circuit,


@@ -290,6 +324,7 @@ const struct circuit *circuits[] = {
        &h_lambda_circuit,
        &asym_butterfly_circuit,
        &big_h_lambda_circuit,
        &h_lambda_circuit,
        &asym_butterfly_circuit,
        &big_h_lambda_circuit,

+       &letter_v_circuit,

        NULL,
 };
 
        NULL,
 };
 


@@ -516,7 +551,7 @@ static int8_t get_path(const struct circuit *circuit,
                start = end + circuit->len - 1;
        }
 
                start = end + circuit->len - 1;
        }
 

-       DPR("face: %s %d\r\n", circuit->name, faceA);
+       DPR("%s(): %s face=%d\r\n", __FUNCTION__, circuit->name, faceA);

 
        /* check that the point is present in the circuit */
        for (curcircuit = start; curcircuit != end; curcircuit += step) {
 
        /* check that the point is present in the circuit */
        for (curcircuit = start; curcircuit != end; curcircuit += step) {


@@ -545,6 +580,8 @@ static int8_t get_path(const struct circuit *circuit,
                        linenum = get_line_num(prev_i, prev_j, dir);
                        circuit_wpline[path_len].line_num = linenum;
                        circuit_wpline[path_len].dir = dir;
                        linenum = get_line_num(prev_i, prev_j, dir);
                        circuit_wpline[path_len].line_num = linenum;
                        circuit_wpline[path_len].dir = dir;

+                       DPR("%s(): %d %d -> %d %d / len=%d num=%d dir=%d\r\n",
+                           __FUNCTION__, prev_i, prev_j, i, j, path_len, linenum, dir);

                        path_len++;
                }
        }
                        path_len++;
                }
        }


@@ -660,7 +697,7 @@ static int8_t evaluate_one_face(const struct circuit *circuit,
        uint8_t ni = 0, nj = 0;
        uint8_t dir, color, idx;
        int8_t step = faceA ? 1 : -1;
        uint8_t ni = 0, nj = 0;
        uint8_t dir, color, idx;
        int8_t step = faceA ? 1 : -1;

-       int16_t x, y, d;
+       int16_t x, y, d, prev_d = 0;

        int16_t oppx, oppy;
 
        *score = 0x8000; /* -int_max */
        int16_t oppx, oppy;
 
        *score = 0x8000; /* -int_max */


@@ -690,13 +727,15 @@ static int8_t evaluate_one_face(const struct circuit *circuit,
        /* get opponent coords */
        if (get_opponent_xy(&oppx, &oppy) < 0)
                oppx = I2C_OPPONENT_NOT_THERE;
        /* get opponent coords */
        if (get_opponent_xy(&oppx, &oppy) < 0)
                oppx = I2C_OPPONENT_NOT_THERE;

+       else
+               DPR("%s() opponent: %d, %d\r\n", __FUNCTION__, oppx, oppy);

 
        /* silent the compiler */
        prev_i = 0xff;
        prev_j = 0xff;
 
        /* browse all points and calculate the score */
 
        /* silent the compiler */
        prev_i = 0xff;
        prev_j = 0xff;
 
        /* browse all points and calculate the score */

-       for (curcircuit = start;
+       for (; /* start at starti,startj */

             curcircuit != end;
             curcircuit += step, len ++, prev_i = i, prev_j = j) {
                i = curcircuit->i;
             curcircuit != end;
             curcircuit += step, len ++, prev_i = i, prev_j = j) {
                i = curcircuit->i;


@@ -706,8 +745,11 @@ static int8_t evaluate_one_face(const struct circuit *circuit,
                ijcoord_to_xycoord(i, j, &x, &y);
                if (oppx != I2C_OPPONENT_NOT_THERE) {
                        d = distance_between(oppx, oppy, x, y);
                ijcoord_to_xycoord(i, j, &x, &y);
                if (oppx != I2C_OPPONENT_NOT_THERE) {
                        d = distance_between(oppx, oppy, x, y);

-                       if (d < 600)
+                       DPR("%s(): opp at %d mm (ij=%d,%d opp=%d,%d pos=%d,%d)\r\n",
+                           __FUNCTION__, d, i, j, oppx, oppy, x, y);
+                       if (d < 600 && d < prev_d)

                                opponent_on_path = 1;
                                opponent_on_path = 1;

+                       prev_d = d;

                }
 
                /* don't try to look cobs/tomato for first point */
                }
 
                /* don't try to look cobs/tomato for first point */


@@ -785,9 +827,9 @@ static int8_t evaluate_one_circuit(const struct circuit *circuit,
 }
 
 /* i,j starting position */
 }
 
 /* i,j starting position */

-int8_t find_best_circuit(uint8_t i, uint8_t j,
-                      const struct circuit **selected_circuit,
-                      int8_t *selected_face)
+static int8_t find_best_circuit(uint8_t i, uint8_t j,
+                               const struct circuit **selected_circuit,
+                               int8_t *selected_face)

 {
        const struct circuit **circuit;
        int16_t scoreA, scoreB;
 {
        const struct circuit **circuit;
        int16_t scoreA, scoreB;


@@ -805,12 +847,12 @@ int8_t find_best_circuit(uint8_t i, uint8_t j,
                if (scoreA > selected_score) {
                        *selected_circuit = *circuit;
                        selected_score = scoreA;
                if (scoreA > selected_score) {
                        *selected_circuit = *circuit;
                        selected_score = scoreA;

-                       *selected_face = 0;
+                       *selected_face = 1;

                }
                if (scoreB > selected_score) {
                        *selected_circuit = *circuit;
                        selected_score = scoreB;
                }
                if (scoreB > selected_score) {
                        *selected_circuit = *circuit;
                        selected_score = scoreB;

-                       *selected_face = 1;
+                       *selected_face = 0;

                }
        }
 
                }
        }
 


@@ -823,6 +865,36 @@ int8_t find_best_circuit(uint8_t i, uint8_t j,
        return found;
 }
 
        return found;
 }
 

+static void test_all_circuits(void)
+{
+       const struct circuit **circuit;
+       const struct wp_coord *cur;
+       const struct wp_coord *start;
+       const struct wp_coord *end;
+       uint8_t prev_i, prev_j, i, j, dir;
+
+       for (circuit = &circuits[0]; *circuit; circuit++) {
+               start = &(*circuit)->path[0];
+               end = start + (*circuit)->len - 1;
+
+               prev_i = start->i;
+               prev_j = start->j;
+               start ++;
+
+               for (cur = start; cur != end;
+                    cur ++, prev_i = i, prev_j = j) {
+
+                       i = cur->i;
+                       j = cur->j;
+
+                       dir = get_dir(prev_i, prev_j, i, j);
+                       if (dir == 0xFF)
+                               printf_P("Bad circuit %s %d %d\r\n", (*circuit)->name, i, j);
+               }
+       }
+}
+
+

 static void dump_circuit_wp(struct wp_line *circuit_wpline, int8_t len)
 {
        int8_t i;
 static void dump_circuit_wp(struct wp_line *circuit_wpline, int8_t len)
 {
        int8_t i;


@@ -849,6 +921,8 @@ uint8_t strat_harvest_circuit(void)
        uint8_t dir, prev_dir;
        uint8_t err;
 
        uint8_t dir, prev_dir;
        uint8_t err;
 

+       strat_set_speed(SPEED_CLITOID_SLOW, SPEED_ANGLE_SLOW);
+

        x = position_get_x_s16(&mainboard.pos);
        y = position_get_y_s16(&mainboard.pos);
 
        x = position_get_x_s16(&mainboard.pos);
        y = position_get_y_s16(&mainboard.pos);
 


@@ -875,36 +949,70 @@ uint8_t strat_harvest_circuit(void)
 
        prev_linenum = circuit_wpline[0].line_num;
        prev_dir = circuit_wpline[0].dir;
 
        prev_linenum = circuit_wpline[0].line_num;
        prev_dir = circuit_wpline[0].dir;

+
+       /* do all lines of circuit */

        for (idx = 1; idx < len; idx ++) {
        for (idx = 1; idx < len; idx ++) {

-       retry:

                linenum = circuit_wpline[idx].line_num;
                dir = circuit_wpline[idx].dir;
 
                linenum = circuit_wpline[idx].line_num;
                dir = circuit_wpline[idx].dir;
 

-               /* XXX basic opponent management */

                DEBUG(E_USER_STRAT, "%s(): line %d dir %d -> line %d dir %d",
                      __FUNCTION__, prev_linenum, prev_dir, linenum, dir);
                err = line2line(prev_linenum, prev_dir, linenum, dir,
                                TRAJ_FLAGS_NO_NEAR);
                DEBUG(E_USER_STRAT, "%s(): line %d dir %d -> line %d dir %d",
                      __FUNCTION__, prev_linenum, prev_dir, linenum, dir);
                err = line2line(prev_linenum, prev_dir, linenum, dir,
                                TRAJ_FLAGS_NO_NEAR);

-               if (!TRAJ_SUCCESS(err)) {
-                       strat_hardstop();
-                       time_wait_ms(2000);
-                       goto retry;
-               }
+               if (!TRAJ_SUCCESS(err))
+                       return err;

 
                prev_linenum = linenum;
                prev_dir = dir;
        }
 
 
                prev_linenum = linenum;
                prev_dir = dir;
        }
 

-       return END_TRAJ; // XXX
+       return END_TRAJ;

 }
 
 }
 

+/* try to unblock in any situation */

 uint8_t strat_unblock(void)
 {
 uint8_t strat_unblock(void)
 {

+       int16_t x, y;
+       uint8_t i, j;
+       uint16_t old_dspeed, old_aspeed;
+       uint8_t err;
+
+       DEBUG(E_USER_STRAT, "%s()", __FUNCTION__);
+
+       strat_want_pack = 1;
+       strat_get_speed(&old_dspeed, &old_aspeed);
+
+       strat_hardstop();
+       strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW);
+       x = position_get_x_s16(&mainboard.pos);
+       y = position_get_y_s16(&mainboard.pos);
+       if (xycoord_to_ijcoord(&x, &y, &i, &j) < 0) {
+               /* aie... go to center... but it's really a bad
+                * idea */
+               x = CENTER_X;
+               y = CENTER_Y;
+       }
+
+       /* XXX if opponent is too close, go back, or wait ? */
+
+       /* 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))
+               return err;
+
+       strat_set_speed(old_dspeed, old_aspeed);
+       strat_want_pack = 0;

        return END_TRAJ;
 }
 
 void test_strat_avoid(void)
 {
        return END_TRAJ;
 }
 
 void test_strat_avoid(void)
 {

+       test_all_circuits();
+

 #ifdef TEST_STRAT_AVOID
        uint8_t i, j;
        const struct circuit *selected_circuit;
 #ifdef TEST_STRAT_AVOID
        uint8_t i, j;
        const struct circuit *selected_circuit;

diff --git a/projects/microb2010/mainboard/strat_avoid.h b/projects/microb2010/mainboard/strat_avoid.h
index 493aca4..56de996 100644 (file)
--- a/projects/microb2010/mainboard/strat_avoid.h
+++ b/projects/microb2010/mainboard/strat_avoid.h
@@ -39,6 +39,9 @@ int8_t browse_circuits(uint8_t i, uint8_t j,
                       const struct wp_coord **selected_circuit,
                       int8_t *selected_face);
 
                       const struct wp_coord **selected_circuit,
                       int8_t *selected_face);
 

+/* try to unblock in any situation */
+uint8_t strat_unblock(void);
+

 /* harvest on the best circuit */
 uint8_t strat_harvest_circuit(void);
 
 /* harvest on the best circuit */
 uint8_t strat_harvest_circuit(void);
 

diff --git a/projects/microb2010/mainboard/strat_base.c b/projects/microb2010/mainboard/strat_base.c
index 6075271..1a89f4d 100644 (file)
--- a/projects/microb2010/mainboard/strat_base.c
+++ b/projects/microb2010/mainboard/strat_base.c
@@ -235,7 +235,7 @@ void strat_limit_speed_disable(void)
        strat_limit_speed_enabled = 0;
 }
 
        strat_limit_speed_enabled = 0;
 }
 

-/* called periodically */
+/* called periodically (note: disabled in 2010) */

 void strat_limit_speed(void)
 {
        uint16_t lim_d = 0, lim_a = 0;
 void strat_limit_speed(void)
 {
        uint16_t lim_d = 0, lim_a = 0;


@@ -326,7 +326,6 @@ void strat_start(void)
 /* 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)
 {

-#if 0

        int16_t x_rel, y_rel;
        int16_t opp_x, opp_y, opp_d, opp_a;
 
        int16_t x_rel, y_rel;
        int16_t opp_x, opp_y, opp_d, opp_a;
 


@@ -335,8 +334,7 @@ uint8_t strat_obstacle(void)
                return 0;
 
        /* no opponent detected */
                return 0;
 
        /* no opponent detected */

-       if (get_opponent_xyda(&opp_x, &opp_y,
-                             &opp_d, &opp_a))
+       if (get_opponent_xyda(&opp_x, &opp_y, &opp_d, &opp_a))

                return 0;
 
        /* save obstacle position */
                return 0;
 
        /* save obstacle position */


@@ -344,28 +342,9 @@ uint8_t strat_obstacle(void)
        opponent_obstacle.y = opp_y;
        opponent_obstacle.d = opp_d;
        opponent_obstacle.a = opp_a;
        opponent_obstacle.y = opp_y;
        opponent_obstacle.d = opp_d;
        opponent_obstacle.a = opp_a;

-#else /* belgium cup only */
-       int16_t x_rel, y_rel;
-       int16_t opp_d, opp_a;
-       double opp_x, opp_y;
-
-#ifdef HOST_VERSION
-       return 0;
-       if (time_get_s() >= 12 && time_get_s() <= 30)
-               return 1;
-#endif
-       return 0; /* XXX disabled */
-
-       if (!sensor_get(S_RCOB_WHITE))
-               return 0;

 
 

-       opp_a = 0;
-       opp_d = 300;
-
-       rel_da_to_abs_xy(opp_d, RAD(opp_a), &opp_x, &opp_y);

        if (!is_in_area(opp_x, opp_y, 250))
                return 0;
        if (!is_in_area(opp_x, opp_y, 250))
                return 0;

-#endif

 
        /* sensor are temporarily disabled */
        if (sensor_obstacle_is_disabled())
 
        /* sensor are temporarily disabled */
        if (sensor_obstacle_is_disabled())

diff --git a/projects/microb2010/mainboard/strat_corn.c b/projects/microb2010/mainboard/strat_corn.c
index 74dd8d1..c5f0523 100644 (file)
--- a/projects/microb2010/mainboard/strat_corn.c
+++ b/projects/microb2010/mainboard/strat_corn.c
@@ -315,7 +315,7 @@ static int8_t strat_calc_clitoid(uint8_t num1, uint8_t dir1,
        }
        /* hard turn, 120 deg */
        else {
        }
        /* hard turn, 120 deg */
        else {

-               radius = 120;//75;
+               radius = 75;

                if (diff_a_deg > 0)
                        beta_deg = 0;
                else
                if (diff_a_deg > 0)
                        beta_deg = 0;
                else


@@ -332,7 +332,6 @@ static int8_t strat_calc_clitoid(uint8_t num1, uint8_t dir1,
                strat_set_speed(SPEED_CLITOID_FAST, SPEED_ANGLE_SLOW);
        }
 
                strat_set_speed(SPEED_CLITOID_FAST, SPEED_ANGLE_SLOW);
        }
 

-       /* XXX check return value !! */

        ret = trajectory_clitoid(&mainboard.traj, l1.p1.x, l1.p1.y,
                                 line1_a_deg, 150., diff_a_deg, beta_deg,
                                 radius, xy_norm(l1.p1.x, l1.p1.y,
        ret = trajectory_clitoid(&mainboard.traj, l1.p1.x, l1.p1.y,
                                 line1_a_deg, 150., diff_a_deg, beta_deg,
                                 radius, xy_norm(l1.p1.x, l1.p1.y,


@@ -350,8 +349,10 @@ uint8_t line2line(uint8_t num1, uint8_t dir1, uint8_t num2,
 
  reprocess:
        ret = strat_calc_clitoid(num1, dir1, num2, dir2, &pack_spickles);
 
  reprocess:
        ret = strat_calc_clitoid(num1, dir1, num2, dir2, &pack_spickles);

-       if (ret < 0)
+       if (ret < 0) {

                DEBUG(E_USER_STRAT, "clitoid failed");
                DEBUG(E_USER_STRAT, "clitoid failed");

+               return END_ERROR;
+       }

 
        /* XXX what to do if cobboard is stucked */
 
 
        /* XXX what to do if cobboard is stucked */
 


@@ -360,8 +361,13 @@ uint8_t line2line(uint8_t num1, uint8_t dir1, uint8_t num2,
                                                     num2, dir2),
                                    flags);
 
                                                     num2, dir2),
                                    flags);
 

+       /* error during traj, or traj finished */
+       if (err != 0)
+               return err;
+

        /* the speed has to change */
        /* the speed has to change */

-       if (trajectory_get_state(&mainboard.traj) != RUNNING_CLITOID_CURVE)
+       if (err == 0 &&
+           trajectory_get_state(&mainboard.traj) != RUNNING_CLITOID_CURVE)

                goto reprocess;
 
        DEBUG(E_USER_STRAT, "clitoid started err=%d pack_spickles=%d",
                goto reprocess;
 
        DEBUG(E_USER_STRAT, "clitoid started err=%d pack_spickles=%d",


@@ -379,7 +385,7 @@ uint8_t line2line(uint8_t num1, uint8_t dir1, uint8_t num2,
                err = wait_traj_end(flags);
        }
 
                err = wait_traj_end(flags);
        }
 

-       DEBUG(E_USER_STRAT, "clitoid finished");
+       DEBUG(E_USER_STRAT, "clitoid finished, err=%d", err);

 
        strat_rpack60 = 0;
        strat_lpack60 = 0;
 
        strat_rpack60 = 0;
        strat_lpack60 = 0;

diff --git a/projects/microb2010/mainboard/strat_db.c b/projects/microb2010/mainboard/strat_db.c
index d6af32e..9fe4329 100644 (file)
--- a/projects/microb2010/mainboard/strat_db.c
+++ b/projects/microb2010/mainboard/strat_db.c
@@ -149,7 +149,10 @@ int8_t ijcoord_to_xycoord(uint8_t i, uint8_t j, int16_t *x, int16_t *y)
        if (i >= WAYPOINTS_NBX && j >= WAYPOINTS_NBY)
                return -1;
        *x = (OFFSET_CORN_X + i*STEP_CORN_X);
        if (i >= WAYPOINTS_NBX && j >= WAYPOINTS_NBY)
                return -1;
        *x = (OFFSET_CORN_X + i*STEP_CORN_X);

-       *y = COLOR_Y(OFFSET_CORN_Y + j*STEP_CORN_Y);
+       if (i&1)
+               *y = COLOR_Y(OFFSET_CORN_Y + j*STEP_CORN_Y + STEP_CORN_Y/2);
+       else
+               *y = COLOR_Y(OFFSET_CORN_Y + j*STEP_CORN_Y);

        return 0;
 }
 
        return 0;
 }