-/*
- * 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
* 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 <zer0@droids-corp.org>
*/
+
#include <stdio.h>
#include <stdlib.h>
-#include <string.h>
+#include <stdint.h>
#include <math.h>
+#include <aversive.h>
#include <aversive/pgmspace.h>
-#include <aversive/wait.h>
#include <aversive/error.h>
#include <ax12.h>
#include <uart.h>
#include <pwm_ng.h>
-#include <time.h>
+#include <clock_time.h>
#include <spi.h>
#include <pid.h>
#include <quadramp.h>
#include <control_system_manager.h>
#include <trajectory_manager.h>
+#include <trajectory_manager_utils.h>
+#include <trajectory_manager_core.h>
#include <vect_base.h>
#include <lines.h>
#include <polygon.h>
#include <robot_system.h>
#include <position_manager.h>
+#include <diagnostic.h>
+
#include <rdline.h>
#include <parse.h>
+#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<CORN_NB; i++) {
+ if (n >= 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<TOMATO_NB; i++) {
+ if (n >= 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 ; i<len ; i++) {
- DEBUG(E_USER_STRAT, "With avoidance %d: x=%"PRIi32" y=%"PRIi32"", i, p->x, 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
}
git.droids-corp.org / aversive.git / blobdiff