]> git.droids-corp.org - aversive.git/commitdiff
rework strat_avoid
authorzer0 <zer0@carbon.local>
Tue, 4 May 2010 17:18:58 +0000 (19:18 +0200)
committerzer0 <zer0@carbon.local>
Tue, 4 May 2010 17:18:58 +0000 (19:18 +0200)
projects/microb2010/mainboard/strat_avoid.c
projects/microb2010/mainboard/strat_avoid.h
projects/microb2010/mainboard/strat_corn.c
projects/microb2010/mainboard/strat_corn.h
projects/microb2010/mainboard/strat_db.c
projects/microb2010/mainboard/strat_utils.c
projects/microb2010/mainboard/strat_utils.h

index 36bc1634448b362d7cbdf26a06e41fee113b2c84..eb14b882fa8b07a7b5538a542fffa3dc87c32a2c 100644 (file)
 #include "sensor.h"
 #include "actuator.h"
 
+//#define VERBOSE
 
-const struct wp_coord circuit1[] = {
-       { .i = 11, .j = 6, .end = 0, },
-       { .i = 10, .j = 6, .end = 0, },
-       { .i = 9, .j = 5, .end = 0, },
-       { .i = 8, .j = 5, .end = 0, },
-       { .i = 7, .j = 4, .end = 0, },
-       { .i = 6, .j = 4, .end = 0, },
-       { .i = 5, .j = 4, .end = 0, },
-       { .i = 4, .j = 5, .end = 0, },
-       { .i = 3, .j = 5, .end = 0, },
-       { .i = 2, .j = 6, .end = 0, },
-       { .i = 1, .j = 6, .end = 0, },
-       { .i = 1, .j = 5, .end = 0, },
-       { .i = 1, .j = 4, .end = 0, },
-       { .i = 1, .j = 3, .end = 0, },
-       { .i = 1, .j = 2, .end = 0, },
-       { .i = 1, .j = 1, .end = 0, },
-       { .i = 2, .j = 2, .end = 0, },
-       { .i = 3, .j = 2, .end = 0, },
-       { .i = 4, .j = 3, .end = 0, },
-       { .i = 5, .j = 3, .end = 0, },
-       { .i = 6, .j = 4, .end = 0, },
-       { .i = 7, .j = 3, .end = 0, },
-       { .i = 8, .j = 3, .end = 0, },
-       { .i = 9, .j = 2, .end = 0, },
-       { .i = 10, .j = 2, .end = 0, },
-       { .i = 11, .j = 1, .end = 0, },
-       { .i = 11, .j = 2, .end = 0, },
-       { .i = 11, .j = 3, .end = 0, },
-       { .i = 11, .j = 4, .end = 0, },
-       { .i = 11, .j = 5, .end = 0, },
-       { .i = 11, .j = 6, .end = 1, },
+#ifdef VERBOSE
+#define DPR(fmt, ...) printf_P(PSTR(fmt), ##__VA_ARGS__)
+#else
+#define DPR(args...) do {} while (0)
+#endif
+
+struct circuit {
+       const char *name;
+       uint8_t len;
+       const struct wp_coord *path;
 };
 
-const struct wp_coord circuit2[] = {
-       { .i = 11, .j = 6, .end = 0, },
-       { .i = 10, .j = 6, .end = 0, },
-       { .i = 9, .j = 5, .end = 0, },
-       { .i = 9, .j = 4, .end = 0, },
-       { .i = 9, .j = 3, .end = 0, },
-       { .i = 10, .j = 4, .end = 0, },
-       { .i = 11, .j = 4, .end = 0, },
-       { .i = 11, .j = 5, .end = 0, },
-       { .i = 11, .j = 6, .end = 1, },
+const struct wp_coord butterfly_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 = 1, .j = 3, },
+       { .i = 1, .j = 2, },
+       { .i = 1, .j = 1, },
+       { .i = 2, .j = 2, },
+       { .i = 3, .j = 2, },
+       { .i = 4, .j = 3, },
+       { .i = 5, .j = 3, },
+       { .i = 6, .j = 4, },
+       { .i = 7, .j = 3, },
+       { .i = 8, .j = 3, },
+       { .i = 9, .j = 2, },
+       { .i = 10, .j = 2, },
+       { .i = 11, .j = 1, },
+       { .i = 11, .j = 2, },
+       { .i = 11, .j = 3, },
+       { .i = 11, .j = 4, },
+       { .i = 11, .j = 5, },
+       { .i = 11, .j = 6, },
+};
+
+const struct circuit butterfly = {
+       .name = "butterfly",
+       .len = sizeof(butterfly_tab)/sizeof(struct wp_coord),
+       .path = butterfly_tab,
+};
+
+const struct wp_coord small_tab[] = {
+       { .i = 11, .j = 6, },
+       { .i = 10, .j = 6, },
+       { .i = 9, .j = 5, },
+       { .i = 9, .j = 4, },
+       { .i = 9, .j = 3, },
+       { .i = 10, .j = 4, },
+       { .i = 11, .j = 4, },
+       { .i = 11, .j = 5, },
+       { .i = 11, .j = 6, },
+};
+
+const struct circuit small = {
+       .name = "small",
+       .len = sizeof(small)/sizeof(struct wp_coord),
+       .path = small_tab,
 };
 
 /* list of all possible circuits */
-const struct wp_coord *circuits[] = {
-       circuit1,
-       circuit2,
+const struct circuit *circuits[] = {
+       &butterfly,
+       &small,
        NULL,
 };
 
@@ -130,7 +155,11 @@ static inline uint8_t opposite_position(uint8_t pos)
        return pos;
 }
 
-#if 0
+#ifdef HOST_VERSION
+//#define TEST_STRAT_AVOID
+#endif
+
+#ifdef TEST_STRAT_AVOID
 static uint8_t cc;
 uint8_t xget_cob_count(void)
 {
@@ -154,6 +183,36 @@ uint8_t xtime_get_s(void)
 #define xtime_get_s() time_get_s()
 #endif
 
+/* return true if turn is at 60 deg */
+uint8_t is_60deg(uint8_t dir1, uint8_t dir2)
+{
+       int8_t turn;
+
+       turn = dir2-dir1;
+       if (turn < 0)
+               turn += 6;
+       if (turn == 1)
+               return 1;
+       if (turn == 5)
+               return 1;
+       return 0;
+}
+
+/* return true if turn is at 60 deg */
+uint8_t is_120deg(uint8_t dir1, uint8_t dir2)
+{
+       int8_t turn;
+
+       turn = dir2-dir1;
+       if (turn < 0)
+               turn += 6;
+       if (turn == 2)
+               return 1;
+       if (turn == 4)
+               return 1;
+       return 0;
+}
+
 /* get the neighbour of the point at specified dir, return -1 if
  * there is no neighbor */
 int8_t wp_get_neigh(uint8_t i, uint8_t j, uint8_t *ni, uint8_t *nj,
@@ -269,13 +328,10 @@ uint8_t corn_count_neigh(uint8_t i, uint8_t j)
        uint8_t dir, n = 0;
        uint8_t ni, nj;
 
-       for (dir = LINE_UP; dir <= LINE_R_DOWN; dir++) {
+       for (dir = LINE_UP; dir <= LINE_R_UP; dir++) {
                if (wp_get_neigh(i, j, &ni, &nj, dir) < 0)
                        continue;
 
-               //printf("i,j=%d,%d dir=%d, ni,nj=%d,%d\r\n",
-               //       i, j, dir, ni, nj);
-
                /* is there a corn cob ? */
                if (strat_db.wp_table[ni][nj].type == WP_TYPE_CORN &&
                    strat_db.wp_table[ni][nj].present &&
@@ -287,48 +343,52 @@ uint8_t corn_count_neigh(uint8_t i, uint8_t j)
 }
 
 
-int8_t get_path(const struct wp_coord *circuit,
-               uint8_t starti, uint8_t startj, uint8_t faceA,
-               struct wp_line *circuit_wpline)
+/* fill circuit_wpline table with waypoints from circuit starting at
+ * i,j and using selected face */
+static int8_t get_path(const struct circuit *circuit,
+                      uint8_t starti, uint8_t startj, uint8_t faceA,
+                      struct wp_line *circuit_wpline)
 {
        const struct wp_coord *curcircuit;
+       const struct wp_coord *start;
+       const struct wp_coord *end;
        uint8_t prev_i, prev_j;
        uint8_t dir, prev_dir = 0xFF;
        uint8_t found = 0, i = 0, j = 0;
        uint8_t linenum;
        int8_t step = faceA ? 1 : -1;
-       int8_t skipfirst=0;
        int8_t path_len = 0;
 
-       printf_P(PSTR("face: %d\r\n"), faceA);
-       if ( !faceA && circuit->i == 11 && circuit->j == 6)
-               skipfirst=1;
+       /* start and end of circuit */
+       if (faceA) {
+               start = &circuit->path[0];
+               end = start + circuit->len - 1;
+       }
+       else {
+               end = &circuit->path[0];
+               start = end + circuit->len - 1;
+       }
+
+       DPR("face: %s %d\r\n", circuit->name, faceA);
 
        /* check that the point is present in the circuit */
-       for (curcircuit = circuit + skipfirst; curcircuit->end == 0; curcircuit ++) {
+       for (curcircuit = start; curcircuit != end; curcircuit += step) {
                if (curcircuit->i == starti && curcircuit->j == startj) {
                        found = 1;
                        break;
                }
        }
-
-       if ( !faceA && curcircuit->i == 11 && curcircuit->j == 6)
-               found = 1;
        if (found == 0)
                return -1;
 
-       /* XXX len must be >= 1 */
-       /* XXX start = 11,6 */
 
+       /* browse the circuit from starti, startj in the specified
+        * direction, and fill the table when direction changes */
        prev_i = starti;
        prev_j = startj;
+       for ( ; curcircuit != end;
+             curcircuit += step, prev_dir = dir, prev_i = i, prev_j = j) {
 
-       curcircuit = curcircuit;
-       while (1) {
-               if (faceA && curcircuit->end)
-                       break;
-               else if (!faceA && curcircuit == circuit)
-                       break;
                i = curcircuit->i;
                j = curcircuit->j;
 
@@ -336,25 +396,20 @@ int8_t get_path(const struct wp_coord *circuit,
 
                if (prev_dir != dir) {
                        linenum = get_line_num(prev_i, prev_j, dir);
-                       /* printf_P(PSTR("COIN %d, %d, dir=%d linenum=%d\r\n"), */
-                       /*                               prev_i, prev_j, dir, linenum); */
                        circuit_wpline[path_len].line_num = linenum;
                        circuit_wpline[path_len].dir = dir;
                        path_len++;
                }
-               prev_dir = dir;
-               prev_i = i;
-               prev_j = j;
-               curcircuit += step;
        }
 
-/*     printf_P(PSTR("COIN %d, %d\r\n"), curcircuit->i, curcircuit->j); */
-
-       return path_len; /* XXX */
+       return path_len;
 }
 
-int16_t get_score(uint32_t wcorn_retrieved, uint32_t ucorn_retrieved,
-                 uint16_t tomato_retrieved, uint8_t len)
+/* process score from retrieved objects number, and circuit len */
+static int16_t get_score(uint32_t wcorn_retrieved,
+                        uint32_t ucorn_retrieved,
+                        uint16_t tomato_retrieved,
+                        uint8_t len, uint8_t opp_on_path)
 {
        int16_t score = 0;
        uint8_t i;
@@ -379,7 +434,8 @@ int16_t get_score(uint32_t wcorn_retrieved, uint32_t ucorn_retrieved,
                mask <<= 1UL;
        }
 
-       printf_P(PSTR("get score: cob %d \r\n"), n);
+       DPR("get score: cob %d (->%d)\r\n", n, n/2);
+
        /* score with tomato */
        n = xget_ball_count();
        mask = 1;
@@ -393,101 +449,200 @@ int16_t get_score(uint32_t wcorn_retrieved, uint32_t ucorn_retrieved,
                mask <<= 1UL;
        }
 
-       printf_P(PSTR("get score: ball %d \r\n"), n);
+       DPR("get score: ball %d\r\n", n);
+
        /* malus for long circuits */
        score -= (len * 20);
+       DPR("malus for length: %d\r\n", len * 20);
 
        /* double malus for long circuits if we don't have much
         * time */
 #define WP_SPEED 1
        if (len * WP_SPEED > (MATCH_TIME - xtime_get_s())) {
-               uint16_t extra;
+               int32_t extra;
                extra = (len * WP_SPEED) - (MATCH_TIME - xtime_get_s());
-               score -= (200 * extra);
+               extra = (200 * extra);
+               if (extra < 0) /* should not happen */
+                       extra = 0;
+               if (extra > 10000)
+                       extra = 10000;
+               score -= extra;
+               DPR("malus for length + time: %d\r\n", extra);
        }
 
-       /* XXX use direction of robot */
+       /* malus if there is opponent on the path */
+       if (opp_on_path) {
+               DPR("malus for opponent: 1000\r\n");
+               score -= 2000;
+       }
 
        return score;
 }
 
+/* return the corn type of specified coords: I2C_COB_WHITE,
+ * I2C_COB_UNKNOWN, or I2C_COB_NONE if it is black or not present */
+static uint8_t get_corn_type(uint8_t i, uint8_t j)
+{
+       uint8_t color;
+       /* is there a corn cob ? */
+       if (strat_db.wp_table[i][j].type == WP_TYPE_CORN &&
+           strat_db.wp_table[i][j].present) {
+               color = strat_db.wp_table[i][j].corn.color;
+               if (color == I2C_COB_WHITE)
+                       return I2C_COB_WHITE;
+               else if (color == I2C_COB_UNKNOWN)
+                       return I2C_COB_UNKNOWN;
+       }
+       return I2C_COB_NONE;
+}
+
 /* i,j: starting position */
-int8_t browse_one_circuit(const struct wp_coord *circuit,
-                         uint8_t starti, uint8_t startj,
-                         int16_t *scoreA, int16_t *scoreB)
+static int8_t evaluate_one_face(const struct circuit *circuit,
+                               uint8_t starti, uint8_t startj,
+                               uint8_t faceA, int16_t *score)
 {
        const struct wp_coord *curcircuit;
+       const struct wp_coord *start;
+       const struct wp_coord *end;
        uint32_t wcorn_retrieved = 0; /* bit mask */
        uint32_t ucorn_retrieved = 0; /* bit mask */
        uint16_t tomato_retrieved = 0; /* bit mask */
-       uint8_t len = 0, found = 0, i, j;
-       uint8_t ni = 0, nj = 0, pos, color;
+       uint8_t opponent_on_path = 0;
+       uint8_t len = 0, found = 0;
+       uint8_t i, j, prev_i, prev_j;
+       uint8_t ni = 0, nj = 0;
+       uint8_t dir, color, idx;
+       int8_t step = faceA ? 1 : -1;
+       int16_t x, y, d;
+       int16_t oppx, oppy;
+
+       *score = 0x8000; /* -int_max */
+
+       /* start and end of circuit */
+       if (faceA) {
+               start = &circuit->path[0];
+               end = start + circuit->len - 1;
+       }
+       else {
+               end = &circuit->path[0];
+               start = end + circuit->len - 1;
+       }
+
+       DPR("%s() face: %s %d\r\n", __FUNCTION__, circuit->name, faceA);
 
        /* check that the point is present in the circuit */
-       for (curcircuit = circuit; curcircuit->end == 0; curcircuit ++) {
+       for (curcircuit = start; curcircuit != end; curcircuit += step) {
                if (curcircuit->i == starti && curcircuit->j == startj) {
                        found = 1;
                        break;
                }
        }
-
        if (found == 0)
                return -1;
 
-       for (curcircuit = circuit; curcircuit->end == 0; curcircuit ++, len ++) {
+       /* get opponent coords */
+       if (get_opponent_xy(&oppx, &oppy) < 0)
+               oppx = I2C_OPPONENT_NOT_THERE;
+
+       /* silent the compiler */
+       prev_i = 0xff;
+       prev_j = 0xff;
+
+       /* browse all points and calculate the score */
+       for (curcircuit = start;
+            curcircuit != end;
+            curcircuit += step, len ++, prev_i = i, prev_j = j) {
                i = curcircuit->i;
                j = curcircuit->j;
 
-/*             printf("cur:%d,%d %x %x %x %d\n", i, j, */
-/*                    wcorn_retrieved, ucorn_retrieved, tomato_retrieved, len); */
-
-               /* face A completed */
-               if (i == starti && j == startj) {
-                       *scoreA = get_score(wcorn_retrieved, ucorn_retrieved,
-                                          tomato_retrieved, len);
-                       wcorn_retrieved = 0; /* bit mask */
-                       ucorn_retrieved = 0; /* bit mask */
-                       tomato_retrieved = 0; /* bit mask */
-                       len = 0;
+               /* is opponent near the point ? */
+               ijcoord_to_xycoord(i, j, &x, &y);
+               if (oppx != I2C_OPPONENT_NOT_THERE) {
+                       d = distance_between(oppx, oppy, x, y);
+                       if (d < 600)
+                               opponent_on_path = 1;
                }
 
+               /* don't try to look cobs/tomato for first point */
+               if (curcircuit == start)
+                       continue;
+
+               /* get current direction, we wil check cobs behind us
+                * on left and right */
+               dir = get_dir(prev_i, prev_j, i, j);
+
+               DPR("%d %d -> %d %d  (%d)\n", prev_i, prev_j, i, j, dir);
+
                /* is there a tomato ? */
                if (strat_db.wp_table[i][j].type == WP_TYPE_TOMATO &&
                    strat_db.wp_table[i][j].present) {
+                       DPR("  TOMATO\n");
                        tomato_retrieved |= (1UL << strat_db.wp_table[i][j].tomato.idx);
                }
 
-               /* browse all neighbours to see if there is cobs */
-               for (pos = LINE_UP; pos <= LINE_R_DOWN; pos++) {
-                       if (wp_get_neigh(i, j, &ni, &nj, pos) < 0)
-                               continue;
-
-                       /* is there a corn cob ? */
-                       if (strat_db.wp_table[ni][nj].type == WP_TYPE_CORN &&
-                           strat_db.wp_table[ni][nj].present) {
-                               color = strat_db.wp_table[ni][nj].corn.color;
-                               if (color == I2C_COB_WHITE)
-                                       wcorn_retrieved |= (1UL << strat_db.wp_table[ni][nj].corn.idx);
-                               else if (color == I2C_COB_UNKNOWN)
-                                       ucorn_retrieved |= (1UL << strat_db.wp_table[ni][nj].corn.idx);
+               /* behind left */
+               if (wp_get_neigh(i, j, &ni, &nj, (dir + 2) % 6) == 0) {
+                       color = get_corn_type(ni, nj);
+                       idx = strat_db.wp_table[ni][nj].corn.idx;
+                       if (color == I2C_COB_WHITE) {
+                               DPR("  LEFT WCORN (%d)\n", idx);
+                               wcorn_retrieved |= (1UL << idx);
+                       }
+                       else if (color == I2C_COB_UNKNOWN) {
+                               DPR("  LEFT UCORN (%d)\n", idx);
+                               ucorn_retrieved |= (1UL << idx);
+                       }
+               }
+
+               /* behind right */
+               if (wp_get_neigh(i, j, &ni, &nj, (dir + 4) % 6) == 0) {
+                       color = get_corn_type(ni, nj);
+                       idx = strat_db.wp_table[ni][nj].corn.idx;
+                       if (color == I2C_COB_WHITE) {
+                               DPR("  RIGHT WCORN (%d)\n", idx);
+                               wcorn_retrieved |= (1UL << idx);
+                       }
+                       else if (color == I2C_COB_UNKNOWN) {
+                               DPR("  RIGHT UCORN (%d)\n", idx);
+                               ucorn_retrieved |= (1UL << idx);
                        }
                }
-       };
 
-       *scoreB = get_score(wcorn_retrieved, ucorn_retrieved,
-                           tomato_retrieved, len);
-       if (circuit->i == starti && circuit->j == startj)
-               *scoreA = *scoreB;
+               /* prev_i, prev_j, len and curcircuit are updated in
+                * for (;;) */
+       }
+
+       /* write score and exit */
+       *score = get_score(wcorn_retrieved, ucorn_retrieved,
+                          tomato_retrieved, len, opponent_on_path);
+       return 0;
+}
+
+/* i,j: starting position */
+static int8_t evaluate_one_circuit(const struct circuit *circuit,
+                                  uint8_t starti, uint8_t startj,
+                                  int16_t *scoreA, int16_t *scoreB)
+{
+       if (evaluate_one_face(circuit, starti, startj, 1, scoreA) < 0)
+               return -1;
+
+       /* we are on eject point, scoreB is the same */
+       if (starti == 11 && startj == 6) {
+               *scoreB = *scoreA;
+               return 0;
+       }
 
+       if (evaluate_one_face(circuit, starti, startj, 0, scoreB) < 0)
+               return -1;
        return 0;
 }
 
 /* i,j starting position */
-int8_t browse_circuits(uint8_t i, uint8_t j,
-                      const struct wp_coord **selected_circuit,
+int8_t find_best_circuit(uint8_t i, uint8_t j,
+                      const struct circuit **selected_circuit,
                       int8_t *selected_face)
 {
-       const struct wp_coord **circuit;
+       const struct circuit **circuit;
        int16_t scoreA, scoreB;
        int16_t selected_score = 0x8000; /* ~-int_max */
        int8_t found = -1;
@@ -495,10 +650,11 @@ int8_t browse_circuits(uint8_t i, uint8_t j,
        *selected_face = 0;
        *selected_circuit = circuits[0] ;
        for (circuit = &circuits[0]; *circuit; circuit++) {
-               if (browse_one_circuit(*circuit, i, j, &scoreA, &scoreB) < 0)
+               if (evaluate_one_circuit(*circuit, i, j, &scoreA, &scoreB) < 0)
                        continue;
                found = 0;
-               printf_P(PSTR("Circuit: %d %d\r\n"), scoreA, scoreB);
+               DEBUG(E_USER_STRAT, "Scores for %s are: faceA=%d, faceB=%d",
+                     (*circuit)->name, scoreA, scoreB);
                if (scoreA > selected_score) {
                        *selected_circuit = *circuit;
                        selected_score = scoreA;
@@ -510,6 +666,13 @@ int8_t browse_circuits(uint8_t i, uint8_t j,
                        *selected_face = 1;
                }
        }
+
+       if (found == -1)
+               DEBUG(E_USER_STRAT, "no circuit found");
+       else
+               DEBUG(E_USER_STRAT, "circuit found: %s, %s",
+                     (*selected_circuit)->name,
+                     (*selected_face) ? "faceA":"faceB");
        return found;
 }
 
@@ -519,15 +682,17 @@ static void dump_circuit_wp(struct wp_line *circuit_wpline, int8_t len)
        if (len <= 0)
                return;
        for (i = 0; i < len; i ++) {
-               printf_P(PSTR("linenum %d dir %d\r\n"), circuit_wpline[i].line_num,
+               DEBUG(E_USER_STRAT, "linenum %d dir %d",
+                     circuit_wpline[i].line_num,
                       circuit_wpline[i].dir);
        }
 
 }
 
+/* choose a circuit, then harvest on this circuit */
 uint8_t strat_harvest_circuit(void)
 {
-       const struct wp_coord *selected_circuit;
+       const struct circuit *selected_circuit;
        int8_t selected_face;
        struct wp_line circuit_wpline[MAX_CIRCUIT_WPLINE];
        int8_t len;
@@ -546,7 +711,12 @@ uint8_t strat_harvest_circuit(void)
                return END_ERROR;
        }
 
-       browse_circuits(i, j, &selected_circuit, &selected_face);
+       if (find_best_circuit(i, j, &selected_circuit, &selected_face) < 0) {
+               DEBUG(E_USER_STRAT, "%s(): cannot find a good circuit",
+                     __FUNCTION__);
+               return END_ERROR;
+       }
+
        len = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
        if (len < 0) {
                DEBUG(E_USER_STRAT, "%s(): cannot find a path",
@@ -583,13 +753,9 @@ uint8_t strat_harvest_circuit(void)
 
 void test_strat_avoid(void)
 {
-
-       //corn_count_neigh(1, 3);
-
-
-#if 0
+#ifdef TEST_STRAT_AVOID
        uint8_t i, j;
-       const struct wp_coord *selected_circuit;
+       const struct circuit *selected_circuit;
        int8_t selected_face;
        struct wp_line circuit_wpline[MAX_CIRCUIT_WPLINE];
        int8_t ret;
@@ -599,37 +765,37 @@ void test_strat_avoid(void)
 
        ts = 0; bc = 0; cc = 0;
        printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
-       browse_circuits(i, j, &selected_circuit, &selected_face);
+       find_best_circuit(i, j, &selected_circuit, &selected_face);
        ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
        dump_circuit_wp(circuit_wpline, ret);
 
        ts = 0; bc = 3; cc = 0;
        printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
-       browse_circuits(i, j, &selected_circuit, &selected_face);
+       find_best_circuit(i, j, &selected_circuit, &selected_face);
        ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
        dump_circuit_wp(circuit_wpline, ret);
 
        ts = 0; bc = 4; cc = 0;
        printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
-       browse_circuits(i, j, &selected_circuit, &selected_face);
+       find_best_circuit(i, j, &selected_circuit, &selected_face);
        ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
        dump_circuit_wp(circuit_wpline, ret);
 
        ts = 0; bc = 3; cc = 5;
        printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
-       browse_circuits(i, j, &selected_circuit, &selected_face);
+       find_best_circuit(i, j, &selected_circuit, &selected_face);
        ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
        dump_circuit_wp(circuit_wpline, ret);
 
        ts = 0; bc = 4; cc = 5;
        printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
-       browse_circuits(i, j, &selected_circuit, &selected_face);
+       find_best_circuit(i, j, &selected_circuit, &selected_face);
        ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
        dump_circuit_wp(circuit_wpline, ret);
 
        ts = 80; bc = 0; cc = 0;
        printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
-       browse_circuits(i, j, &selected_circuit, &selected_face);
+       find_best_circuit(i, j, &selected_circuit, &selected_face);
        ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
        dump_circuit_wp(circuit_wpline, ret);
 
@@ -638,19 +804,19 @@ void test_strat_avoid(void)
 
        ts = 0; bc = 0; cc = 0;
        printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
-       browse_circuits(i, j, &selected_circuit, &selected_face);
+       find_best_circuit(i, j, &selected_circuit, &selected_face);
        ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
        dump_circuit_wp(circuit_wpline, ret);
 
        ts = 0; bc = 3; cc = 0;
        printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
-       browse_circuits(i, j, &selected_circuit, &selected_face);
+       find_best_circuit(i, j, &selected_circuit, &selected_face);
        ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
        dump_circuit_wp(circuit_wpline, ret);
 
        ts = 80; bc = 0; cc = 0;
        printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
-       browse_circuits(i, j, &selected_circuit, &selected_face);
+       find_best_circuit(i, j, &selected_circuit, &selected_face);
        ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
        dump_circuit_wp(circuit_wpline, ret);
 
@@ -659,19 +825,19 @@ void test_strat_avoid(void)
 
        ts = 0; bc = 0; cc = 0;
        printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
-       browse_circuits(i, j, &selected_circuit, &selected_face);
+       find_best_circuit(i, j, &selected_circuit, &selected_face);
        ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
        dump_circuit_wp(circuit_wpline, ret);
 
        ts = 0; bc = 3; cc = 0;
        printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
-       browse_circuits(i, j, &selected_circuit, &selected_face);
+       find_best_circuit(i, j, &selected_circuit, &selected_face);
        ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
        dump_circuit_wp(circuit_wpline, ret);
 
        ts = 80; bc = 0; cc = 0;
        printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
-       browse_circuits(i, j, &selected_circuit, &selected_face);
+       find_best_circuit(i, j, &selected_circuit, &selected_face);
        ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
        dump_circuit_wp(circuit_wpline, ret);
 #endif
index d7ac4ac025f3b837b2e2d1155d6c92b6f6bb8ecf..493aca4d9bfe50aca4136167ab1ba9ff7bab56d2 100644 (file)
@@ -27,7 +27,7 @@
 struct wp_coord {
        uint8_t i:4;
        uint8_t j:3;
-       uint8_t end:1;
+       uint8_t reserved:1;
 };
 
 struct wp_line {
@@ -54,6 +54,9 @@ uint8_t corn_count_neigh(uint8_t i, uint8_t j);
 /* 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);
 
+uint8_t is_60deg(uint8_t dir1, uint8_t dir2);
+uint8_t is_120deg(uint8_t dir1, uint8_t dir2);
+
 void test_strat_avoid(void);
 
 #endif
index 1f5e1d347c4868af3093b634cb242ef842498480..48eff29a45efc24b2f7cf9131d913ac75bb3c2c6 100644 (file)
@@ -168,15 +168,23 @@ static uint8_t clitoid_select_speed(uint8_t num1, uint8_t dir1,
                return 1;
        }
 
-/*     if (time_get_s() > 39) */
-/*             DEBUG(E_USER_STRAT, "i,j = (%d %d), count=%d", i, j, corn_count_neigh(i, j)); */
+/*     if (time_get_s() > 32) */
+/*             DEBUG(E_USER_STRAT, "i,j = (%d %d), count=%d", i, j, */
+/*                   corn_count_neigh(i, j)); */
 
        if (corn_count_neigh(i, j) == 2)
                return 1;
 
+       /* we are on intersection, let's go slow... but as we enter in
+        * the curve-part of the clitoid, we should not go there */
        if (wp_belongs_to_line(i, j, num2, dir2))
                return 0;
 
+       /* we can ge fast if it's a 60deg angle and if we checked the
+        * current point */
+       if (is_60deg(dir1, dir2))
+               return 0;
+
        /* get next point */
        if (wp_get_neigh(i, j, &i2, &j2, dir1) < 0) {
                DEBUG(E_USER_STRAT, "%s(): cannot get neigh1",
@@ -184,7 +192,7 @@ static uint8_t clitoid_select_speed(uint8_t num1, uint8_t dir1,
                return 1;
        }
 
-       /* if (i2, j2) belongs to next line, check corn in opposition */
+       /* if (i2, j2) belongs to next line, check corns */
        if (wp_belongs_to_line(i2, j2, num2, dir2)) {
                if (corn_count_neigh(i2, j2) > 0)
                        return 1;
@@ -199,7 +207,7 @@ static uint8_t clitoid_select_speed(uint8_t num1, uint8_t dir1,
                return 1;
        }
 
-       /* if (i3, j3) belongs to next line, check corn in opposition */
+       /* if (i3, j3) belongs to next line, check corns */
        if (wp_belongs_to_line(i3, j3, num2, dir2)) {
                if (corn_count_neigh(i2, j2) > 0 ||
                    corn_count_neigh(i3, j3) > 0)
@@ -316,11 +324,11 @@ static int8_t strat_calc_clitoid(uint8_t num1, uint8_t dir1,
 
        clitoid_slow = clitoid_select_speed(num1, dir1, num2, dir2);
        if (clitoid_slow) {
-               DEBUG(E_USER_STRAT, "slow clito\n");
+               DEBUG(E_USER_STRAT, "slow clito");
                strat_set_speed(SPEED_CLITOID_SLOW, SPEED_ANGLE_SLOW);
        }
        else {
-               DEBUG(E_USER_STRAT, "fast clito\n");
+               DEBUG(E_USER_STRAT, "fast clito");
                strat_set_speed(SPEED_CLITOID_FAST, SPEED_ANGLE_SLOW);
        }
 
index e84b83d34befb34c96c7b530147033b08b666a1d..6f597f1fd1190c05645ab40fced19cea6d4b6595 100644 (file)
  */
 
 #define LINE_UP     0
-#define LINE_DOWN   1
-#define LINE_R_UP   2
-#define LINE_L_DOWN 3
-#define LINE_L_UP   4
-#define LINE_R_DOWN 5
+#define LINE_L_UP   1
+#define LINE_L_DOWN 2
+#define LINE_DOWN   3
+#define LINE_R_DOWN 4
+#define LINE_R_UP   5
 
 struct line_2pts {
        point_t p1;
index f21dd575680310f78f25e2bc51ac3f53e1d54056..d6af32e188d316083ed71f4a6980fc9fabaecd4e 100644 (file)
@@ -495,7 +495,6 @@ void strat_db_init(void)
                        /* tomato */
                        idx = ijcoord_to_tomato_idx(i, j);
                        if (idx >= 0) {
-                               printf("%d %d\n", i, j);
                                wp->type = WP_TYPE_TOMATO;
                                wp->present = 1;
                                wp->tomato.idx = idx;
index 41def2cead75f9bd303a71fb68ec87cad5b6e0d3..33c1474ee5e995a7e6a52bfe26f266873a9ea566 100644 (file)
@@ -319,6 +319,14 @@ uint8_t opponent_is_behind(void)
        return 0;
 }
 
+int16_t distance_from_opponent(int16_t x, int16_t y)
+{
+       int16_t oppx, oppy;
+       if (get_opponent_xy(&oppx, &oppy) < 0)
+               return -1;
+       return distance_between(x, y, oppx, oppy);
+}
+
 uint8_t get_ball_count(void)
 {
        return ballboard.ball_count;
index 7bb43a747cbd173ce91d79f3980f84449e3d4227..020b7f1e2f4a0ee8c90e69504fad532211ea827d 100644 (file)
@@ -57,6 +57,7 @@ uint8_t get_opponent_color(void);
 int8_t get_opponent_xy(int16_t *x, int16_t *y);
 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);
+int16_t distance_from_opponent(int16_t x, int16_t y);
 uint8_t opponent_is_behind(void);
 
 uint8_t get_ball_count(void);