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