projects/microb2010/mainboard/strat_avoid.c

   1 /*
   2  *  Copyright Droids Corporation, Microb Technology (2009)
   3  *
   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.
   8  *
   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.
  13  *
  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
  17  *
  18  *  Revision : $Id: strat_avoid.c,v 1.5 2009-11-08 17:24:33 zer0 Exp $
  19  *
  20  */
  21
  22 #include <stdio.h>
  23 #include <stdlib.h>
  24 #include <string.h>
  25 #include <math.h>
  26
  27 #include <aversive/pgmspace.h>
  28 #include <aversive/wait.h>
  29 #include <aversive/error.h>
  30
  31 #include <ax12.h>
  32 #include <uart.h>
  33 #include <pwm_ng.h>
  34 #include <time.h>
  35 #include <spi.h>
  36
  37 #include <pid.h>
  38 #include <quadramp.h>
  39 #include <control_system_manager.h>
  40 #include <trajectory_manager.h>
  41 #include <vect_base.h>
  42 #include <lines.h>
  43 #include <polygon.h>
  44 #include <obstacle_avoidance.h>
  45 #include <blocking_detection_manager.h>
  46 #include <robot_system.h>
  47 #include <position_manager.h>
  48
  49 #include <rdline.h>
  50 #include <parse.h>
  51
  52 #include "main.h"
  53 #include "strat.h"
  54 #include "strat_base.h"
  55 #include "strat_utils.h"
  56 #include "sensor.h"
  57
  58 #define EDGE_NUMBER 5
  59 void set_rotated_pentagon(poly_t *pol, const point_t *robot_pt,
  60                           int16_t radius, int16_t x, int16_t y)
  61 {
  62
  63         double c_a, s_a;
  64         uint8_t i;
  65         double px1, py1, px2, py2;
  66         double a_rad;
  67
  68         a_rad = atan2(y - robot_pt->y, x - robot_pt->x);
  69
  70         /* generate pentagon  */
  71         c_a = cos(-2*M_PI/EDGE_NUMBER);
  72         s_a = sin(-2*M_PI/EDGE_NUMBER);
  73
  74         /*
  75         px1 = radius;
  76         py1 = 0;
  77         */
  78         px1 = radius * cos(a_rad + 2*M_PI/(2*EDGE_NUMBER));
  79         py1 = radius * sin(a_rad + 2*M_PI/(2*EDGE_NUMBER));
  80
  81
  82         for (i = 0; i < EDGE_NUMBER; i++){
  83                 oa_poly_set_point(pol, x + px1, y + py1, i);
  84
  85                 px2 = px1*c_a + py1*s_a;
  86                 py2 = -px1*s_a + py1*c_a;
  87
  88                 px1 = px2;
  89                 py1 = py2;
  90         }
  91 }
  92
  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)
  95 {
  96         double tmp_x, tmp_y;
  97         double a_rad;
  98
  99         a_rad = atan2(y - robot_pt->y, x - robot_pt->x);
 100
 101         DEBUG(E_USER_STRAT, "%s() x,y=%d,%d a_rad=%2.2f",
 102               __FUNCTION__, x, y, a_rad);
 103
 104         /* point 1 */
 105         tmp_x = w;
 106         tmp_y = l;
 107         rotate(&tmp_x, &tmp_y, a_rad);
 108         tmp_x += x;
 109         tmp_y += y;
 110         oa_poly_set_point(pol, tmp_x, tmp_y, 0);
 111
 112         /* point 2 */
 113         tmp_x = -w;
 114         tmp_y = l;
 115         rotate(&tmp_x, &tmp_y, a_rad);
 116         tmp_x += x;
 117         tmp_y += y;
 118         oa_poly_set_point(pol, tmp_x, tmp_y, 1);
 119
 120         /* point 3 */
 121         tmp_x = -w;
 122         tmp_y = -l;
 123         rotate(&tmp_x, &tmp_y, a_rad);
 124         tmp_x += x;
 125         tmp_y += y;
 126         oa_poly_set_point(pol, tmp_x, tmp_y, 2);
 127
 128         /* point 4 */
 129         tmp_x = w;
 130         tmp_y = -l;
 131         rotate(&tmp_x, &tmp_y, a_rad);
 132         tmp_x += x;
 133         tmp_y += y;
 134         oa_poly_set_point(pol, tmp_x, tmp_y, 3);
 135 }
 136
 137 #define DISC_X CENTER_X
 138 #define DISC_Y CENTER_Y
 139
 140 void set_central_disc_poly(poly_t *pol, const point_t *robot_pt)
 141 {
 142         set_rotated_pentagon(pol, robot_pt, DISC_PENTA_DIAG,
 143                              DISC_X, DISC_Y);
 144 }
 145
 146 #ifdef HOMOLOGATION
 147 /* /!\ half size */
 148 #define O_WIDTH  400
 149 #define O_LENGTH 550
 150 #else
 151 /* /!\ half size */
 152 #define O_WIDTH  360
 153 #define O_LENGTH 500
 154 #endif
 155
 156 void set_opponent_poly(poly_t *pol, const point_t *robot_pt, int16_t w, int16_t l)
 157 {
 158         int16_t x, y;
 159         get_opponent_xy(&x, &y);
 160         DEBUG(E_USER_STRAT, "oponent at: %d %d", x, y);
 161
 162         /* place poly even if invalid, because it's -100 */
 163         set_rotated_poly(pol, robot_pt, w, l, x, y);
 164 }
 165
 166 /* don't care about polygons further than this distance for escape */
 167 #define ESCAPE_POLY_THRES 1000
 168
 169 /* don't reduce opp if opp is too far */
 170 #define REDUCE_POLY_THRES 600
 171
 172 /* has to be longer than any poly */
 173 #define ESCAPE_VECT_LEN 3000
 174
 175 /*
 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.
 181  */
 182 static int8_t go_in_area(point_t *robot_pt)
 183 {
 184         point_t poly_pts_area[4];
 185         poly_t poly_area;
 186         point_t disc_pt, dst_pt;
 187
 188         disc_pt.x = DISC_X;
 189         disc_pt.y = DISC_Y;
 190
 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);
 195
 196                 poly_area.l = 4;
 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;
 200
 201                 poly_pts_area[1].x = strat_infos.area_bbox.x2;
 202                 poly_pts_area[1].y = strat_infos.area_bbox.y1;
 203
 204                 poly_pts_area[2].x = strat_infos.area_bbox.x2;
 205                 poly_pts_area[2].y = strat_infos.area_bbox.y2;
 206
 207                 poly_pts_area[3].x = strat_infos.area_bbox.x1;
 208                 poly_pts_area[3].y = strat_infos.area_bbox.y2;
 209
 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);
 212
 213                 strat_goto_xy_force(dst_pt.x, dst_pt.y);
 214
 215                 robot_pt->x = dst_pt.x;
 216                 robot_pt->y = dst_pt.y;
 217
 218                 NOTICE(E_USER_STRAT, "GOTO %"PRIi32",%"PRIi32"",
 219                        dst_pt.x, dst_pt.y);
 220
 221                 return 1;
 222         }
 223
 224         return 0;
 225 }
 226
 227
 228 /*
 229  * Escape from polygons if needed.
 230  * robot_pt is the current position of the robot, it will be
 231  * updated.
 232  */
 233 static int8_t escape_from_poly(point_t *robot_pt,
 234                                poly_t *pol_disc,
 235                                int16_t opp_x, int16_t opp_y,
 236                                int16_t opp_w, int16_t opp_l,
 237                                poly_t *pol_opp)
 238 {
 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;
 243         double len;
 244         point_t opp_pt, disc_pt, dst_pt;
 245         point_t intersect_disc_pt, intersect_opp_pt;
 246
 247         opp_pt.x = opp_x;
 248         opp_pt.y = opp_y;
 249         disc_pt.x = DISC_X;
 250         disc_pt.y = DISC_Y;
 251
 252         /* escape from other poly if necessary */
 253         if (is_in_poly(robot_pt, pol_disc) == 1)
 254                 in_disc = 1;
 255         if (is_in_poly(robot_pt, pol_opp) == 1)
 256                 in_opp = 1;
 257
 258         if (in_disc == 0 && in_opp == 0) {
 259                 NOTICE(E_USER_STRAT, "no need to escape");
 260                 return 0;
 261         }
 262
 263         NOTICE(E_USER_STRAT, "in_disc=%d, in_opp=%d", in_disc, in_opp);
 264
 265         /* process escape vector */
 266
 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",
 271                        disc_dx, disc_dy);
 272                 len = norm(disc_dx, disc_dy);
 273                 if (len != 0) {
 274                         disc_dx /= len;
 275                         disc_dy /= len;
 276                 }
 277                 else {
 278                         disc_dx = 1.0;
 279                         disc_dy = 0.0;
 280                 }
 281                 escape_dx += disc_dx;
 282                 escape_dy += disc_dy;
 283         }
 284
 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",
 289                        opp_dx, opp_dy);
 290                 len = norm(opp_dx, opp_dy);
 291                 if (len != 0) {
 292                         opp_dx /= len;
 293                         opp_dy /= len;
 294                 }
 295                 else {
 296                         opp_dx = 1.0;
 297                         opp_dy = 0.0;
 298                 }
 299                 escape_dx += opp_dx;
 300                 escape_dy += opp_dy;
 301         }
 302
 303         /* normalize escape vector */
 304         len = norm(escape_dx, escape_dy);
 305         if (len != 0) {
 306                 escape_dx /= len;
 307                 escape_dy /= len;
 308         }
 309         else {
 310                 if (pol_disc != NULL) {
 311                         /* rotate 90° */
 312                         escape_dx = disc_dy;
 313                         escape_dy = disc_dx;
 314                 }
 315                 else if (pol_opp != NULL) {
 316                         /* rotate 90° */
 317                         escape_dx = opp_dy;
 318                         escape_dy = opp_dx;
 319                 }
 320                 else { /* should not happen */
 321                         opp_dx = 1.0;
 322                         opp_dy = 0.0;
 323                 }
 324         }
 325
 326         NOTICE(E_USER_STRAT, " escape vect = %2.2f,%2.2f",
 327                escape_dx, escape_dy);
 328
 329         /* process the correct len of escape vector */
 330
 331         dst_pt.x = robot_pt->x + escape_dx * ESCAPE_VECT_LEN;
 332         dst_pt.y = robot_pt->y + escape_dy * ESCAPE_VECT_LEN;
 333
 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,
 337                dst_pt.x, dst_pt.y);
 338
 339         if (in_disc) {
 340                 if (is_crossing_poly(*robot_pt, dst_pt, &intersect_disc_pt,
 341                                      pol_disc) == 1) {
 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) {
 346
 347                                 if (!is_in_boundingbox(&dst_pt))
 348                                         return -1;
 349
 350                                 NOTICE(E_USER_STRAT, "GOTO %"PRIi32",%"PRIi32"",
 351                                        dst_pt.x, dst_pt.y);
 352
 353                                 strat_goto_xy_force(dst_pt.x, dst_pt.y);
 354
 355                                 robot_pt->x = dst_pt.x;
 356                                 robot_pt->y = dst_pt.y;
 357
 358                                 return 0;
 359                         }
 360                 }
 361         }
 362
 363         if (in_opp) {
 364                 if (is_crossing_poly(*robot_pt, dst_pt, &intersect_opp_pt,
 365                                      pol_opp) == 1) {
 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;
 369
 370                         if (is_point_in_poly(pol_disc, dst_pt.x, dst_pt.y) != 1) {
 371
 372                                 if (!is_in_boundingbox(&dst_pt))
 373                                         return -1;
 374
 375                                 NOTICE(E_USER_STRAT, "GOTO %"PRIi32",%"PRIi32"",
 376                                        dst_pt.x, dst_pt.y);
 377
 378                                 strat_goto_xy_force(dst_pt.x, dst_pt.y);
 379
 380                                 robot_pt->x = dst_pt.x;
 381                                 robot_pt->y = dst_pt.y;
 382
 383                                 return 0;
 384                         }
 385                 }
 386         }
 387
 388         /* should not happen */
 389         return -1;
 390 }
 391
 392
 393 static int8_t __goto_and_avoid(int16_t x, int16_t y,
 394                                uint8_t flags_intermediate,
 395                                uint8_t flags_final,
 396                                uint8_t forward)
 397 {
 398         int8_t len = -1, i;
 399         point_t *p;
 400         poly_t *pol_disc, *pol_opp;
 401         int8_t ret;
 402         int16_t opp_w, opp_l, opp_x, opp_y;
 403         point_t p_dst, robot_pt;
 404
 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);
 407
 408  retry:
 409         get_opponent_xy(&opp_x, &opp_y);
 410         opp_w = O_WIDTH;
 411         opp_l = O_LENGTH;
 412
 413         robot_pt.x = position_get_x_s16(&mainboard.pos);
 414         robot_pt.y = position_get_y_s16(&mainboard.pos);
 415
 416         oa_init();
 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);
 421
 422         /* If we are not in the limited area, try to go in it. */
 423         ret = go_in_area(&robot_pt);
 424
 425         /* check that destination is valid */
 426         p_dst.x = x;
 427         p_dst.y = y;
 428         if (!is_in_boundingbox(&p_dst)) {
 429                 NOTICE(E_USER_STRAT, " dst is not in playground");
 430                 return END_ERROR;
 431         }
 432         if (is_point_in_poly(pol_disc, x, y)) {
 433                 NOTICE(E_USER_STRAT, " dst is in disc");
 434                 return END_ERROR;
 435         }
 436         if (is_point_in_poly(pol_opp, x, y)) {
 437                 NOTICE(E_USER_STRAT, " dst is in opp");
 438                 return END_ERROR;
 439         }
 440
 441         /* now start to avoid */
 442         while (opp_w && opp_l) {
 443
 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);
 448                 if (ret == 0) {
 449                         oa_reset();
 450                         oa_start_end_points(robot_pt.x, robot_pt.y, x, y);
 451                         /* oa_dump(); */
 452
 453                         len = oa_process();
 454                         if (len >= 0)
 455                                 break;
 456                 }
 457                 if (distance_between(robot_pt.x, robot_pt.y, opp_x, opp_y) < REDUCE_POLY_THRES ) {
 458                         if (opp_w == 0)
 459                                 opp_l /= 2;
 460                         opp_w /= 2;
 461                 }
 462                 else {
 463                         NOTICE(E_USER_STRAT, "oa_process() returned %d", len);
 464                         return END_ERROR;
 465                 }
 466
 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);
 469         }
 470
 471         p = oa_get_path();
 472         for (i=0 ; i<len ; i++) {
 473                 DEBUG(E_USER_STRAT, "With avoidance %d: x=%"PRIi32" y=%"PRIi32"", i, p->x, p->y);
 474
 475                 if (forward)
 476                         trajectory_goto_forward_xy_abs(&mainboard.traj, p->x, p->y);
 477                 else
 478                         trajectory_goto_backward_xy_abs(&mainboard.traj, p->x, p->y);
 479
 480                 /* no END_NEAR for the last point */
 481                 if (i == len - 1)
 482                         ret = wait_traj_end(flags_final);
 483                 else
 484                         ret = wait_traj_end(flags_intermediate);
 485
 486                 if (ret == END_BLOCKING) {
 487                         DEBUG(E_USER_STRAT, "Retry avoidance %s(%d,%d)",
 488                               __FUNCTION__, x, y);
 489                         goto retry;
 490                 }
 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)",
 494                               __FUNCTION__, x, y);
 495                         goto retry;
 496                 }
 497                 /* else if it is not END_TRAJ or END_NEAR, return */
 498                 else if (!TRAJ_SUCCESS(ret)) {
 499                         return ret;
 500                 }
 501                 p++;
 502         }
 503
 504         return END_TRAJ;
 505 }
 506
 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,
 509                                uint8_t flags_final)
 510 {
 511         return __goto_and_avoid(x, y, flags_intermediate, flags_final, 1);
 512 }
 513
 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,
 516                        uint8_t flags_final)
 517 {
 518         return __goto_and_avoid(x, y, flags_intermediate, flags_final, 0);
 519 }
 520
 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,
 524                                uint8_t flags_final)
 525 {
 526         double d,a;
 527         abs_xy_to_rel_da(x, y, &d, &a);
 528
 529         if (d < 300 && a < RAD(90) && a > RAD(-90))
 530                 return __goto_and_avoid(x, y, flags_intermediate,
 531                                         flags_final, 1);
 532         else
 533                 return __goto_and_avoid(x, y, flags_intermediate,
 534                                         flags_final, 0);
 535 }