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
 238         /* read sensors from ballboard */
 239         IRQ_LOCK(flags);
 240         lcob = ballboard.lcob;
 241         ballboard.lcob = I2C_COB_NONE;
 242         rcob = ballboard.rcob;
 243         ballboard.rcob = I2C_COB_NONE;
 244         IRQ_UNLOCK(flags);
 245
 246         /* check present cobs once per second */
 247         cur_time = time_get_s();
 248         if (cur_time != prev_check_time) {
 249                 uint8_t i;
 250
 251                 /* only useful for simu */
 252                 for (i = 0; i < CORN_NB; i++) {
 253                         if (strat_db.corn_table[i]->present == 1 &&
 254                             strat_db.corn_table[i]->time_removed != -1 &&
 255                             strat_db.corn_table[i]->time_removed + 2 <= time_get_s()) {
 256 #ifdef HOST_VERSION
 257                                 printf("remove cob %d\n", i);
 258 #endif
 259                                 strat_db.corn_table[i]->present = 0;
 260                         }
 261                 }
 262                 prev_check_time = cur_time;
 263         }
 264
 265         /* detect cob on left side */
 266         lcob_near = corn_is_near(&lidx, I2C_LEFT_SIDE);
 267         if (lcob_near && lcob != I2C_COB_NONE) {
 268                 if (strat_db.corn_table[lidx]->corn.color == I2C_COB_UNKNOWN)
 269                         DEBUG(E_USER_STRAT, "lcob %s %d",
 270                               lcob == I2C_COB_WHITE ? "white" : "black", lidx);
 271                 corn_set_color(strat_db.corn_table[lidx], lcob);
 272         }
 273
 274         /* detect cob on right side */
 275         rcob_near = corn_is_near(&ridx, I2C_RIGHT_SIDE);
 276         if (rcob_near && rcob != I2C_COB_NONE) {
 277                 if (strat_db.corn_table[ridx]->corn.color == I2C_COB_UNKNOWN)
 278                         DEBUG(E_USER_STRAT, "rcob %s %d",
 279                               rcob == I2C_COB_WHITE ? "white" : "black", ridx);
 280                 corn_set_color(strat_db.corn_table[ridx], rcob);
 281         }
 282
 283         /* control the cobboard mode for left spickle */
 284         need_lpack = get_cob_count() >= 5 || strat_want_pack ||
 285                 strat_lpack60 || strat_opponent_lpack;
 286         if (lcob_near && strat_db.corn_table[lidx]->present) {
 287                 if (need_lpack) {
 288                         /* nothing  */
 289                 }
 290                 else {
 291                         /* deploy spickle and harvest white ones */
 292                         if (strat_db.corn_table[lidx]->corn.color == I2C_COB_WHITE) {
 293                                 i2c_cobboard_autoharvest_nomove(I2C_LEFT_SIDE);
 294                                 if (strat_db.corn_table[lidx]->time_removed == -1
 295 #ifndef HOST_VERSION
 296                                     && cobboard.status == I2C_COBBOARD_STATUS_LBUSY
 297 #endif
 298                                     ) {
 299                                         strat_db.corn_table[lidx]->time_removed = time_get_s();
 300 #ifdef HOST_VERSION
 301                                         cobboard.cob_count ++;
 302                                         printf("add cob %d\n", lidx);
 303 #else
 304                                         strat_db.corn_table[lidx]->present = 0;
 305 #endif
 306                                 }
 307                         }
 308                         else
 309                                 i2c_cobboard_deploy_nomove(I2C_LEFT_SIDE);
 310                 }
 311         }
 312         else {
 313                 /* no cob near us, we can pack or deploy freely */
 314                 if (need_lpack)
 315                         i2c_cobboard_pack_weak(I2C_LEFT_SIDE);
 316                 else
 317                         i2c_cobboard_deploy(I2C_LEFT_SIDE);
 318         }
 319
 320         /* control the cobboard mode for right spickle */
 321         need_rpack = get_cob_count() >= 5 || strat_want_pack ||
 322                 strat_rpack60 || strat_opponent_rpack;
 323         if (rcob_near && strat_db.corn_table[ridx]->present) {
 324                 if (need_rpack) {
 325                         /* nothing */
 326                 }
 327                 else {
 328                         /* deploy spickle and harvest white ones */
 329                         if (strat_db.corn_table[ridx]->corn.color == I2C_COB_WHITE) {
 330                                 i2c_cobboard_autoharvest_nomove(I2C_RIGHT_SIDE);
 331                                 if (strat_db.corn_table[ridx]->time_removed == -1
 332 #ifndef HOST_VERSION
 333                                     && cobboard.status == I2C_COBBOARD_STATUS_RBUSY
 334 #endif
 335                                     ) {
 336                                         strat_db.corn_table[ridx]->time_removed = time_get_s();
 337 #ifdef HOST_VERSION
 338                                         cobboard.cob_count ++;
 339                                         printf("add cob %d\n", ridx);
 340 #else
 341                                         strat_db.corn_table[ridx]->present = 0;
 342 #endif
 343                                 }
 344                         }
 345                         else
 346                                 i2c_cobboard_deploy_nomove(I2C_RIGHT_SIDE);
 347                 }
 348         }
 349         else {
 350                 /* no cob near us, we can pack or deploy freely */
 351                 if (need_rpack)
 352                         i2c_cobboard_pack_weak(I2C_RIGHT_SIDE);
 353                 else
 354                         i2c_cobboard_deploy(I2C_RIGHT_SIDE);
 355         }
 356 }
 357
 358 /* check opponent position */
 359 void check_opponent(void)
 360 {
 361         int16_t opp_x, opp_y;
 362         int16_t opp_d, opp_a;
 363         uint8_t i, j;
 364
 365         strat_opponent_lpack = 0;
 366         strat_opponent_rpack = 0;
 367
 368         if (get_opponent_xyda(&opp_x, &opp_y, &opp_d, &opp_a) < 0)
 369                 return;
 370
 371         /* pack spickles if opponent too close */
 372         if (opp_d < 600) {
 373                 if (opp_a > 45 && opp_a < 135)
 374                         strat_opponent_lpack = 1;
 375                 if (opp_a > 225 && opp_a < 315)
 376                         strat_opponent_rpack = 1;
 377         }
 378
 379         /* check for oranges after 5 seconds */
 380         if (time_get_s() > 5) {
 381                 if (mainboard.our_color == I2C_COLOR_YELLOW) {
 382                         if (opp_y < 500 && opp_x < 500)
 383                                 strat_db.our_oranges_count = 0;
 384                         if (opp_y < 500 && opp_x > AREA_X - 500)
 385                                 strat_db.opp_oranges_count = 0;
 386                 }
 387                 else {
 388                         if (opp_y > AREA_Y - 500 && opp_x < 500)
 389                                 strat_db.our_oranges_count = 0;
 390                         if (opp_y > AREA_Y - 500 && opp_x > AREA_X - 500)
 391                                 strat_db.opp_oranges_count = 0;
 392                 }
 393         }
 394
 395         /* malus for some tomatoes and cobs, visited by opponent */
 396         if (xycoord_to_ijcoord(&opp_x, &opp_y, &i, &j) < 0)
 397                 return;
 398
 399         strat_db.wp_table[i][j].opp_visited = 1;
 400 }
 401
 402 /* called periodically (10ms) */
 403 void strat_event(void *dummy)
 404 {
 405         /* ignore when strat is not running */
 406         if (strat_running == 0)
 407                 return;
 408
 409         check_opponent();
 410         check_tomato();
 411         check_corn();
 412
 413         /* limit speed when opponent is near */
 414         /* disabled for 2010, we are already slow :) */
 415         //strat_limit_speed();
 416 }
 417
 418 /* check that we are on an eject line */
 419 static uint8_t robot_is_on_eject_line(void)
 420 {
 421         int16_t x, y;
 422         uint8_t i, j;
 423
 424         x = position_get_x_s16(&mainboard.pos);
 425         y = position_get_y_s16(&mainboard.pos);
 426
 427         if (xycoord_to_ijcoord_not_corn(&x, &y, &i, &j) < 0)
 428                 return 0;
 429
 430         if (!wp_belongs_to_line(i, j, 5, LINE_UP) &&
 431             !wp_belongs_to_line(i, j, 2, LINE_R_UP))
 432                 return 0;
 433
 434         return 1;
 435 }
 436
 437 /* 0 = fast, 1 = slow */
 438 static uint8_t eject_select_speed(void)
 439 {
 440         int16_t x, y;
 441         uint8_t i, j;
 442
 443         x = position_get_x_s16(&mainboard.pos);
 444         y = position_get_y_s16(&mainboard.pos);
 445
 446         if (get_cob_count() >= 5) {
 447                 strat_want_pack = 1;
 448                 return 0; /* fast */
 449         }
 450
 451         if (xycoord_to_ijcoord_not_corn(&x, &y, &i, &j) < 0) {
 452                 DEBUG(E_USER_STRAT, "%s(): cannot find waypoint at %d,%d",
 453                       __FUNCTION__, x, y);
 454                 return 1; /* slow */
 455         }
 456
 457         if (corn_count_neigh(i, j) == 2)
 458                 return 1; /* slow */
 459
 460         return 0; /* fast */
 461 }
 462
 463 /* called multiple times while we are waiting to reach the ejection
 464  * point */
 465 static uint8_t speedify_eject(void)
 466 {
 467         if (eject_select_speed())
 468                 strat_set_speed(SPEED_CLITOID_SLOW, SPEED_ANGLE_SLOW);
 469         else
 470                 strat_set_speed(SPEED_CLITOID_FAST, SPEED_ANGLE_SLOW);
 471         return 0;
 472 }
 473
 474 /* must be called from a terminal line */
 475 static uint8_t strat_eject(void)
 476 {
 477         uint8_t err;
 478
 479         DEBUG(E_USER_STRAT, "%s() cob_count=%d ball_count=%d",
 480               __FUNCTION__, get_cob_count(), get_ball_count());
 481
 482         /* check that we are called from an eject line */
 483         if (!robot_is_on_eject_line()) {
 484                 DEBUG(E_USER_STRAT, "%s() not on eject line", __FUNCTION__);
 485                 return END_ERROR;
 486         }
 487
 488         /* go to eject point */
 489         trajectory_goto_forward_xy_abs(&mainboard.traj, 2625, COLOR_Y(1847));
 490         err = WAIT_COND_OR_TRAJ_END(speedify_eject(),
 491                                     TRAJ_FLAGS_NO_NEAR);
 492         /* err is never == 0 because speedify_eject() always return 0 */
 493         if (!TRAJ_SUCCESS(err))
 494                 return err;
 495
 496         /* pack arms (force), and disable strat_event */
 497         strat_event_disable();
 498         i2c_cobboard_pack_weak(I2C_LEFT_SIDE);
 499         i2c_cobboard_pack_weak(I2C_RIGHT_SIDE);
 500
 501         /* ball ejection */
 502         if (get_ball_count() > 0) {
 503                 i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_EJECT);
 504                 trajectory_a_abs(&mainboard.traj, COLOR_A(70));
 505                 err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
 506                 if (!TRAJ_SUCCESS(err))
 507                         goto fail;
 508
 509                 DEBUG(E_USER_STRAT, "%s():%d", __FUNCTION__, __LINE__);
 510                 strat_hardstop();
 511 #ifdef HOST_VERSION
 512                 time_wait_ms(2000);
 513 #else
 514                 WAIT_COND_OR_TIMEOUT(ballboard.status == I2C_BALLBOARD_STATUS_F_BUSY,
 515                                      2000);
 516                 WAIT_COND_OR_TIMEOUT(ballboard.status == I2C_BALLBOARD_STATUS_F_READY,
 517                                      2000);
 518 #endif
 519         }
 520
 521         if (get_cob_count() > 0) {
 522                 /* half turn */
 523                 trajectory_a_abs(&mainboard.traj, COLOR_A(-110));
 524                 err = wait_traj_end(END_INTR|END_TRAJ);
 525                 if (!TRAJ_SUCCESS(err))
 526                         goto fail;
 527
 528                 /* cob ejection */
 529                 trajectory_d_rel(&mainboard.traj, -70);
 530                 err = wait_traj_end(END_INTR|END_TRAJ);
 531                 if (!TRAJ_SUCCESS(err))
 532                         goto fail;
 533
 534                 i2c_cobboard_set_mode(I2C_COBBOARD_MODE_EJECT);
 535                 time_wait_ms(2000);
 536
 537
 538                 /* cob ejection */
 539                 trajectory_d_rel(&mainboard.traj, 70);
 540                 err = wait_traj_end(END_INTR|END_TRAJ);
 541                 if (!TRAJ_SUCCESS(err))
 542                         goto fail;
 543         }
 544
 545         strat_db_dump(__FUNCTION__);
 546         err = END_TRAJ;
 547
 548  fail:
 549         strat_event_enable();
 550         strat_want_pack = 0;
 551         return err;
 552 }
 553
 554 static uint8_t strat_beginning(uint8_t do_initturn)
 555 {
 556         uint8_t err;
 557
 558         strat_set_acc(ACC_DIST, ACC_ANGLE);
 559
 560         if (do_initturn) {
 561                 //strat_set_speed(600, 60);
 562                 strat_set_speed(450, 50);
 563                 trajectory_d_a_rel(&mainboard.traj, 1000, COLOR_A(20));
 564                 err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj),
 565                                             TRAJ_FLAGS_STD);
 566         }
 567
 568         strat_set_acc(ACC_DIST, ACC_ANGLE);
 569         strat_set_speed(SPEED_CLITOID_SLOW, SPEED_ANGLE_SLOW);
 570
 571  l1:
 572         err = line2line(0, LINE_UP, 2, LINE_R_DOWN, TRAJ_FLAGS_NO_NEAR);
 573         if (err == END_OBSTACLE &&
 574             strat_conf.flags & STRAT_CONF_WAIT_OBSTACLE &&
 575             time_get_s() > strat_conf.prev_wait_obstacle + 5) {
 576                 strat_conf.prev_wait_obstacle = time_get_s();
 577                 time_wait_ms(2000);
 578                 goto l1;
 579         }
 580         if (!TRAJ_SUCCESS(err))
 581                 return err;
 582
 583  l2:
 584         err = line2line(2, LINE_R_DOWN, 2, LINE_R_UP, TRAJ_FLAGS_NO_NEAR);
 585         if (err == END_OBSTACLE &&
 586             strat_conf.flags & STRAT_CONF_WAIT_OBSTACLE &&
 587             time_get_s() > strat_conf.prev_wait_obstacle + 5) {
 588                 strat_conf.prev_wait_obstacle = time_get_s();
 589                 time_wait_ms(2000);
 590                 goto l2;
 591         }
 592         if (!TRAJ_SUCCESS(err)) {
 593                 return err;
 594         }
 595
 596         return END_TRAJ;
 597 }
 598
 599 static uint8_t strat_beginning2(uint8_t do_initturn)
 600 {
 601         uint8_t err;
 602
 603         strat_set_acc(ACC_DIST, ACC_ANGLE);
 604
 605         if (do_initturn) {
 606                 strat_set_speed(600, 95); /* OK */
 607                 trajectory_d_a_rel(&mainboard.traj, 1000, COLOR_A(-40));
 608                 err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj),
 609                                             TRAJ_FLAGS_STD);
 610                 if (err == 0)
 611                         return END_TRAJ;
 612         }
 613         else {
 614                 trajectory_goto_forward_xy_abs(&mainboard.traj,
 615                                                375, COLOR_Y(597));
 616                 err = wait_traj_end(TRAJ_FLAGS_STD);
 617         }
 618         return err;
 619 }
 620
 621 /* dump state (every 5 s max) */
 622 #define DUMP_RATE_LIMIT(dump, last_print)               \
 623         do {                                            \
 624                 if (time_get_s() - last_print > 5) {    \
 625                         dump();                         \
 626                         last_print = time_get_s();      \
 627                 }                                       \
 628         } while (0)
 629
 630
 631 #if 0
 632 /* return true if we need to grab some more elements */
 633 static uint8_t need_more_elements(void)
 634 {
 635         if (time_get_s() <= 75) {
 636                 /* we have enough time left */
 637                 if (get_ball_count() >= 4)
 638                         return 0;
 639                 if (get_cob_count() >= 4)
 640                         return 0;
 641                 if ((get_ball_count() >= 2) &&
 642                     (get_cob_count() >= 2))
 643                         return 0;
 644                 return 1;
 645         }
 646         else {
 647                 /* not much time remaining */
 648                 if ((get_ball_count() >= 1) &&
 649                     (get_cob_count() >= 1))
 650                         return 0;
 651                 return 1;
 652         }
 653 }
 654 #endif
 655
 656 /* get tomatoes near our goals (12,5 and 12,3) */
 657 uint8_t get_opp_oranges(void)
 658 {
 659         int16_t x, y;
 660         uint8_t i, j;
 661         uint8_t err;
 662
 663         DEBUG(E_USER_STRAT, "%s()", __FUNCTION__);
 664
 665         /* only if oranges are present */
 666         if (strat_db.opp_oranges_count == 0)
 667                 return END_TRAJ;
 668
 669         strat_db.opp_oranges_count = 0;
 670         x = position_get_x_s16(&mainboard.pos);
 671         y = position_get_y_s16(&mainboard.pos);
 672
 673         if (xycoord_to_ijcoord_not_corn(&x, &y, &i, &j) < 0)
 674                 return END_ERROR;
 675
 676         /* not on eject point */
 677         if (i != 11 || j != 6)
 678                 return END_ERROR;
 679
 680         strat_want_pack = 1;
 681         strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
 682
 683         /* turn in the correct direction */
 684         trajectory_a_abs(&mainboard.traj, COLOR_A(-90));
 685         err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
 686         if (!TRAJ_SUCCESS(err))
 687                 goto fail;
 688
 689         trajectory_goto_forward_xy_abs(&mainboard.traj, 2625, COLOR_Y(597));
 690         err = wait_traj_end(TRAJ_FLAGS_STD);
 691         if (!TRAJ_SUCCESS(err))
 692                 goto fail;
 693
 694         strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW);
 695         trajectory_goto_forward_xy_abs(&mainboard.traj, 2750, COLOR_Y(250));
 696         err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
 697         if (!TRAJ_SUCCESS(err))
 698                 goto fail;
 699
 700         err = run_to_the_hills(get_opponent_color());
 701
 702  fail:
 703         strat_want_pack = 0;
 704         return err;
 705 }
 706
 707 /* get tomatoes near our goals (12,5 and 12,3) */
 708 uint8_t get_orphan_tomatoes(void)
 709 {
 710 #define CLITOID_TOMATO_RADIUS 100.
 711 #define TOMATO_BACK_X 2780
 712 #define TOMATO_BACK_LEN 200
 713
 714         int16_t x, y, a;
 715         uint8_t i, j;
 716         uint8_t err;
 717         int8_t ret;
 718
 719         DEBUG(E_USER_STRAT, "%s()", __FUNCTION__);
 720
 721         /* only go if both tomatoes are present */
 722         if (!strat_db.wp_table[12][5].present ||
 723             !strat_db.wp_table[12][3].present) {
 724                 return END_TRAJ;
 725         }
 726
 727         x = position_get_x_s16(&mainboard.pos);
 728         y = position_get_y_s16(&mainboard.pos);
 729
 730         if (xycoord_to_ijcoord_not_corn(&x, &y, &i, &j) < 0)
 731                 return END_ERROR;
 732
 733         /* not on eject point */
 734         if (i != 11 || j != 6)
 735                 return END_ERROR;
 736
 737         strat_want_pack = 1;
 738         strat_set_speed(SPEED_CLITOID_FAST, SPEED_ANGLE_SLOW);
 739
 740         /* turn in the correct direction */
 741         trajectory_a_abs(&mainboard.traj, COLOR_A(-90));
 742         err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
 743
 744         /* clitoid to turn and take the first ball */
 745         ret = trajectory_clitoid(&mainboard.traj, 2625, COLOR_Y(1847),
 746                                  COLOR_A(-90), 150., COLOR_A(90), 0,
 747                                  CLITOID_TOMATO_RADIUS, 3*125);
 748         if (ret < 0) {
 749                 err = END_ERROR;
 750                 goto fail;
 751         }
 752
 753         err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
 754         if (!TRAJ_SUCCESS(err))
 755                 goto fail;
 756
 757         strat_set_speed(SPEED_CLITOID_SLOW, SPEED_ANGLE_SLOW);
 758         err = strat_calib(300, END_TRAJ|END_BLOCKING);
 759         a = position_get_a_deg_s16(&mainboard.pos);
 760         if (ABS(a) < 10)
 761                 strat_reset_pos(AREA_X - ROBOT_HALF_LENGTH_FRONT,
 762                                 DO_NOT_SET_POS,
 763                                 COLOR_A(0) + ROBOT_ANGLE_FRONT);
 764
 765         strat_set_speed(SPEED_CLITOID_FAST, SPEED_ANGLE_FAST);
 766         trajectory_d_rel(&mainboard.traj, -250);
 767         err = WAIT_COND_OR_TRAJ_END(!x_is_more_than(TOMATO_BACK_X),
 768                                     TRAJ_FLAGS_NO_NEAR);
 769
 770         if (err != 0 && !TRAJ_SUCCESS(err))
 771                 goto fail;
 772
 773         trajectory_d_a_rel(&mainboard.traj, -TOMATO_BACK_LEN, COLOR_A(-90));
 774         err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
 775
 776         /* next ball */
 777
 778         /* clitoid to turn and take the first ball */
 779         strat_set_speed(SPEED_CLITOID_FAST, SPEED_ANGLE_SLOW);
 780         ret = trajectory_clitoid(&mainboard.traj, 2625, COLOR_Y(1847),
 781                                  COLOR_A(-90), 150., COLOR_A(90), 0,
 782                                  CLITOID_TOMATO_RADIUS, 7*125);
 783         if (ret < 0) {
 784                 err = END_ERROR;
 785                 goto fail;
 786         }
 787         err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
 788         if (!TRAJ_SUCCESS(err))
 789                 goto fail;
 790
 791         strat_hardstop();
 792         strat_set_speed(SPEED_CLITOID_FAST, SPEED_ANGLE_FAST);
 793         trajectory_d_rel(&mainboard.traj, -250);
 794         err = WAIT_COND_OR_TRAJ_END(!x_is_more_than(TOMATO_BACK_X),
 795                                     TRAJ_FLAGS_NO_NEAR);
 796         trajectory_d_a_rel(&mainboard.traj, -TOMATO_BACK_LEN, COLOR_A(-90));
 797         err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
 798
 799 fail:
 800         strat_want_pack = 0;
 801         return err;
 802 }
 803
 804
 805 #define HILL_LEN 1000
 806
 807 #define HILL_ANGLE 0
 808 #define HILL_POSY_YELLOW 310
 809 #define HILL_POSY_BLUE 190
 810
 811 #define HILL_POSX_BALLS_DOWN1 830
 812 #define HILL_POSX_BALLS_DOWN2 920
 813 #define HILL_POSX_BALLS_DOWN3 730
 814 #define HILL_START_POSX 580
 815
 816 uint8_t prepare_hill(uint8_t orange_color, int16_t posx)
 817 {
 818         int16_t startx, starty;
 819         uint8_t our_color = get_color();
 820         uint8_t err;
 821
 822         strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW);
 823         if (orange_color == I2C_COLOR_YELLOW)
 824                 starty = HILL_POSY_YELLOW;
 825         else
 826                 starty = HILL_POSY_BLUE;
 827         if (orange_color == our_color)
 828                 startx = posx;
 829         else
 830                 startx = AREA_X - posx;
 831         trajectory_goto_forward_xy_abs(&mainboard.traj, startx, COLOR_Y(starty));
 832         err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
 833         if (!TRAJ_SUCCESS(err))
 834                 return err;
 835
 836         /* turn to the hills */
 837         if (orange_color == our_color)
 838                 trajectory_a_abs(&mainboard.traj, COLOR_A(HILL_ANGLE));
 839         else
 840                 trajectory_a_abs(&mainboard.traj, COLOR_A(-180+HILL_ANGLE));
 841         err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
 842         if (!TRAJ_SUCCESS(err))
 843                 return err;
 844
 845         return END_TRAJ;
 846 }
 847
 848 /* get oranges, must be called near game area */
 849 uint8_t run_to_the_hills(uint8_t orange_color)
 850 {
 851         double aa, ad;
 852         uint16_t sa, sd;
 853         uint8_t err;
 854         uint8_t our_color = get_color();
 855         int32_t p = pid_get_gain_P(&mainboard.angle.pid);
 856         int32_t i = pid_get_gain_I(&mainboard.angle.pid);
 857         int32_t d = pid_get_gain_D(&mainboard.angle.pid);
 858         int32_t max_in = pid_get_max_in(&mainboard.angle.pid);
 859         int32_t max_i = pid_get_max_I(&mainboard.angle.pid);
 860         int32_t max_out = pid_get_max_out(&mainboard.angle.pid);
 861
 862         strat_want_pack = 1;
 863         strat_get_acc(&ad, &aa);
 864         strat_get_speed(&sd, &sa);
 865
 866         DEBUG(E_USER_STRAT, "%s()", __FUNCTION__);
 867
 868         strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
 869         err = prepare_hill(orange_color, HILL_START_POSX);
 870         if (!TRAJ_SUCCESS(err))
 871                 return err;
 872
 873         strat_set_acc(5, ACC_ANGLE);
 874         strat_set_speed(300, SPEED_ANGLE_SLOW);
 875         bd_set_current_thresholds(&mainboard.distance.bd, 500, 8000, 2000000, 80);
 876         bd_set_current_thresholds(&mainboard.angle.bd, 500, 8000, 2000000, 80);
 877         bd_set_speed_threshold(&mainboard.distance.bd, 10);
 878         support_balls_pack();
 879
 880         /* decrease angle gains */
 881         pid_set_gains(&mainboard.angle.pid, 200, 0, 2000);
 882
 883         /* here it is difficult to handle return values, because we
 884          * are on the hill */
 885         i2c_cobboard_set_mode(I2C_COBBOARD_MODE_KICKSTAND_DOWN);
 886         trajectory_d_rel(&mainboard.traj, HILL_LEN);
 887         if (orange_color == our_color)
 888                 err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos) >
 889                                             HILL_POSX_BALLS_DOWN1,
 890                                             TRAJ_FLAGS_SMALL_DIST);
 891         else
 892                 err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos) <
 893                                             AREA_X - HILL_POSX_BALLS_DOWN1,
 894                                             TRAJ_FLAGS_SMALL_DIST);
 895         DEBUG(E_USER_STRAT, "deploy support balls");
 896         i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_PREP_FORK);
 897         support_balls_deploy();
 898         i2c_cobboard_set_mode(I2C_COBBOARD_MODE_KICKSTAND_UP);
 899         trajectory_only_a_rel(&mainboard.traj, 2);
 900         err = WAIT_COND_OR_TE_TO(0, 0, 2200);
 901
 902         i2c_cobboard_set_mode(I2C_COBBOARD_MODE_KICKSTAND_DOWN);
 903         strat_set_acc(3, 3);
 904         strat_hardstop();
 905         i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_TAKE_FORK);
 906
 907         time_wait_ms(1800);
 908
 909         /* reach top, go down */
 910         trajectory_d_rel(&mainboard.traj, -HILL_LEN);
 911
 912         if (orange_color == our_color)
 913                 err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos) <
 914                                             HILL_POSX_BALLS_DOWN2,
 915                                             TRAJ_FLAGS_SMALL_DIST);
 916         else
 917                 err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos) >
 918                                             AREA_X - HILL_POSX_BALLS_DOWN2,
 919                                             TRAJ_FLAGS_SMALL_DIST);
 920         DEBUG(E_USER_STRAT, "pack support balls");
 921         support_balls_pack();
 922         if (orange_color == our_color)
 923                 err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos) <
 924                                             HILL_POSX_BALLS_DOWN3,
 925                                             TRAJ_FLAGS_SMALL_DIST);
 926         else
 927                 err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos) >
 928                                             AREA_X - HILL_POSX_BALLS_DOWN3,
 929                                             TRAJ_FLAGS_SMALL_DIST);
 930
 931         DEBUG(E_USER_STRAT, "deploy support balls");
 932         strat_set_acc(ad, aa);
 933         strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
 934         support_balls_deploy();
 935
 936         /* wait to be near the wall */
 937         if (orange_color == our_color)
 938                 err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos) < 300,
 939                                             TRAJ_FLAGS_SMALL_DIST);
 940         else
 941                 err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos) >
 942                                             AREA_X - 300,
 943                                             TRAJ_FLAGS_SMALL_DIST);
 944
 945         /* restore BD coefs */
 946         bd_set_current_thresholds(&mainboard.distance.bd, 500, 8000, 1000000, 20);
 947         bd_set_current_thresholds(&mainboard.distance.bd, 500, 8000, 1000000, 20);
 948         bd_set_speed_threshold(&mainboard.distance.bd, 60);
 949
 950         /* calibrate position on the wall */
 951         strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_SLOW);
 952
 953         i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST);
 954
 955         trajectory_a_abs(&mainboard.traj, COLOR_A(-90));
 956         err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
 957         time_wait_ms(200);
 958
 959         err = strat_calib(400, TRAJ_FLAGS_SMALL_DIST);
 960         strat_reset_pos(DO_NOT_SET_POS,
 961                         COLOR_Y(ROBOT_HALF_LENGTH_FRONT),
 962                         COLOR_A(-90) + ROBOT_ANGLE_FRONT);
 963         strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
 964
 965         trajectory_d_rel(&mainboard.traj, -250);
 966         err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
 967         if (orange_color == our_color)
 968                 trajectory_a_abs(&mainboard.traj, 180);
 969         else
 970                 trajectory_a_abs(&mainboard.traj, 0);
 971         err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
 972         time_wait_ms(200);
 973
 974         strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST);
 975         err = strat_calib(400, TRAJ_FLAGS_SMALL_DIST);
 976         if (orange_color == our_color)
 977                 strat_reset_pos(ROBOT_HALF_LENGTH_FRONT,
 978                                 DO_NOT_SET_POS,
 979                                 180 + ROBOT_ANGLE_FRONT);
 980         else
 981                 strat_reset_pos(AREA_X - ROBOT_HALF_LENGTH_FRONT,
 982                                 DO_NOT_SET_POS,
 983                                 0 + ROBOT_ANGLE_FRONT);
 984         strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
 985
 986         trajectory_d_rel(&mainboard.traj, -250);
 987         err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
 988
 989         /* revert acceleration and speed */
 990         pid_set_gains(&mainboard.angle.pid, p, i, d);
 991         pid_set_maximums(&mainboard.distance.pid, max_in, max_i, max_out);
 992         strat_want_pack = 0;
 993         strat_set_speed(sd, sa);
 994         support_balls_deploy();
 995         return err;
 996 }
 997
 998 uint8_t strat_main(void)
 999 {
1000         uint8_t err, do_initturn = 1;
1001
1002         /* get oranges */
1003         if (strat_conf.flags & STRAT_CONF_OUR_ORANGE) {
1004                 err = run_to_the_hills(get_color());
1005                 strat_db.our_oranges_count = 0;
1006                 do_initturn = 0;
1007         }
1008
1009         /* harvest the first cobs + balls */
1010         if (strat_conf.flags & STRAT_CONF_STRAIGHT_BEGIN)
1011                 err = strat_beginning2(do_initturn);
1012         else
1013                 err = strat_beginning(do_initturn);
1014
1015         if (!TRAJ_SUCCESS(err))
1016                 strat_unblock();
1017         else
1018                 err = strat_eject();
1019
1020         /* choose circuit, and harvest on it */
1021         while (1) {
1022
1023                 DEBUG(E_USER_STRAT, "start main loop");
1024
1025                 /* if it's time to get tomatoes, do it */
1026                 if (time_get_s() > strat_conf.orphan_tomato) {
1027                         err = get_orphan_tomatoes();
1028                         if (err == END_ERROR) {
1029                                 DEBUG(E_USER_STRAT,
1030                                       "get_orphan_tomatoes returned END_ERROR");
1031                         }
1032                         else if (err == END_TIMER) {
1033                                 DEBUG(E_USER_STRAT, "End of time");
1034                                 strat_exit();
1035                                 break;
1036                         }
1037                         else if (!TRAJ_SUCCESS(err)) {
1038                                 /* don't retry these tomatoes if it failed */
1039                                 strat_conf.orphan_tomato = 90;
1040                                 strat_unblock();
1041                         }
1042                 }
1043
1044                 /* if it's time to get opponent oranges, do it */
1045                 if (time_get_s() > strat_conf.opp_orange) {
1046                         err = get_opp_oranges();
1047                         if (err == END_ERROR) {
1048                                 DEBUG(E_USER_STRAT,
1049                                       "get_opp_oranges returned END_ERROR");
1050                         }
1051                         else if (err == END_TIMER) {
1052                                 DEBUG(E_USER_STRAT, "End of time");
1053                                 strat_exit();
1054                                 break;
1055                         }
1056                         else if (!TRAJ_SUCCESS(err)) {
1057                                 /* don't retry oranges if it failed */
1058                                 strat_conf.opp_orange = 90;
1059                                 strat_unblock();
1060                         }
1061                 }
1062
1063                 /**********************/
1064                 /* harvest on circuit */
1065                 /**********************/
1066
1067                 err = strat_harvest_circuit();
1068                 if (err == END_TIMER) {
1069                         DEBUG(E_USER_STRAT, "End of time");
1070                         strat_exit();
1071                         break;
1072                 }
1073                 if (!TRAJ_SUCCESS(err)) {
1074                         strat_unblock();
1075                         continue;
1076                 }
1077
1078                 /***********************/
1079                 /* eject game elements */
1080                 /***********************/
1081
1082                 err = strat_eject();
1083                 /* end of time exit ! */
1084                 if (err == END_TIMER) {
1085                         DEBUG(E_USER_STRAT, "End of time");
1086                         strat_exit();
1087                         break;
1088                 }
1089                 if (!TRAJ_SUCCESS(err)) {
1090                         strat_unblock();
1091                         continue;
1092                 }
1093         }
1094
1095         return err;
1096 }