2 * Copyright Droids, Microb Technology (2010)
4 * This program is free software; you can redistribute it and/or modify
5 * it under the terms of the GNU General Public License as published by
6 * the Free Software Foundation; either version 2 of the License, or
7 * (at your option) any later version.
9 * This program is distributed in the hope that it will be useful,
10 * but WITHOUT ANY WARRANTY; without even the implied warranty of
11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 * GNU General Public License for more details.
14 * You should have received a copy of the GNU General Public License
15 * along with this program; if not, write to the Free Software
16 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
18 * Revision : $Id: strat.c,v 1.6 2009-11-08 17:24:33 zer0 Exp $
20 * Olivier MATZ <zer0@droids-corp.org>
30 #include <aversive/pgmspace.h>
31 #include <aversive/error.h>
36 #include <clock_time.h>
41 #include <control_system_manager.h>
42 #include <trajectory_manager.h>
43 #include <trajectory_manager_utils.h>
44 #include <trajectory_manager_core.h>
45 #include <vect_base.h>
48 #include <obstacle_avoidance.h>
49 #include <blocking_detection_manager.h>
50 #include <robot_system.h>
51 #include <position_manager.h>
53 #include <diagnostic.h>
58 #include "../common/i2c_commands.h"
59 #include "i2c_protocol.h"
63 #include "strat_base.h"
64 #include "strat_corn.h"
65 #include "strat_avoid.h"
66 #include "strat_utils.h"
73 #define DPR(fmt, ...) printf_P(PSTR(fmt), ##__VA_ARGS__)
75 #define DPR(args...) do {} while (0)
81 const struct wp_coord *path;
84 const struct wp_coord butterfly_tab[] = {
109 { .i = 10, .j = 2, },
110 { .i = 11, .j = 1, },
111 { .i = 11, .j = 2, },
112 { .i = 11, .j = 3, },
113 { .i = 11, .j = 4, },
114 { .i = 11, .j = 5, },
115 { .i = 11, .j = 6, },
118 const struct circuit butterfly = {
120 .len = sizeof(butterfly_tab)/sizeof(struct wp_coord),
121 .path = butterfly_tab,
124 const struct wp_coord small_tab[] = {
125 { .i = 11, .j = 6, },
126 { .i = 10, .j = 6, },
130 { .i = 10, .j = 4, },
131 { .i = 11, .j = 4, },
132 { .i = 11, .j = 5, },
133 { .i = 11, .j = 6, },
136 const struct circuit small = {
138 .len = sizeof(small)/sizeof(struct wp_coord),
142 /* list of all possible circuits */
143 const struct circuit *circuits[] = {
149 /* symetric neighbor position */
150 static inline uint8_t opposite_position(uint8_t pos)
159 //#define TEST_STRAT_AVOID
162 #ifdef TEST_STRAT_AVOID
164 uint8_t xget_cob_count(void)
170 uint8_t xget_ball_count(void)
176 uint8_t xtime_get_s(void)
181 #define xget_cob_count() get_cob_count()
182 #define xget_ball_count() get_ball_count()
183 #define xtime_get_s() time_get_s()
186 /* return true if turn is at 60 deg */
187 uint8_t is_60deg(uint8_t dir1, uint8_t dir2)
201 /* return true if turn is at 60 deg */
202 uint8_t is_120deg(uint8_t dir1, uint8_t dir2)
216 /* get the neighbour of the point at specified dir, return -1 if
217 * there is no neighbor */
218 int8_t wp_get_neigh(uint8_t i, uint8_t j, uint8_t *ni, uint8_t *nj,
247 if (i >= WAYPOINTS_NBX || j >= WAYPOINTS_NBY)
255 static uint8_t get_line_num(int8_t i, int8_t j, uint8_t dir)
276 static uint8_t get_dir(uint8_t prev_i, uint8_t prev_j,
277 uint8_t i, uint8_t j)
279 int8_t diff_i, diff_j;
284 if (diff_i == 0 && diff_j == 1)
286 if (diff_i == 0 && diff_j == -1)
289 if ((prev_i & 1) == 0) {
290 if (diff_i == 1 && diff_j == 0)
292 if (diff_i == 1 && diff_j == -1)
294 if (diff_i == -1 && diff_j == 0)
296 if (diff_i == -1 && diff_j == -1)
300 if (diff_i == 1 && diff_j == 1)
302 if (diff_i == 1 && diff_j == 0)
304 if (diff_i == -1 && diff_j == 1)
306 if (diff_i == -1 && diff_j == 0)
314 /* return true if a waypoint belongs to a line */
315 uint8_t wp_belongs_to_line(uint8_t i, uint8_t j, uint8_t linenum, uint8_t dir)
318 ln = get_line_num(i, j, dir);
324 /* count the number of non-black corns which are neighbors of
326 uint8_t corn_count_neigh(uint8_t i, uint8_t j)
331 for (dir = LINE_UP; dir <= LINE_R_UP; dir++) {
332 if (wp_get_neigh(i, j, &ni, &nj, dir) < 0)
335 /* is there a corn cob ? */
336 if (strat_db.wp_table[ni][nj].type == WP_TYPE_CORN &&
337 strat_db.wp_table[ni][nj].present &&
338 strat_db.wp_table[ni][nj].corn.color != I2C_COB_BLACK)
346 /* fill circuit_wpline table with waypoints from circuit starting at
347 * i,j and using selected face */
348 static int8_t get_path(const struct circuit *circuit,
349 uint8_t starti, uint8_t startj, uint8_t faceA,
350 struct wp_line *circuit_wpline)
352 const struct wp_coord *curcircuit;
353 const struct wp_coord *start;
354 const struct wp_coord *end;
355 uint8_t prev_i, prev_j;
356 uint8_t dir, prev_dir = 0xFF;
357 uint8_t found = 0, i = 0, j = 0;
359 int8_t step = faceA ? 1 : -1;
362 /* start and end of circuit */
364 start = &circuit->path[0];
365 end = start + circuit->len - 1;
368 end = &circuit->path[0];
369 start = end + circuit->len - 1;
372 DPR("face: %s %d\r\n", circuit->name, faceA);
374 /* check that the point is present in the circuit */
375 for (curcircuit = start; curcircuit != end; curcircuit += step) {
376 if (curcircuit->i == starti && curcircuit->j == startj) {
385 /* browse the circuit from starti, startj in the specified
386 * direction, and fill the table when direction changes */
389 for ( ; curcircuit != end;
390 curcircuit += step, prev_dir = dir, prev_i = i, prev_j = j) {
395 dir = get_dir(prev_i, prev_j, i, j);
397 if (prev_dir != dir) {
398 linenum = get_line_num(prev_i, prev_j, dir);
399 circuit_wpline[path_len].line_num = linenum;
400 circuit_wpline[path_len].dir = dir;
408 /* process score from retrieved objects number, and circuit len */
409 static int16_t get_score(uint32_t wcorn_retrieved,
410 uint32_t ucorn_retrieved,
411 uint16_t tomato_retrieved,
412 uint8_t len, uint8_t opp_on_path)
419 /* score with corn */
420 n = xget_cob_count() * 2;
421 for (i = 0; i<CORN_NB; i++) {
424 if (wcorn_retrieved & mask) {
430 if (ucorn_retrieved & mask) {
437 DPR("get score: cob %d (->%d)\r\n", n, n/2);
439 /* score with tomato */
440 n = xget_ball_count();
442 for (i = 0; i<TOMATO_NB; i++) {
445 if (tomato_retrieved & mask) {
452 DPR("get score: ball %d\r\n", n);
454 /* malus for long circuits */
456 DPR("malus for length: %d\r\n", len * 20);
458 /* double malus for long circuits if we don't have much
461 if (len * WP_SPEED > (MATCH_TIME - xtime_get_s())) {
463 extra = (len * WP_SPEED) - (MATCH_TIME - xtime_get_s());
464 extra = (200 * extra);
465 if (extra < 0) /* should not happen */
470 DPR("malus for length + time: %d\r\n", extra);
473 /* malus if there is opponent on the path */
475 DPR("malus for opponent: 1000\r\n");
482 /* return the corn type of specified coords: I2C_COB_WHITE,
483 * I2C_COB_UNKNOWN, or I2C_COB_NONE if it is black or not present */
484 static uint8_t get_corn_type(uint8_t i, uint8_t j)
487 /* is there a corn cob ? */
488 if (strat_db.wp_table[i][j].type == WP_TYPE_CORN &&
489 strat_db.wp_table[i][j].present) {
490 color = strat_db.wp_table[i][j].corn.color;
491 if (color == I2C_COB_WHITE)
492 return I2C_COB_WHITE;
493 else if (color == I2C_COB_UNKNOWN)
494 return I2C_COB_UNKNOWN;
499 /* i,j: starting position */
500 static int8_t evaluate_one_face(const struct circuit *circuit,
501 uint8_t starti, uint8_t startj,
502 uint8_t faceA, int16_t *score)
504 const struct wp_coord *curcircuit;
505 const struct wp_coord *start;
506 const struct wp_coord *end;
507 uint32_t wcorn_retrieved = 0; /* bit mask */
508 uint32_t ucorn_retrieved = 0; /* bit mask */
509 uint16_t tomato_retrieved = 0; /* bit mask */
510 uint8_t opponent_on_path = 0;
511 uint8_t len = 0, found = 0;
512 uint8_t i, j, prev_i, prev_j;
513 uint8_t ni = 0, nj = 0;
514 uint8_t dir, color, idx;
515 int8_t step = faceA ? 1 : -1;
519 *score = 0x8000; /* -int_max */
521 /* start and end of circuit */
523 start = &circuit->path[0];
524 end = start + circuit->len - 1;
527 end = &circuit->path[0];
528 start = end + circuit->len - 1;
531 DPR("%s() face: %s %d\r\n", __FUNCTION__, circuit->name, faceA);
533 /* check that the point is present in the circuit */
534 for (curcircuit = start; curcircuit != end; curcircuit += step) {
535 if (curcircuit->i == starti && curcircuit->j == startj) {
543 /* get opponent coords */
544 if (get_opponent_xy(&oppx, &oppy) < 0)
545 oppx = I2C_OPPONENT_NOT_THERE;
547 /* silent the compiler */
551 /* browse all points and calculate the score */
552 for (curcircuit = start;
554 curcircuit += step, len ++, prev_i = i, prev_j = j) {
558 /* is opponent near the point ? */
559 ijcoord_to_xycoord(i, j, &x, &y);
560 if (oppx != I2C_OPPONENT_NOT_THERE) {
561 d = distance_between(oppx, oppy, x, y);
563 opponent_on_path = 1;
566 /* don't try to look cobs/tomato for first point */
567 if (curcircuit == start)
570 /* get current direction, we wil check cobs behind us
571 * on left and right */
572 dir = get_dir(prev_i, prev_j, i, j);
574 DPR("%d %d -> %d %d (%d)\n", prev_i, prev_j, i, j, dir);
576 /* is there a tomato ? */
577 if (strat_db.wp_table[i][j].type == WP_TYPE_TOMATO &&
578 strat_db.wp_table[i][j].present) {
580 tomato_retrieved |= (1UL << strat_db.wp_table[i][j].tomato.idx);
584 if (wp_get_neigh(i, j, &ni, &nj, (dir + 2) % 6) == 0) {
585 color = get_corn_type(ni, nj);
586 idx = strat_db.wp_table[ni][nj].corn.idx;
587 if (color == I2C_COB_WHITE) {
588 DPR(" LEFT WCORN (%d)\n", idx);
589 wcorn_retrieved |= (1UL << idx);
591 else if (color == I2C_COB_UNKNOWN) {
592 DPR(" LEFT UCORN (%d)\n", idx);
593 ucorn_retrieved |= (1UL << idx);
598 if (wp_get_neigh(i, j, &ni, &nj, (dir + 4) % 6) == 0) {
599 color = get_corn_type(ni, nj);
600 idx = strat_db.wp_table[ni][nj].corn.idx;
601 if (color == I2C_COB_WHITE) {
602 DPR(" RIGHT WCORN (%d)\n", idx);
603 wcorn_retrieved |= (1UL << idx);
605 else if (color == I2C_COB_UNKNOWN) {
606 DPR(" RIGHT UCORN (%d)\n", idx);
607 ucorn_retrieved |= (1UL << idx);
611 /* prev_i, prev_j, len and curcircuit are updated in
615 /* write score and exit */
616 *score = get_score(wcorn_retrieved, ucorn_retrieved,
617 tomato_retrieved, len, opponent_on_path);
621 /* i,j: starting position */
622 static int8_t evaluate_one_circuit(const struct circuit *circuit,
623 uint8_t starti, uint8_t startj,
624 int16_t *scoreA, int16_t *scoreB)
626 if (evaluate_one_face(circuit, starti, startj, 1, scoreA) < 0)
629 /* we are on eject point, scoreB is the same */
630 if (starti == 11 && startj == 6) {
635 if (evaluate_one_face(circuit, starti, startj, 0, scoreB) < 0)
640 /* i,j starting position */
641 int8_t find_best_circuit(uint8_t i, uint8_t j,
642 const struct circuit **selected_circuit,
643 int8_t *selected_face)
645 const struct circuit **circuit;
646 int16_t scoreA, scoreB;
647 int16_t selected_score = 0x8000; /* ~-int_max */
651 *selected_circuit = circuits[0] ;
652 for (circuit = &circuits[0]; *circuit; circuit++) {
653 if (evaluate_one_circuit(*circuit, i, j, &scoreA, &scoreB) < 0)
656 DEBUG(E_USER_STRAT, "Scores for %s are: faceA=%d, faceB=%d",
657 (*circuit)->name, scoreA, scoreB);
658 if (scoreA > selected_score) {
659 *selected_circuit = *circuit;
660 selected_score = scoreA;
663 if (scoreB > selected_score) {
664 *selected_circuit = *circuit;
665 selected_score = scoreB;
671 DEBUG(E_USER_STRAT, "no circuit found");
673 DEBUG(E_USER_STRAT, "circuit found: %s, %s",
674 (*selected_circuit)->name,
675 (*selected_face) ? "faceA":"faceB");
679 static void dump_circuit_wp(struct wp_line *circuit_wpline, int8_t len)
684 for (i = 0; i < len; i ++) {
685 DEBUG(E_USER_STRAT, "linenum %d dir %d",
686 circuit_wpline[i].line_num,
687 circuit_wpline[i].dir);
692 /* choose a circuit, then harvest on this circuit */
693 uint8_t strat_harvest_circuit(void)
695 const struct circuit *selected_circuit;
696 int8_t selected_face;
697 struct wp_line circuit_wpline[MAX_CIRCUIT_WPLINE];
701 uint8_t linenum, prev_linenum;
702 uint8_t dir, prev_dir;
705 x = position_get_x_s16(&mainboard.pos);
706 y = position_get_y_s16(&mainboard.pos);
708 if (xycoord_to_ijcoord(&x, &y, &i, &j) < 0) {
709 DEBUG(E_USER_STRAT, "%s(): cannot find waypoint at %d,%d",
714 if (find_best_circuit(i, j, &selected_circuit, &selected_face) < 0) {
715 DEBUG(E_USER_STRAT, "%s(): cannot find a good circuit",
720 len = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
722 DEBUG(E_USER_STRAT, "%s(): cannot find a path",
727 dump_circuit_wp(circuit_wpline, len);
729 prev_linenum = circuit_wpline[0].line_num;
730 prev_dir = circuit_wpline[0].dir;
731 for (idx = 1; idx < len; idx ++) {
733 linenum = circuit_wpline[idx].line_num;
734 dir = circuit_wpline[idx].dir;
736 /* XXX basic opponent management */
737 DEBUG(E_USER_STRAT, "%s(): line %d dir %d -> line %d dir %d",
738 __FUNCTION__, prev_linenum, prev_dir, linenum, dir);
739 err = line2line(prev_linenum, prev_dir, linenum, dir,
741 if (!TRAJ_SUCCESS(err)) {
747 prev_linenum = linenum;
751 return END_TRAJ; // XXX
754 void test_strat_avoid(void)
756 #ifdef TEST_STRAT_AVOID
758 const struct circuit *selected_circuit;
759 int8_t selected_face;
760 struct wp_line circuit_wpline[MAX_CIRCUIT_WPLINE];
764 printf_P(PSTR("========= i=%d, j=%d\r\n"), i, j);
766 ts = 0; bc = 0; cc = 0;
767 printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
768 find_best_circuit(i, j, &selected_circuit, &selected_face);
769 ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
770 dump_circuit_wp(circuit_wpline, ret);
772 ts = 0; bc = 3; cc = 0;
773 printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
774 find_best_circuit(i, j, &selected_circuit, &selected_face);
775 ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
776 dump_circuit_wp(circuit_wpline, ret);
778 ts = 0; bc = 4; cc = 0;
779 printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
780 find_best_circuit(i, j, &selected_circuit, &selected_face);
781 ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
782 dump_circuit_wp(circuit_wpline, ret);
784 ts = 0; bc = 3; cc = 5;
785 printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
786 find_best_circuit(i, j, &selected_circuit, &selected_face);
787 ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
788 dump_circuit_wp(circuit_wpline, ret);
790 ts = 0; bc = 4; cc = 5;
791 printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
792 find_best_circuit(i, j, &selected_circuit, &selected_face);
793 ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
794 dump_circuit_wp(circuit_wpline, ret);
796 ts = 80; bc = 0; cc = 0;
797 printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
798 find_best_circuit(i, j, &selected_circuit, &selected_face);
799 ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
800 dump_circuit_wp(circuit_wpline, ret);
803 printf_P(PSTR("========= i=%d, j=%d\r\n"), i, j);
805 ts = 0; bc = 0; cc = 0;
806 printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
807 find_best_circuit(i, j, &selected_circuit, &selected_face);
808 ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
809 dump_circuit_wp(circuit_wpline, ret);
811 ts = 0; bc = 3; cc = 0;
812 printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
813 find_best_circuit(i, j, &selected_circuit, &selected_face);
814 ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
815 dump_circuit_wp(circuit_wpline, ret);
817 ts = 80; bc = 0; cc = 0;
818 printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
819 find_best_circuit(i, j, &selected_circuit, &selected_face);
820 ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
821 dump_circuit_wp(circuit_wpline, ret);
824 printf_P(PSTR("========= i=%d, j=%d\r\n"), i, j);
826 ts = 0; bc = 0; cc = 0;
827 printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
828 find_best_circuit(i, j, &selected_circuit, &selected_face);
829 ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
830 dump_circuit_wp(circuit_wpline, ret);
832 ts = 0; bc = 3; cc = 0;
833 printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
834 find_best_circuit(i, j, &selected_circuit, &selected_face);
835 ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
836 dump_circuit_wp(circuit_wpline, ret);
838 ts = 80; bc = 0; cc = 0;
839 printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
840 find_best_circuit(i, j, &selected_circuit, &selected_face);
841 ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
842 dump_circuit_wp(circuit_wpline, ret);