X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fmainboard%2Fstrat_avoid.c;h=61151ff4fd1936bf9fdbe86af7e678ebe4e05dae;hp=6e75dd0e55375e862fdcfa0e2ca80a3f6668b075;hb=878d0fc33d940d5d02fc65dfd0e56798c97de18d;hpb=5918edd6f4f713ef3c8b0b0020dd30a4fb8222ae diff --git a/projects/microb2010/mainboard/strat_avoid.c b/projects/microb2010/mainboard/strat_avoid.c index 6e75dd0..61151ff 100644 --- a/projects/microb2010/mainboard/strat_avoid.c +++ b/projects/microb2010/mainboard/strat_avoid.c @@ -1,6 +1,6 @@ -/* - * Copyright Droids Corporation, Microb Technology (2009) - * +/* + * Copyright Droids, Microb Technology (2010) + * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation; either version 2 of the License, or @@ -15,29 +15,33 @@ * along with this program; if not, write to the Free Software * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA * - * Revision : $Id: strat_avoid.c,v 1.5 2009-11-08 17:24:33 zer0 Exp $ + * Revision : $Id: strat.c,v 1.6 2009-11-08 17:24:33 zer0 Exp $ * + * Olivier MATZ */ + #include #include -#include +#include #include +#include #include -#include #include #include #include #include -#include +#include #include #include #include #include #include +#include +#include #include #include #include @@ -46,490 +50,942 @@ #include #include +#include + #include #include +#include "../common/i2c_commands.h" +#include "i2c_protocol.h" #include "main.h" #include "strat.h" +#include "strat_db.h" #include "strat_base.h" +#include "strat_corn.h" +#include "strat_avoid.h" #include "strat_utils.h" #include "sensor.h" +#include "actuator.h" -#define EDGE_NUMBER 5 -void set_rotated_pentagon(poly_t *pol, const point_t *robot_pt, - int16_t radius, int16_t x, int16_t y) -{ +//#define VERBOSE - double c_a, s_a; - uint8_t i; - double px1, py1, px2, py2; - double a_rad; - - a_rad = atan2(y - robot_pt->y, x - robot_pt->x); - - /* generate pentagon */ - c_a = cos(-2*M_PI/EDGE_NUMBER); - s_a = sin(-2*M_PI/EDGE_NUMBER); +#ifdef VERBOSE +#define DPR(fmt, ...) printf_P(PSTR(fmt), ##__VA_ARGS__) +#else +#define DPR(args...) do {} while (0) +#endif - /* - px1 = radius; - py1 = 0; - */ - px1 = radius * cos(a_rad + 2*M_PI/(2*EDGE_NUMBER)); - py1 = radius * sin(a_rad + 2*M_PI/(2*EDGE_NUMBER)); +struct circuit { + const char *name; + uint8_t len; + const struct wp_coord *path; +}; + +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 = 3, }, + { .i = 4, .j = 4, }, + { .i = 3, .j = 2, }, + { .i = 2, .j = 2, }, + { .i = 1, .j = 1, }, + { .i = 1, .j = 2, }, + { .i = 1, .j = 3, }, + { .i = 1, .j = 4, }, + { .i = 1, .j = 5, }, + { .i = 1, .j = 6, }, + { .i = 2, .j = 6, }, + { .i = 3, .j = 5, }, + { .i = 4, .j = 5, }, + { .i = 5, .j = 4, }, + { .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_circuit = { + .name = "butterfly", + .len = sizeof(butterfly_tab)/sizeof(struct wp_coord), + .path = butterfly_tab, +}; + +const struct wp_coord losange_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 losange_circuit = { + .name = "losange", + .len = sizeof(losange_tab)/sizeof(struct wp_coord), + .path = losange_tab, +}; + +const struct wp_coord triangle_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 = 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 triangle_circuit = { + .name = "triangle", + .len = sizeof(triangle_tab)/sizeof(struct wp_coord), + .path = triangle_tab, +}; + +const struct wp_coord answer_d_tab[] = { + { .i = 11, .j = 6, }, + { .i = 11, .j = 5, }, + { .i = 11, .j = 4, }, + { .i = 11, .j = 3, }, + { .i = 11, .j = 2, }, + { .i = 11, .j = 1, }, + { .i = 10, .j = 2, }, + { .i = 9, .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 answer_d_circuit = { + .name = "answer_d", + .len = sizeof(answer_d_tab)/sizeof(struct wp_coord), + .path = answer_d_tab, +}; + +const struct wp_coord h_lambda_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 = 3, }, + { .i = 5, .j = 4, }, + { .i = 5, .j = 5, }, + { .i = 5, .j = 6, }, + { .i = 6, .j = 6, }, + { .i = 7, .j = 5, }, + { .i = 8, .j = 5, }, + { .i = 9, .j = 5, }, + { .i = 10, .j = 6, }, + { .i = 11, .j = 6, }, +}; + +const struct circuit h_lambda_circuit = { + .name = "h_lambda", + .len = sizeof(h_lambda_tab)/sizeof(struct wp_coord), + .path = h_lambda_tab, +}; + +const struct wp_coord asym_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 = 3, }, + { .i = 4, .j = 3, }, + { .i = 3, .j = 2, }, + { .i = 3, .j = 3, }, + { .i = 3, .j = 4, }, + { .i = 3, .j = 5, }, + { .i = 4, .j = 5, }, + { .i = 5, .j = 4, }, + { .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 asym_butterfly_circuit = { + .name = "asym_butterfly", + .len = sizeof(asym_butterfly_tab)/sizeof(struct wp_coord), + .path = asym_butterfly_tab, +}; + +const struct wp_coord big_h_lambda_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 = 4, }, + { .i = 8, .j = 5, }, + { .i = 9, .j = 5, }, + { .i = 10, .j = 6, }, + { .i = 11, .j = 6, }, +}; + +const struct circuit big_h_lambda_circuit = { + .name = "big_h_lambda", + .len = sizeof(big_h_lambda_tab)/sizeof(struct wp_coord), + .path = big_h_lambda_tab, +}; + +/* list of all possible circuits */ +const struct circuit *circuits[] = { + &butterfly_circuit, + &losange_circuit, + &triangle_circuit, + &answer_d_circuit, + &h_lambda_circuit, + &asym_butterfly_circuit, + &big_h_lambda_circuit, + NULL, +}; + +/* symetric neighbor position */ +static inline uint8_t opposite_position(uint8_t pos) +{ + pos += 3; + if (pos > LINE_L_UP) + pos -= 6; + return pos; +} +#ifdef HOST_VERSION +//#define TEST_STRAT_AVOID +#endif - for (i = 0; i < EDGE_NUMBER; i++){ - oa_poly_set_point(pol, x + px1, y + py1, i); - - px2 = px1*c_a + py1*s_a; - py2 = -px1*s_a + py1*c_a; +#ifdef TEST_STRAT_AVOID +static uint8_t cc; +uint8_t xget_cob_count(void) +{ + return cc; +} - px1 = px2; - py1 = py2; - } +static uint8_t bc; +uint8_t xget_ball_count(void) +{ + return bc; } -void set_rotated_poly(poly_t *pol, const point_t *robot_pt, - int16_t w, int16_t l, int16_t x, int16_t y) +static uint32_t ts; +uint8_t xtime_get_s(void) { - double tmp_x, tmp_y; - double a_rad; - - a_rad = atan2(y - robot_pt->y, x - robot_pt->x); - - DEBUG(E_USER_STRAT, "%s() x,y=%d,%d a_rad=%2.2f", - __FUNCTION__, x, y, a_rad); - - /* point 1 */ - tmp_x = w; - tmp_y = l; - rotate(&tmp_x, &tmp_y, a_rad); - tmp_x += x; - tmp_y += y; - oa_poly_set_point(pol, tmp_x, tmp_y, 0); - - /* point 2 */ - tmp_x = -w; - tmp_y = l; - rotate(&tmp_x, &tmp_y, a_rad); - tmp_x += x; - tmp_y += y; - oa_poly_set_point(pol, tmp_x, tmp_y, 1); - - /* point 3 */ - tmp_x = -w; - tmp_y = -l; - rotate(&tmp_x, &tmp_y, a_rad); - tmp_x += x; - tmp_y += y; - oa_poly_set_point(pol, tmp_x, tmp_y, 2); - - /* point 4 */ - tmp_x = w; - tmp_y = -l; - rotate(&tmp_x, &tmp_y, a_rad); - tmp_x += x; - tmp_y += y; - oa_poly_set_point(pol, tmp_x, tmp_y, 3); + return ts; } +#else +#define xget_cob_count() get_cob_count() +#define xget_ball_count() get_ball_count() +#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; -#define DISC_X CENTER_X -#define DISC_Y CENTER_Y + turn = dir2-dir1; + if (turn < 0) + turn += 6; + if (turn == 1) + return 1; + if (turn == 5) + return 1; + return 0; +} -void set_central_disc_poly(poly_t *pol, const point_t *robot_pt) +/* return true if turn is at 60 deg */ +uint8_t is_120deg(uint8_t dir1, uint8_t dir2) { - set_rotated_pentagon(pol, robot_pt, DISC_PENTA_DIAG, - DISC_X, DISC_Y); + int8_t turn; + + turn = dir2-dir1; + if (turn < 0) + turn += 6; + if (turn == 2) + return 1; + if (turn == 4) + return 1; + return 0; } -#ifdef HOMOLOGATION -/* /!\ half size */ -#define O_WIDTH 400 -#define O_LENGTH 550 -#else -/* /!\ half size */ -#define O_WIDTH 360 -#define O_LENGTH 500 -#endif +/* 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, + uint8_t dir) +{ + switch (dir) { + case LINE_UP: + j++; + break; + case LINE_R_UP: + if ((i & 1)) j++; + i++; + break; + case LINE_R_DOWN: + if (!(i & 1)) j--; + i++; + break; + case LINE_DOWN: + j--; + break; + case LINE_L_DOWN: + if (!(i & 1)) j--; + i--; + break; + case LINE_L_UP: + if ((i & 1)) j++; + i--; + break; + default: + return -1; + } + if (i >= WAYPOINTS_NBX || j >= WAYPOINTS_NBY) + return -1; -void set_opponent_poly(poly_t *pol, const point_t *robot_pt, int16_t w, int16_t l) + *ni = i; + *nj = j; + return 0; +} + +static uint8_t get_line_num(int8_t i, int8_t j, uint8_t dir) { - int16_t x, y; - get_opponent_xy(&x, &y); - DEBUG(E_USER_STRAT, "oponent at: %d %d", x, y); - - /* place poly even if invalid, because it's -100 */ - set_rotated_poly(pol, robot_pt, w, l, x, y); + switch (dir) { + case LINE_UP: + case LINE_DOWN: + return i/2; + case LINE_R_UP: + case LINE_L_DOWN: + i &= 0xfe; + j -= i/2; + return (5-j)/2; + case LINE_R_DOWN: + case LINE_L_UP: + i &= 0xfe; + j += i/2; + return (11-j)/2; + default: + return -1; + } } -/* don't care about polygons further than this distance for escape */ -#define ESCAPE_POLY_THRES 1000 +static uint8_t get_dir(uint8_t prev_i, uint8_t prev_j, + uint8_t i, uint8_t j) +{ + int8_t diff_i, diff_j; + + diff_i = i - prev_i; + diff_j = j - prev_j; + + if (diff_i == 0 && diff_j == 1) + return LINE_UP; + if (diff_i == 0 && diff_j == -1) + return LINE_DOWN; + + if ((prev_i & 1) == 0) { + if (diff_i == 1 && diff_j == 0) + return LINE_R_UP; + if (diff_i == 1 && diff_j == -1) + return LINE_R_DOWN; + if (diff_i == -1 && diff_j == 0) + return LINE_L_UP; + if (diff_i == -1 && diff_j == -1) + return LINE_L_DOWN; + } + else { + if (diff_i == 1 && diff_j == 1) + return LINE_R_UP; + if (diff_i == 1 && diff_j == 0) + return LINE_R_DOWN; + if (diff_i == -1 && diff_j == 1) + return LINE_L_UP; + if (diff_i == -1 && diff_j == 0) + return LINE_L_DOWN; + } -/* don't reduce opp if opp is too far */ -#define REDUCE_POLY_THRES 600 + /* invalid value */ + return 0xFF; +} -/* has to be longer than any poly */ -#define ESCAPE_VECT_LEN 3000 +/* 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 ln; + ln = get_line_num(i, j, dir); + if (ln == linenum) + return 1; + return 0; +} -/* - * Go in playground, loop until out of poly. The argument robot_pt is - * the pointer to the current position of the robot. - * Return 0 if there was nothing to do. - * Return 1 if we had to move. In this case, update the theorical - * position of the robot in robot_pt. - */ -static int8_t go_in_area(point_t *robot_pt) +/* count the number of non-black corns which are neighbors of + * specified cob */ +uint8_t corn_count_neigh(uint8_t i, uint8_t j) { - point_t poly_pts_area[4]; - poly_t poly_area; - point_t disc_pt, dst_pt; + uint8_t dir, n = 0; + uint8_t ni, nj; + + for (dir = LINE_UP; dir <= LINE_R_UP; dir++) { + if (wp_get_neigh(i, j, &ni, &nj, dir) < 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 && + strat_db.wp_table[ni][nj].corn.color != I2C_COB_BLACK) + n ++; + } - disc_pt.x = DISC_X; - disc_pt.y = DISC_Y; + return n; +} - /* Go in playground */ - if (!is_in_boundingbox(robot_pt)){ - NOTICE(E_USER_STRAT, "not in playground %"PRIi32", %"PRIi32"", - robot_pt->x, robot_pt->y); - poly_area.l = 4; - poly_area.pts = poly_pts_area; - poly_pts_area[0].x = strat_infos.area_bbox.x1; - poly_pts_area[0].y = strat_infos.area_bbox.y1; +/* 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 path_len = 0; + + /* 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; + } - poly_pts_area[1].x = strat_infos.area_bbox.x2; - poly_pts_area[1].y = strat_infos.area_bbox.y1; + DPR("face: %s %d\r\n", circuit->name, faceA); - poly_pts_area[2].x = strat_infos.area_bbox.x2; - poly_pts_area[2].y = strat_infos.area_bbox.y2; + /* check that the point is present in the circuit */ + for (curcircuit = start; curcircuit != end; curcircuit += step) { + if (curcircuit->i == starti && curcircuit->j == startj) { + found = 1; + break; + } + } + if (found == 0) + return -1; - poly_pts_area[3].x = strat_infos.area_bbox.x1; - poly_pts_area[3].y = strat_infos.area_bbox.y2; - is_crossing_poly(*robot_pt, disc_pt, &dst_pt, &poly_area); - NOTICE(E_USER_STRAT, "pt dst %"PRIi32", %"PRIi32"", dst_pt.x, dst_pt.y); - - strat_goto_xy_force(dst_pt.x, dst_pt.y); + /* 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) { - robot_pt->x = dst_pt.x; - robot_pt->y = dst_pt.y; + i = curcircuit->i; + j = curcircuit->j; - NOTICE(E_USER_STRAT, "GOTO %"PRIi32",%"PRIi32"", - dst_pt.x, dst_pt.y); + dir = get_dir(prev_i, prev_j, i, j); - return 1; + if (prev_dir != dir) { + linenum = get_line_num(prev_i, prev_j, dir); + circuit_wpline[path_len].line_num = linenum; + circuit_wpline[path_len].dir = dir; + path_len++; + } } - return 0; + return path_len; } - -/* - * Escape from polygons if needed. - * robot_pt is the current position of the robot, it will be - * updated. - */ -static int8_t escape_from_poly(point_t *robot_pt, - poly_t *pol_disc, - int16_t opp_x, int16_t opp_y, - int16_t opp_w, int16_t opp_l, - poly_t *pol_opp) +/* 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) { - uint8_t in_disc = 0, in_opp = 0; - double escape_dx = 0, escape_dy = 0; - double disc_dx = 0, disc_dy = 0; - double opp_dx = 0, opp_dy = 0; - double len; - point_t opp_pt, disc_pt, dst_pt; - point_t intersect_disc_pt, intersect_opp_pt; - - opp_pt.x = opp_x; - opp_pt.y = opp_y; - disc_pt.x = DISC_X; - disc_pt.y = DISC_Y; - - /* escape from other poly if necessary */ - if (is_in_poly(robot_pt, pol_disc) == 1) - in_disc = 1; - if (is_in_poly(robot_pt, pol_opp) == 1) - in_opp = 1; - - if (in_disc == 0 && in_opp == 0) { - NOTICE(E_USER_STRAT, "no need to escape"); - return 0; - } - - NOTICE(E_USER_STRAT, "in_disc=%d, in_opp=%d", in_disc, in_opp); - - /* process escape vector */ - - if (distance_between(robot_pt->x, robot_pt->y, DISC_X, DISC_Y) < ESCAPE_POLY_THRES) { - disc_dx = robot_pt->x - DISC_X; - disc_dy = robot_pt->y - DISC_Y; - NOTICE(E_USER_STRAT, " robot is near disc: vect=%2.2f,%2.2f", - disc_dx, disc_dy); - len = norm(disc_dx, disc_dy); - if (len != 0) { - disc_dx /= len; - disc_dy /= len; + int16_t score = 0; + uint8_t i; + uint32_t mask = 1; + uint8_t n; + + /* score with corn */ + n = xget_cob_count() * 2; + for (i = 0; i= 10) + break; + if (wcorn_retrieved & mask) { + score += 250; + n += 2; } - else { - disc_dx = 1.0; - disc_dy = 0.0; + if (n >= 10) + break; + if (ucorn_retrieved & mask) { + score += 125; + n += 1; } - escape_dx += disc_dx; - escape_dy += disc_dy; + mask <<= 1UL; } - if (distance_between(robot_pt->x, robot_pt->y, opp_x, opp_y) < ESCAPE_POLY_THRES) { - opp_dx = robot_pt->x - opp_x; - opp_dy = robot_pt->y - opp_y; - NOTICE(E_USER_STRAT, " robot is near opp: vect=%2.2f,%2.2f", - opp_dx, opp_dy); - len = norm(opp_dx, opp_dy); - if (len != 0) { - opp_dx /= len; - opp_dy /= len; - } - else { - opp_dx = 1.0; - opp_dy = 0.0; + DPR("get score: cob %d (->%d)\r\n", n, n/2); + + /* score with tomato */ + n = xget_ball_count(); + mask = 1; + for (i = 0; i= 4) + break; + if (tomato_retrieved & mask) { + score += 150; + n += 1; } - escape_dx += opp_dx; - escape_dy += opp_dy; + mask <<= 1UL; } - /* normalize escape vector */ - len = norm(escape_dx, escape_dy); - if (len != 0) { - escape_dx /= len; - escape_dy /= len; + 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())) { + int32_t extra; + extra = (len * WP_SPEED) - (MATCH_TIME - xtime_get_s()); + 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); } - else { - if (pol_disc != NULL) { - /* rotate 90° */ - escape_dx = disc_dy; - escape_dy = disc_dx; - } - else if (pol_opp != NULL) { - /* rotate 90° */ - escape_dx = opp_dy; - escape_dy = opp_dx; - } - else { /* should not happen */ - opp_dx = 1.0; - opp_dy = 0.0; - } + + /* malus if there is opponent on the path */ + if (opp_on_path) { + DPR("malus for opponent: 1000\r\n"); + score -= 2000; } - NOTICE(E_USER_STRAT, " escape vect = %2.2f,%2.2f", - escape_dx, escape_dy); + return score; +} - /* process the correct len of escape vector */ +/* 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; +} - dst_pt.x = robot_pt->x + escape_dx * ESCAPE_VECT_LEN; - dst_pt.y = robot_pt->y + escape_dy * ESCAPE_VECT_LEN; +/* i,j: starting position */ +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 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; + } - NOTICE(E_USER_STRAT, "robot pt %"PRIi32" %"PRIi32, - robot_pt->x, robot_pt->y); - NOTICE(E_USER_STRAT, "dst point %"PRIi32",%"PRIi32, - dst_pt.x, dst_pt.y); + DPR("%s() face: %s %d\r\n", __FUNCTION__, circuit->name, faceA); - if (in_disc) { - if (is_crossing_poly(*robot_pt, dst_pt, &intersect_disc_pt, - pol_disc) == 1) { - /* we add 2 mm to be sure we are out of th polygon */ - dst_pt.x = intersect_disc_pt.x + escape_dx * 2; - dst_pt.y = intersect_disc_pt.y + escape_dy * 2; - if (is_point_in_poly(pol_opp, dst_pt.x, dst_pt.y) != 1) { + /* check that the point is present in the circuit */ + for (curcircuit = start; curcircuit != end; curcircuit += step) { + if (curcircuit->i == starti && curcircuit->j == startj) { + found = 1; + break; + } + } + if (found == 0) + return -1; + + /* 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; + + /* 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; - if (!is_in_boundingbox(&dst_pt)) - return -1; - - NOTICE(E_USER_STRAT, "GOTO %"PRIi32",%"PRIi32"", - dst_pt.x, dst_pt.y); + /* get current direction, we wil check cobs behind us + * on left and right */ + dir = get_dir(prev_i, prev_j, i, j); - strat_goto_xy_force(dst_pt.x, dst_pt.y); + DPR("%d %d -> %d %d (%d)\n", prev_i, prev_j, i, j, dir); - robot_pt->x = dst_pt.x; - robot_pt->y = dst_pt.y; + /* 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); + } - return 0; + /* 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); } } - } - if (in_opp) { - if (is_crossing_poly(*robot_pt, dst_pt, &intersect_opp_pt, - pol_opp) == 1) { - /* we add 2 cm to be sure we are out of th polygon */ - dst_pt.x = intersect_opp_pt.x + escape_dx * 2; - dst_pt.y = intersect_opp_pt.y + escape_dy * 2; + /* 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); + } + } - if (is_point_in_poly(pol_disc, dst_pt.x, dst_pt.y) != 1) { + /* prev_i, prev_j, len and curcircuit are updated in + * for (;;) */ + } - if (!is_in_boundingbox(&dst_pt)) - return -1; - - NOTICE(E_USER_STRAT, "GOTO %"PRIi32",%"PRIi32"", - dst_pt.x, dst_pt.y); + /* write score and exit */ + *score = get_score(wcorn_retrieved, ucorn_retrieved, + tomato_retrieved, len, opponent_on_path); + return 0; +} - strat_goto_xy_force(dst_pt.x, dst_pt.y); +/* 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; - robot_pt->x = dst_pt.x; - robot_pt->y = dst_pt.y; + /* we are on eject point, scoreB is the same */ + if (starti == 11 && startj == 6) { + *scoreB = *scoreA; + return 0; + } - return 0; - } + if (evaluate_one_face(circuit, starti, startj, 0, scoreB) < 0) + return -1; + return 0; +} + +/* i,j starting position */ +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; + int16_t selected_score = 0x8000; /* ~-int_max */ + int8_t found = -1; + + *selected_face = 0; + *selected_circuit = circuits[0] ; + for (circuit = &circuits[0]; *circuit; circuit++) { + if (evaluate_one_circuit(*circuit, i, j, &scoreA, &scoreB) < 0) + continue; + found = 0; + 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; + *selected_face = 0; + } + if (scoreB > selected_score) { + *selected_circuit = *circuit; + selected_score = scoreB; + *selected_face = 1; } } - /* should not happen */ - return -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; } +static void dump_circuit_wp(struct wp_line *circuit_wpline, int8_t len) +{ + int8_t i; + if (len <= 0) + return; + for (i = 0; i < len; i ++) { + DEBUG(E_USER_STRAT, "linenum %d dir %d", + circuit_wpline[i].line_num, + circuit_wpline[i].dir); + } + +} -static int8_t __goto_and_avoid(int16_t x, int16_t y, - uint8_t flags_intermediate, - uint8_t flags_final, - uint8_t forward) +/* choose a circuit, then harvest on this circuit */ +uint8_t strat_harvest_circuit(void) { - int8_t len = -1, i; - point_t *p; - poly_t *pol_disc, *pol_opp; - int8_t ret; - int16_t opp_w, opp_l, opp_x, opp_y; - point_t p_dst, robot_pt; - - DEBUG(E_USER_STRAT, "%s(%d,%d) flags_i=%x flags_f=%x forw=%d", - __FUNCTION__, x, y, flags_intermediate, flags_final, forward); - - retry: - get_opponent_xy(&opp_x, &opp_y); - opp_w = O_WIDTH; - opp_l = O_LENGTH; - - robot_pt.x = position_get_x_s16(&mainboard.pos); - robot_pt.y = position_get_y_s16(&mainboard.pos); - - oa_init(); - pol_disc = oa_new_poly(5); - set_central_disc_poly(pol_disc, &robot_pt); - pol_opp = oa_new_poly(4); - set_opponent_poly(pol_opp, &robot_pt, O_WIDTH, O_LENGTH); - - /* If we are not in the limited area, try to go in it. */ - ret = go_in_area(&robot_pt); - - /* check that destination is valid */ - p_dst.x = x; - p_dst.y = y; - if (!is_in_boundingbox(&p_dst)) { - NOTICE(E_USER_STRAT, " dst is not in playground"); + const struct circuit *selected_circuit; + int8_t selected_face; + struct wp_line circuit_wpline[MAX_CIRCUIT_WPLINE]; + int8_t len; + uint8_t i, j, idx; + int16_t x, y; + uint8_t linenum, prev_linenum; + uint8_t dir, prev_dir; + uint8_t err; + + x = position_get_x_s16(&mainboard.pos); + y = position_get_y_s16(&mainboard.pos); + + if (xycoord_to_ijcoord(&x, &y, &i, &j) < 0) { + DEBUG(E_USER_STRAT, "%s(): cannot find waypoint at %d,%d", + __FUNCTION__, x, y); return END_ERROR; } - if (is_point_in_poly(pol_disc, x, y)) { - NOTICE(E_USER_STRAT, " dst is in disc"); + + 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; } - if (is_point_in_poly(pol_opp, x, y)) { - NOTICE(E_USER_STRAT, " dst is in opp"); + + len = get_path(selected_circuit, i, j, selected_face, circuit_wpline); + if (len < 0) { + DEBUG(E_USER_STRAT, "%s(): cannot find a path", + __FUNCTION__); return END_ERROR; } - /* now start to avoid */ - while (opp_w && opp_l) { - - /* robot_pt is not updated if it fails */ - ret = escape_from_poly(&robot_pt, - pol_disc, opp_x, opp_y, - opp_w, opp_l, pol_opp); - if (ret == 0) { - oa_reset(); - oa_start_end_points(robot_pt.x, robot_pt.y, x, y); - /* oa_dump(); */ - - len = oa_process(); - if (len >= 0) - break; - } - if (distance_between(robot_pt.x, robot_pt.y, opp_x, opp_y) < REDUCE_POLY_THRES ) { - if (opp_w == 0) - opp_l /= 2; - opp_w /= 2; - } - else { - NOTICE(E_USER_STRAT, "oa_process() returned %d", len); - return END_ERROR; - } - - NOTICE(E_USER_STRAT, "reducing opponent %d %d", opp_w, opp_l); - set_opponent_poly(pol_opp, &robot_pt, opp_w, opp_l); - } - - p = oa_get_path(); - for (i=0 ; ix, p->y); - - if (forward) - trajectory_goto_forward_xy_abs(&mainboard.traj, p->x, p->y); - else - trajectory_goto_backward_xy_abs(&mainboard.traj, p->x, p->y); - - /* no END_NEAR for the last point */ - if (i == len - 1) - ret = wait_traj_end(flags_final); - else - ret = wait_traj_end(flags_intermediate); - - if (ret == END_BLOCKING) { - DEBUG(E_USER_STRAT, "Retry avoidance %s(%d,%d)", - __FUNCTION__, x, y); + dump_circuit_wp(circuit_wpline, len); + + prev_linenum = circuit_wpline[0].line_num; + prev_dir = circuit_wpline[0].dir; + for (idx = 1; idx < len; idx ++) { + retry: + 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); + if (!TRAJ_SUCCESS(err)) { + strat_hardstop(); + time_wait_ms(2000); goto retry; } - else if (ret == END_OBSTACLE) { - /* brake and wait the speed to be slow */ - DEBUG(E_USER_STRAT, "Retry avoidance %s(%d,%d)", - __FUNCTION__, x, y); - goto retry; - } - /* else if it is not END_TRAJ or END_NEAR, return */ - else if (!TRAJ_SUCCESS(ret)) { - return ret; - } - p++; - } - - return END_TRAJ; -} -/* go forward to a x,y point. use current speed for that */ -uint8_t goto_and_avoid_forward(int16_t x, int16_t y, uint8_t flags_intermediate, - uint8_t flags_final) -{ - return __goto_and_avoid(x, y, flags_intermediate, flags_final, 1); -} + prev_linenum = linenum; + prev_dir = dir; + } -/* go backward to a x,y point. use current speed for that */ -uint8_t goto_and_avoid_backward(int16_t x, int16_t y, uint8_t flags_intermediate, - uint8_t flags_final) -{ - return __goto_and_avoid(x, y, flags_intermediate, flags_final, 0); + return END_TRAJ; // XXX } -/* go to a x,y point. prefer backward but go forward if the point is - * near and in front of us */ -uint8_t goto_and_avoid(int16_t x, int16_t y, uint8_t flags_intermediate, - uint8_t flags_final) +void test_strat_avoid(void) { - double d,a; - abs_xy_to_rel_da(x, y, &d, &a); +#ifdef TEST_STRAT_AVOID + uint8_t i, j; + const struct circuit *selected_circuit; + int8_t selected_face; + struct wp_line circuit_wpline[MAX_CIRCUIT_WPLINE]; + int8_t ret; - if (d < 300 && a < RAD(90) && a > RAD(-90)) - return __goto_and_avoid(x, y, flags_intermediate, - flags_final, 1); - else - return __goto_and_avoid(x, y, flags_intermediate, - flags_final, 0); + i = 1; j = 1; + printf_P(PSTR("========= i=%d, j=%d\r\n"), i, j); + + ts = 0; bc = 0; cc = 0; + printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc); + 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); + 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); + 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); + 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); + 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); + 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); + + i = 4; j = 3; + printf_P(PSTR("========= i=%d, j=%d\r\n"), i, j); + + ts = 0; bc = 0; cc = 0; + printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc); + 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); + 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); + 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); + + i = 11; j = 6; + printf_P(PSTR("========= i=%d, j=%d\r\n"), i, j); + + ts = 0; bc = 0; cc = 0; + printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc); + 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); + 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); + 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 }