projects/microb2010/mainboard/strat.c

   1 /*
   2  *  Copyright Droids, 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.c,v 1.6 2009-11-08 17:24:33 zer0 Exp $
  19  *
  20  *  Olivier MATZ <zer0@droids-corp.org>
  21  */
  22
  23 #include <stdio.h>
  24 #include <stdlib.h>
  25 #include <string.h>
  26 #include <math.h>
  27
  28 #include <aversive/pgmspace.h>
  29 #include <aversive/queue.h>
  30 #include <aversive/wait.h>
  31 #include <aversive/error.h>
  32
  33 #include <ax12.h>
  34 #include <uart.h>
  35 #include <pwm_ng.h>
  36 #include <clock_time.h>
  37 #include <spi.h>
  38
  39 #include <pid.h>
  40 #include <quadramp.h>
  41 #include <control_system_manager.h>
  42 #include <trajectory_manager.h>
  43 #include <trajectory_manager_utils.h>
  44 #include <trajectory_manager_core.h>
  45 #include <vect_base.h>
  46 #include <lines.h>
  47 #include <polygon.h>
  48 #include <obstacle_avoidance.h>
  49 #include <blocking_detection_manager.h>
  50 #include <robot_system.h>
  51 #include <position_manager.h>
  52
  53 #include <diagnostic.h>
  54
  55 #include <rdline.h>
  56 #include <parse.h>
  57
  58 #include "../common/i2c_commands.h"
  59 #include "i2c_protocol.h"
  60 #include "main.h"
  61 #include "cs.h"
  62 #include "strat.h"
  63 #include "strat_db.h"
  64 #include "strat_base.h"
  65 #include "strat_corn.h"
  66 #include "strat_utils.h"
  67 #include "strat_avoid.h"
  68 #include "sensor.h"
  69 #include "actuator.h"
  70
  71 #define COL_DISP_MARGIN 400 /* stop 40 cm in front of dispenser */
  72 #define COL_SCAN_PRE_MARGIN 250
  73
  74 static volatile uint8_t strat_running = 0;
  75 volatile uint8_t strat_want_pack = 0;
  76 volatile uint8_t strat_lpack60 = 0;
  77 volatile uint8_t strat_rpack60 = 0;
  78
  79 volatile uint8_t strat_opponent_lpack = 0;
  80 volatile uint8_t strat_opponent_rpack = 0;
  81
  82 struct strat_conf strat_conf = {
  83         .dump_enabled = 0,
  84         .opp_orange = 90,
  85         .orphan_tomato = 45,
  86         .flags = 0,
  87 };
  88
  89 /*************************************************************/
  90
  91 /*                  INIT                                     */
  92
  93 /*************************************************************/
  94
  95 /* called before each strat, and before the start switch */
  96 void strat_preinit(void)
  97 {
  98         time_reset();
  99         interrupt_traj_reset();
 100         mainboard.flags =  DO_ENCODERS | DO_CS | DO_RS |
 101                 DO_POS | DO_BD | DO_POWER;
 102
 103         strat_db_init();
 104         strat_conf.prev_wait_obstacle = -5;
 105         strat_conf_dump(__FUNCTION__);
 106         strat_db_dump(__FUNCTION__);
 107 }
 108
 109 void strat_conf_dump(const char *caller)
 110 {
 111         if (!strat_conf.dump_enabled)
 112                 return;
 113
 114         printf_P(PSTR("-- conf --\r\n"));
 115         printf_P(PSTR("our_orange = %s\r\n"),
 116                  (strat_conf.flags & STRAT_CONF_OUR_ORANGE) ? "y":"n");
 117         printf_P(PSTR("wait_obstacle = %s\r\n"),
 118                  (strat_conf.flags & STRAT_CONF_WAIT_OBSTACLE) ? "y":"n");
 119         printf_P(PSTR("straight begin = %s\r\n"),
 120                  (strat_conf.flags & STRAT_CONF_STRAIGHT_BEGIN) ? "y":"n");
 121         printf_P(PSTR("opp_orange = %d\r\n"), strat_conf.opp_orange);
 122         printf_P(PSTR("orphan_tomato = %d\r\n"), strat_conf.orphan_tomato);
 123 }
 124
 125 void strat_event_enable(void)
 126 {
 127         strat_running = 1;
 128 }
 129
 130 void strat_event_disable(void)
 131 {
 132         strat_running = 0;
 133 }
 134
 135 /* call it just before launching the strat */
 136 void strat_init(void)
 137 {
 138 #ifdef HOST_VERSION
 139         position_set(&mainboard.pos, 298.16,
 140                      COLOR_Y(308.78), COLOR_A(70.00));
 141 #endif
 142
 143         /* we consider that the color is correctly set */
 144         strat_running = 1;
 145         strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
 146         time_reset();
 147         interrupt_traj_reset();
 148
 149         i2c_cobboard_deploy(I2C_LEFT_SIDE);
 150         i2c_cobboard_deploy(I2C_RIGHT_SIDE);
 151         i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST);
 152         i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_HARVEST);
 153
 154         /* used in strat_base for END_TIMER */
 155         mainboard.flags = DO_ENCODERS | DO_CS | DO_RS |
 156                 DO_POS | DO_BD | DO_TIMER | DO_POWER;
 157 }
 158
 159 /* call it after each strat */
 160 void strat_exit(void)
 161 {
 162 #ifndef HOST_VERSION
 163         uint8_t flags;
 164 #endif
 165
 166         strat_running = 0;
 167         mainboard.flags &= ~(DO_TIMER);
 168         strat_hardstop();
 169         time_reset();
 170         wait_ms(100);
 171 #ifndef HOST_VERSION
 172         IRQ_LOCK(flags);
 173         mainboard.flags &= ~(DO_CS);
 174         IRQ_UNLOCK(flags);
 175         pwm_ng_set(LEFT_PWM, 0);
 176         pwm_ng_set(RIGHT_PWM, 0);
 177 #endif
 178 }
 179
 180 /* mark tomato as not present */
 181 static void check_tomato(void)
 182 {
 183         int16_t x, y;
 184         uint8_t i, j;
 185         static uint8_t prev_check_time;
 186         uint8_t cur_time;
 187
 188         /* check present tomatoes once per second */
 189         cur_time = time_get_s();
 190         if (cur_time != prev_check_time) {
 191                 uint8_t k;
 192
 193                 for (k = 0; k < TOMATO_NB; k++) {
 194                         if (strat_db.tomato_table[k]->present == 1 &&
 195                             strat_db.tomato_table[k]->time_removed != -1 &&
 196                             strat_db.tomato_table[k]->time_removed + 2 <= time_get_s()) {
 197 #ifdef HOST_VERSION
 198                                 printf("remove tomato %d\n", k);
 199 #endif
 200                                 strat_db.tomato_table[k]->present = 0;
 201                         }
 202                 }
 203                 prev_check_time = cur_time;
 204         }
 205
 206         x = position_get_x_s16(&mainboard.pos);
 207         y = position_get_y_s16(&mainboard.pos);
 208
 209         if (xycoord_to_ijcoord(&x, &y, &i, &j) < 0)
 210                 return;
 211
 212         if (strat_db.wp_table[i][j].type != WP_TYPE_TOMATO)
 213                 return;
 214
 215         if (strat_db.wp_table[i][j].present == 0)
 216                 return;
 217
 218         strat_db.wp_table[i][j].time_removed = time_get_s();
 219         strat_db.wp_table[i][j].present = 0;
 220 #ifdef HOST_VERSION
 221         ballboard.ball_count ++;
 222         printf("add ball %d,%d\n", i, j);
 223 #endif
 224 }
 225
 226 /* mark corn as not present and give correct commands to the cobboard
 227  * for spickles */
 228 static void check_corn(void)
 229 {
 230         uint8_t flags;
 231         int8_t lcob_near, rcob_near;
 232         uint8_t lcob, rcob;
 233         uint8_t lidx, ridx;
 234         static uint8_t prev_check_time;
 235         uint8_t cur_time;
 236         uint8_t need_lpack, need_rpack;
 237         int16_t l_xspickle, l_yspickle;
 238         int16_t r_xspickle, r_yspickle;
 239         uint8_t l_y_too_high_pack = 0, r_y_too_high_pack = 0;
 240
 241         /* read sensors from ballboard */
 242         IRQ_LOCK(flags);
 243         lcob = ballboard.lcob;
 244         ballboard.lcob = I2C_COB_NONE;
 245         rcob = ballboard.rcob;
 246         ballboard.rcob = I2C_COB_NONE;
 247         IRQ_UNLOCK(flags);
 248
 249         /* check present cobs once per second */
 250         cur_time = time_get_s();
 251         if (cur_time != prev_check_time) {
 252                 uint8_t i;
 253
 254                 /* only useful for simu */
 255                 for (i = 0; i < CORN_NB; i++) {
 256                         if (strat_db.corn_table[i]->present == 1 &&
 257                             strat_db.corn_table[i]->time_removed != -1 &&
 258                             strat_db.corn_table[i]->time_removed + 2 <= time_get_s()) {
 259 #ifdef HOST_VERSION
 260                                 printf("remove cob %d\n", i);
 261 #endif
 262                                 strat_db.corn_table[i]->present = 0;
 263                         }
 264                 }
 265                 prev_check_time = cur_time;
 266         }
 267
 268         /* detect cob on left side */
 269         lcob_near = corn_is_near(&lidx, I2C_LEFT_SIDE,
 270                                  &l_xspickle, &l_yspickle);
 271         if (lcob_near && lcob != I2C_COB_NONE) {
 272                 if (strat_db.corn_table[lidx]->corn.color == I2C_COB_UNKNOWN)
 273                         DEBUG(E_USER_STRAT, "lcob %s %d",
 274                               lcob == I2C_COB_WHITE ? "white" : "black", lidx);
 275                 corn_set_color(strat_db.corn_table[lidx], lcob);
 276         }
 277         if (!__y_is_more_than(l_yspickle, 600))
 278                 l_y_too_high_pack = 1;
 279
 280         /* detect cob on right side */
 281         rcob_near = corn_is_near(&ridx, I2C_RIGHT_SIDE,
 282                                  &r_xspickle, &r_yspickle);
 283         if (rcob_near && rcob != I2C_COB_NONE) {
 284                 if (strat_db.corn_table[ridx]->corn.color == I2C_COB_UNKNOWN)
 285                         DEBUG(E_USER_STRAT, "rcob %s %d",
 286                               rcob == I2C_COB_WHITE ? "white" : "black", ridx);
 287                 corn_set_color(strat_db.corn_table[ridx], rcob);
 288         }
 289         if (!__y_is_more_than(r_yspickle, 600))
 290                 r_y_too_high_pack = 1;
 291
 292         /* control the cobboard mode for left spickle */
 293         need_lpack = get_cob_count() >= 5 || strat_want_pack ||
 294                 strat_lpack60 || strat_opponent_lpack || l_y_too_high_pack;
 295
 296         if (lcob_near && strat_db.corn_table[lidx]->present) {
 297                 if (need_lpack) {
 298                         /* nothing  */
 299                 }
 300                 else {
 301                         /* deploy spickle and harvest white ones */
 302                         if (strat_db.corn_table[lidx]->corn.color == I2C_COB_WHITE) {
 303                                 i2c_cobboard_autoharvest_nomove(I2C_LEFT_SIDE);
 304                                 if (strat_db.corn_table[lidx]->time_removed == -1
 305 #ifndef HOST_VERSION
 306                                     && cobboard.status == I2C_COBBOARD_STATUS_LBUSY
 307 #endif
 308                                     ) {
 309                                         strat_db.corn_table[lidx]->time_removed = time_get_s();
 310 #ifdef HOST_VERSION
 311                                         cobboard.cob_count ++;
 312                                         printf("add cob %d\n", lidx);
 313 #else
 314                                         strat_db.corn_table[lidx]->present = 0;
 315 #endif
 316                                 }
 317                         }
 318                         else
 319                                 i2c_cobboard_deploy_nomove(I2C_LEFT_SIDE);
 320                 }
 321         }
 322         else {
 323                 /* no cob near us, we can pack or deploy freely */
 324                 if (need_lpack)
 325                         i2c_cobboard_pack_weak(I2C_LEFT_SIDE);
 326                 else
 327                         i2c_cobboard_deploy(I2C_LEFT_SIDE);
 328         }
 329
 330         /* control the cobboard mode for right spickle */
 331         need_rpack = get_cob_count() >= 5 || strat_want_pack ||
 332                 strat_rpack60 || strat_opponent_rpack || r_y_too_high_pack;
 333         if (rcob_near && strat_db.corn_table[ridx]->present) {
 334                 if (need_rpack) {
 335                         /* nothing */
 336                 }
 337                 else {
 338                         /* deploy spickle and harvest white ones */
 339                         if (strat_db.corn_table[ridx]->corn.color == I2C_COB_WHITE) {
 340                                 i2c_cobboard_autoharvest_nomove(I2C_RIGHT_SIDE);
 341                                 if (strat_db.corn_table[ridx]->time_removed == -1
 342 #ifndef HOST_VERSION
 343                                     && cobboard.status == I2C_COBBOARD_STATUS_RBUSY
 344 #endif
 345                                     ) {
 346                                         strat_db.corn_table[ridx]->time_removed = time_get_s();
 347 #ifdef HOST_VERSION
 348                                         cobboard.cob_count ++;
 349                                         printf("add cob %d\n", ridx);
 350 #else
 351                                         strat_db.corn_table[ridx]->present = 0;
 352 #endif
 353                                 }
 354                         }
 355                         else
 356                                 i2c_cobboard_deploy_nomove(I2C_RIGHT_SIDE);
 357                 }
 358         }
 359         else {
 360                 /* no cob near us, we can pack or deploy freely */
 361                 if (need_rpack)
 362                         i2c_cobboard_pack_weak(I2C_RIGHT_SIDE);
 363                 else
 364                         i2c_cobboard_deploy(I2C_RIGHT_SIDE);
 365         }
 366 }
 367
 368 /* check opponent position */
 369 void check_opponent(void)
 370 {
 371         int16_t opp_x, opp_y;
 372         int16_t opp_d, opp_a;
 373         uint8_t i, j;
 374
 375         strat_opponent_lpack = 0;
 376         strat_opponent_rpack = 0;
 377
 378         if (get_opponent_xyda(&opp_x, &opp_y, &opp_d, &opp_a) < 0)
 379                 return;
 380
 381         /* pack spickles if opponent too close */
 382         if (opp_d < 600) {
 383                 if (opp_a > 45 && opp_a < 135)
 384                         strat_opponent_lpack = 1;
 385                 if (opp_a > 225 && opp_a < 315)
 386                         strat_opponent_rpack = 1;
 387         }
 388
 389         /* check for oranges after 5 seconds */
 390         if (time_get_s() > 5) {
 391                 if (mainboard.our_color == I2C_COLOR_YELLOW) {
 392                         if (opp_y < 500 && opp_x < 500)
 393                                 strat_db.our_oranges_count = 0;
 394                         if (opp_y < 500 && opp_x > AREA_X - 500)
 395                                 strat_db.opp_oranges_count = 0;
 396                 }
 397                 else {
 398                         if (opp_y > AREA_Y - 500 && opp_x < 500)
 399                                 strat_db.our_oranges_count = 0;
 400                         if (opp_y > AREA_Y - 500 && opp_x > AREA_X - 500)
 401                                 strat_db.opp_oranges_count = 0;
 402                 }
 403         }
 404
 405         /* malus for some tomatoes and cobs, visited by opponent */
 406         if (xycoord_to_ijcoord(&opp_x, &opp_y, &i, &j) < 0)
 407                 return;
 408
 409         strat_db.wp_table[i][j].opp_visited = 1;
 410 }
 411
 412 /* called periodically (10ms) */
 413 void strat_event(void *dummy)
 414 {
 415         /* ignore when strat is not running */
 416         if (strat_running == 0)
 417                 return;
 418
 419         check_opponent();
 420         check_tomato();
 421         check_corn();
 422
 423         /* limit speed when opponent is near */
 424         /* disabled for 2010, we are already slow :) */
 425         //strat_limit_speed();
 426 }
 427
 428 /* check that we are on an eject line */
 429 static uint8_t robot_is_on_eject_line(void)
 430 {
 431         int16_t x, y;
 432         uint8_t i, j;
 433
 434         x = position_get_x_s16(&mainboard.pos);
 435         y = position_get_y_s16(&mainboard.pos);
 436
 437         if (xycoord_to_ijcoord_not_corn(&x, &y, &i, &j) < 0)
 438                 return 0;
 439
 440         if (!wp_belongs_to_line(i, j, 5, LINE_UP) &&
 441             !wp_belongs_to_line(i, j, 2, LINE_R_UP))
 442                 return 0;
 443
 444         return 1;
 445 }
 446
 447 /* 0 = fast, 1 = slow */
 448 static uint8_t eject_select_speed(void)
 449 {
 450         int16_t x, y;
 451         uint8_t i, j;
 452
 453         x = position_get_x_s16(&mainboard.pos);
 454         y = position_get_y_s16(&mainboard.pos);
 455
 456         if (get_cob_count() >= 5) {
 457                 strat_want_pack = 1;
 458                 return 0; /* fast */
 459         }
 460
 461         if (xycoord_to_ijcoord_not_corn(&x, &y, &i, &j) < 0) {
 462                 DEBUG(E_USER_STRAT, "%s(): cannot find waypoint at %d,%d",
 463                       __FUNCTION__, x, y);
 464                 return 1; /* slow */
 465         }
 466
 467         if (corn_count_neigh(i, j) == 2)
 468                 return 1; /* slow */
 469
 470         return 0; /* fast */
 471 }
 472
 473 /* called multiple times while we are waiting to reach the ejection
 474  * point */
 475 static uint8_t speedify_eject(void)
 476 {
 477         if (eject_select_speed())
 478                 strat_set_speed(SPEED_CLITOID_SLOW, SPEED_ANGLE_SLOW);
 479         else
 480                 strat_set_speed(SPEED_CLITOID_FAST, SPEED_ANGLE_SLOW);
 481         return 0;
 482 }
 483
 484 /* must be called from a terminal line */
 485 static uint8_t strat_eject(void)
 486 {
 487         uint8_t err;
 488
 489         DEBUG(E_USER_STRAT, "%s() cob_count=%d ball_count=%d",
 490               __FUNCTION__, get_cob_count(), get_ball_count());
 491
 492         /* check that we are called from an eject line */
 493         if (!robot_is_on_eject_line()) {
 494                 DEBUG(E_USER_STRAT, "%s() not on eject line", __FUNCTION__);
 495                 return END_ERROR;
 496         }
 497
 498         /* go to eject point */
 499         trajectory_goto_forward_xy_abs(&mainboard.traj, 2625, COLOR_Y(1847));
 500         err = WAIT_COND_OR_TRAJ_END(speedify_eject(),
 501                                     TRAJ_FLAGS_NO_NEAR);
 502         /* err is never == 0 because speedify_eject() always return 0 */
 503         if (!TRAJ_SUCCESS(err))
 504                 return err;
 505
 506         /* pack arms (force), and disable strat_event */
 507         strat_event_disable();
 508         i2c_cobboard_pack_weak(I2C_LEFT_SIDE);
 509         i2c_cobboard_pack_weak(I2C_RIGHT_SIDE);
 510
 511         /* ball ejection */
 512         if (get_ball_count() > 0) {
 513                 i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_EJECT);
 514                 time_wait_ms(300);
 515                 trajectory_a_abs(&mainboard.traj, COLOR_A(70));
 516                 err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
 517                 if (!TRAJ_SUCCESS(err))
 518                         goto fail;
 519
 520                 DEBUG(E_USER_STRAT, "%s():%d", __FUNCTION__, __LINE__);
 521                 strat_hardstop();
 522 #ifdef HOST_VERSION
 523                 time_wait_ms(2000);
 524 #else
 525                 WAIT_COND_OR_TIMEOUT(ballboard.status == I2C_BALLBOARD_STATUS_F_BUSY,
 526                                      2000);
 527                 WAIT_COND_OR_TIMEOUT(ballboard.status == I2C_BALLBOARD_STATUS_F_READY,
 528                                      2000);
 529 #endif
 530         }
 531         else {
 532                 /* to pack spickles */
 533                 time_wait_ms(300);
 534         }
 535
 536         if (get_cob_count() > 0) {
 537                 /* half turn */
 538                 trajectory_a_abs(&mainboard.traj, COLOR_A(-110));
 539                 err = wait_traj_end(END_INTR|END_TRAJ);
 540                 if (!TRAJ_SUCCESS(err))
 541                         goto fail;
 542
 543                 /* cob ejection */
 544                 trajectory_d_rel(&mainboard.traj, -70);
 545                 err = wait_traj_end(END_INTR|END_TRAJ);
 546                 if (!TRAJ_SUCCESS(err))
 547                         goto fail;
 548
 549                 i2c_cobboard_set_mode(I2C_COBBOARD_MODE_EJECT);
 550                 time_wait_ms(2000);
 551
 552
 553                 /* cob ejection */
 554                 trajectory_d_rel(&mainboard.traj, 70);
 555                 err = wait_traj_end(END_INTR|END_TRAJ);
 556                 if (!TRAJ_SUCCESS(err))
 557                         goto fail;
 558         }
 559
 560         strat_db_dump(__FUNCTION__);
 561         err = END_TRAJ;
 562
 563  fail:
 564         strat_event_enable();
 565         strat_want_pack = 0;
 566         return err;
 567 }
 568
 569 static uint8_t strat_beginning(uint8_t do_initturn)
 570 {
 571         uint8_t err;
 572
 573         strat_set_acc(ACC_DIST, ACC_ANGLE);
 574
 575         if (do_initturn) {
 576                 //strat_set_speed(600, 60);
 577                 strat_set_speed(450, 50);
 578                 trajectory_d_a_rel(&mainboard.traj, 1000, COLOR_A(20));
 579                 err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj),
 580                                             TRAJ_FLAGS_STD);
 581         }
 582
 583         strat_set_acc(ACC_DIST, ACC_ANGLE);
 584         strat_set_speed(SPEED_CLITOID_SLOW, SPEED_ANGLE_SLOW);
 585
 586  l1:
 587         err = line2line(0, LINE_UP, 2, LINE_R_DOWN, TRAJ_FLAGS_NO_NEAR);
 588         if (err == END_OBSTACLE &&
 589             strat_conf.flags & STRAT_CONF_WAIT_OBSTACLE &&
 590             time_get_s() > strat_conf.prev_wait_obstacle + 5) {
 591                 strat_conf.prev_wait_obstacle = time_get_s();
 592                 time_wait_ms(2000);
 593                 goto l1;
 594         }
 595         if (!TRAJ_SUCCESS(err))
 596                 return err;
 597
 598  l2:
 599         err = line2line(2, LINE_R_DOWN, 2, LINE_R_UP, TRAJ_FLAGS_NO_NEAR);
 600         if (err == END_OBSTACLE &&
 601             strat_conf.flags & STRAT_CONF_WAIT_OBSTACLE &&
 602             time_get_s() > strat_conf.prev_wait_obstacle + 5) {
 603                 strat_conf.prev_wait_obstacle = time_get_s();
 604                 time_wait_ms(2000);
 605                 goto l2;
 606         }
 607         if (!TRAJ_SUCCESS(err)) {
 608                 return err;
 609         }
 610
 611         return END_TRAJ;
 612 }
 613
 614 static uint8_t strat_beginning2(uint8_t do_initturn)
 615 {
 616         uint8_t err;
 617
 618         strat_set_acc(ACC_DIST, ACC_ANGLE);
 619
 620         if (do_initturn) {
 621                 strat_set_speed(600, 95); /* OK */
 622                 trajectory_d_a_rel(&mainboard.traj, 1000, COLOR_A(-40));
 623                 err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj),
 624                                             TRAJ_FLAGS_STD);
 625                 if (err == 0)
 626                         return END_TRAJ;
 627         }
 628         else {
 629                 trajectory_goto_forward_xy_abs(&mainboard.traj,
 630                                                375, COLOR_Y(597));
 631                 err = wait_traj_end(TRAJ_FLAGS_STD);
 632         }
 633         return err;
 634 }
 635
 636 /* dump state (every 5 s max) */
 637 #define DUMP_RATE_LIMIT(dump, last_print)               \
 638         do {                                            \
 639                 if (time_get_s() - last_print > 5) {    \
 640                         dump();                         \
 641                         last_print = time_get_s();      \
 642                 }                                       \
 643         } while (0)
 644
 645
 646 #if 0
 647 /* return true if we need to grab some more elements */
 648 static uint8_t need_more_elements(void)
 649 {
 650         if (time_get_s() <= 75) {
 651                 /* we have enough time left */
 652                 if (get_ball_count() >= 4)
 653                         return 0;
 654                 if (get_cob_count() >= 4)
 655                         return 0;
 656                 if ((get_ball_count() >= 2) &&
 657                     (get_cob_count() >= 2))
 658                         return 0;
 659                 return 1;
 660         }
 661         else {
 662                 /* not much time remaining */
 663                 if ((get_ball_count() >= 1) &&
 664                     (get_cob_count() >= 1))
 665                         return 0;
 666                 return 1;
 667         }
 668 }
 669 #endif
 670
 671 /* get tomatoes near our goals (12,5 and 12,3) */
 672 uint8_t get_opp_oranges(void)
 673 {
 674         int16_t x, y;
 675         uint8_t i, j;
 676         uint8_t err;
 677
 678         DEBUG(E_USER_STRAT, "%s()", __FUNCTION__);
 679
 680         /* only if oranges are present */
 681         if (strat_db.opp_oranges_count == 0)
 682                 return END_TRAJ;
 683
 684         strat_db.opp_oranges_count = 0;
 685         x = position_get_x_s16(&mainboard.pos);
 686         y = position_get_y_s16(&mainboard.pos);
 687
 688         if (xycoord_to_ijcoord_not_corn(&x, &y, &i, &j) < 0)
 689                 return END_ERROR;
 690
 691         /* not on eject point */
 692         if (i != 11 || j != 6)
 693                 return END_ERROR;
 694
 695         strat_want_pack = 1;
 696         strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
 697
 698         /* turn in the correct direction */
 699         trajectory_a_abs(&mainboard.traj, COLOR_A(-90));
 700         err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
 701         if (!TRAJ_SUCCESS(err))
 702                 goto fail;
 703
 704         trajectory_goto_forward_xy_abs(&mainboard.traj, 2625, COLOR_Y(597));
 705         err = wait_traj_end(TRAJ_FLAGS_STD);
 706         if (!TRAJ_SUCCESS(err))
 707                 goto fail;
 708
 709         strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW);
 710         trajectory_goto_forward_xy_abs(&mainboard.traj, 2750, COLOR_Y(250));
 711         err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
 712         if (!TRAJ_SUCCESS(err))
 713                 goto fail;
 714
 715         err = run_to_the_hills(get_opponent_color());
 716
 717  fail:
 718         strat_want_pack = 0;
 719         return err;
 720 }
 721
 722 /* get tomatoes near our goals (12,5 and 12,3) */
 723 uint8_t get_orphan_tomatoes(void)
 724 {
 725 #define CLITOID_TOMATO_RADIUS 100.
 726 #define TOMATO_BACK_X 2780
 727 #define TOMATO_BACK_LEN 200
 728
 729         int16_t x, y, a;
 730         uint8_t i, j;
 731         uint8_t err;
 732         int8_t ret;
 733
 734         DEBUG(E_USER_STRAT, "%s()", __FUNCTION__);
 735
 736         /* only go if both tomatoes are present */
 737         if (!strat_db.wp_table[12][5].present ||
 738             !strat_db.wp_table[12][3].present) {
 739                 return END_TRAJ;
 740         }
 741
 742         x = position_get_x_s16(&mainboard.pos);
 743         y = position_get_y_s16(&mainboard.pos);
 744
 745         if (xycoord_to_ijcoord_not_corn(&x, &y, &i, &j) < 0)
 746                 return END_ERROR;
 747
 748         /* not on eject point */
 749         if (i != 11 || j != 6)
 750                 return END_ERROR;
 751
 752         strat_want_pack = 1;
 753         strat_set_speed(SPEED_CLITOID_FAST, SPEED_ANGLE_SLOW);
 754
 755         /* turn in the correct direction */
 756         trajectory_a_abs(&mainboard.traj, COLOR_A(-90));
 757         err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
 758
 759         /* clitoid to turn and take the first ball */
 760         ret = trajectory_clitoid(&mainboard.traj, 2625, COLOR_Y(1847),
 761                                  COLOR_A(-90), 150., COLOR_A(90), 0,
 762                                  CLITOID_TOMATO_RADIUS, 3*125);
 763         if (ret < 0) {
 764                 err = END_ERROR;
 765                 goto fail;
 766         }
 767
 768         err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
 769         if (!TRAJ_SUCCESS(err))
 770                 goto fail;
 771
 772         strat_set_speed(SPEED_CLITOID_SLOW, SPEED_ANGLE_SLOW);
 773         err = strat_calib(300, END_TRAJ|END_BLOCKING);
 774         a = position_get_a_deg_s16(&mainboard.pos);
 775         if (ABS(a) < 10)
 776                 strat_reset_pos(AREA_X - ROBOT_HALF_LENGTH_FRONT,
 777                                 DO_NOT_SET_POS,
 778                                 COLOR_A(0) + ROBOT_ANGLE_FRONT);
 779
 780         strat_set_speed(SPEED_CLITOID_FAST, SPEED_ANGLE_FAST);
 781         trajectory_d_rel(&mainboard.traj, -250);
 782         err = WAIT_COND_OR_TRAJ_END(!x_is_more_than(TOMATO_BACK_X),
 783                                     TRAJ_FLAGS_NO_NEAR);
 784
 785         if (err != 0 && !TRAJ_SUCCESS(err))
 786                 goto fail;
 787
 788         trajectory_d_a_rel(&mainboard.traj, -TOMATO_BACK_LEN, COLOR_A(-90));
 789         err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
 790
 791         /* next ball */
 792
 793         /* clitoid to turn and take the first ball */
 794         strat_set_speed(SPEED_CLITOID_FAST, SPEED_ANGLE_SLOW);
 795         ret = trajectory_clitoid(&mainboard.traj, 2625, COLOR_Y(1847),
 796                                  COLOR_A(-90), 150., COLOR_A(90), 0,
 797                                  CLITOID_TOMATO_RADIUS, 7*125);
 798         if (ret < 0) {
 799                 err = END_ERROR;
 800                 goto fail;
 801         }
 802         err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
 803         if (!TRAJ_SUCCESS(err))
 804                 goto fail;
 805
 806         strat_hardstop();
 807         strat_set_speed(SPEED_CLITOID_FAST, SPEED_ANGLE_FAST);
 808         trajectory_d_rel(&mainboard.traj, -250);
 809         err = WAIT_COND_OR_TRAJ_END(!x_is_more_than(TOMATO_BACK_X),
 810                                     TRAJ_FLAGS_NO_NEAR);
 811         trajectory_d_a_rel(&mainboard.traj, -TOMATO_BACK_LEN, COLOR_A(-90));
 812         err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
 813
 814 fail:
 815         strat_want_pack = 0;
 816         return err;
 817 }
 818
 819
 820 #define HILL_LEN 1000
 821
 822 #define HILL_ANGLE 0
 823 #define HILL_POSY_YELLOW 310
 824 #define HILL_POSY_BLUE 190
 825
 826 #define HILL_POSX_BALLS_DOWN1 830
 827 #define HILL_POSX_BALLS_DOWN2 920
 828 #define HILL_POSX_BALLS_DOWN3 730
 829 #define HILL_START_POSX 580
 830
 831 uint8_t prepare_hill(uint8_t orange_color, int16_t posx)
 832 {
 833         int16_t startx, starty;
 834         uint8_t our_color = get_color();
 835         uint8_t err;
 836
 837         strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW);
 838         if (orange_color == I2C_COLOR_YELLOW)
 839                 starty = HILL_POSY_YELLOW;
 840         else
 841                 starty = HILL_POSY_BLUE;
 842         if (orange_color == our_color)
 843                 startx = posx;
 844         else
 845                 startx = AREA_X - posx;
 846         trajectory_goto_forward_xy_abs(&mainboard.traj, startx, COLOR_Y(starty));
 847         err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
 848         if (!TRAJ_SUCCESS(err))
 849                 return err;
 850
 851         /* turn to the hills */
 852         if (orange_color == our_color)
 853                 trajectory_a_abs(&mainboard.traj, COLOR_A(HILL_ANGLE));
 854         else
 855                 trajectory_a_abs(&mainboard.traj, COLOR_A(-180+HILL_ANGLE));
 856         err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
 857         if (!TRAJ_SUCCESS(err))
 858                 return err;
 859
 860         return END_TRAJ;
 861 }
 862
 863 /* get oranges, must be called near game area */
 864 uint8_t run_to_the_hills(uint8_t orange_color)
 865 {
 866         double aa, ad;
 867         uint16_t sa, sd;
 868         uint8_t err;
 869         uint8_t our_color = get_color();
 870         int32_t p = pid_get_gain_P(&mainboard.angle.pid);
 871         int32_t i = pid_get_gain_I(&mainboard.angle.pid);
 872         int32_t d = pid_get_gain_D(&mainboard.angle.pid);
 873         int32_t max_in = pid_get_max_in(&mainboard.angle.pid);
 874         int32_t max_i = pid_get_max_I(&mainboard.angle.pid);
 875         int32_t max_out = pid_get_max_out(&mainboard.angle.pid);
 876
 877         strat_want_pack = 1;
 878         strat_get_acc(&ad, &aa);
 879         strat_get_speed(&sd, &sa);
 880
 881         DEBUG(E_USER_STRAT, "%s()", __FUNCTION__);
 882
 883         strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
 884         err = prepare_hill(orange_color, HILL_START_POSX);
 885         if (!TRAJ_SUCCESS(err))
 886                 return err;
 887
 888         strat_set_acc(5, ACC_ANGLE);
 889         strat_set_speed(300, SPEED_ANGLE_SLOW);
 890         bd_set_current_thresholds(&mainboard.distance.bd, 500, 8000, 2000000, 80);
 891         bd_set_current_thresholds(&mainboard.angle.bd, 500, 8000, 2000000, 80);
 892         bd_set_speed_threshold(&mainboard.distance.bd, 10);
 893         support_balls_pack();
 894
 895         /* decrease angle gains */
 896         pid_set_gains(&mainboard.angle.pid, 200, 0, 2000);
 897
 898         /* here it is difficult to handle return values, because we
 899          * are on the hill */
 900         i2c_cobboard_set_mode(I2C_COBBOARD_MODE_KICKSTAND_DOWN);
 901         trajectory_d_rel(&mainboard.traj, HILL_LEN);
 902         if (orange_color == our_color)
 903                 err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos) >
 904                                             HILL_POSX_BALLS_DOWN1,
 905                                             TRAJ_FLAGS_SMALL_DIST);
 906         else
 907                 err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos) <
 908                                             AREA_X - HILL_POSX_BALLS_DOWN1,
 909                                             TRAJ_FLAGS_SMALL_DIST);
 910         DEBUG(E_USER_STRAT, "deploy support balls");
 911         i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_PREP_FORK);
 912         support_balls_deploy();
 913         i2c_cobboard_set_mode(I2C_COBBOARD_MODE_KICKSTAND_UP);
 914         trajectory_only_a_rel(&mainboard.traj, 2);
 915         err = WAIT_COND_OR_TE_TO(0, 0, 2200);
 916
 917         i2c_cobboard_set_mode(I2C_COBBOARD_MODE_KICKSTAND_DOWN);
 918         strat_set_acc(3, 3);
 919         strat_hardstop();
 920         i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_TAKE_FORK);
 921
 922         time_wait_ms(1100);
 923
 924         trajectory_d_rel(&mainboard.traj, 15);
 925         time_wait_ms(400);
 926         strat_hardstop();
 927         time_wait_ms(200);
 928
 929         /* reach top, go down */
 930         trajectory_d_rel(&mainboard.traj, -HILL_LEN);
 931
 932         if (orange_color == our_color)
 933                 err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos) <
 934                                             HILL_POSX_BALLS_DOWN2,
 935                                             TRAJ_FLAGS_SMALL_DIST);
 936         else
 937                 err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos) >
 938                                             AREA_X - HILL_POSX_BALLS_DOWN2,
 939                                             TRAJ_FLAGS_SMALL_DIST);
 940         DEBUG(E_USER_STRAT, "pack support balls");
 941         support_balls_pack();
 942         if (orange_color == our_color)
 943                 err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos) <
 944                                             HILL_POSX_BALLS_DOWN3,
 945                                             TRAJ_FLAGS_SMALL_DIST);
 946         else
 947                 err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos) >
 948                                             AREA_X - HILL_POSX_BALLS_DOWN3,
 949                                             TRAJ_FLAGS_SMALL_DIST);
 950
 951         DEBUG(E_USER_STRAT, "deploy support balls");
 952         strat_set_acc(ad, aa);
 953         strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
 954         support_balls_deploy();
 955
 956         /* wait to be near the wall */
 957         if (orange_color == our_color)
 958                 err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos) < 300,
 959                                             TRAJ_FLAGS_SMALL_DIST);
 960         else
 961                 err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos) >
 962                                             AREA_X - 300,
 963                                             TRAJ_FLAGS_SMALL_DIST);
 964
 965         /* restore BD coefs */
 966         bd_set_current_thresholds(&mainboard.distance.bd, 500, 8000, 1000000, 20);
 967         bd_set_current_thresholds(&mainboard.distance.bd, 500, 8000, 1000000, 20);
 968         bd_set_speed_threshold(&mainboard.distance.bd, 60);
 969
 970         /* calibrate position on the wall */
 971         strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_SLOW);
 972
 973         i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST);
 974
 975         trajectory_a_abs(&mainboard.traj, COLOR_A(-90));
 976         err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
 977         time_wait_ms(200);
 978
 979         err = strat_calib(400, TRAJ_FLAGS_SMALL_DIST);
 980         strat_reset_pos(DO_NOT_SET_POS,
 981                         COLOR_Y(ROBOT_HALF_LENGTH_FRONT),
 982                         COLOR_A(-90) + ROBOT_ANGLE_FRONT);
 983         strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
 984
 985         trajectory_d_rel(&mainboard.traj, -250);
 986         err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
 987         if (orange_color == our_color)
 988                 trajectory_a_abs(&mainboard.traj, 180);
 989         else
 990                 trajectory_a_abs(&mainboard.traj, 0);
 991         err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
 992         time_wait_ms(200);
 993
 994         strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST);
 995         err = strat_calib(400, TRAJ_FLAGS_SMALL_DIST);
 996         if (orange_color == our_color)
 997                 strat_reset_pos(ROBOT_HALF_LENGTH_FRONT,
 998                                 DO_NOT_SET_POS,
 999                                 180 + ROBOT_ANGLE_FRONT);
1000         else
1001                 strat_reset_pos(AREA_X - ROBOT_HALF_LENGTH_FRONT,
1002                                 DO_NOT_SET_POS,
1003                                 0 + ROBOT_ANGLE_FRONT);
1004         strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
1005
1006         trajectory_d_rel(&mainboard.traj, -250);
1007         err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
1008
1009         /* revert acceleration and speed */
1010         pid_set_gains(&mainboard.angle.pid, p, i, d);
1011         pid_set_maximums(&mainboard.distance.pid, max_in, max_i, max_out);
1012         strat_want_pack = 0;
1013         strat_set_speed(sd, sa);
1014         support_balls_deploy();
1015         return err;
1016 }
1017
1018 uint8_t strat_main(void)
1019 {
1020         uint8_t err, do_initturn = 1;
1021
1022         /* get oranges */
1023         if (strat_conf.flags & STRAT_CONF_OUR_ORANGE) {
1024                 err = run_to_the_hills(get_color());
1025                 strat_db.our_oranges_count = 0;
1026                 do_initturn = 0;
1027         }
1028
1029         /* harvest the first cobs + balls */
1030         if (strat_conf.flags & STRAT_CONF_STRAIGHT_BEGIN)
1031                 err = strat_beginning2(do_initturn);
1032         else
1033                 err = strat_beginning(do_initturn);
1034
1035         if (!TRAJ_SUCCESS(err))
1036                 strat_unblock();
1037         else
1038                 err = strat_eject();
1039
1040         /* choose circuit, and harvest on it */
1041         while (1) {
1042
1043                 DEBUG(E_USER_STRAT, "start main loop");
1044
1045                 /* if it's time to get tomatoes, do it */
1046                 if (time_get_s() > strat_conf.orphan_tomato) {
1047                         err = get_orphan_tomatoes();
1048                         if (err == END_ERROR) {
1049                                 DEBUG(E_USER_STRAT,
1050                                       "get_orphan_tomatoes returned END_ERROR");
1051                         }
1052                         else if (err == END_TIMER) {
1053                                 DEBUG(E_USER_STRAT, "End of time");
1054                                 strat_exit();
1055                                 break;
1056                         }
1057                         else if (!TRAJ_SUCCESS(err)) {
1058                                 /* don't retry these tomatoes if it failed */
1059                                 strat_conf.orphan_tomato = 90;
1060                                 strat_unblock();
1061                         }
1062                 }
1063
1064                 /* if it's time to get opponent oranges, do it */
1065                 if (time_get_s() > strat_conf.opp_orange) {
1066                         err = get_opp_oranges();
1067                         if (err == END_ERROR) {
1068                                 DEBUG(E_USER_STRAT,
1069                                       "get_opp_oranges returned END_ERROR");
1070                         }
1071                         else if (err == END_TIMER) {
1072                                 DEBUG(E_USER_STRAT, "End of time");
1073                                 strat_exit();
1074                                 break;
1075                         }
1076                         else if (!TRAJ_SUCCESS(err)) {
1077                                 /* don't retry oranges if it failed */
1078                                 strat_conf.opp_orange = 90;
1079                                 strat_unblock();
1080                         }
1081                 }
1082
1083                 /**********************/
1084                 /* harvest on circuit */
1085                 /**********************/
1086
1087                 err = strat_harvest_circuit();
1088                 if (err == END_TIMER) {
1089                         DEBUG(E_USER_STRAT, "End of time");
1090                         strat_exit();
1091                         break;
1092                 }
1093                 if (!TRAJ_SUCCESS(err)) {
1094                         strat_unblock();
1095                         continue;
1096                 }
1097
1098                 /***********************/
1099                 /* eject game elements */
1100                 /***********************/
1101
1102                 err = strat_eject();
1103                 /* end of time exit ! */
1104                 if (err == END_TIMER) {
1105                         DEBUG(E_USER_STRAT, "End of time");
1106                         strat_exit();
1107                         break;
1108                 }
1109                 if (!TRAJ_SUCCESS(err)) {
1110                         strat_unblock();
1111                         continue;
1112                 }
1113         }
1114
1115         return err;
1116 }