X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fmainboard%2Fstrat_avoid.c;h=3c128bfaaf0a689032a8484b14a8947c20b430c0;hp=6e75dd0e55375e862fdcfa0e2ca80a3f6668b075;hb=4e7801883ed4076cb14b63a0571467747894c0f8;hpb=5918edd6f4f713ef3c8b0b0020dd30a4fb8222ae diff --git a/projects/microb2010/mainboard/strat_avoid.c b/projects/microb2010/mainboard/strat_avoid.c index 6e75dd0..3c128bf 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,32 @@ * 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 +49,364 @@ #include #include +#include + #include #include +#include "../common/i2c_commands.h" +#include "i2c_protocol.h" #include "main.h" #include "strat.h" #include "strat_base.h" +#include "strat_corn.h" #include "strat_utils.h" #include "sensor.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) +#include "actuator.h" + +/* XXX TODO +static +const +change x,y -> i,j to avoid confusion with coords +could be optimized in mem space: it is not needed to store the x,y coord, + we can process it from idx. however it will be less optimized for speed + +*/ + +#define OFFSET_CORN_X 150 +#define OFFSET_CORN_Y 222 +#define STEP_CORN_X 225 +#define STEP_CORN_Y 250 + +#define CORN_NB 18 + +#define WAYPOINTS_NBX 13 +#define WAYPOINTS_NBY 8 + +/* enum is better */ +#define TYPE_WAYPOINT 0 +#define TYPE_DANGEROUS 1 +#define TYPE_WHITE_CORN 2 +#define TYPE_BLACK_CORN 3 +#define TYPE_OBSTACLE 4 +#define TYPE_UNKNOWN 5 + +/* XXX enum possible ? else just rename */ +#define START 0 +#define UP 1 +#define UP_RIGHT 2 +#define DOWN_RIGHT 3 +#define DOWN 4 +#define DOWN_LEFT 5 +#define UP_LEFT 6 +#define END 7 + +struct djpoint { + uint16_t weight; + struct djpoint *parent; + + uint8_t type:3; + uint8_t parent_pos:3; + uint8_t updated:1; + uint8_t todo:1; +}; + +uint8_t corn_table[CORN_NB]; + +static struct djpoint djpoints[WAYPOINTS_NBX][WAYPOINTS_NBY]; + +/* return index from neigh pointer */ +#define PT2IDX(neigh) ( ((void *)(neigh)-(void *)(&djpoints)) / sizeof(*neigh) ) + +void dump(void) { + int8_t i, j; + struct djpoint *pt; - 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); - - /* - 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)); - - - 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; - - px1 = px2; - py1 = py2; + printf_P(PSTR(" ")); + for (i=0; i=0; j--) { + printf_P(PSTR("%3d "), j/2); + + if ((j&1) == 0) + printf_P(PSTR(" ")); + + for (i=0; itype == TYPE_OBSTACLE) + printf_P(PSTR(" X ")); + else if (pt->type == TYPE_DANGEROUS) + printf_P(PSTR(" D ")); + else if (pt->type == TYPE_WHITE_CORN) + printf_P(PSTR(" W ")); + else if (pt->type == TYPE_BLACK_CORN) + printf_P(PSTR(" B ")); + else if (pt->type == TYPE_WAYPOINT) + printf_P(PSTR(" %5d "), pt->weight); + else + printf_P(PSTR(" ? ")); + } + printf_P(PSTR("\r\n")); } } -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 inline uint8_t opposite_position(uint8_t pos) { - 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); + pos += 3; + if (pos > UP_LEFT) + pos -= 6; + return pos; } -#define DISC_X CENTER_X -#define DISC_Y CENTER_Y - -void set_central_disc_poly(poly_t *pol, const point_t *robot_pt) +/* return coord of the entry in the table from the pointer */ +static void djpoint2ij(struct djpoint *pt, int8_t *x, int8_t *y) { - set_rotated_pentagon(pol, robot_pt, DISC_PENTA_DIAG, - DISC_X, DISC_Y); + int8_t idx = PT2IDX(pt); + *x = idx / WAYPOINTS_NBY; + *y = idx % WAYPOINTS_NBY; } -#ifdef HOMOLOGATION -/* /!\ half size */ -#define O_WIDTH 400 -#define O_LENGTH 550 -#else -/* /!\ half size */ -#define O_WIDTH 360 -#define O_LENGTH 500 -#endif - -void set_opponent_poly(poly_t *pol, const point_t *robot_pt, int16_t w, int16_t l) +/* get the neighbour of the point at specified position */ +static struct djpoint *get_neigh(struct djpoint *pt, + uint8_t position) { - 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); -} + int8_t i,j; + struct djpoint *neigh; + + djpoint2ij(pt, &i, &j); + + switch (position) { + case UP: + j++; + break; + case UP_RIGHT: + if (!(i & 1)) j++; + i++; + break; + case DOWN_RIGHT: + if (i & 1) j--; + i++; + break; + case DOWN: + j--; + break; + case DOWN_LEFT: + if (i & 1) j--; + i--; + break; + case UP_LEFT: + if (!(i & 1)) j++; + i--; + break; + default: + return NULL; + } + if (i < 0 || j < 0 || i >= WAYPOINTS_NBX || j >= WAYPOINTS_NBY) + return NULL; -/* don't care about polygons further than this distance for escape */ -#define ESCAPE_POLY_THRES 1000 + neigh = &djpoints[i][j]; -/* don't reduce opp if opp is too far */ -#define REDUCE_POLY_THRES 600 + if (neigh->type != TYPE_WAYPOINT) + return NULL; -/* has to be longer than any poly */ -#define ESCAPE_VECT_LEN 3000 + return neigh; +} -/* - * 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) +static struct djpoint *get_next_neigh(struct djpoint *pt, uint8_t *position) { - point_t poly_pts_area[4]; - poly_t poly_area; - point_t disc_pt, dst_pt; + struct djpoint *neigh = NULL; + uint8_t pos = *position + 1; - disc_pt.x = DISC_X; - disc_pt.y = DISC_Y; - - /* 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; - - poly_pts_area[1].x = strat_infos.area_bbox.x2; - poly_pts_area[1].y = strat_infos.area_bbox.y1; - - poly_pts_area[2].x = strat_infos.area_bbox.x2; - poly_pts_area[2].y = strat_infos.area_bbox.y2; - - poly_pts_area[3].x = strat_infos.area_bbox.x1; - poly_pts_area[3].y = strat_infos.area_bbox.y2; + for (pos = *position + 1; pos < END; pos++) { + neigh = get_neigh(pt, pos); + if (neigh != NULL) + break; + } - 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); + *position = pos; + return neigh; +} - robot_pt->x = dst_pt.x; - robot_pt->y = dst_pt.y; +/* browse all points */ +#define POINT_FOREACH(cur) \ + for (cur = &djpoints[0][0]; \ + cur < &djpoints[WAYPOINTS_NBX][WAYPOINTS_NBY]; \ + cur ++) - NOTICE(E_USER_STRAT, "GOTO %"PRIi32",%"PRIi32"", - dst_pt.x, dst_pt.y); - - return 1; - } +/* XXX comment */ +#define NEIGH_FOREACH(neigh, pos, point) \ + for (pos = START, neigh = get_next_neigh(point, &pos); \ + neigh; \ + neigh = get_next_neigh(point, &pos)) +int dijkstra_init(void) +{ return 0; } +static uint16_t dist(struct djpoint *p1, struct djpoint *p2) +{ + double vx, vy; + vx = p2->pos.x - p1->pos.x; + vy = p2->pos.y - p1->pos.y; + return sqrt(vx * vx + vy * vy); +} -/* - * 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) +void dijkstra_process_neighs(struct djpoint *pt) { - 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; + struct djpoint *neigh; + uint8_t pos, parent_pos; + uint16_t weight; + int8_t i,j; + + djpoint2ij(pt, &i, &j); + printf_P(PSTR("at %d %d:\r\n"), i, j); + + NEIGH_FOREACH(neigh, pos, pt) { + weight = pt->weight + dist(pt, neigh); + parent_pos = opposite_position(pos); + + /* bonus if we keep the same direction */ + if (parent_pos == pt->parent_pos || + pt->parent_pos == END) + weight = weight - 1; + + printf_P(PSTR(" pos=%d: ppos=%d opos=%d nw=%d w=%d\r\n"), pos, + pt->parent_pos, parent_pos, + neigh->weight, weight); + if (neigh->weight == 0 || weight < neigh->weight) { + djpoint2ij(neigh, &i, &j); + //printf_P(PSTR(" %d,%d updated\r\n"), i, j); + neigh->weight = weight; + neigh->parent_pos = parent_pos; + neigh->updated = 1; } - else { - disc_dx = 1.0; - disc_dy = 0.0; - } - escape_dx += disc_dx; - escape_dy += disc_dy; } +} - 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; +int dijkstra(struct djpoint *start) +{ + struct djpoint *cur; + uint8_t todolist = 1; + + start->todo = 1; + + while (todolist) { + printf_P(PSTR("\r\n")); + dump(); + /* process all neighbours of todo list */ + POINT_FOREACH(cur) { + if (!cur->todo) + continue; + dijkstra_process_neighs(cur); + cur->todo = 0; } - escape_dx += opp_dx; - escape_dy += opp_dy; - } - /* normalize escape vector */ - len = norm(escape_dx, escape_dy); - if (len != 0) { - escape_dx /= len; - escape_dy /= len; - } - 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; + /* convert updated list in todo list */ + todolist = 0; + POINT_FOREACH(cur) { + if (!cur->updated) + continue; + todolist = 1; + cur->todo = 1; + cur->updated = 0; } } + return 0; /* XXX */ +} - NOTICE(E_USER_STRAT, " escape vect = %2.2f,%2.2f", - escape_dx, escape_dy); - - /* process the correct len of escape vector */ - - dst_pt.x = robot_pt->x + escape_dx * ESCAPE_VECT_LEN; - dst_pt.y = robot_pt->y + escape_dy * ESCAPE_VECT_LEN; - - 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); - - 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) { - - if (!is_in_boundingbox(&dst_pt)) - return -1; - - NOTICE(E_USER_STRAT, "GOTO %"PRIi32",%"PRIi32"", - dst_pt.x, dst_pt.y); +/* init waypoints position */ +void init_djpoints(void) +{ + int8_t i, j; + struct djpoint *pt; - strat_goto_xy_force(dst_pt.x, dst_pt.y); + for (i=0; ix = dst_pt.x; - robot_pt->y = dst_pt.y; + for (j=0; jtype = TYPE_WAYPOINT; + pt->parent_pos = END; + pt->updated = 0; + pt->todo = 0; } } +} - 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; - - if (is_point_in_poly(pol_disc, dst_pt.x, dst_pt.y) != 1) { +/* update the type and weight of waypoints, before starting + * dijkstra */ +void update_djpoints(void) +{ + int8_t i, j, c; + struct djpoint *pt; - if (!is_in_boundingbox(&dst_pt)) - return -1; - - NOTICE(E_USER_STRAT, "GOTO %"PRIi32",%"PRIi32"", - dst_pt.x, dst_pt.y); + for (i=0; ix = dst_pt.x; - robot_pt->y = dst_pt.y; + pt->weight = 0; - return 0; + /* corn */ + c = ijcoord_to_corn_idx(i, j); + if (c >= 0) { + pt->type = corn_table[c]; + continue; + } + /* too close of border */ + if ((i & 1) == 1 && j == (WAYPOINTS_NBY-1)) { + pt->type = TYPE_OBSTACLE; + continue; } + /* hill */ + if (i >= 2 && i < (WAYPOINTS_NBX-2) && j < 2) { + pt->type = TYPE_OBSTACLE; + continue; + } + /* dangerous points */ + if (i == 0 || i == (WAYPOINTS_NBX-1)) { + pt->type = TYPE_DANGEROUS; + continue; + } + if ( (i&1) == 0 && j == (WAYPOINTS_NBY-1)) { + pt->type = TYPE_DANGEROUS; + continue; + } + pt->type = TYPE_WAYPOINT; } } - - /* should not happen */ - return -1; } - -static int8_t __goto_and_avoid(int16_t x, int16_t y, - uint8_t flags_intermediate, - uint8_t flags_final, - uint8_t forward) +int get_path(struct djpoint *start, struct djpoint *end) { - 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"); - return END_ERROR; - } - if (is_point_in_poly(pol_disc, x, y)) { - NOTICE(E_USER_STRAT, " dst is in disc"); - return END_ERROR; - } - if (is_point_in_poly(pol_opp, x, y)) { - NOTICE(E_USER_STRAT, " dst is in opp"); - return END_ERROR; - } + struct djpoint *cur; + uint8_t prev_direction = 0; + int8_t idx; + int16_t x, y; - /* 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; + for (cur = start; + cur != NULL && cur->parent_pos != END && cur != end; + cur = get_neigh(cur, cur->parent_pos)) { + if (prev_direction != cur->parent_pos) { + idx = PT2IDX(cur); + corn_idx_to_coordxy(idx, &x, &y); + printf_P(PSTR("%d %d (%d)\r\n"), + x, y, cur->parent_pos); } - 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); + prev_direction = cur->parent_pos; } - - 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); - 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); -} + idx = PT2IDX(end); + corn_idx_to_coordxy(idx, &x, &y); + printf_P(PSTR("%d %d\r\n"), x, y); -/* 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); -} - -/* 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) -{ - double d,a; - abs_xy_to_rel_da(x, y, &d, &a); - - 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); + return 0; /* XXX */ }