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"
71 const struct wp_coord circuit1[] = {
72 { .i = 11, .j = 6, .end = 0, },
73 { .i = 10, .j = 6, .end = 0, },
74 { .i = 9, .j = 5, .end = 0, },
75 { .i = 8, .j = 5, .end = 0, },
76 { .i = 7, .j = 4, .end = 0, },
77 { .i = 6, .j = 4, .end = 0, },
78 { .i = 5, .j = 4, .end = 0, },
79 { .i = 4, .j = 5, .end = 0, },
80 { .i = 3, .j = 5, .end = 0, },
81 { .i = 2, .j = 6, .end = 0, },
82 { .i = 1, .j = 6, .end = 0, },
83 { .i = 1, .j = 5, .end = 0, },
84 { .i = 1, .j = 4, .end = 0, },
85 { .i = 1, .j = 3, .end = 0, },
86 { .i = 1, .j = 2, .end = 0, },
87 { .i = 1, .j = 1, .end = 0, },
88 { .i = 2, .j = 2, .end = 0, },
89 { .i = 3, .j = 2, .end = 0, },
90 { .i = 4, .j = 3, .end = 0, },
91 { .i = 5, .j = 3, .end = 0, },
92 { .i = 6, .j = 4, .end = 0, },
93 { .i = 7, .j = 3, .end = 0, },
94 { .i = 8, .j = 3, .end = 0, },
95 { .i = 9, .j = 2, .end = 0, },
96 { .i = 10, .j = 2, .end = 0, },
97 { .i = 11, .j = 1, .end = 0, },
98 { .i = 11, .j = 2, .end = 0, },
99 { .i = 11, .j = 3, .end = 0, },
100 { .i = 11, .j = 4, .end = 0, },
101 { .i = 11, .j = 5, .end = 0, },
102 { .i = 11, .j = 6, .end = 1, },
105 const struct wp_coord circuit2[] = {
106 { .i = 11, .j = 6, .end = 0, },
107 { .i = 10, .j = 6, .end = 0, },
108 { .i = 9, .j = 5, .end = 0, },
109 { .i = 9, .j = 4, .end = 0, },
110 { .i = 9, .j = 3, .end = 0, },
111 { .i = 10, .j = 4, .end = 0, },
112 { .i = 11, .j = 4, .end = 0, },
113 { .i = 11, .j = 5, .end = 0, },
114 { .i = 11, .j = 6, .end = 1, },
117 /* list of all possible circuits */
118 const struct wp_coord *circuits[] = {
124 /* symetric neighbor position */
125 static inline uint8_t opposite_position(uint8_t pos)
135 uint8_t xget_cob_count(void)
141 uint8_t xget_ball_count(void)
147 uint8_t xtime_get_s(void)
152 #define xget_cob_count() get_cob_count()
153 #define xget_ball_count() get_ball_count()
154 #define xtime_get_s() time_get_s()
157 /* get the neighbour of the point at specified dir, return -1 if
158 * there is no neighbor */
159 int8_t wp_get_neigh(uint8_t i, uint8_t j, uint8_t *ni, uint8_t *nj,
188 if (i >= WAYPOINTS_NBX || j >= WAYPOINTS_NBY)
196 static uint8_t get_line_num(int8_t i, int8_t j, uint8_t dir)
217 static uint8_t get_dir(uint8_t prev_i, uint8_t prev_j,
218 uint8_t i, uint8_t j)
220 int8_t diff_i, diff_j;
225 if (diff_i == 0 && diff_j == 1)
227 if (diff_i == 0 && diff_j == -1)
230 if ((prev_i & 1) == 0) {
231 if (diff_i == 1 && diff_j == 0)
233 if (diff_i == 1 && diff_j == -1)
235 if (diff_i == -1 && diff_j == 0)
237 if (diff_i == -1 && diff_j == -1)
241 if (diff_i == 1 && diff_j == 1)
243 if (diff_i == 1 && diff_j == 0)
245 if (diff_i == -1 && diff_j == 1)
247 if (diff_i == -1 && diff_j == 0)
255 /* return true if a waypoint belongs to a line */
256 uint8_t wp_belongs_to_line(uint8_t i, uint8_t j, uint8_t linenum, uint8_t dir)
259 ln = get_line_num(i, j, dir);
265 /* count the number of non-black corns which are neighbors of
267 uint8_t corn_count_neigh(uint8_t i, uint8_t j)
272 for (dir = LINE_UP; dir <= LINE_R_DOWN; dir++) {
273 if (wp_get_neigh(i, j, &ni, &nj, dir) < 0)
276 //printf("i,j=%d,%d dir=%d, ni,nj=%d,%d\r\n",
277 // i, j, dir, ni, nj);
279 /* is there a corn cob ? */
280 if (strat_db.wp_table[ni][nj].type == WP_TYPE_CORN &&
281 strat_db.wp_table[ni][nj].present &&
282 strat_db.wp_table[ni][nj].corn.color != I2C_COB_BLACK)
290 int8_t get_path(const struct wp_coord *circuit,
291 uint8_t starti, uint8_t startj, uint8_t faceA,
292 struct wp_line *circuit_wpline)
294 const struct wp_coord *curcircuit;
295 uint8_t prev_i, prev_j;
296 uint8_t dir, prev_dir = 0xFF;
297 uint8_t found = 0, i = 0, j = 0;
299 int8_t step = faceA ? 1 : -1;
303 printf_P(PSTR("face: %d\r\n"), faceA);
304 if ( !faceA && circuit->i == 11 && circuit->j == 6)
307 /* check that the point is present in the circuit */
308 for (curcircuit = circuit + skipfirst; curcircuit->end == 0; curcircuit ++) {
309 if (curcircuit->i == starti && curcircuit->j == startj) {
315 if ( !faceA && curcircuit->i == 11 && curcircuit->j == 6)
320 /* XXX len must be >= 1 */
321 /* XXX start = 11,6 */
326 curcircuit = curcircuit;
328 if (faceA && curcircuit->end)
330 else if (!faceA && curcircuit == circuit)
335 dir = get_dir(prev_i, prev_j, i, j);
337 if (prev_dir != dir) {
338 linenum = get_line_num(prev_i, prev_j, dir);
339 /* printf_P(PSTR("COIN %d, %d, dir=%d linenum=%d\r\n"), */
340 /* prev_i, prev_j, dir, linenum); */
341 circuit_wpline[path_len].line_num = linenum;
342 circuit_wpline[path_len].dir = dir;
351 /* printf_P(PSTR("COIN %d, %d\r\n"), curcircuit->i, curcircuit->j); */
353 return path_len; /* XXX */
356 int16_t get_score(uint32_t wcorn_retrieved, uint32_t ucorn_retrieved,
357 uint16_t tomato_retrieved, uint8_t len)
364 /* score with corn */
365 n = xget_cob_count() * 2;
366 for (i = 0; i<CORN_NB; i++) {
369 if (wcorn_retrieved & mask) {
375 if (ucorn_retrieved & mask) {
382 printf_P(PSTR("get score: cob %d \r\n"), n);
383 /* score with tomato */
384 n = xget_ball_count();
386 for (i = 0; i<TOMATO_NB; i++) {
389 if (tomato_retrieved & mask) {
396 printf_P(PSTR("get score: ball %d \r\n"), n);
397 /* malus for long circuits */
400 /* double malus for long circuits if we don't have much
403 if (len * WP_SPEED > (MATCH_TIME - xtime_get_s())) {
405 extra = (len * WP_SPEED) - (MATCH_TIME - xtime_get_s());
406 score -= (200 * extra);
409 /* XXX use direction of robot */
414 /* i,j: starting position */
415 int8_t browse_one_circuit(const struct wp_coord *circuit,
416 uint8_t starti, uint8_t startj,
417 int16_t *scoreA, int16_t *scoreB)
419 const struct wp_coord *curcircuit;
420 uint32_t wcorn_retrieved = 0; /* bit mask */
421 uint32_t ucorn_retrieved = 0; /* bit mask */
422 uint16_t tomato_retrieved = 0; /* bit mask */
423 uint8_t len = 0, found = 0, i, j;
424 uint8_t ni = 0, nj = 0, pos, color;
426 /* check that the point is present in the circuit */
427 for (curcircuit = circuit; curcircuit->end == 0; curcircuit ++) {
428 if (curcircuit->i == starti && curcircuit->j == startj) {
437 for (curcircuit = circuit; curcircuit->end == 0; curcircuit ++, len ++) {
441 /* printf("cur:%d,%d %x %x %x %d\n", i, j, */
442 /* wcorn_retrieved, ucorn_retrieved, tomato_retrieved, len); */
444 /* face A completed */
445 if (i == starti && j == startj) {
446 *scoreA = get_score(wcorn_retrieved, ucorn_retrieved,
447 tomato_retrieved, len);
448 wcorn_retrieved = 0; /* bit mask */
449 ucorn_retrieved = 0; /* bit mask */
450 tomato_retrieved = 0; /* bit mask */
454 /* is there a tomato ? */
455 if (strat_db.wp_table[i][j].type == WP_TYPE_TOMATO &&
456 strat_db.wp_table[i][j].present) {
457 tomato_retrieved |= (1UL << strat_db.wp_table[i][j].tomato.idx);
460 /* browse all neighbours to see if there is cobs */
461 for (pos = LINE_UP; pos <= LINE_R_DOWN; pos++) {
462 if (wp_get_neigh(i, j, &ni, &nj, pos) < 0)
465 /* is there a corn cob ? */
466 if (strat_db.wp_table[ni][nj].type == WP_TYPE_CORN &&
467 strat_db.wp_table[ni][nj].present) {
468 color = strat_db.wp_table[ni][nj].corn.color;
469 if (color == I2C_COB_WHITE)
470 wcorn_retrieved |= (1UL << strat_db.wp_table[ni][nj].corn.idx);
471 else if (color == I2C_COB_UNKNOWN)
472 ucorn_retrieved |= (1UL << strat_db.wp_table[ni][nj].corn.idx);
477 *scoreB = get_score(wcorn_retrieved, ucorn_retrieved,
478 tomato_retrieved, len);
479 if (circuit->i == starti && circuit->j == startj)
485 /* i,j starting position */
486 int8_t browse_circuits(uint8_t i, uint8_t j,
487 const struct wp_coord **selected_circuit,
488 int8_t *selected_face)
490 const struct wp_coord **circuit;
491 int16_t scoreA, scoreB;
492 int16_t selected_score = 0x8000; /* ~-int_max */
496 *selected_circuit = circuits[0] ;
497 for (circuit = &circuits[0]; *circuit; circuit++) {
498 if (browse_one_circuit(*circuit, i, j, &scoreA, &scoreB) < 0)
501 printf_P(PSTR("Circuit: %d %d\r\n"), scoreA, scoreB);
502 if (scoreA > selected_score) {
503 *selected_circuit = *circuit;
504 selected_score = scoreA;
507 if (scoreB > selected_score) {
508 *selected_circuit = *circuit;
509 selected_score = scoreB;
516 static void dump_circuit_wp(struct wp_line *circuit_wpline, int8_t len)
521 for (i = 0; i < len; i ++) {
522 printf_P(PSTR("linenum %d dir %d\r\n"), circuit_wpline[i].line_num,
523 circuit_wpline[i].dir);
528 uint8_t strat_harvest_circuit(void)
530 const struct wp_coord *selected_circuit;
531 int8_t selected_face;
532 struct wp_line circuit_wpline[MAX_CIRCUIT_WPLINE];
536 uint8_t linenum, prev_linenum;
537 uint8_t dir, prev_dir;
540 x = position_get_x_s16(&mainboard.pos);
541 y = position_get_y_s16(&mainboard.pos);
543 if (xycoord_to_ijcoord(&x, &y, &i, &j) < 0) {
544 DEBUG(E_USER_STRAT, "%s(): cannot find waypoint at %d,%d",
549 browse_circuits(i, j, &selected_circuit, &selected_face);
550 len = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
552 DEBUG(E_USER_STRAT, "%s(): cannot find a path",
557 dump_circuit_wp(circuit_wpline, len);
559 prev_linenum = circuit_wpline[0].line_num;
560 prev_dir = circuit_wpline[0].dir;
561 for (idx = 1; idx < len; idx ++) {
563 linenum = circuit_wpline[idx].line_num;
564 dir = circuit_wpline[idx].dir;
566 /* XXX basic opponent management */
567 DEBUG(E_USER_STRAT, "%s(): line %d dir %d -> line %d dir %d",
568 __FUNCTION__, prev_linenum, prev_dir, linenum, dir);
569 err = line2line(prev_linenum, prev_dir, linenum, dir,
571 if (!TRAJ_SUCCESS(err)) {
577 prev_linenum = linenum;
581 return END_TRAJ; // XXX
584 void test_strat_avoid(void)
587 //corn_count_neigh(1, 3);
592 const struct wp_coord *selected_circuit;
593 int8_t selected_face;
594 struct wp_line circuit_wpline[MAX_CIRCUIT_WPLINE];
598 printf_P(PSTR("========= i=%d, j=%d\r\n"), i, j);
600 ts = 0; bc = 0; cc = 0;
601 printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
602 browse_circuits(i, j, &selected_circuit, &selected_face);
603 ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
604 dump_circuit_wp(circuit_wpline, ret);
606 ts = 0; bc = 3; cc = 0;
607 printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
608 browse_circuits(i, j, &selected_circuit, &selected_face);
609 ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
610 dump_circuit_wp(circuit_wpline, ret);
612 ts = 0; bc = 4; cc = 0;
613 printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
614 browse_circuits(i, j, &selected_circuit, &selected_face);
615 ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
616 dump_circuit_wp(circuit_wpline, ret);
618 ts = 0; bc = 3; cc = 5;
619 printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
620 browse_circuits(i, j, &selected_circuit, &selected_face);
621 ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
622 dump_circuit_wp(circuit_wpline, ret);
624 ts = 0; bc = 4; cc = 5;
625 printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
626 browse_circuits(i, j, &selected_circuit, &selected_face);
627 ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
628 dump_circuit_wp(circuit_wpline, ret);
630 ts = 80; bc = 0; cc = 0;
631 printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
632 browse_circuits(i, j, &selected_circuit, &selected_face);
633 ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
634 dump_circuit_wp(circuit_wpline, ret);
637 printf_P(PSTR("========= i=%d, j=%d\r\n"), i, j);
639 ts = 0; bc = 0; cc = 0;
640 printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
641 browse_circuits(i, j, &selected_circuit, &selected_face);
642 ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
643 dump_circuit_wp(circuit_wpline, ret);
645 ts = 0; bc = 3; cc = 0;
646 printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
647 browse_circuits(i, j, &selected_circuit, &selected_face);
648 ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
649 dump_circuit_wp(circuit_wpline, ret);
651 ts = 80; bc = 0; cc = 0;
652 printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
653 browse_circuits(i, j, &selected_circuit, &selected_face);
654 ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
655 dump_circuit_wp(circuit_wpline, ret);
658 printf_P(PSTR("========= i=%d, j=%d\r\n"), i, j);
660 ts = 0; bc = 0; cc = 0;
661 printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
662 browse_circuits(i, j, &selected_circuit, &selected_face);
663 ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
664 dump_circuit_wp(circuit_wpline, ret);
666 ts = 0; bc = 3; cc = 0;
667 printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
668 browse_circuits(i, j, &selected_circuit, &selected_face);
669 ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
670 dump_circuit_wp(circuit_wpline, ret);
672 ts = 80; bc = 0; cc = 0;
673 printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
674 browse_circuits(i, j, &selected_circuit, &selected_face);
675 ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
676 dump_circuit_wp(circuit_wpline, ret);