projects/microb2009/tests/oa/main.c

   1 #include <math.h>
   2 #include <stdlib.h>
   3 #include <stdio.h>
   4 #include <stdarg.h>
   5
   6 #include <aversive.h>
   7 #include <aversive/error.h>
   8
   9 #include <vect_base.h>
  10 #include <lines.h>
  11 #include <polygon.h>
  12 #include <obstacle_avoidance.h>
  13
  14 #ifndef HOST_VERSION
  15 #error only for host
  16 #endif
  17
  18 #define M_2PI (M_PI*2)
  19 #define E_USER_STRAT 200
  20
  21 #define ROBOT_X 1000
  22 #define ROBOT_Y 1000
  23 #define ROBOT_A 0.5   /* radian */
  24 #define ROBOT_A_DEG ((int)((ROBOT_A*180)/3.14159))
  25
  26 #define OPP_X 2000
  27 #define OPP_Y 1500
  28
  29 #define DST_X 1500
  30 #define DST_Y 2000
  31
  32 #define PLAYGROUND_X_MIN 250
  33 #define PLAYGROUND_X_MAX 2750
  34 #define PLAYGROUND_Y_MIN 250
  35 #define PLAYGROUND_Y_MAX 1850
  36
  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;
  41
  42 int16_t opp_x = OPP_X;
  43 int16_t opp_y = OPP_Y;
  44
  45 int16_t dst_x = DST_X;
  46 int16_t dst_y = DST_Y;
  47
  48
  49 #define EDGE_NUMBER 5
  50
  51
  52 double norm(double x, double y)
  53 {
  54         return sqrt(x*x + y*y);
  55 }
  56
  57 /* return the distance between two points */
  58 int16_t distance_between(int16_t x1, int16_t y1, int16_t x2, int16_t y2)
  59 {
  60         int32_t x,y;
  61         x = (x2-x1);
  62         x = x*x;
  63         y = (y2-y1);
  64         y = y*y;
  65         return sqrt(x+y);
  66 }
  67
  68 void rotate(double *x, double *y, double rot)
  69 {
  70         double l, a;
  71
  72         l = norm(*x, *y);
  73         a = atan2(*y, *x);
  74
  75         a += rot;
  76         *x = l * cos(a);
  77         *y = l * sin(a);
  78 }
  79
  80
  81 void set_rotated_pentagon(poly_t *pol, int16_t radius,
  82                               int16_t x, int16_t y)
  83 {
  84
  85         double c_a, s_a;
  86         uint8_t i;
  87         double px1, py1, px2, py2;
  88         double a_rad;
  89
  90         a_rad = atan2(y - robot_y, x - robot_x);
  91
  92
  93         /* generate pentagon  */
  94         c_a = cos(-2*M_PI/EDGE_NUMBER);
  95         s_a = sin(-2*M_PI/EDGE_NUMBER);
  96
  97         /*
  98         px1 = radius;
  99         py1 = 0;
 100         */
 101         px1 = radius * cos(a_rad + 2*M_PI/(2*EDGE_NUMBER));
 102         py1 = radius * sin(a_rad + 2*M_PI/(2*EDGE_NUMBER));
 103
 104
 105         for (i = 0; i < EDGE_NUMBER; i++){
 106                 oa_poly_set_point(pol, x + px1, y + py1, i);
 107
 108                 px2 = px1*c_a + py1*s_a;
 109                 py2 = -px1*s_a + py1*c_a;
 110
 111                 px1 = px2;
 112                 py1 = py2;
 113         }
 114
 115 }
 116
 117 void set_rotated_poly(poly_t *pol, int16_t w, int16_t l,
 118                       int16_t x, int16_t y)
 119 {
 120         double tmp_x, tmp_y;
 121         double a_rad;
 122
 123         a_rad = atan2(y - robot_y, x - robot_x);
 124
 125         /* point 1 */
 126         tmp_x = w;
 127         tmp_y = l;
 128         rotate(&tmp_x, &tmp_y, a_rad);
 129         tmp_x += x;
 130         tmp_y += y;
 131         oa_poly_set_point(pol, tmp_x, tmp_y, 0);
 132
 133         /* point 2 */
 134         tmp_x = -w;
 135         tmp_y = l;
 136         rotate(&tmp_x, &tmp_y, a_rad);
 137         tmp_x += x;
 138         tmp_y += y;
 139         oa_poly_set_point(pol, tmp_x, tmp_y, 1);
 140
 141         /* point 3 */
 142         tmp_x = -w;
 143         tmp_y = -l;
 144         rotate(&tmp_x, &tmp_y, a_rad);
 145         tmp_x += x;
 146         tmp_y += y;
 147         oa_poly_set_point(pol, tmp_x, tmp_y, 2);
 148
 149         /* point 4 */
 150         tmp_x = w;
 151         tmp_y = -l;
 152         rotate(&tmp_x, &tmp_y, a_rad);
 153         tmp_x += x;
 154         tmp_y += y;
 155         oa_poly_set_point(pol, tmp_x, tmp_y, 3);
 156 }
 157
 158 #define DISC_PENTA_DIAG 500
 159 #define DISC_X 1500
 160 #define DISC_Y 1050
 161
 162 void set_central_disc_poly(poly_t *pol)
 163 {
 164         set_rotated_pentagon(pol, DISC_PENTA_DIAG,
 165                              DISC_X, DISC_Y);
 166 }
 167
 168 /* /!\ half size */
 169 #define O_WIDTH  360
 170 #define O_LENGTH 500
 171
 172 void set_opponent_poly(poly_t *pol, int16_t w, int16_t l)
 173 {
 174         int16_t x, y;
 175         x = opp_x;
 176         y = opp_y;
 177         DEBUG(E_USER_STRAT, "oponent at: %d %d", x, y);
 178
 179         /* place poly even if invalid, because it's -100 */
 180         set_rotated_poly(pol, w, l, x, y);
 181 }
 182
 183 /* don't care about polygons further than this distance for
 184  * strat_escape */
 185 #define ESCAPE_POLY_THRES 1000
 186
 187 /* don't reduce opp if opp is too far */
 188 #define REDUCE_POLY_THRES 600
 189
 190 /* has to be longer than any poly */
 191 #define ESCAPE_VECT_LEN 3000
 192
 193 /* go in playground, loop until out of poly */
 194 /* XXX return end timer??? */
 195 static int8_t go_in_area(void)
 196 {
 197         point_t poly_pts_area[4];
 198         poly_t poly_area;
 199
 200         point_t robot_pt, disc_pt, dst_pt;
 201
 202         robot_pt.x = robot_x;
 203         robot_pt.y = robot_y;
 204         disc_pt.x = DISC_X;
 205         disc_pt.y = DISC_Y;
 206
 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);
 210
 211                 poly_area.l = 4;
 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;
 215
 216                 poly_pts_area[1].x = PLAYGROUND_X_MAX;
 217                 poly_pts_area[1].y = PLAYGROUND_Y_MIN;
 218
 219                 poly_pts_area[2].x = PLAYGROUND_X_MAX;
 220                 poly_pts_area[2].y = PLAYGROUND_Y_MAX;
 221
 222                 poly_pts_area[3].x = PLAYGROUND_X_MIN;
 223                 poly_pts_area[3].y = PLAYGROUND_Y_MAX;
 224
 225
 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);
 228
 229                 /* XXX */
 230                 robot_pt.x = dst_pt.x;
 231                 robot_pt.y = dst_pt.y;
 232
 233                 robot_x = dst_pt.x;
 234                 robot_y = dst_pt.y;
 235
 236                 NOTICE(E_USER_STRAT, "GOTO %d,%d",
 237                        dst_pt.x, dst_pt.y);
 238
 239                 return 1;
 240         }
 241
 242         return 0;
 243 }
 244
 245
 246 /*
 247  * Escape from polygons if needed.
 248  */
 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,
 252                                poly_t *pol_opp)
 253 {
 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;
 258         double len;
 259
 260         point_t robot_pt, opp_pt, disc_pt, dst_pt;
 261         point_t intersect_disc_pt, intersect_opp_pt;
 262
 263         robot_pt.x = robot_x;
 264         robot_pt.y = robot_y;
 265         opp_pt.x = opp_x;
 266         opp_pt.y = opp_y;
 267         disc_pt.x = DISC_X;
 268         disc_pt.y = DISC_Y;
 269
 270         /* escape from other poly if necessary */
 271         if (is_point_in_poly(pol_disc, robot_x, robot_y) == 1)
 272                 in_disc = 1;
 273         if (is_point_in_poly(pol_opp, robot_x, robot_y) == 1)
 274                 in_opp = 1;
 275
 276         if (in_disc == 0 && in_opp == 0) {
 277                 NOTICE(E_USER_STRAT, "no need to escape");
 278                 return 0;
 279         }
 280
 281         NOTICE(E_USER_STRAT, "in_disc=%d, in_opp=%d", in_disc, in_opp);
 282
 283         /* process escape vector */
 284
 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",
 289                        disc_dx, disc_dy);
 290                 len = norm(disc_dx, disc_dy);
 291                 if (len != 0) {
 292                         disc_dx /= len;
 293                         disc_dy /= len;
 294                 }
 295                 else {
 296                         disc_dx = 1.0;
 297                         disc_dy = 0.0;
 298                 }
 299                 escape_dx += disc_dx;
 300                 escape_dy += disc_dy;
 301         }
 302
 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",
 307                        opp_dx, opp_dy);
 308                 len = norm(opp_dx, opp_dy);
 309                 if (len != 0) {
 310                         opp_dx /= len;
 311                         opp_dy /= len;
 312                 }
 313                 else {
 314                         opp_dx = 1.0;
 315                         opp_dy = 0.0;
 316                 }
 317                 escape_dx += opp_dx;
 318                 escape_dy += opp_dy;
 319         }
 320
 321         /* normalize escape vector */
 322         len = norm(escape_dx, escape_dy);
 323         if (len != 0) {
 324                 escape_dx /= len;
 325                 escape_dy /= len;
 326         }
 327         else {
 328                 if (pol_disc != NULL) {
 329                         /* rotate 90° */
 330                         escape_dx = disc_dy;
 331                         escape_dy = disc_dx;
 332                 }
 333                 else if (pol_opp != NULL) {
 334                         /* rotate 90° */
 335                         escape_dx = opp_dy;
 336                         escape_dy = opp_dx;
 337                 }
 338                 else { /* should not happen */
 339                         opp_dx = 1.0;
 340                         opp_dy = 0.0;
 341                 }
 342         }
 343
 344         NOTICE(E_USER_STRAT, " escape vect = %2.2f,%2.2f",
 345                escape_dx, escape_dy);
 346
 347         /* process the correct len of escape vector */
 348
 349         dst_pt.x = robot_pt.x + escape_dx * ESCAPE_VECT_LEN;
 350         dst_pt.y = robot_pt.y + escape_dy * ESCAPE_VECT_LEN;
 351
 352         NOTICE(E_USER_STRAT, "robot pt %d %d \r\ndst point %d,%d",
 353                robot_pt.x, robot_pt.y,
 354                dst_pt.x, dst_pt.y);
 355
 356         if (in_disc) {
 357                 if (is_crossing_poly(robot_pt, dst_pt, &intersect_disc_pt,
 358                                      pol_disc) == 1) {
 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) {
 363
 364                                 if (!is_in_boundingbox(&dst_pt))
 365                                         return -1;
 366
 367                                 robot_x = dst_pt.x;
 368                                 robot_y = dst_pt.y;
 369
 370                                 /* XXX */
 371                                 NOTICE(E_USER_STRAT, "GOTO %d,%d (%d)",
 372                                        robot_x, robot_y,
 373                                        is_point_in_poly(pol_opp, robot_x, robot_y));
 374                                 return 0;
 375                         }
 376                 }
 377         }
 378
 379         if (in_opp) {
 380                 if (is_crossing_poly(robot_pt, dst_pt, &intersect_opp_pt,
 381                                      pol_opp) == 1) {
 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;
 385
 386                         if (is_point_in_poly(pol_disc, dst_pt.x, dst_pt.y) != 1) {
 387
 388                                 if (!is_in_boundingbox(&dst_pt))
 389                                         return -1;
 390
 391                                 robot_x = dst_pt.x;
 392                                 robot_y = dst_pt.y;
 393
 394                                 /* XXX */
 395                                 NOTICE(E_USER_STRAT, "GOTO %d,%d (%d)",
 396                                        robot_x, robot_y,
 397                                        is_point_in_poly(pol_opp, robot_x, robot_y));
 398                                 return 0;
 399                         }
 400                 }
 401         }
 402
 403         /* should not happen */
 404         return -1;
 405 }
 406
 407
 408
 409
 410 static int8_t goto_and_avoid(int16_t x, int16_t y)
 411 {
 412         int8_t len = -1, i;
 413         point_t *p;
 414         poly_t *pol_disc, *pol_opp;
 415         int16_t posx, posy;
 416         int8_t ret;
 417         int16_t opp_w;
 418         int16_t opp_l;
 419         point_t p_dst;
 420
 421         opp_w = O_WIDTH;
 422         opp_l = O_LENGTH;
 423
 424         posx = robot_x;
 425         posy = robot_y;
 426
 427         oa_init();
 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);
 432
 433         go_in_area();
 434
 435         p_dst.x = x;
 436         p_dst.y = y;
 437         if (!is_in_boundingbox(&p_dst)) {
 438                 NOTICE(E_USER_STRAT, " dst is not in playground");
 439                 return -1;
 440         }
 441
 442         if (is_point_in_poly(pol_disc, x, y)) {
 443                 NOTICE(E_USER_STRAT, " dst is in disc");
 444                 return -1;
 445         }
 446         if (is_point_in_poly(pol_opp, x, y)) {
 447                 NOTICE(E_USER_STRAT, " dst is in opp");
 448                 return -1;
 449         }
 450         while (opp_w && opp_l) {
 451
 452                 ret = escape_from_poly(pol_disc,
 453                                        opp_x, opp_y,
 454                                        opp_w, opp_l,
 455                                        pol_opp);
 456                 if (ret == 0) {
 457                         oa_reset();
 458                         oa_start_end_points(robot_x, robot_y, x, y);
 459                         oa_dump();
 460
 461                         len = oa_process();
 462                         if (len >= 0)
 463                                 break;
 464                 }
 465                 if (distance_between(robot_x, robot_y, opp_x, opp_y) < REDUCE_POLY_THRES ) {
 466                         if (opp_w == 0)
 467                                 opp_l /= 2;
 468                         opp_w /= 2;
 469                 }
 470                 else {
 471                         NOTICE(E_USER_STRAT, "oa_process() returned %d", len);
 472
 473                         /* XXX */
 474                         return -1;
 475                 }
 476
 477                 NOTICE(E_USER_STRAT, "reducing opponent %d %d", opp_w, opp_l);
 478                 set_opponent_poly(pol_opp, opp_w, opp_l);
 479         }
 480
 481         p = oa_get_path();
 482         for (i=0 ; i<len ; i++) {
 483                 DEBUG(E_USER_STRAT, "With avoidance %d: x=%d y=%d", i, p->x, p->y);
 484                 p++;
 485         }
 486
 487         return 0;
 488 }
 489
 490 /* log function, add a command to configure
 491  * it dynamically */
 492 void mylog(struct error * e, ...)
 493 {
 494         va_list ap;
 495
 496         va_start(ap, e);
 497
 498         vfprintf(stdout, e->text, ap);
 499         printf_P(PSTR("\r\n"));
 500         va_end(ap);
 501 }
 502
 503 #ifdef HOST_VERSION
 504 int main(int argc, char **argv)
 505 #else
 506 int main(void)
 507 #endif
 508 {
 509 #ifdef HOST_VERSION
 510         if (argc != 8) {
 511                 printf("bad args\n");
 512                 return -1;
 513         }
 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]);
 522 #endif
 523         /* LOGS */
 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);
 529
 530         polygon_set_boundingbox(PLAYGROUND_X_MIN, PLAYGROUND_Y_MIN,
 531                                 PLAYGROUND_X_MAX, PLAYGROUND_Y_MAX);
 532
 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);
 535
 536         return 0;
 537 }