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_circuit = {
120 .len = sizeof(butterfly_tab)/sizeof(struct wp_coord),
121 .path = butterfly_tab,
124 const struct wp_coord losange_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 losange_circuit = {
138 .len = sizeof(losange_tab)/sizeof(struct wp_coord),
142 const struct wp_coord triangle_tab[] = {
143 { .i = 11, .j = 6, },
144 { .i = 10, .j = 6, },
152 { .i = 10, .j = 2, },
153 { .i = 11, .j = 1, },
154 { .i = 11, .j = 2, },
155 { .i = 11, .j = 3, },
156 { .i = 11, .j = 4, },
157 { .i = 11, .j = 5, },
158 { .i = 11, .j = 6, },
161 const struct circuit triangle_circuit = {
163 .len = sizeof(triangle_tab)/sizeof(struct wp_coord),
164 .path = triangle_tab,
167 const struct wp_coord answer_d_tab[] = {
168 { .i = 11, .j = 6, },
169 { .i = 11, .j = 5, },
170 { .i = 11, .j = 4, },
171 { .i = 11, .j = 3, },
172 { .i = 11, .j = 2, },
173 { .i = 11, .j = 1, },
174 { .i = 10, .j = 2, },
178 { .i = 10, .j = 4, },
179 { .i = 11, .j = 4, },
180 { .i = 11, .j = 5, },
181 { .i = 11, .j = 6, },
184 const struct circuit answer_d_circuit = {
186 .len = sizeof(answer_d_tab)/sizeof(struct wp_coord),
187 .path = answer_d_tab,
190 const struct wp_coord h_lambda_tab[] = {
191 { .i = 11, .j = 6, },
192 { .i = 10, .j = 6, },
205 { .i = 10, .j = 6, },
206 { .i = 11, .j = 6, },
209 const struct circuit h_lambda_circuit = {
211 .len = sizeof(h_lambda_tab)/sizeof(struct wp_coord),
212 .path = h_lambda_tab,
215 const struct wp_coord asym_butterfly_tab[] = {
216 { .i = 11, .j = 6, },
217 { .i = 10, .j = 6, },
234 { .i = 10, .j = 2, },
235 { .i = 11, .j = 1, },
236 { .i = 11, .j = 2, },
237 { .i = 11, .j = 3, },
238 { .i = 11, .j = 4, },
239 { .i = 11, .j = 5, },
240 { .i = 11, .j = 6, },
243 const struct circuit asym_butterfly_circuit = {
244 .name = "asym_butterfly",
245 .len = sizeof(asym_butterfly_tab)/sizeof(struct wp_coord),
246 .path = asym_butterfly_tab,
249 const struct wp_coord big_h_lambda_tab[] = {
250 { .i = 11, .j = 6, },
251 { .i = 10, .j = 6, },
274 { .i = 10, .j = 6, },
275 { .i = 11, .j = 6, },
278 const struct circuit big_h_lambda_circuit = {
279 .name = "big_h_lambda",
280 .len = sizeof(big_h_lambda_tab)/sizeof(struct wp_coord),
281 .path = big_h_lambda_tab,
284 const struct wp_coord letter_v_tab[] = {
285 { .i = 11, .j = 6, },
286 { .i = 10, .j = 6, },
306 { .i = 10, .j = 4, },
307 { .i = 11, .j = 4, },
308 { .i = 11, .j = 5, },
309 { .i = 11, .j = 6, },
312 const struct circuit letter_v_circuit = {
314 .len = sizeof(letter_v_tab)/sizeof(struct wp_coord),
315 .path = letter_v_tab,
318 const struct wp_coord sperma_tab[] = {
319 { .i = 11, .j = 6, },
320 { .i = 10, .j = 6, },
341 { .i = 10, .j = 6, },
342 { .i = 11, .j = 6, },
345 const struct circuit sperma_circuit = {
347 .len = sizeof(sperma_tab)/sizeof(struct wp_coord),
351 /* list of all possible circuits */
352 const struct circuit *circuits[] = {
358 &asym_butterfly_circuit,
359 &big_h_lambda_circuit,
365 /* symetric neighbor position */
366 static inline uint8_t opposite_position(uint8_t pos)
375 //#define TEST_STRAT_AVOID
378 #ifdef TEST_STRAT_AVOID
380 uint8_t xget_cob_count(void)
386 uint8_t xget_ball_count(void)
392 uint8_t xtime_get_s(void)
397 #define xget_cob_count() get_cob_count()
398 #define xget_ball_count() get_ball_count()
399 #define xtime_get_s() time_get_s()
402 /* return true if turn is at 60 deg */
403 uint8_t is_60deg(uint8_t dir1, uint8_t dir2)
417 /* return true if turn is at 60 deg */
418 uint8_t is_120deg(uint8_t dir1, uint8_t dir2)
432 /* get the neighbour of the point at specified dir, return -1 if
433 * there is no neighbor */
434 int8_t wp_get_neigh(uint8_t i, uint8_t j, uint8_t *ni, uint8_t *nj,
463 if (i >= WAYPOINTS_NBX || j >= WAYPOINTS_NBY)
471 static int8_t get_line_num(int8_t i, int8_t j, uint8_t dir)
484 if ((mod == 0 || mod == 1) && ((j & 1) == 0))
486 if ((mod == 2 || mod == 3) && ((j & 1) == 1))
494 if ((mod == 0 || mod == 3) && ((j & 1) == 0))
496 if ((mod == 1 || mod == 2) && ((j & 1) == 1))
506 static uint8_t get_dir(uint8_t prev_i, uint8_t prev_j,
507 uint8_t i, uint8_t j)
509 int8_t diff_i, diff_j;
514 if (diff_i == 0 && diff_j == 1)
516 if (diff_i == 0 && diff_j == -1)
519 if ((prev_i & 1) == 0) {
520 if (diff_i == 1 && diff_j == 0)
522 if (diff_i == 1 && diff_j == -1)
524 if (diff_i == -1 && diff_j == 0)
526 if (diff_i == -1 && diff_j == -1)
530 if (diff_i == 1 && diff_j == 1)
532 if (diff_i == 1 && diff_j == 0)
534 if (diff_i == -1 && diff_j == 1)
536 if (diff_i == -1 && diff_j == 0)
544 /* return approximative angle of line */
545 int16_t linedir2angle(uint8_t dir)
559 return COLOR_A(-150);
565 int16_t get_nearest_dir_angle(int16_t a)
567 uint8_t dir, min_dir = 0;
568 int16_t min_diff = 0x7FFF, diff;
570 for (dir = LINE_UP; dir <= LINE_R_UP; dir++) {
571 diff = abs(linedir2angle(dir) - a);
576 if (diff < min_diff) {
581 return linedir2angle(min_dir);
584 /* return true if a waypoint belongs to a line */
585 uint8_t wp_belongs_to_line(uint8_t i, uint8_t j, uint8_t linenum, uint8_t dir)
588 ln = get_line_num(i, j, dir);
596 /* count the number of non-black corns which are neighbors of
598 uint8_t corn_count_neigh(uint8_t i, uint8_t j)
603 for (dir = LINE_UP; dir <= LINE_R_UP; dir++) {
604 if (wp_get_neigh(i, j, &ni, &nj, dir) < 0)
607 /* is there a corn cob removed for more than 2 secs ? */
608 if (strat_db.wp_table[ni][nj].type == WP_TYPE_CORN &&
609 strat_db.wp_table[ni][nj].corn.color != I2C_COB_BLACK &&
610 (strat_db.wp_table[ni][nj].present ||
611 strat_db.wp_table[ni][nj].time_removed + 2 > time_get_s()))
619 /* fill circuit_wpline table with waypoints from circuit starting at
620 * i,j and using selected face */
621 static int8_t get_path(const struct circuit *circuit,
622 uint8_t starti, uint8_t startj, uint8_t faceA,
623 struct wp_line *circuit_wpline)
625 const struct wp_coord *curcircuit;
626 const struct wp_coord *start;
627 const struct wp_coord *end;
628 uint8_t prev_i, prev_j;
629 uint8_t dir, prev_dir = 0xFF;
630 uint8_t found = 0, i = 0, j = 0;
632 int8_t step = faceA ? 1 : -1;
635 /* start and end of circuit */
637 start = &circuit->path[0];
638 end = start + circuit->len - 1;
641 end = &circuit->path[0];
642 start = end + circuit->len - 1;
645 DPR("%s(): %s face=%d\r\n", __FUNCTION__, circuit->name, faceA);
647 /* check that the point is present in the circuit */
648 for (curcircuit = start; curcircuit != end; curcircuit += step) {
649 if (curcircuit->i == starti && curcircuit->j == startj) {
658 /* browse the circuit from starti, startj in the specified
659 * direction, and fill the table when direction changes */
662 for ( ; curcircuit != end;
663 curcircuit += step, prev_dir = dir, prev_i = i, prev_j = j) {
668 dir = get_dir(prev_i, prev_j, i, j);
670 if (prev_dir != dir) {
671 linenum = get_line_num(prev_i, prev_j, dir);
672 circuit_wpline[path_len].line_num = linenum;
673 circuit_wpline[path_len].dir = dir;
674 DPR("%s(): %d %d -> %d %d / len=%d num=%d dir=%d\r\n",
675 __FUNCTION__, prev_i, prev_j, i, j, path_len, linenum, dir);
683 /* process score from retrieved objects number, and circuit len */
684 static int16_t get_score(uint32_t wcorn_retrieved,
685 uint32_t ucorn_retrieved,
686 uint16_t tomato_retrieved,
687 uint16_t utomato_retrieved,
688 uint8_t len, uint8_t opp_on_path)
695 /* score with corn */
696 n = xget_cob_count() * 2;
697 for (i = 0; i<CORN_NB; i++) {
700 if (wcorn_retrieved & mask) {
706 if (ucorn_retrieved & mask) {
713 DPR("get score: cob %d (->%d)\r\n", n, n/2);
715 /* score with tomato */
716 n = xget_ball_count();
718 for (i = 0; i<TOMATO_NB; i++) {
721 if (tomato_retrieved & mask) {
728 DPR("get score: ball %d\r\n", n);
730 /* malus for long circuits */
732 DPR("malus for length: %d\r\n", len * 20);
734 /* double malus for long circuits if we don't have much
737 if (len * WP_SPEED > (MATCH_TIME - xtime_get_s())) {
739 extra = (len * WP_SPEED) - (MATCH_TIME - xtime_get_s());
740 extra = (200 * extra);
741 if (extra < 0) /* should not happen */
746 DPR("malus for length + time: %d\r\n", extra);
749 /* malus if there is opponent on the path */
751 DPR("malus for opponent: %d\r\n", (500 * opp_on_path));
752 score -= (500 * opp_on_path);
758 /* return the corn type of specified coords: I2C_COB_WHITE,
759 * I2C_COB_UNKNOWN, or I2C_COB_NONE if it is black or not present */
760 static uint8_t get_corn_type(uint8_t i, uint8_t j)
763 /* is there a corn cob ? */
764 if (strat_db.wp_table[i][j].type == WP_TYPE_CORN &&
765 strat_db.wp_table[i][j].present) {
766 color = strat_db.wp_table[i][j].corn.color;
767 if (color == I2C_COB_WHITE)
768 return I2C_COB_WHITE;
769 else if (color == I2C_COB_UNKNOWN)
770 return I2C_COB_UNKNOWN;
775 /* i,j: starting position */
776 static int8_t evaluate_one_face(const struct circuit *circuit,
777 uint8_t starti, uint8_t startj,
778 uint8_t faceA, int16_t *score)
780 const struct wp_coord *curcircuit;
781 const struct wp_coord *start;
782 const struct wp_coord *end;
783 uint32_t wcorn_retrieved = 0; /* bit mask */
784 uint32_t ucorn_retrieved = 0; /* bit mask */
785 uint16_t tomato_retrieved = 0; /* bit mask */
786 uint16_t utomato_retrieved = 0; /* bit mask */
787 uint8_t opponent_on_path = 0;
788 uint8_t len = 0, found = 0;
789 uint8_t i, j, prev_i, prev_j;
790 uint8_t ni = 0, nj = 0;
791 uint8_t dir, color, idx, visited;
792 int8_t step = faceA ? 1 : -1;
794 int32_t d, prev_d = 0;
797 *score = 0x8000; /* -int_max */
799 /* start and end of circuit */
801 start = &circuit->path[0];
802 end = start + circuit->len - 1;
805 end = &circuit->path[0];
806 start = end + circuit->len - 1;
809 DPR("%s() face: %s %d\r\n", __FUNCTION__, circuit->name, faceA);
811 /* check that the point is present in the circuit */
812 for (curcircuit = start; curcircuit != end; curcircuit += step) {
813 if (curcircuit->i == starti && curcircuit->j == startj) {
821 /* get opponent coords */
822 if (get_opponent_xy(&oppx, &oppy) < 0)
823 oppx = I2C_OPPONENT_NOT_THERE;
825 DPR("%s() opponent: %d, %d\r\n", __FUNCTION__, oppx, oppy);
827 /* silent the compiler */
831 /* browse all points and calculate the score */
832 for (; /* start at starti,startj */
834 curcircuit += step, len ++, prev_i = i, prev_j = j) {
838 /* is opponent near the point ? */
839 ijcoord_to_xycoord(i, j, &x, &y);
840 if (oppx != I2C_OPPONENT_NOT_THERE) {
841 d = quad_distance_between(oppx, oppy, x, y);
842 DPR("%s(): opp at %d mm (ij=%d,%d opp=%d,%d pos=%d,%d)\r\n",
843 __FUNCTION__, d, i, j, oppx, oppy, x, y);
844 if (d < (250L*250L) && d < prev_d)
845 opponent_on_path += 3;
846 else if (d < (500L*500L) && d < prev_d)
851 /* don't try to look cobs/tomato for first point */
852 if (curcircuit == start)
855 /* get current direction, we wil check cobs behind us
856 * on left and right */
857 dir = get_dir(prev_i, prev_j, i, j);
859 DPR("%d %d -> %d %d (%d)\n", prev_i, prev_j, i, j, dir);
861 /* is there a tomato ? */
862 if (strat_db.wp_table[i][j].type == WP_TYPE_TOMATO &&
863 strat_db.wp_table[i][j].present) {
864 if (strat_db.wp_table[i][j].opp_visited) {
865 DPR(" TOMATO (opp visited)\n");
866 utomato_retrieved |= (1UL << strat_db.wp_table[i][j].tomato.idx);
870 tomato_retrieved |= (1UL << strat_db.wp_table[i][j].tomato.idx);
875 if (wp_get_neigh(i, j, &ni, &nj, (dir + 2) % 6) == 0) {
876 color = get_corn_type(ni, nj);
877 idx = strat_db.wp_table[ni][nj].corn.idx;
878 visited = strat_db.wp_table[ni][nj].opp_visited;
879 if (color == I2C_COB_WHITE && !visited) {
880 DPR(" LEFT WCORN (%d)\n", idx);
881 wcorn_retrieved |= (1UL << idx);
883 else if (color == I2C_COB_WHITE && visited) {
884 DPR(" LEFT CORN visited (%d)\n", idx);
885 ucorn_retrieved |= (1UL << idx);
887 else if (color == I2C_COB_UNKNOWN) {
888 DPR(" LEFT UCORN (%d)\n", idx);
889 ucorn_retrieved |= (1UL << idx);
894 if (wp_get_neigh(i, j, &ni, &nj, (dir + 4) % 6) == 0) {
895 color = get_corn_type(ni, nj);
896 idx = strat_db.wp_table[ni][nj].corn.idx;
897 visited = strat_db.wp_table[ni][nj].opp_visited;
898 if (color == I2C_COB_WHITE && !visited) {
899 DPR(" RIGHT WCORN (%d)\n", idx);
900 wcorn_retrieved |= (1UL << idx);
902 else if (color == I2C_COB_WHITE && visited) {
903 DPR(" RIGHT CORN visited (%d)\n", idx);
904 ucorn_retrieved |= (1UL << idx);
906 else if (color == I2C_COB_UNKNOWN) {
907 DPR(" RIGHT UCORN (%d)\n", idx);
908 ucorn_retrieved |= (1UL << idx);
912 /* prev_i, prev_j, len and curcircuit are updated in
916 /* write score and exit */
917 *score = get_score(wcorn_retrieved, ucorn_retrieved,
918 tomato_retrieved, utomato_retrieved,
919 len, opponent_on_path);
923 /* i,j: starting position */
924 static int8_t evaluate_one_circuit(const struct circuit *circuit,
925 uint8_t starti, uint8_t startj,
926 int16_t *scoreA, int16_t *scoreB)
928 if (evaluate_one_face(circuit, starti, startj, 1, scoreA) < 0)
931 /* we are on eject point, scoreB is the same */
932 if (starti == 11 && startj == 6) {
937 if (evaluate_one_face(circuit, starti, startj, 0, scoreB) < 0)
942 /* i,j starting position */
943 static int8_t find_best_circuit(uint8_t i, uint8_t j,
944 const struct circuit **selected_circuit,
945 int8_t *selected_face)
947 const struct circuit **circuit;
948 int16_t scoreA, scoreB;
949 int16_t selected_score = 0x8000; /* ~-int_max */
953 *selected_circuit = circuits[0] ;
954 for (circuit = &circuits[0]; *circuit; circuit++) {
955 if (evaluate_one_circuit(*circuit, i, j, &scoreA, &scoreB) < 0)
958 DEBUG(E_USER_STRAT, "Scores for %s are: faceA=%d, faceB=%d",
959 (*circuit)->name, scoreA, scoreB);
960 if (scoreA > selected_score) {
961 *selected_circuit = *circuit;
962 selected_score = scoreA;
965 if (scoreB > selected_score) {
966 *selected_circuit = *circuit;
967 selected_score = scoreB;
973 DEBUG(E_USER_STRAT, "no circuit found");
975 DEBUG(E_USER_STRAT, "circuit found: %s, %s",
976 (*selected_circuit)->name,
977 (*selected_face) ? "faceA":"faceB");
981 static void init_all_circuits(void)
983 const struct circuit **circuit;
984 const struct wp_coord *cur;
985 const struct wp_coord *start;
986 const struct wp_coord *end;
987 uint8_t prev_i, prev_j, i, j, dir;
989 for (circuit = &circuits[0]; *circuit; circuit++) {
990 start = &(*circuit)->path[0];
991 end = start + (*circuit)->len - 1;
997 for (cur = start; cur != end;
998 cur ++, prev_i = i, prev_j = j) {
1003 strat_db.wp_table[i][j].on_circuit = 1;
1005 dir = get_dir(prev_i, prev_j, i, j);
1007 printf_P("Bad circuit %s %d %d\r\n", (*circuit)->name, i, j);
1013 static void dump_circuit_wp(struct wp_line *circuit_wpline, int8_t len)
1018 for (i = 0; i < len; i ++) {
1019 DEBUG(E_USER_STRAT, "linenum %d dir %d",
1020 circuit_wpline[i].line_num,
1021 circuit_wpline[i].dir);
1026 /* choose a circuit, then harvest on this circuit */
1027 uint8_t strat_harvest_circuit(void)
1029 const struct circuit *selected_circuit;
1030 int8_t selected_face;
1031 struct wp_line circuit_wpline[MAX_CIRCUIT_WPLINE];
1035 uint8_t linenum, prev_linenum;
1036 uint8_t dir, prev_dir;
1039 strat_set_speed(SPEED_CLITOID_SLOW, SPEED_ANGLE_SLOW);
1040 strat_want_pack = 1;
1042 x = position_get_x_s16(&mainboard.pos);
1043 y = position_get_y_s16(&mainboard.pos);
1045 if (xycoord_to_ijcoord_not_corn(&x, &y, &i, &j) < 0) {
1046 DEBUG(E_USER_STRAT, "%s(): cannot find waypoint at %d,%d",
1047 __FUNCTION__, x, y);
1052 if (find_best_circuit(i, j, &selected_circuit, &selected_face) < 0) {
1053 DEBUG(E_USER_STRAT, "%s(): cannot find a good circuit",
1059 len = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
1061 DEBUG(E_USER_STRAT, "%s(): cannot find a path",
1067 dump_circuit_wp(circuit_wpline, len);
1069 prev_linenum = circuit_wpline[0].line_num;
1070 prev_dir = circuit_wpline[0].dir;
1072 /* fix orientation first */
1073 trajectory_a_abs(&mainboard.traj, linedir2angle(prev_dir));
1074 err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
1075 if (!TRAJ_SUCCESS(err))
1078 strat_want_pack = 0;
1080 /* do all lines of circuit */
1081 for (idx = 1; idx < len; idx ++) {
1083 linenum = circuit_wpline[idx].line_num;
1084 dir = circuit_wpline[idx].dir;
1086 DEBUG(E_USER_STRAT, "%s(): line %d dir %d -> line %d dir %d",
1087 __FUNCTION__, prev_linenum, prev_dir, linenum, dir);
1088 err = line2line(prev_linenum, prev_dir, linenum, dir,
1089 TRAJ_FLAGS_NO_NEAR);
1091 /* in some cases it is better to wait that obstacle is
1092 * gone before starting to avoid it */
1093 if (err == END_OBSTACLE &&
1094 strat_conf.flags & STRAT_CONF_WAIT_OBSTACLE &&
1095 time_get_s() > strat_conf.prev_wait_obstacle + 5) {
1096 strat_conf.prev_wait_obstacle = time_get_s();
1100 if (!TRAJ_SUCCESS(err))
1103 prev_linenum = linenum;
1109 strat_want_pack = 0;
1113 /* list of waypoints when we are not on a circuit */
1114 const struct xy_point unblock_pts[] = {
1115 { .x = 375, .y = 597 }, /* 1,1 */
1116 { .x = 2625, .y = 597 }, /* 11,1 */
1117 { .x = 1500, .y = 722 }, /* 6,2 */
1118 { .x = 375, .y = 1097 }, /* 1,3 */
1119 { .x = 375, .y = 1597 }, /* 1,5 */
1120 { .x = 2625, .y = 1097 }, /* 11,3 */
1121 { .x = 2625, .y = 1597 }, /* 11,5 */
1122 { .x = 1500, .y = 1722 }, /* 6,6 */
1126 /* try to unblock in any situation */
1127 uint8_t strat_unblock(void)
1129 int16_t x, y, posx, posy, posa;
1130 uint8_t i, j, k, cpt;
1131 uint16_t old_dspeed, old_aspeed;
1133 uint16_t d_min = 0x7FFF, d;
1134 const struct xy_point *pt;
1136 DEBUG(E_USER_STRAT, "%s()", __FUNCTION__);
1138 strat_want_pack = 1;
1139 strat_get_speed(&old_dspeed, &old_aspeed);
1142 posa = position_get_a_deg_s16(&mainboard.pos);
1144 strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW);
1145 posx = position_get_x_s16(&mainboard.pos);
1146 posy = position_get_y_s16(&mainboard.pos);
1150 if (xycoord_to_ijcoord_not_corn(&x, &y, &i, &j) < 0)
1152 else if (strat_db.wp_table[i][j].on_circuit == 0)
1155 /* find the nearest unblock point */
1158 /* browse all points and find the nearest */
1159 for (k = 0; k < sizeof(unblock_pts)/sizeof(*unblock_pts); k++) {
1160 pt = &unblock_pts[k];
1161 d = distance_between(posx, posy, pt->x, COLOR_Y(pt->y));
1169 DEBUG(E_USER_STRAT, "%s() unblock point is %d,%d",
1170 __FUNCTION__, x, y);
1172 for (cpt = 0; cpt < 2; cpt++) {
1174 /* go to nearest waypoint */
1175 trajectory_goto_xy_abs(&mainboard.traj, x, y);
1176 err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
1177 if (err == END_TIMER)
1180 if (TRAJ_SUCCESS(err))
1187 trajectory_d_a_rel(&mainboard.traj, 100, 20);
1188 err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
1189 trajectory_d_a_rel(&mainboard.traj, 100, -20);
1190 err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
1191 trajectory_d_a_rel(&mainboard.traj, -100, -20);
1192 err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
1193 trajectory_d_a_rel(&mainboard.traj, -100, 20);
1194 err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
1197 trajectory_a_abs(&mainboard.traj, get_nearest_dir_angle(posa));
1198 err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
1199 if (err == END_TIMER)
1202 if (!TRAJ_SUCCESS(err))
1205 strat_set_speed(old_dspeed, old_aspeed);
1209 void strat_avoid_init(void)
1211 init_all_circuits();
1213 #ifdef TEST_STRAT_AVOID
1215 const struct circuit *selected_circuit;
1216 int8_t selected_face;
1217 struct wp_line circuit_wpline[MAX_CIRCUIT_WPLINE];
1221 printf_P(PSTR("========= i=%d, j=%d\r\n"), i, j);
1223 ts = 0; bc = 0; cc = 0;
1224 printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
1225 find_best_circuit(i, j, &selected_circuit, &selected_face);
1226 ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
1227 dump_circuit_wp(circuit_wpline, ret);
1229 ts = 0; bc = 3; cc = 0;
1230 printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
1231 find_best_circuit(i, j, &selected_circuit, &selected_face);
1232 ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
1233 dump_circuit_wp(circuit_wpline, ret);
1235 ts = 0; bc = 4; cc = 0;
1236 printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
1237 find_best_circuit(i, j, &selected_circuit, &selected_face);
1238 ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
1239 dump_circuit_wp(circuit_wpline, ret);
1241 ts = 0; bc = 3; cc = 5;
1242 printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
1243 find_best_circuit(i, j, &selected_circuit, &selected_face);
1244 ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
1245 dump_circuit_wp(circuit_wpline, ret);
1247 ts = 0; bc = 4; cc = 5;
1248 printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
1249 find_best_circuit(i, j, &selected_circuit, &selected_face);
1250 ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
1251 dump_circuit_wp(circuit_wpline, ret);
1253 ts = 80; bc = 0; cc = 0;
1254 printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
1255 find_best_circuit(i, j, &selected_circuit, &selected_face);
1256 ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
1257 dump_circuit_wp(circuit_wpline, ret);
1260 printf_P(PSTR("========= i=%d, j=%d\r\n"), i, j);
1262 ts = 0; bc = 0; cc = 0;
1263 printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
1264 find_best_circuit(i, j, &selected_circuit, &selected_face);
1265 ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
1266 dump_circuit_wp(circuit_wpline, ret);
1268 ts = 0; bc = 3; cc = 0;
1269 printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
1270 find_best_circuit(i, j, &selected_circuit, &selected_face);
1271 ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
1272 dump_circuit_wp(circuit_wpline, ret);
1274 ts = 80; bc = 0; cc = 0;
1275 printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
1276 find_best_circuit(i, j, &selected_circuit, &selected_face);
1277 ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
1278 dump_circuit_wp(circuit_wpline, ret);
1281 printf_P(PSTR("========= i=%d, j=%d\r\n"), i, j);
1283 ts = 0; bc = 0; cc = 0;
1284 printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
1285 find_best_circuit(i, j, &selected_circuit, &selected_face);
1286 ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
1287 dump_circuit_wp(circuit_wpline, ret);
1289 ts = 0; bc = 3; cc = 0;
1290 printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
1291 find_best_circuit(i, j, &selected_circuit, &selected_face);
1292 ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
1293 dump_circuit_wp(circuit_wpline, ret);
1295 ts = 80; bc = 0; cc = 0;
1296 printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
1297 find_best_circuit(i, j, &selected_circuit, &selected_face);
1298 ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
1299 dump_circuit_wp(circuit_wpline, ret);