2 * Copyright Droids Corporation, Microb Technology (2009)
4 * This program is free software; you can redistribute it and/or modify
5 * it under the terms of the GNU General Public License as published by
6 * the Free Software Foundation; either version 2 of the License, or
7 * (at your option) any later version.
9 * This program is distributed in the hope that it will be useful,
10 * but WITHOUT ANY WARRANTY; without even the implied warranty of
11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 * GNU General Public License for more details.
14 * You should have received a copy of the GNU General Public License
15 * along with this program; if not, write to the Free Software
16 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
18 * Revision : $Id: strat_avoid.c,v 1.5 2009-11-08 17:24:33 zer0 Exp $
27 #include <aversive/pgmspace.h>
28 #include <aversive/wait.h>
29 #include <aversive/error.h>
39 #include <control_system_manager.h>
40 #include <trajectory_manager.h>
41 #include <vect_base.h>
44 #include <obstacle_avoidance.h>
45 #include <blocking_detection_manager.h>
46 #include <robot_system.h>
47 #include <position_manager.h>
54 #include "strat_base.h"
55 #include "strat_utils.h"
59 void set_rotated_pentagon(poly_t *pol, const point_t *robot_pt,
60 int16_t radius, int16_t x, int16_t y)
65 double px1, py1, px2, py2;
68 a_rad = atan2(y - robot_pt->y, x - robot_pt->x);
70 /* generate pentagon */
71 c_a = cos(-2*M_PI/EDGE_NUMBER);
72 s_a = sin(-2*M_PI/EDGE_NUMBER);
78 px1 = radius * cos(a_rad + 2*M_PI/(2*EDGE_NUMBER));
79 py1 = radius * sin(a_rad + 2*M_PI/(2*EDGE_NUMBER));
82 for (i = 0; i < EDGE_NUMBER; i++){
83 oa_poly_set_point(pol, x + px1, y + py1, i);
85 px2 = px1*c_a + py1*s_a;
86 py2 = -px1*s_a + py1*c_a;
93 void set_rotated_poly(poly_t *pol, const point_t *robot_pt,
94 int16_t w, int16_t l, int16_t x, int16_t y)
99 a_rad = atan2(y - robot_pt->y, x - robot_pt->x);
101 DEBUG(E_USER_STRAT, "%s() x,y=%d,%d a_rad=%2.2f",
102 __FUNCTION__, x, y, a_rad);
107 rotate(&tmp_x, &tmp_y, a_rad);
110 oa_poly_set_point(pol, tmp_x, tmp_y, 0);
115 rotate(&tmp_x, &tmp_y, a_rad);
118 oa_poly_set_point(pol, tmp_x, tmp_y, 1);
123 rotate(&tmp_x, &tmp_y, a_rad);
126 oa_poly_set_point(pol, tmp_x, tmp_y, 2);
131 rotate(&tmp_x, &tmp_y, a_rad);
134 oa_poly_set_point(pol, tmp_x, tmp_y, 3);
137 #define DISC_X CENTER_X
138 #define DISC_Y CENTER_Y
140 void set_central_disc_poly(poly_t *pol, const point_t *robot_pt)
142 set_rotated_pentagon(pol, robot_pt, DISC_PENTA_DIAG,
156 void set_opponent_poly(poly_t *pol, const point_t *robot_pt, int16_t w, int16_t l)
159 get_opponent_xy(&x, &y);
160 DEBUG(E_USER_STRAT, "oponent at: %d %d", x, y);
162 /* place poly even if invalid, because it's -100 */
163 set_rotated_poly(pol, robot_pt, w, l, x, y);
166 /* don't care about polygons further than this distance for escape */
167 #define ESCAPE_POLY_THRES 1000
169 /* don't reduce opp if opp is too far */
170 #define REDUCE_POLY_THRES 600
172 /* has to be longer than any poly */
173 #define ESCAPE_VECT_LEN 3000
176 * Go in playground, loop until out of poly. The argument robot_pt is
177 * the pointer to the current position of the robot.
178 * Return 0 if there was nothing to do.
179 * Return 1 if we had to move. In this case, update the theorical
180 * position of the robot in robot_pt.
182 static int8_t go_in_area(point_t *robot_pt)
184 point_t poly_pts_area[4];
186 point_t disc_pt, dst_pt;
191 /* Go in playground */
192 if (!is_in_boundingbox(robot_pt)){
193 NOTICE(E_USER_STRAT, "not in playground %"PRIi32", %"PRIi32"",
194 robot_pt->x, robot_pt->y);
197 poly_area.pts = poly_pts_area;
198 poly_pts_area[0].x = strat_infos.area_bbox.x1;
199 poly_pts_area[0].y = strat_infos.area_bbox.y1;
201 poly_pts_area[1].x = strat_infos.area_bbox.x2;
202 poly_pts_area[1].y = strat_infos.area_bbox.y1;
204 poly_pts_area[2].x = strat_infos.area_bbox.x2;
205 poly_pts_area[2].y = strat_infos.area_bbox.y2;
207 poly_pts_area[3].x = strat_infos.area_bbox.x1;
208 poly_pts_area[3].y = strat_infos.area_bbox.y2;
210 is_crossing_poly(*robot_pt, disc_pt, &dst_pt, &poly_area);
211 NOTICE(E_USER_STRAT, "pt dst %"PRIi32", %"PRIi32"", dst_pt.x, dst_pt.y);
213 strat_goto_xy_force(dst_pt.x, dst_pt.y);
215 robot_pt->x = dst_pt.x;
216 robot_pt->y = dst_pt.y;
218 NOTICE(E_USER_STRAT, "GOTO %"PRIi32",%"PRIi32"",
229 * Escape from polygons if needed.
230 * robot_pt is the current position of the robot, it will be
233 static int8_t escape_from_poly(point_t *robot_pt,
235 int16_t opp_x, int16_t opp_y,
236 int16_t opp_w, int16_t opp_l,
239 uint8_t in_disc = 0, in_opp = 0;
240 double escape_dx = 0, escape_dy = 0;
241 double disc_dx = 0, disc_dy = 0;
242 double opp_dx = 0, opp_dy = 0;
244 point_t opp_pt, disc_pt, dst_pt;
245 point_t intersect_disc_pt, intersect_opp_pt;
252 /* escape from other poly if necessary */
253 if (is_in_poly(robot_pt, pol_disc) == 1)
255 if (is_in_poly(robot_pt, pol_opp) == 1)
258 if (in_disc == 0 && in_opp == 0) {
259 NOTICE(E_USER_STRAT, "no need to escape");
263 NOTICE(E_USER_STRAT, "in_disc=%d, in_opp=%d", in_disc, in_opp);
265 /* process escape vector */
267 if (distance_between(robot_pt->x, robot_pt->y, DISC_X, DISC_Y) < ESCAPE_POLY_THRES) {
268 disc_dx = robot_pt->x - DISC_X;
269 disc_dy = robot_pt->y - DISC_Y;
270 NOTICE(E_USER_STRAT, " robot is near disc: vect=%2.2f,%2.2f",
272 len = norm(disc_dx, disc_dy);
281 escape_dx += disc_dx;
282 escape_dy += disc_dy;
285 if (distance_between(robot_pt->x, robot_pt->y, opp_x, opp_y) < ESCAPE_POLY_THRES) {
286 opp_dx = robot_pt->x - opp_x;
287 opp_dy = robot_pt->y - opp_y;
288 NOTICE(E_USER_STRAT, " robot is near opp: vect=%2.2f,%2.2f",
290 len = norm(opp_dx, opp_dy);
303 /* normalize escape vector */
304 len = norm(escape_dx, escape_dy);
310 if (pol_disc != NULL) {
315 else if (pol_opp != NULL) {
320 else { /* should not happen */
326 NOTICE(E_USER_STRAT, " escape vect = %2.2f,%2.2f",
327 escape_dx, escape_dy);
329 /* process the correct len of escape vector */
331 dst_pt.x = robot_pt->x + escape_dx * ESCAPE_VECT_LEN;
332 dst_pt.y = robot_pt->y + escape_dy * ESCAPE_VECT_LEN;
334 NOTICE(E_USER_STRAT, "robot pt %"PRIi32" %"PRIi32,
335 robot_pt->x, robot_pt->y);
336 NOTICE(E_USER_STRAT, "dst point %"PRIi32",%"PRIi32,
340 if (is_crossing_poly(*robot_pt, dst_pt, &intersect_disc_pt,
342 /* we add 2 mm to be sure we are out of th polygon */
343 dst_pt.x = intersect_disc_pt.x + escape_dx * 2;
344 dst_pt.y = intersect_disc_pt.y + escape_dy * 2;
345 if (is_point_in_poly(pol_opp, dst_pt.x, dst_pt.y) != 1) {
347 if (!is_in_boundingbox(&dst_pt))
350 NOTICE(E_USER_STRAT, "GOTO %"PRIi32",%"PRIi32"",
353 strat_goto_xy_force(dst_pt.x, dst_pt.y);
355 robot_pt->x = dst_pt.x;
356 robot_pt->y = dst_pt.y;
364 if (is_crossing_poly(*robot_pt, dst_pt, &intersect_opp_pt,
366 /* we add 2 cm to be sure we are out of th polygon */
367 dst_pt.x = intersect_opp_pt.x + escape_dx * 2;
368 dst_pt.y = intersect_opp_pt.y + escape_dy * 2;
370 if (is_point_in_poly(pol_disc, dst_pt.x, dst_pt.y) != 1) {
372 if (!is_in_boundingbox(&dst_pt))
375 NOTICE(E_USER_STRAT, "GOTO %"PRIi32",%"PRIi32"",
378 strat_goto_xy_force(dst_pt.x, dst_pt.y);
380 robot_pt->x = dst_pt.x;
381 robot_pt->y = dst_pt.y;
388 /* should not happen */
393 static int8_t __goto_and_avoid(int16_t x, int16_t y,
394 uint8_t flags_intermediate,
400 poly_t *pol_disc, *pol_opp;
402 int16_t opp_w, opp_l, opp_x, opp_y;
403 point_t p_dst, robot_pt;
405 DEBUG(E_USER_STRAT, "%s(%d,%d) flags_i=%x flags_f=%x forw=%d",
406 __FUNCTION__, x, y, flags_intermediate, flags_final, forward);
409 get_opponent_xy(&opp_x, &opp_y);
413 robot_pt.x = position_get_x_s16(&mainboard.pos);
414 robot_pt.y = position_get_y_s16(&mainboard.pos);
417 pol_disc = oa_new_poly(5);
418 set_central_disc_poly(pol_disc, &robot_pt);
419 pol_opp = oa_new_poly(4);
420 set_opponent_poly(pol_opp, &robot_pt, O_WIDTH, O_LENGTH);
422 /* If we are not in the limited area, try to go in it. */
423 ret = go_in_area(&robot_pt);
425 /* check that destination is valid */
428 if (!is_in_boundingbox(&p_dst)) {
429 NOTICE(E_USER_STRAT, " dst is not in playground");
432 if (is_point_in_poly(pol_disc, x, y)) {
433 NOTICE(E_USER_STRAT, " dst is in disc");
436 if (is_point_in_poly(pol_opp, x, y)) {
437 NOTICE(E_USER_STRAT, " dst is in opp");
441 /* now start to avoid */
442 while (opp_w && opp_l) {
444 /* robot_pt is not updated if it fails */
445 ret = escape_from_poly(&robot_pt,
446 pol_disc, opp_x, opp_y,
447 opp_w, opp_l, pol_opp);
450 oa_start_end_points(robot_pt.x, robot_pt.y, x, y);
457 if (distance_between(robot_pt.x, robot_pt.y, opp_x, opp_y) < REDUCE_POLY_THRES ) {
463 NOTICE(E_USER_STRAT, "oa_process() returned %d", len);
467 NOTICE(E_USER_STRAT, "reducing opponent %d %d", opp_w, opp_l);
468 set_opponent_poly(pol_opp, &robot_pt, opp_w, opp_l);
472 for (i=0 ; i<len ; i++) {
473 DEBUG(E_USER_STRAT, "With avoidance %d: x=%"PRIi32" y=%"PRIi32"", i, p->x, p->y);
476 trajectory_goto_forward_xy_abs(&mainboard.traj, p->x, p->y);
478 trajectory_goto_backward_xy_abs(&mainboard.traj, p->x, p->y);
480 /* no END_NEAR for the last point */
482 ret = wait_traj_end(flags_final);
484 ret = wait_traj_end(flags_intermediate);
486 if (ret == END_BLOCKING) {
487 DEBUG(E_USER_STRAT, "Retry avoidance %s(%d,%d)",
491 else if (ret == END_OBSTACLE) {
492 /* brake and wait the speed to be slow */
493 DEBUG(E_USER_STRAT, "Retry avoidance %s(%d,%d)",
497 /* else if it is not END_TRAJ or END_NEAR, return */
498 else if (!TRAJ_SUCCESS(ret)) {
507 /* go forward to a x,y point. use current speed for that */
508 uint8_t goto_and_avoid_forward(int16_t x, int16_t y, uint8_t flags_intermediate,
511 return __goto_and_avoid(x, y, flags_intermediate, flags_final, 1);
514 /* go backward to a x,y point. use current speed for that */
515 uint8_t goto_and_avoid_backward(int16_t x, int16_t y, uint8_t flags_intermediate,
518 return __goto_and_avoid(x, y, flags_intermediate, flags_final, 0);
521 /* go to a x,y point. prefer backward but go forward if the point is
522 * near and in front of us */
523 uint8_t goto_and_avoid(int16_t x, int16_t y, uint8_t flags_intermediate,
527 abs_xy_to_rel_da(x, y, &d, &a);
529 if (d < 300 && a < RAD(90) && a > RAD(-90))
530 return __goto_and_avoid(x, y, flags_intermediate,
533 return __goto_and_avoid(x, y, flags_intermediate,