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