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                 time_wait_ms(300);
 505                 trajectory_a_abs(&mainboard.traj, COLOR_A(70));
 506                 err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
 507                 if (!TRAJ_SUCCESS(err))
 508                         goto fail;
 509
 510                 DEBUG(E_USER_STRAT, "%s():%d", __FUNCTION__, __LINE__);
 511                 strat_hardstop();
 512 #ifdef HOST_VERSION
 513                 time_wait_ms(2000);
 514 #else
 515                 WAIT_COND_OR_TIMEOUT(ballboard.status == I2C_BALLBOARD_STATUS_F_BUSY,
 516                                      2000);
 517                 WAIT_COND_OR_TIMEOUT(ballboard.status == I2C_BALLBOARD_STATUS_F_READY,
 518                                      2000);
 519 #endif
 520         }
 521         else {
 522                 /* to pack spickles */
 523                 time_wait_ms(300);
 524         }
 525
 526         if (get_cob_count() > 0) {
 527                 /* half turn */
 528                 trajectory_a_abs(&mainboard.traj, COLOR_A(-110));
 529                 err = wait_traj_end(END_INTR|END_TRAJ);
 530                 if (!TRAJ_SUCCESS(err))
 531                         goto fail;
 532
 533                 /* cob ejection */
 534                 trajectory_d_rel(&mainboard.traj, -70);
 535                 err = wait_traj_end(END_INTR|END_TRAJ);
 536                 if (!TRAJ_SUCCESS(err))
 537                         goto fail;
 538
 539                 i2c_cobboard_set_mode(I2C_COBBOARD_MODE_EJECT);
 540                 time_wait_ms(2000);
 541
 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
 550         strat_db_dump(__FUNCTION__);
 551         err = END_TRAJ;
 552
 553  fail:
 554         strat_event_enable();
 555         strat_want_pack = 0;
 556         return err;
 557 }
 558
 559 static uint8_t strat_beginning(uint8_t do_initturn)
 560 {
 561         uint8_t err;
 562
 563         strat_set_acc(ACC_DIST, ACC_ANGLE);
 564
 565         if (do_initturn) {
 566                 //strat_set_speed(600, 60);
 567                 strat_set_speed(450, 50);
 568                 trajectory_d_a_rel(&mainboard.traj, 1000, COLOR_A(20));
 569                 err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj),
 570                                             TRAJ_FLAGS_STD);
 571         }
 572
 573         strat_set_acc(ACC_DIST, ACC_ANGLE);
 574         strat_set_speed(SPEED_CLITOID_SLOW, SPEED_ANGLE_SLOW);
 575
 576  l1:
 577         err = line2line(0, LINE_UP, 2, LINE_R_DOWN, TRAJ_FLAGS_NO_NEAR);
 578         if (err == END_OBSTACLE &&
 579             strat_conf.flags & STRAT_CONF_WAIT_OBSTACLE &&
 580             time_get_s() > strat_conf.prev_wait_obstacle + 5) {
 581                 strat_conf.prev_wait_obstacle = time_get_s();
 582                 time_wait_ms(2000);
 583                 goto l1;
 584         }
 585         if (!TRAJ_SUCCESS(err))
 586                 return err;
 587
 588  l2:
 589         err = line2line(2, LINE_R_DOWN, 2, LINE_R_UP, TRAJ_FLAGS_NO_NEAR);
 590         if (err == END_OBSTACLE &&
 591             strat_conf.flags & STRAT_CONF_WAIT_OBSTACLE &&
 592             time_get_s() > strat_conf.prev_wait_obstacle + 5) {
 593                 strat_conf.prev_wait_obstacle = time_get_s();
 594                 time_wait_ms(2000);
 595                 goto l2;
 596         }
 597         if (!TRAJ_SUCCESS(err)) {
 598                 return err;
 599         }
 600
 601         return END_TRAJ;
 602 }
 603
 604 static uint8_t strat_beginning2(uint8_t do_initturn)
 605 {
 606         uint8_t err;
 607
 608         strat_set_acc(ACC_DIST, ACC_ANGLE);
 609
 610         if (do_initturn) {
 611                 strat_set_speed(600, 95); /* OK */
 612                 trajectory_d_a_rel(&mainboard.traj, 1000, COLOR_A(-40));
 613                 err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj),
 614                                             TRAJ_FLAGS_STD);
 615                 if (err == 0)
 616                         return END_TRAJ;
 617         }
 618         else {
 619                 trajectory_goto_forward_xy_abs(&mainboard.traj,
 620                                                375, COLOR_Y(597));
 621                 err = wait_traj_end(TRAJ_FLAGS_STD);
 622         }
 623         return err;
 624 }
 625
 626 /* dump state (every 5 s max) */
 627 #define DUMP_RATE_LIMIT(dump, last_print)               \
 628         do {                                            \
 629                 if (time_get_s() - last_print > 5) {    \
 630                         dump();                         \
 631                         last_print = time_get_s();      \
 632                 }                                       \
 633         } while (0)
 634
 635
 636 #if 0
 637 /* return true if we need to grab some more elements */
 638 static uint8_t need_more_elements(void)
 639 {
 640         if (time_get_s() <= 75) {
 641                 /* we have enough time left */
 642                 if (get_ball_count() >= 4)
 643                         return 0;
 644                 if (get_cob_count() >= 4)
 645                         return 0;
 646                 if ((get_ball_count() >= 2) &&
 647                     (get_cob_count() >= 2))
 648                         return 0;
 649                 return 1;
 650         }
 651         else {
 652                 /* not much time remaining */
 653                 if ((get_ball_count() >= 1) &&
 654                     (get_cob_count() >= 1))
 655                         return 0;
 656                 return 1;
 657         }
 658 }
 659 #endif
 660
 661 /* get tomatoes near our goals (12,5 and 12,3) */
 662 uint8_t get_opp_oranges(void)
 663 {
 664         int16_t x, y;
 665         uint8_t i, j;
 666         uint8_t err;
 667
 668         DEBUG(E_USER_STRAT, "%s()", __FUNCTION__);
 669
 670         /* only if oranges are present */
 671         if (strat_db.opp_oranges_count == 0)
 672                 return END_TRAJ;
 673
 674         strat_db.opp_oranges_count = 0;
 675         x = position_get_x_s16(&mainboard.pos);
 676         y = position_get_y_s16(&mainboard.pos);
 677
 678         if (xycoord_to_ijcoord_not_corn(&x, &y, &i, &j) < 0)
 679                 return END_ERROR;
 680
 681         /* not on eject point */
 682         if (i != 11 || j != 6)
 683                 return END_ERROR;
 684
 685         strat_want_pack = 1;
 686         strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
 687
 688         /* turn in the correct direction */
 689         trajectory_a_abs(&mainboard.traj, COLOR_A(-90));
 690         err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
 691         if (!TRAJ_SUCCESS(err))
 692                 goto fail;
 693
 694         trajectory_goto_forward_xy_abs(&mainboard.traj, 2625, COLOR_Y(597));
 695         err = wait_traj_end(TRAJ_FLAGS_STD);
 696         if (!TRAJ_SUCCESS(err))
 697                 goto fail;
 698
 699         strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW);
 700         trajectory_goto_forward_xy_abs(&mainboard.traj, 2750, COLOR_Y(250));
 701         err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
 702         if (!TRAJ_SUCCESS(err))
 703                 goto fail;
 704
 705         err = run_to_the_hills(get_opponent_color());
 706
 707  fail:
 708         strat_want_pack = 0;
 709         return err;
 710 }
 711
 712 /* get tomatoes near our goals (12,5 and 12,3) */
 713 uint8_t get_orphan_tomatoes(void)
 714 {
 715 #define CLITOID_TOMATO_RADIUS 100.
 716 #define TOMATO_BACK_X 2780
 717 #define TOMATO_BACK_LEN 200
 718
 719         int16_t x, y, a;
 720         uint8_t i, j;
 721         uint8_t err;
 722         int8_t ret;
 723
 724         DEBUG(E_USER_STRAT, "%s()", __FUNCTION__);
 725
 726         /* only go if both tomatoes are present */
 727         if (!strat_db.wp_table[12][5].present ||
 728             !strat_db.wp_table[12][3].present) {
 729                 return END_TRAJ;
 730         }
 731
 732         x = position_get_x_s16(&mainboard.pos);
 733         y = position_get_y_s16(&mainboard.pos);
 734
 735         if (xycoord_to_ijcoord_not_corn(&x, &y, &i, &j) < 0)
 736                 return END_ERROR;
 737
 738         /* not on eject point */
 739         if (i != 11 || j != 6)
 740                 return END_ERROR;
 741
 742         strat_want_pack = 1;
 743         strat_set_speed(SPEED_CLITOID_FAST, SPEED_ANGLE_SLOW);
 744
 745         /* turn in the correct direction */
 746         trajectory_a_abs(&mainboard.traj, COLOR_A(-90));
 747         err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
 748
 749         /* clitoid to turn and take the first ball */
 750         ret = trajectory_clitoid(&mainboard.traj, 2625, COLOR_Y(1847),
 751                                  COLOR_A(-90), 150., COLOR_A(90), 0,
 752                                  CLITOID_TOMATO_RADIUS, 3*125);
 753         if (ret < 0) {
 754                 err = END_ERROR;
 755                 goto fail;
 756         }
 757
 758         err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
 759         if (!TRAJ_SUCCESS(err))
 760                 goto fail;
 761
 762         strat_set_speed(SPEED_CLITOID_SLOW, SPEED_ANGLE_SLOW);
 763         err = strat_calib(300, END_TRAJ|END_BLOCKING);
 764         a = position_get_a_deg_s16(&mainboard.pos);
 765         if (ABS(a) < 10)
 766                 strat_reset_pos(AREA_X - ROBOT_HALF_LENGTH_FRONT,
 767                                 DO_NOT_SET_POS,
 768                                 COLOR_A(0) + ROBOT_ANGLE_FRONT);
 769
 770         strat_set_speed(SPEED_CLITOID_FAST, SPEED_ANGLE_FAST);
 771         trajectory_d_rel(&mainboard.traj, -250);
 772         err = WAIT_COND_OR_TRAJ_END(!x_is_more_than(TOMATO_BACK_X),
 773                                     TRAJ_FLAGS_NO_NEAR);
 774
 775         if (err != 0 && !TRAJ_SUCCESS(err))
 776                 goto fail;
 777
 778         trajectory_d_a_rel(&mainboard.traj, -TOMATO_BACK_LEN, COLOR_A(-90));
 779         err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
 780
 781         /* next ball */
 782
 783         /* clitoid to turn and take the first ball */
 784         strat_set_speed(SPEED_CLITOID_FAST, SPEED_ANGLE_SLOW);
 785         ret = trajectory_clitoid(&mainboard.traj, 2625, COLOR_Y(1847),
 786                                  COLOR_A(-90), 150., COLOR_A(90), 0,
 787                                  CLITOID_TOMATO_RADIUS, 7*125);
 788         if (ret < 0) {
 789                 err = END_ERROR;
 790                 goto fail;
 791         }
 792         err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
 793         if (!TRAJ_SUCCESS(err))
 794                 goto fail;
 795
 796         strat_hardstop();
 797         strat_set_speed(SPEED_CLITOID_FAST, SPEED_ANGLE_FAST);
 798         trajectory_d_rel(&mainboard.traj, -250);
 799         err = WAIT_COND_OR_TRAJ_END(!x_is_more_than(TOMATO_BACK_X),
 800                                     TRAJ_FLAGS_NO_NEAR);
 801         trajectory_d_a_rel(&mainboard.traj, -TOMATO_BACK_LEN, COLOR_A(-90));
 802         err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
 803
 804 fail:
 805         strat_want_pack = 0;
 806         return err;
 807 }
 808
 809
 810 #define HILL_LEN 1000
 811
 812 #define HILL_ANGLE 0
 813 #define HILL_POSY_YELLOW 310
 814 #define HILL_POSY_BLUE 190
 815
 816 #define HILL_POSX_BALLS_DOWN1 830
 817 #define HILL_POSX_BALLS_DOWN2 920
 818 #define HILL_POSX_BALLS_DOWN3 730
 819 #define HILL_START_POSX 580
 820
 821 uint8_t prepare_hill(uint8_t orange_color, int16_t posx)
 822 {
 823         int16_t startx, starty;
 824         uint8_t our_color = get_color();
 825         uint8_t err;
 826
 827         strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW);
 828         if (orange_color == I2C_COLOR_YELLOW)
 829                 starty = HILL_POSY_YELLOW;
 830         else
 831                 starty = HILL_POSY_BLUE;
 832         if (orange_color == our_color)
 833                 startx = posx;
 834         else
 835                 startx = AREA_X - posx;
 836         trajectory_goto_forward_xy_abs(&mainboard.traj, startx, COLOR_Y(starty));
 837         err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
 838         if (!TRAJ_SUCCESS(err))
 839                 return err;
 840
 841         /* turn to the hills */
 842         if (orange_color == our_color)
 843                 trajectory_a_abs(&mainboard.traj, COLOR_A(HILL_ANGLE));
 844         else
 845                 trajectory_a_abs(&mainboard.traj, COLOR_A(-180+HILL_ANGLE));
 846         err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
 847         if (!TRAJ_SUCCESS(err))
 848                 return err;
 849
 850         return END_TRAJ;
 851 }
 852
 853 /* get oranges, must be called near game area */
 854 uint8_t run_to_the_hills(uint8_t orange_color)
 855 {
 856         double aa, ad;
 857         uint16_t sa, sd;
 858         uint8_t err;
 859         uint8_t our_color = get_color();
 860         int32_t p = pid_get_gain_P(&mainboard.angle.pid);
 861         int32_t i = pid_get_gain_I(&mainboard.angle.pid);
 862         int32_t d = pid_get_gain_D(&mainboard.angle.pid);
 863         int32_t max_in = pid_get_max_in(&mainboard.angle.pid);
 864         int32_t max_i = pid_get_max_I(&mainboard.angle.pid);
 865         int32_t max_out = pid_get_max_out(&mainboard.angle.pid);
 866
 867         strat_want_pack = 1;
 868         strat_get_acc(&ad, &aa);
 869         strat_get_speed(&sd, &sa);
 870
 871         DEBUG(E_USER_STRAT, "%s()", __FUNCTION__);
 872
 873         strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
 874         err = prepare_hill(orange_color, HILL_START_POSX);
 875         if (!TRAJ_SUCCESS(err))
 876                 return err;
 877
 878         strat_set_acc(5, ACC_ANGLE);
 879         strat_set_speed(300, SPEED_ANGLE_SLOW);
 880         bd_set_current_thresholds(&mainboard.distance.bd, 500, 8000, 2000000, 80);
 881         bd_set_current_thresholds(&mainboard.angle.bd, 500, 8000, 2000000, 80);
 882         bd_set_speed_threshold(&mainboard.distance.bd, 10);
 883         support_balls_pack();
 884
 885         /* decrease angle gains */
 886         pid_set_gains(&mainboard.angle.pid, 200, 0, 2000);
 887
 888         /* here it is difficult to handle return values, because we
 889          * are on the hill */
 890         i2c_cobboard_set_mode(I2C_COBBOARD_MODE_KICKSTAND_DOWN);
 891         trajectory_d_rel(&mainboard.traj, HILL_LEN);
 892         if (orange_color == our_color)
 893                 err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos) >
 894                                             HILL_POSX_BALLS_DOWN1,
 895                                             TRAJ_FLAGS_SMALL_DIST);
 896         else
 897                 err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos) <
 898                                             AREA_X - HILL_POSX_BALLS_DOWN1,
 899                                             TRAJ_FLAGS_SMALL_DIST);
 900         DEBUG(E_USER_STRAT, "deploy support balls");
 901         i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_PREP_FORK);
 902         support_balls_deploy();
 903         i2c_cobboard_set_mode(I2C_COBBOARD_MODE_KICKSTAND_UP);
 904         trajectory_only_a_rel(&mainboard.traj, 2);
 905         err = WAIT_COND_OR_TE_TO(0, 0, 2200);
 906
 907         i2c_cobboard_set_mode(I2C_COBBOARD_MODE_KICKSTAND_DOWN);
 908         strat_set_acc(3, 3);
 909         strat_hardstop();
 910         i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_TAKE_FORK);
 911
 912         time_wait_ms(1100);
 913
 914         trajectory_d_rel(&mainboard.traj, 15);
 915         time_wait_ms(400);
 916         strat_hardstop();
 917         time_wait_ms(200);
 918
 919         /* reach top, go down */
 920         trajectory_d_rel(&mainboard.traj, -HILL_LEN);
 921
 922         if (orange_color == our_color)
 923                 err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos) <
 924                                             HILL_POSX_BALLS_DOWN2,
 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_DOWN2,
 929                                             TRAJ_FLAGS_SMALL_DIST);
 930         DEBUG(E_USER_STRAT, "pack support balls");
 931         support_balls_pack();
 932         if (orange_color == our_color)
 933                 err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos) <
 934                                             HILL_POSX_BALLS_DOWN3,
 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_DOWN3,
 939                                             TRAJ_FLAGS_SMALL_DIST);
 940
 941         DEBUG(E_USER_STRAT, "deploy support balls");
 942         strat_set_acc(ad, aa);
 943         strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
 944         support_balls_deploy();
 945
 946         /* wait to be near the wall */
 947         if (orange_color == our_color)
 948                 err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos) < 300,
 949                                             TRAJ_FLAGS_SMALL_DIST);
 950         else
 951                 err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos) >
 952                                             AREA_X - 300,
 953                                             TRAJ_FLAGS_SMALL_DIST);
 954
 955         /* restore BD coefs */
 956         bd_set_current_thresholds(&mainboard.distance.bd, 500, 8000, 1000000, 20);
 957         bd_set_current_thresholds(&mainboard.distance.bd, 500, 8000, 1000000, 20);
 958         bd_set_speed_threshold(&mainboard.distance.bd, 60);
 959
 960         /* calibrate position on the wall */
 961         strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_SLOW);
 962
 963         i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST);
 964
 965         trajectory_a_abs(&mainboard.traj, COLOR_A(-90));
 966         err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
 967         time_wait_ms(200);
 968
 969         err = strat_calib(400, TRAJ_FLAGS_SMALL_DIST);
 970         strat_reset_pos(DO_NOT_SET_POS,
 971                         COLOR_Y(ROBOT_HALF_LENGTH_FRONT),
 972                         COLOR_A(-90) + ROBOT_ANGLE_FRONT);
 973         strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
 974
 975         trajectory_d_rel(&mainboard.traj, -250);
 976         err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
 977         if (orange_color == our_color)
 978                 trajectory_a_abs(&mainboard.traj, 180);
 979         else
 980                 trajectory_a_abs(&mainboard.traj, 0);
 981         err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
 982         time_wait_ms(200);
 983
 984         strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST);
 985         err = strat_calib(400, TRAJ_FLAGS_SMALL_DIST);
 986         if (orange_color == our_color)
 987                 strat_reset_pos(ROBOT_HALF_LENGTH_FRONT,
 988                                 DO_NOT_SET_POS,
 989                                 180 + ROBOT_ANGLE_FRONT);
 990         else
 991                 strat_reset_pos(AREA_X - ROBOT_HALF_LENGTH_FRONT,
 992                                 DO_NOT_SET_POS,
 993                                 0 + ROBOT_ANGLE_FRONT);
 994         strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
 995
 996         trajectory_d_rel(&mainboard.traj, -250);
 997         err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
 998
 999         /* revert acceleration and speed */
1000         pid_set_gains(&mainboard.angle.pid, p, i, d);
1001         pid_set_maximums(&mainboard.distance.pid, max_in, max_i, max_out);
1002         strat_want_pack = 0;
1003         strat_set_speed(sd, sa);
1004         support_balls_deploy();
1005         return err;
1006 }
1007
1008 uint8_t strat_main(void)
1009 {
1010         uint8_t err, do_initturn = 1;
1011
1012         /* get oranges */
1013         if (strat_conf.flags & STRAT_CONF_OUR_ORANGE) {
1014                 err = run_to_the_hills(get_color());
1015                 strat_db.our_oranges_count = 0;
1016                 do_initturn = 0;
1017         }
1018
1019         /* harvest the first cobs + balls */
1020         if (strat_conf.flags & STRAT_CONF_STRAIGHT_BEGIN)
1021                 err = strat_beginning2(do_initturn);
1022         else
1023                 err = strat_beginning(do_initturn);
1024
1025         if (!TRAJ_SUCCESS(err))
1026                 strat_unblock();
1027         else
1028                 err = strat_eject();
1029
1030         /* choose circuit, and harvest on it */
1031         while (1) {
1032
1033                 DEBUG(E_USER_STRAT, "start main loop");
1034
1035                 /* if it's time to get tomatoes, do it */
1036                 if (time_get_s() > strat_conf.orphan_tomato) {
1037                         err = get_orphan_tomatoes();
1038                         if (err == END_ERROR) {
1039                                 DEBUG(E_USER_STRAT,
1040                                       "get_orphan_tomatoes returned END_ERROR");
1041                         }
1042                         else if (err == END_TIMER) {
1043                                 DEBUG(E_USER_STRAT, "End of time");
1044                                 strat_exit();
1045                                 break;
1046                         }
1047                         else if (!TRAJ_SUCCESS(err)) {
1048                                 /* don't retry these tomatoes if it failed */
1049                                 strat_conf.orphan_tomato = 90;
1050                                 strat_unblock();
1051                         }
1052                 }
1053
1054                 /* if it's time to get opponent oranges, do it */
1055                 if (time_get_s() > strat_conf.opp_orange) {
1056                         err = get_opp_oranges();
1057                         if (err == END_ERROR) {
1058                                 DEBUG(E_USER_STRAT,
1059                                       "get_opp_oranges returned END_ERROR");
1060                         }
1061                         else if (err == END_TIMER) {
1062                                 DEBUG(E_USER_STRAT, "End of time");
1063                                 strat_exit();
1064                                 break;
1065                         }
1066                         else if (!TRAJ_SUCCESS(err)) {
1067                                 /* don't retry oranges if it failed */
1068                                 strat_conf.opp_orange = 90;
1069                                 strat_unblock();
1070                         }
1071                 }
1072
1073                 /**********************/
1074                 /* harvest on circuit */
1075                 /**********************/
1076
1077                 err = strat_harvest_circuit();
1078                 if (err == END_TIMER) {
1079                         DEBUG(E_USER_STRAT, "End of time");
1080                         strat_exit();
1081                         break;
1082                 }
1083                 if (!TRAJ_SUCCESS(err)) {
1084                         strat_unblock();
1085                         continue;
1086                 }
1087
1088                 /***********************/
1089                 /* eject game elements */
1090                 /***********************/
1091
1092                 err = strat_eject();
1093                 /* end of time exit ! */
1094                 if (err == END_TIMER) {
1095                         DEBUG(E_USER_STRAT, "End of time");
1096                         strat_exit();
1097                         break;
1098                 }
1099                 if (!TRAJ_SUCCESS(err)) {
1100                         strat_unblock();
1101                         continue;
1102                 }
1103         }
1104
1105         return err;
1106 }