-/*
- * 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_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
+
+*/
+
+/* 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 parent_pos:3;
+ uint8_t updated:1;
+ uint8_t todo:1;
+ uint8_t reserved:3;
+};
+
+/* database for dijkstra */
+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;
+ struct waypoint_db *wp;
- 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<WAYPOINTS_NBX; i++) {
+ printf_P(PSTR(" %2d "), i);
+ }
+ printf_P(PSTR("\r\n"));
+
+ for (j=WAYPOINTS_NBY*2-1; j>=0; j--) {
+ printf_P(PSTR("%3d "), j/2);
+
+ if ((j&1) == 0)
+ printf_P(PSTR(" "));
+
+ for (i=0; i<WAYPOINTS_NBX; i++) {
+ pt = &djpoints[i][j/2];
+ wp = &strat_db.wp_table[i][j/2];
+
+ if (((i+j) & 1) == 0)
+ continue;
+
+ if (wp->type == WP_TYPE_OBSTACLE)
+ printf_P(PSTR(" X "));
+ else if (wp->dangerous)
+ printf_P(PSTR(" D "));
+ else if (wp->type == WP_TYPE_CORN &&
+ wp->corn.color == I2C_COB_WHITE)
+ printf_P(PSTR(" W "));
+ else if (wp->type == WP_TYPE_CORN &&
+ wp->corn.color == I2C_COB_BLACK)
+ printf_P(PSTR(" B "));
+ else if (wp->type == WP_TYPE_CORN &&
+ wp->corn.color == I2C_COB_UNKNOWN)
+ printf_P(PSTR(" U "));
+ else if (wp->type == WP_TYPE_WAYPOINT ||
+ wp->type == WP_TYPE_TOMATO)
+ 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)
+/* is point reachable by the robot ? */
+static uint8_t is_reachable(uint8_t i, uint8_t j)
{
- set_rotated_pentagon(pol, robot_pt, DISC_PENTA_DIAG,
- DISC_X, DISC_Y);
+ struct waypoint_db *wp;
+
+ wp = &strat_db.wp_table[i][j];
+ if (wp->type == WP_TYPE_WAYPOINT)
+ return 1;
+ if (wp->type == WP_TYPE_TOMATO)
+ return 1;
+ if (wp->type == WP_TYPE_CORN &&
+ wp->present == 0)
+ 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
-
-void set_opponent_poly(poly_t *pol, const point_t *robot_pt, int16_t w, int16_t l)
+/* return coord of the entry in the table from the pointer */
+static void djpoint2ij(struct djpoint *pt, uint8_t *i, uint8_t *j)
{
- 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 idx = PT2IDX(pt);
+ *i = idx / WAYPOINTS_NBY;
+ *j = idx % WAYPOINTS_NBY;
}
-/* don't care about polygons further than this distance for escape */
-#define ESCAPE_POLY_THRES 1000
-
-/* don't reduce opp if opp is too far */
-#define REDUCE_POLY_THRES 600
-
-/* has to be longer than any poly */
-#define ESCAPE_VECT_LEN 3000
-
-/*
- * 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)
+/* get the neighbour of the point at specified position */
+static struct djpoint *get_neigh(struct djpoint *pt,
+ uint8_t position)
{
- point_t poly_pts_area[4];
- poly_t poly_area;
- point_t disc_pt, dst_pt;
-
- 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;
-
- 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);
-
- robot_pt->x = dst_pt.x;
- robot_pt->y = dst_pt.y;
-
- NOTICE(E_USER_STRAT, "GOTO %"PRIi32",%"PRIi32"",
- dst_pt.x, dst_pt.y);
-
- return 1;
+ uint8_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;
- return 0;
-}
+ if (is_reachable(i, j) == 0)
+ return NULL;
+ neigh = &djpoints[i][j];
+ return neigh;
+}
-/*
- * 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)
+static struct djpoint *get_next_neigh(struct djpoint *pt, uint8_t *position)
{
- 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;
- }
- else {
- disc_dx = 1.0;
- disc_dy = 0.0;
- }
- escape_dx += disc_dx;
- escape_dy += disc_dy;
- }
+ struct djpoint *neigh = NULL;
+ uint8_t pos = *position + 1;
- 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;
- }
- escape_dx += opp_dx;
- escape_dy += opp_dy;
+ for (pos = *position + 1; pos < END; pos++) {
+ neigh = get_neigh(pt, pos);
+ if (neigh != NULL)
+ break;
}
- /* 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;
- }
- }
-
- NOTICE(E_USER_STRAT, " escape vect = %2.2f,%2.2f",
- escape_dx, escape_dy);
+ *position = pos;
+ return neigh;
+}
- /* process the correct len of escape vector */
+/* browse all points */
+#define POINT_FOREACH(cur) \
+ for (cur = &djpoints[0][0]; \
+ cur < &djpoints[WAYPOINTS_NBX][WAYPOINTS_NBY]; \
+ cur ++)
- dst_pt.x = robot_pt->x + escape_dx * ESCAPE_VECT_LEN;
- dst_pt.y = robot_pt->y + escape_dy * ESCAPE_VECT_LEN;
+/* XXX comment */
+#define NEIGH_FOREACH(neigh, pos, point) \
+ for (pos = START, neigh = get_next_neigh(point, &pos); \
+ neigh; \
+ neigh = get_next_neigh(point, &pos))
- 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);
+int dijkstra_init(void)
+{
+ return 0;
+}
- 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) {
+/* return distance between p1 and p2 */
+static uint16_t dist(struct djpoint *p1, struct djpoint *p2)
+{
+ int16_t x1, y1, x2, y2;
+ double vx, vy;
+ uint8_t i, j;
- if (!is_in_boundingbox(&dst_pt))
- return -1;
-
- NOTICE(E_USER_STRAT, "GOTO %"PRIi32",%"PRIi32"",
- dst_pt.x, dst_pt.y);
+ djpoint2ij(p1, &i, &j);
+ ijcoord_to_xycoord(i, j, &x1, &y1);
- strat_goto_xy_force(dst_pt.x, dst_pt.y);
+ djpoint2ij(p2, &i, &j);
+ ijcoord_to_xycoord(i, j, &x2, &y2);
- robot_pt->x = dst_pt.x;
- robot_pt->y = dst_pt.y;
+ vx = x2 - x1;
+ vy = y2 - y1;
+ return sqrt(vx * vx + vy * vy);
+}
- return 0;
- }
+void dijkstra_process_neighs(struct djpoint *pt)
+{
+ struct djpoint *neigh;
+ uint8_t pos, parent_pos;
+ uint16_t weight;
+ uint8_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;
}
}
+}
- 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) {
-
- if (!is_in_boundingbox(&dst_pt))
- return -1;
-
- NOTICE(E_USER_STRAT, "GOTO %"PRIi32",%"PRIi32"",
- dst_pt.x, dst_pt.y);
-
- strat_goto_xy_force(dst_pt.x, dst_pt.y);
-
- robot_pt->x = dst_pt.x;
- robot_pt->y = dst_pt.y;
+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;
+ }
- return 0;
- }
+ /* convert updated list in todo list */
+ todolist = 0;
+ POINT_FOREACH(cur) {
+ if (!cur->updated)
+ continue;
+ todolist = 1;
+ cur->todo = 1;
+ cur->updated = 0;
}
}
-
- /* should not happen */
- return -1;
+ return 0; /* XXX */
}
-
-static int8_t __goto_and_avoid(int16_t x, int16_t y,
- uint8_t flags_intermediate,
- uint8_t flags_final,
- uint8_t forward)
+/* init waypoints position */
+void init_djpoints(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");
- 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;
- }
+ int8_t i, j;
+ struct djpoint *pt;
- /* 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;
- }
+ for (i=0; i<WAYPOINTS_NBX; i++) {
- 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);
- 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;
+ for (j=0; j<WAYPOINTS_NBY; j++) {
+ pt = &djpoints[i][j];
+ pt->parent_pos = END;
+ pt->updated = 0;
+ pt->todo = 0;
+ pt->weight = 0;
}
- 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)
+int get_path(struct djpoint *start, struct djpoint *end)
{
- return __goto_and_avoid(x, y, flags_intermediate, flags_final, 1);
-}
+ struct djpoint *cur;
+ uint8_t prev_direction = 0;
+ int8_t idx;
+ int16_t 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);
-}
+ 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_xycoord(idx, &x, &y);
+ printf_P(PSTR("%d %d (%d)\r\n"),
+ x, y, cur->parent_pos);
+ }
+ prev_direction = cur->parent_pos;
+ }
+ idx = PT2IDX(end);
+ corn_idx_to_xycoord(idx, &x, &y);
+ printf_P(PSTR("%d %d\r\n"), x, y);
-/* 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 */
}
git.droids-corp.org / aversive.git / blobdiff