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