7 #include <aversive/error.h>
12 #include <obstacle_avoidance.h>
18 #define M_2PI (M_PI*2)
19 #define E_USER_STRAT 200
23 #define ROBOT_A 0.5 /* radian */
24 #define ROBOT_A_DEG ((int)((ROBOT_A*180)/3.14159))
32 #define PLAYGROUND_X_MIN 250
33 #define PLAYGROUND_X_MAX 2750
34 #define PLAYGROUND_Y_MIN 250
35 #define PLAYGROUND_Y_MAX 1850
37 int16_t robot_x = ROBOT_X;
38 int16_t robot_y = ROBOT_Y;
39 double robot_a = ROBOT_A;
40 int16_t robot_a_deg = ROBOT_A_DEG;
42 int16_t opp_x = OPP_X;
43 int16_t opp_y = OPP_Y;
45 int16_t dst_x = DST_X;
46 int16_t dst_y = DST_Y;
52 double norm(double x, double y)
54 return sqrt(x*x + y*y);
57 /* return the distance between two points */
58 int16_t distance_between(int16_t x1, int16_t y1, int16_t x2, int16_t y2)
68 void rotate(double *x, double *y, double rot)
81 void set_rotated_pentagon(poly_t *pol, int16_t radius,
87 double px1, py1, px2, py2;
90 a_rad = atan2(y - robot_y, x - robot_x);
93 /* generate pentagon */
94 c_a = cos(-2*M_PI/EDGE_NUMBER);
95 s_a = sin(-2*M_PI/EDGE_NUMBER);
101 px1 = radius * cos(a_rad + 2*M_PI/(2*EDGE_NUMBER));
102 py1 = radius * sin(a_rad + 2*M_PI/(2*EDGE_NUMBER));
105 for (i = 0; i < EDGE_NUMBER; i++){
106 oa_poly_set_point(pol, x + px1, y + py1, i);
108 px2 = px1*c_a + py1*s_a;
109 py2 = -px1*s_a + py1*c_a;
117 void set_rotated_poly(poly_t *pol, int16_t w, int16_t l,
118 int16_t x, int16_t y)
123 a_rad = atan2(y - robot_y, x - robot_x);
128 rotate(&tmp_x, &tmp_y, a_rad);
131 oa_poly_set_point(pol, tmp_x, tmp_y, 0);
136 rotate(&tmp_x, &tmp_y, a_rad);
139 oa_poly_set_point(pol, tmp_x, tmp_y, 1);
144 rotate(&tmp_x, &tmp_y, a_rad);
147 oa_poly_set_point(pol, tmp_x, tmp_y, 2);
152 rotate(&tmp_x, &tmp_y, a_rad);
155 oa_poly_set_point(pol, tmp_x, tmp_y, 3);
158 #define DISC_PENTA_DIAG 500
162 void set_central_disc_poly(poly_t *pol)
164 set_rotated_pentagon(pol, DISC_PENTA_DIAG,
172 void set_opponent_poly(poly_t *pol, int16_t w, int16_t l)
177 DEBUG(E_USER_STRAT, "oponent at: %d %d", x, y);
179 /* place poly even if invalid, because it's -100 */
180 set_rotated_poly(pol, w, l, x, y);
183 /* don't care about polygons further than this distance for
185 #define ESCAPE_POLY_THRES 1000
187 /* don't reduce opp if opp is too far */
188 #define REDUCE_POLY_THRES 600
190 /* has to be longer than any poly */
191 #define ESCAPE_VECT_LEN 3000
193 /* go in playground, loop until out of poly */
194 /* XXX return end timer??? */
195 static int8_t go_in_area(void)
197 point_t poly_pts_area[4];
200 point_t robot_pt, disc_pt, dst_pt;
202 robot_pt.x = robot_x;
203 robot_pt.y = robot_y;
207 /* Go in playground */
208 if (!is_in_boundingbox(&robot_pt)){
209 NOTICE(E_USER_STRAT, "not in playground %d, %d", robot_x, robot_y);
212 poly_area.pts = poly_pts_area;
213 poly_pts_area[0].x = PLAYGROUND_X_MIN;
214 poly_pts_area[0].y = PLAYGROUND_Y_MIN;
216 poly_pts_area[1].x = PLAYGROUND_X_MAX;
217 poly_pts_area[1].y = PLAYGROUND_Y_MIN;
219 poly_pts_area[2].x = PLAYGROUND_X_MAX;
220 poly_pts_area[2].y = PLAYGROUND_Y_MAX;
222 poly_pts_area[3].x = PLAYGROUND_X_MIN;
223 poly_pts_area[3].y = PLAYGROUND_Y_MAX;
226 is_crossing_poly(robot_pt, disc_pt, &dst_pt, &poly_area);
227 NOTICE(E_USER_STRAT, "pt dst %d, %d", dst_pt.x, dst_pt.y);
230 robot_pt.x = dst_pt.x;
231 robot_pt.y = dst_pt.y;
236 NOTICE(E_USER_STRAT, "GOTO %d,%d",
247 * Escape from polygons if needed.
249 static int8_t escape_from_poly(poly_t *pol_disc,
250 int16_t opp_x, int16_t opp_y,
251 int16_t opp_w, int16_t opp_l,
254 uint8_t in_disc = 0, in_opp = 0;
255 double escape_dx = 0, escape_dy = 0;
256 double disc_dx = 0, disc_dy = 0;
257 double opp_dx = 0, opp_dy = 0;
260 point_t robot_pt, opp_pt, disc_pt, dst_pt;
261 point_t intersect_disc_pt, intersect_opp_pt;
263 robot_pt.x = robot_x;
264 robot_pt.y = robot_y;
270 /* escape from other poly if necessary */
271 if (is_point_in_poly(pol_disc, robot_x, robot_y) == 1)
273 if (is_point_in_poly(pol_opp, robot_x, robot_y) == 1)
276 if (in_disc == 0 && in_opp == 0) {
277 NOTICE(E_USER_STRAT, "no need to escape");
281 NOTICE(E_USER_STRAT, "in_disc=%d, in_opp=%d", in_disc, in_opp);
283 /* process escape vector */
285 if (distance_between(robot_x, robot_y, DISC_X, DISC_Y) < ESCAPE_POLY_THRES) {
286 disc_dx = robot_x - DISC_X;
287 disc_dy = robot_y - DISC_Y;
288 NOTICE(E_USER_STRAT, " robot is near disc: vect=%2.2f,%2.2f",
290 len = norm(disc_dx, disc_dy);
299 escape_dx += disc_dx;
300 escape_dy += disc_dy;
303 if (distance_between(robot_x, robot_y, opp_x, opp_y) < ESCAPE_POLY_THRES) {
304 opp_dx = robot_x - opp_x;
305 opp_dy = robot_y - opp_y;
306 NOTICE(E_USER_STRAT, " robot is near opp: vect=%2.2f,%2.2f",
308 len = norm(opp_dx, opp_dy);
321 /* normalize escape vector */
322 len = norm(escape_dx, escape_dy);
328 if (pol_disc != NULL) {
333 else if (pol_opp != NULL) {
338 else { /* should not happen */
344 NOTICE(E_USER_STRAT, " escape vect = %2.2f,%2.2f",
345 escape_dx, escape_dy);
347 /* process the correct len of escape vector */
349 dst_pt.x = robot_pt.x + escape_dx * ESCAPE_VECT_LEN;
350 dst_pt.y = robot_pt.y + escape_dy * ESCAPE_VECT_LEN;
352 NOTICE(E_USER_STRAT, "robot pt %d %d \r\ndst point %d,%d",
353 robot_pt.x, robot_pt.y,
357 if (is_crossing_poly(robot_pt, dst_pt, &intersect_disc_pt,
359 /* we add 2 mm to be sure we are out of th polygon */
360 dst_pt.x = intersect_disc_pt.x + escape_dx * 2;
361 dst_pt.y = intersect_disc_pt.y + escape_dy * 2;
362 if (is_point_in_poly(pol_opp, dst_pt.x, dst_pt.y) != 1) {
364 if (!is_in_boundingbox(&dst_pt))
371 NOTICE(E_USER_STRAT, "GOTO %d,%d (%d)",
373 is_point_in_poly(pol_opp, robot_x, robot_y));
380 if (is_crossing_poly(robot_pt, dst_pt, &intersect_opp_pt,
382 /* we add 2 cm to be sure we are out of th polygon */
383 dst_pt.x = intersect_opp_pt.x + escape_dx * 2;
384 dst_pt.y = intersect_opp_pt.y + escape_dy * 2;
386 if (is_point_in_poly(pol_disc, dst_pt.x, dst_pt.y) != 1) {
388 if (!is_in_boundingbox(&dst_pt))
395 NOTICE(E_USER_STRAT, "GOTO %d,%d (%d)",
397 is_point_in_poly(pol_opp, robot_x, robot_y));
403 /* should not happen */
410 static int8_t goto_and_avoid(int16_t x, int16_t y)
414 poly_t *pol_disc, *pol_opp;
428 pol_disc = oa_new_poly(EDGE_NUMBER);
429 set_central_disc_poly(pol_disc);
430 pol_opp = oa_new_poly(4);
431 set_opponent_poly(pol_opp, O_WIDTH, O_LENGTH);
437 if (!is_in_boundingbox(&p_dst)) {
438 NOTICE(E_USER_STRAT, " dst is not in playground");
442 if (is_point_in_poly(pol_disc, x, y)) {
443 NOTICE(E_USER_STRAT, " dst is in disc");
446 if (is_point_in_poly(pol_opp, x, y)) {
447 NOTICE(E_USER_STRAT, " dst is in opp");
450 while (opp_w && opp_l) {
452 ret = escape_from_poly(pol_disc,
458 oa_start_end_points(robot_x, robot_y, x, y);
465 if (distance_between(robot_x, robot_y, opp_x, opp_y) < REDUCE_POLY_THRES ) {
471 NOTICE(E_USER_STRAT, "oa_process() returned %d", len);
477 NOTICE(E_USER_STRAT, "reducing opponent %d %d", opp_w, opp_l);
478 set_opponent_poly(pol_opp, opp_w, opp_l);
482 for (i=0 ; i<len ; i++) {
483 DEBUG(E_USER_STRAT, "With avoidance %d: x=%d y=%d", i, p->x, p->y);
490 /* log function, add a command to configure
492 void mylog(struct error * e, ...)
498 vfprintf(stdout, e->text, ap);
499 printf_P(PSTR("\r\n"));
504 int main(int argc, char **argv)
511 printf("bad args\n");
514 robot_x = atoi(argv[1]);
515 robot_y = atoi(argv[2]);
516 robot_a_deg = atoi(argv[3]);
517 robot_a = (((double)robot_a_deg*M_PI)/3.14159);
518 dst_x = atoi(argv[4]);
519 dst_y = atoi(argv[5]);
520 opp_x = atoi(argv[6]);
521 opp_y = atoi(argv[7]);
524 error_register_emerg(mylog);
525 error_register_error(mylog);
526 error_register_warning(mylog);
527 error_register_notice(mylog);
528 error_register_debug(mylog);
530 polygon_set_boundingbox(PLAYGROUND_X_MIN, PLAYGROUND_Y_MIN,
531 PLAYGROUND_X_MAX, PLAYGROUND_Y_MAX);
533 DEBUG(E_USER_STRAT, "robot at: %d %d %d", robot_x, robot_y, robot_a_deg);
534 goto_and_avoid(dst_x, dst_y);