projects/microb2010/tests/hostsim/strat_base.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_base.c,v 1.8 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 <uart.h>
  32 #include <clock_time.h>
  33
  34 #include <pid.h>
  35 #include <quadramp.h>
  36 #include <control_system_manager.h>
  37 #include <trajectory_manager.h>
  38 #include <vect_base.h>
  39 #include <lines.h>
  40 #include <polygon.h>
  41 #include <obstacle_avoidance.h>
  42 #include <blocking_detection_manager.h>
  43 #include <robot_system.h>
  44 #include <position_manager.h>
  45
  46 #include <rdline.h>
  47 #include <parse.h>
  48
  49 #include "main.h"
  50 #include "cmdline.h"
  51 #include "strat_utils.h"
  52 #include "strat_base.h"
  53 #include "strat.h"
  54
  55 /* true if we want to interrupt a trajectory */
  56 static uint8_t traj_intr=0;
  57
  58 /* filled when a END_OBSTACLE is returned */
  59 struct opponent_obstacle opponent_obstacle;
  60
  61 /* asked speed */
  62 static volatile int16_t strat_speed_a = SPEED_DIST_FAST;
  63 static volatile int16_t strat_speed_d = SPEED_ANGLE_FAST;
  64 static volatile uint16_t strat_limit_speed_a = 0; /* no limit */
  65 static volatile uint16_t strat_limit_speed_d = 0;
  66
  67 static volatile uint8_t strat_limit_speed_enabled = 1;
  68
  69
  70 /* Strings that match the end traj cause */
  71 /* /!\ keep it sync with stat_base.h */
  72 const char *err_tab []= {
  73         "END_TRAJ",
  74         "END_BLOCKING",
  75         "END_NEAR",
  76         "END_OBSTACLE",
  77         "END_ERROR",
  78         "END_INTR",
  79         "END_TIMER",
  80         "END_RESERVED",
  81 };
  82
  83 /* return string from end traj type num */
  84 const char *get_err(uint8_t err)
  85 {
  86         uint8_t i;
  87         if (err == 0)
  88                 return "SUCCESS";
  89         for (i=0 ; i<8; i++) {
  90                 if (err & (1 <<i))
  91                         return err_tab[i];
  92         }
  93         return "END_UNKNOWN";
  94 }
  95
  96 void strat_hardstop(void)
  97 {
  98         trajectory_hardstop(&mainboard.traj);
  99         pid_reset(&mainboard.angle.pid);
 100         pid_reset(&mainboard.distance.pid);
 101         bd_reset(&mainboard.angle.bd);
 102         bd_reset(&mainboard.distance.bd);
 103
 104         while ((ABS(mainboard.speed_d) > 200) ||
 105                (ABS(mainboard.speed_a) > 200))
 106
 107         trajectory_hardstop(&mainboard.traj);
 108         pid_reset(&mainboard.angle.pid);
 109         pid_reset(&mainboard.distance.pid);
 110         bd_reset(&mainboard.angle.bd);
 111         bd_reset(&mainboard.distance.bd);
 112 }
 113
 114 /* go to an x,y point without checking for obstacle or blocking. It
 115  * should be used for very small dist only. Return END_TRAJ if we
 116  * reach destination, or END_BLOCKING if the robot blocked more than 3
 117  * times. */
 118 uint8_t strat_goto_xy_force(int16_t x, int16_t y)
 119 {
 120         uint8_t i, err;
 121
 122 #ifdef HOMOLOGATION
 123         int8_t serr;
 124         uint8_t hardstop = 0;
 125         microseconds us = time_get_us2();
 126         int16_t opp_a, opp_d, opp_x, opp_y;
 127
 128         while (1) {
 129                 serr = get_opponent_xyda(&opp_x, &opp_y,
 130                                         &opp_d, &opp_a);
 131                 if (serr == -1)
 132                         break;
 133                 if (opp_d < 600)
 134                         break;
 135                 if (hardstop == 0) {
 136                         strat_hardstop();
 137                         hardstop = 1;
 138                 }
 139                 if ((time_get_us2() - us) > 3000000L)
 140                         break;
 141         }
 142 #endif
 143         for (i=0; i<3; i++) {
 144                 trajectory_goto_xy_abs(&mainboard.traj, x, y);
 145                 err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
 146                 if (TRAJ_SUCCESS(err))
 147                         return END_TRAJ;
 148                 if (err == END_BLOCKING) {
 149                         time_wait_ms(500);
 150                         strat_hardstop();
 151                 }
 152         }
 153         return END_BLOCKING;
 154 }
 155
 156 /* reset position */
 157 void strat_reset_pos(int16_t x, int16_t y, int16_t a)
 158 {
 159         int16_t posx = position_get_x_s16(&mainboard.pos);
 160         int16_t posy = position_get_y_s16(&mainboard.pos);
 161         int16_t posa = position_get_a_deg_s16(&mainboard.pos);
 162
 163         if (x == DO_NOT_SET_POS)
 164                 x = posx;
 165         if (y == DO_NOT_SET_POS)
 166                 y = posy;
 167         if (a == DO_NOT_SET_POS)
 168                 a = posa;
 169
 170         DEBUG(E_USER_STRAT, "reset pos (%s%s%s)",
 171               x == DO_NOT_SET_POS ? "" : "x",
 172               y == DO_NOT_SET_POS ? "" : "y",
 173               a == DO_NOT_SET_POS ? "" : "a");
 174         position_set(&mainboard.pos, x, y, a);
 175         DEBUG(E_USER_STRAT, "pos resetted", __FUNCTION__);
 176 }
 177
 178 /*
 179  * decrease gain on angle PID, and go forward until we reach the
 180  * border.
 181  */
 182 uint8_t strat_calib(int16_t dist, uint8_t flags)
 183 {
 184         int32_t p = pid_get_gain_P(&mainboard.angle.pid);
 185         int32_t i = pid_get_gain_I(&mainboard.angle.pid);
 186         int32_t d = pid_get_gain_D(&mainboard.angle.pid);
 187         uint8_t err;
 188
 189         pid_set_gains(&mainboard.angle.pid, 150, 0, 2000);
 190         trajectory_d_rel(&mainboard.traj, dist);
 191         err = wait_traj_end(flags);
 192         pid_set_gains(&mainboard.angle.pid, p, i, d);
 193         return err;
 194 }
 195
 196 /* escape from zone, and don't brake, so we can continue with another
 197  * traj */
 198 uint8_t strat_escape(struct build_zone *zone, uint8_t flags)
 199 {
 200         uint8_t err;
 201         uint16_t old_spdd, old_spda;
 202
 203         strat_get_speed(&old_spdd, &old_spda);
 204
 205         err = WAIT_COND_OR_TIMEOUT(!opponent_is_behind(), 1000);
 206         if (err == 0) {
 207                 strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST);
 208                 trajectory_d_rel(&mainboard.traj, -150);
 209                 err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
 210                 strat_set_speed(old_spdd, old_spda);
 211                 return err;
 212         }
 213
 214         strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
 215
 216         if (zone->flags & ZONE_F_DISC) {
 217                 trajectory_d_rel(&mainboard.traj, -350);
 218                 err = WAIT_COND_OR_TRAJ_END(!robot_is_near_disc(), flags);
 219         }
 220         else {
 221                 trajectory_d_rel(&mainboard.traj, -300);
 222                 err = wait_traj_end(flags);
 223         }
 224
 225         strat_set_speed(old_spdd, old_spda);
 226         if (err == 0)
 227                 return END_NEAR;
 228         return err;
 229 }
 230
 231 static void strat_update_traj_speed(void)
 232 {
 233         uint16_t d, a;
 234
 235         d = strat_speed_d;
 236         if (strat_limit_speed_d && d > strat_limit_speed_d)
 237                 d = strat_limit_speed_d;
 238         a = strat_speed_a;
 239         if (strat_limit_speed_a && a > strat_limit_speed_a)
 240                 a = strat_limit_speed_a;
 241
 242         trajectory_set_speed(&mainboard.traj, d, a);
 243 }
 244
 245 void strat_set_speed(uint16_t d, uint16_t a)
 246 {
 247         uint8_t flags;
 248         IRQ_LOCK(flags);
 249         strat_speed_d = d;
 250         strat_speed_a = a;
 251         strat_update_traj_speed();
 252         IRQ_UNLOCK(flags);
 253 }
 254
 255 void strat_get_speed(uint16_t *d, uint16_t *a)
 256 {
 257         uint8_t flags;
 258         IRQ_LOCK(flags);
 259         *d = strat_speed_d;
 260         *a = strat_speed_a;
 261         IRQ_UNLOCK(flags);
 262 }
 263
 264 void strat_limit_speed_enable(void)
 265 {
 266         strat_limit_speed_enabled = 1;
 267 }
 268
 269 void strat_limit_speed_disable(void)
 270 {
 271         strat_limit_speed_enabled = 0;
 272 }
 273
 274 /* called periodically */
 275 void strat_limit_speed(void)
 276 {
 277         uint16_t lim_d = 0, lim_a = 0;
 278         int16_t opp_d, opp_a;
 279
 280         if (strat_limit_speed_enabled == 0)
 281                 goto update;
 282
 283         if (get_opponent_da(&opp_d, &opp_a) != 0)
 284                 goto update;
 285
 286 #ifdef HOMOLOGATION
 287         if (opp_d < 600) {
 288                 lim_d = 150;
 289                 lim_a = 200;
 290         }
 291 #else
 292         if (opp_d < 500) {
 293                 if (mainboard.speed_d > 0 && (opp_a > 290 || opp_a < 70)) {
 294                         lim_d = SPEED_DIST_VERY_SLOW;
 295                         lim_a = SPEED_ANGLE_VERY_SLOW;
 296                 }
 297                 else if (mainboard.speed_d < 0 && (opp_a < 250 && opp_a > 110)) {
 298                         lim_d = SPEED_DIST_VERY_SLOW;
 299                         lim_a = SPEED_ANGLE_VERY_SLOW;
 300                 }
 301                 else {
 302                         lim_d = SPEED_DIST_SLOW;
 303                         lim_a = SPEED_ANGLE_VERY_SLOW;
 304                 }
 305         }
 306 #endif
 307         else if (opp_d < 800) {
 308                 if (mainboard.speed_d > 0 && (opp_a > 290 || opp_a < 70)) {
 309                         lim_d = SPEED_DIST_SLOW;
 310                         lim_a = SPEED_ANGLE_SLOW;
 311                 }
 312                 else if (mainboard.speed_d < 0 && (opp_a < 250 && opp_a > 110)) {
 313                         lim_d = SPEED_DIST_SLOW;
 314                         lim_a = SPEED_ANGLE_SLOW;
 315                 }
 316         }
 317
 318  update:
 319         if (lim_d != strat_limit_speed_d ||
 320             lim_a != strat_limit_speed_a) {
 321                 strat_limit_speed_d = lim_d;
 322                 strat_limit_speed_a = lim_a;
 323                 DEBUG(E_USER_STRAT, "new speed limit da=%d,%d", lim_d, lim_a);
 324                 strat_update_traj_speed();
 325         }
 326 }
 327
 328 /* start the strat */
 329 void strat_start(void)
 330 {
 331 #ifdef HOST_VERSION
 332         printf("not implemented\n");
 333 #else
 334         int8_t i;
 335         uint8_t err;
 336
 337         strat_preinit();
 338
 339         /* if start sw not plugged */
 340         if (sensor_get(S_START_SWITCH)) {
 341                 printf_P(PSTR("No start switch, press a key or plug it\r\n"));
 342
 343                 /* while start sw not plugged */
 344                 while (sensor_get(S_START_SWITCH)) {
 345                         if (! cmdline_keypressed())
 346                                 continue;
 347
 348                         for (i=3; i>0; i--) {
 349                                 printf_P(PSTR("%d\r\n"), i);
 350                                 time_wait_ms(1000);
 351                         }
 352                         break;
 353                 }
 354         }
 355
 356         /* if start sw plugged */
 357         if (!sensor_get(S_START_SWITCH)) {
 358                 printf_P(PSTR("Ready, unplug start switch to start\r\n"));
 359                 /* while start sw plugged */
 360                 while (!sensor_get(S_START_SWITCH));
 361         }
 362
 363         strat_init();
 364         err = strat_main();
 365         NOTICE(E_USER_STRAT, "Finished !! returned %s", get_err(err));
 366         strat_exit();
 367 #endif
 368 }
 369
 370 /* return true if we have to brake due to an obstacle */
 371 uint8_t strat_obstacle(void)
 372 {
 373         int16_t x_rel, y_rel;
 374         int16_t opp_x, opp_y, opp_d, opp_a;
 375
 376         /* too slow */
 377         if (ABS(mainboard.speed_d) < 150)
 378                 return 0;
 379
 380         /* no opponent detected */
 381         if (get_opponent_xyda(&opp_x, &opp_y,
 382                               &opp_d, &opp_a))
 383                 return 0;
 384
 385         /* save obstacle position */
 386         opponent_obstacle.x = opp_x;
 387         opponent_obstacle.y = opp_y;
 388         opponent_obstacle.d = opp_d;
 389         opponent_obstacle.a = opp_a;
 390
 391 #ifndef HOST_VERSION
 392         /* sensor are temporarily disabled */
 393         if (sensor_obstacle_is_disabled())
 394                 return 0;
 395 #endif
 396
 397         /* relative position */
 398         x_rel = cos(RAD(opp_a)) * (double)opp_d;
 399         y_rel = sin(RAD(opp_a)) * (double)opp_d;
 400
 401         /* opponent too far */
 402         if (opp_d > 600)
 403                 return 0;
 404
 405         /* opponent is in front of us */
 406         if (mainboard.speed_d > 0 && (opp_a > 325 || opp_a < 35)) {
 407                 DEBUG(E_USER_STRAT, "opponent front d=%d, a=%d "
 408                       "xrel=%d yrel=%d (speed_d=%d)",
 409                       opp_d, opp_a, x_rel, y_rel, mainboard.speed_d);
 410 #ifndef HOST_VERSION
 411                 sensor_obstacle_disable();
 412 #endif
 413                 return 1;
 414         }
 415         /* opponent is behind us */
 416         if (mainboard.speed_d < 0 && (opp_a < 215 && opp_a > 145)) {
 417                 DEBUG(E_USER_STRAT, "opponent behind d=%d, a=%d xrel=%d yrel=%d",
 418                       opp_d, opp_a, x_rel, y_rel);
 419 #ifndef HOST_VERSION
 420                 sensor_obstacle_disable();
 421 #endif
 422                 return 1;
 423         }
 424
 425         return 0;
 426 }
 427
 428 void interrupt_traj(void)
 429 {
 430         traj_intr = 1;
 431 }
 432
 433 void interrupt_traj_reset(void)
 434 {
 435         traj_intr = 0;
 436 }
 437
 438 uint8_t test_traj_end(uint8_t why)
 439 {
 440         uint16_t cur_timer;
 441         point_t robot_pt;
 442
 443         robot_pt.x = position_get_x_s16(&mainboard.pos);
 444         robot_pt.y = position_get_y_s16(&mainboard.pos);
 445
 446         /* trigger an event at 3 sec before the end of the match if we
 447          * have balls in the barrel */
 448         cur_timer = time_get_s();
 449
 450         if ((mainboard.flags & DO_TIMER) && (why & END_TIMER)) {
 451                 /* end of match */
 452                 if (cur_timer >= MATCH_TIME)
 453                         return END_TIMER;
 454         }
 455
 456         if ((why & END_INTR) && traj_intr) {
 457                 interrupt_traj_reset();
 458                 return END_INTR;
 459         }
 460
 461         if ((why & END_TRAJ) && trajectory_finished(&mainboard.traj))
 462                 return END_TRAJ;
 463
 464         /* we are near the destination point (depends on current
 465          * speed) AND the robot is in the area bounding box. */
 466         if (why & END_NEAR) {
 467                 int16_t d_near = 100;
 468
 469                 if (mainboard.speed_d > 2000)
 470                         d_near = 150;
 471
 472                 if (trajectory_in_window(&mainboard.traj, d_near, RAD(5.0)) &&
 473                     is_in_boundingbox(&robot_pt))
 474                         return END_NEAR;
 475         }
 476
 477         if ((why & END_BLOCKING) && bd_get(&mainboard.angle.bd)) {
 478                 strat_hardstop();
 479                 return END_BLOCKING;
 480         }
 481
 482         if ((why & END_BLOCKING) && bd_get(&mainboard.distance.bd)) {
 483                 strat_hardstop();
 484                 return END_BLOCKING;
 485         }
 486
 487         if ((why & END_OBSTACLE) && strat_obstacle()) {
 488                 strat_hardstop();
 489                 return END_OBSTACLE;
 490         }
 491
 492         return 0;
 493 }
 494
 495 uint8_t __wait_traj_end_debug(uint8_t why, uint16_t line)
 496 {
 497         uint8_t ret = 0;
 498         int16_t opp_x, opp_y, opp_d, opp_a;
 499
 500         while (ret == 0)
 501                 ret = test_traj_end(why);
 502
 503         if (ret == END_OBSTACLE) {
 504                 if (get_opponent_xyda(&opp_x, &opp_y,
 505                                       &opp_d, &opp_a) == 0)
 506                         DEBUG(E_USER_STRAT, "Got %s at line %d"
 507                               " xy=(%d,%d) da=(%d,%d)", get_err(ret),
 508                               line, opp_x, opp_y, opp_d, opp_a);
 509         }
 510         else {
 511                 DEBUG(E_USER_STRAT, "Got %s at line %d",
 512                       get_err(ret), line);
 513         }
 514         return ret;
 515 }