projects/microb2009/mainboard/strat_building.c

   1 /*
   2  *  Copyright Droids, Microb Technology (2008)
   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_building.c,v 1.5 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 <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 <vect_base.h>
  44 #include <lines.h>
  45 #include <polygon.h>
  46 #include <obstacle_avoidance.h>
  47 #include <blocking_detection_manager.h>
  48 #include <robot_system.h>
  49 #include <position_manager.h>
  50
  51 #include <rdline.h>
  52 #include <parse.h>
  53
  54
  55 #include "../common/i2c_commands.h"
  56 #include "main.h"
  57 #include "cmdline.h"
  58 #include "i2c_protocol.h"
  59 #include "strat.h"
  60 #include "strat_base.h"
  61 #include "strat_utils.h"
  62 #include "strat_avoid.h"
  63 #include "sensor.h"
  64
  65
  66 #define DISC_DIST_NEED_GOTO_AVOID 1000
  67 #define DISC_DIST_PREPARE_BUILD   700
  68 #define DISC_DIST_SLOW            500
  69
  70 #define ERROUT(e) do {                          \
  71                 err = e;                        \
  72                 goto end;                       \
  73         } while(0)
  74
  75 static uint8_t is_ready_for_prepare_build(void)
  76 {
  77         double d, a;
  78         if (distance_from_robot(CENTER_X, CENTER_Y) >
  79             DISC_DIST_PREPARE_BUILD)
  80                 return 0;
  81         abs_xy_to_rel_da(CENTER_X, CENTER_Y, &d, &a);
  82         if (a < RAD(-30))
  83                 return 0;
  84         if (a > RAD(30))
  85                 return 0;
  86         return 1;
  87 }
  88
  89 /* go to the nearest place on the disc. Also prepare the arms for
  90  * building at the correct level. If level==-1, don't move the
  91  * arms. */
  92 uint8_t strat_goto_disc(int8_t level)
  93 {
  94         uint8_t err;
  95         uint16_t old_spdd, old_spda;
  96         double d, a, x, y;
  97
  98         DEBUG(E_USER_STRAT, "%s()", __FUNCTION__);
  99
 100         strat_get_speed(&old_spdd, &old_spda);
 101         strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
 102
 103         /* workaround for some static cols configurations */
 104         if ((strat_infos.conf.flags & STRAT_CONF_EARLY_SCAN) == 0) {
 105                 if (time_get_s() > 15)
 106                         i2c_mechboard_mode_loaded();
 107         }
 108
 109         /* if we are far from the disc, goto backward faster */
 110         abs_xy_to_rel_da(CENTER_X, CENTER_Y, &d, &a);
 111         if (d > DISC_DIST_NEED_GOTO_AVOID) {
 112                 rel_da_to_abs_xy(d - DISC_DIST_PREPARE_BUILD, a, &x, &y);
 113                 err = goto_and_avoid(x, y, TRAJ_FLAGS_STD,
 114                                      TRAJ_FLAGS_NO_NEAR);
 115                 if (!TRAJ_SUCCESS(err))
 116                         ERROUT(err);
 117         }
 118
 119 #ifdef HOMOLOGATION
 120         {
 121                 int16_t opp_d, opp_a;
 122                 trajectory_turnto_xy(&mainboard.traj,
 123                                      CENTER_X, CENTER_Y);
 124                 err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
 125
 126                 time_wait_ms(500);
 127
 128                 err = get_opponent_da(&opp_d, &opp_a);
 129                 if (err == 0 && opp_d < 600 &&
 130                     (opp_a > 325 || opp_a < 35))
 131                         return END_ERROR;
 132         }
 133 #endif
 134
 135         trajectory_goto_forward_xy_abs(&mainboard.traj,
 136                                        CENTER_X, CENTER_Y);
 137         err = WAIT_COND_OR_TRAJ_END(is_ready_for_prepare_build(),
 138                                     TRAJ_FLAGS_NO_NEAR);
 139
 140         if (err == END_BLOCKING)
 141                 ERROUT(END_BLOCKING);
 142         if (TRAJ_SUCCESS(err)) /* should not reach dest */
 143                 ERROUT(END_ERROR);
 144
 145         strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW);
 146         if (level != -1)
 147                 i2c_mechboard_mode_prepare_build_both(level);
 148
 149         err = WAIT_COND_OR_TRAJ_END(distance_from_robot(CENTER_X,
 150                                                         CENTER_Y) < DISC_DIST_SLOW,
 151                                     TRAJ_FLAGS_NO_NEAR);
 152
 153         if (err == END_BLOCKING)
 154                 ERROUT(END_BLOCKING);
 155         if (TRAJ_SUCCESS(err)) /* should not reach dest */
 156                 ERROUT(END_ERROR);
 157
 158         strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_VERY_SLOW);
 159         err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
 160
 161         if (err == END_BLOCKING)
 162                 ERROUT(END_TRAJ);
 163         if (TRAJ_SUCCESS(err)) /* should not reach dest */
 164                 ERROUT(END_ERROR);
 165
 166         ERROUT(err);
 167  end:
 168         strat_set_speed(old_spdd, old_spda);
 169         return err;
 170 }
 171
 172 /* must be called from the checkpoint before zone 1. */
 173 static uint8_t strat_goto_build_zone1_near(uint8_t level)
 174 {
 175         uint8_t err;
 176
 177         /* turn to build zone */
 178         strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
 179         trajectory_a_abs(&mainboard.traj, COLOR_A(90));
 180         err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
 181         if (!TRAJ_SUCCESS(err))
 182                 return err;
 183
 184         /* move forward to reach the build zone */
 185         strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_SLOW);
 186         i2c_mechboard_mode_prepare_build_both(level);
 187         err = strat_calib(500, TRAJ_FLAGS_SMALL_DIST);
 188         if (err == END_BLOCKING) {
 189                 err = END_TRAJ;
 190         }
 191
 192         DEBUG(E_USER_STRAT, "build zone reached");
 193         return err;
 194 }
 195
 196 /* must be called from the checkpoint before zone 0 */
 197 static uint8_t strat_goto_build_zone0_near(uint8_t level)
 198 {
 199         uint8_t err;
 200 #ifdef OLD_STYLE
 201         int16_t cur_y, diff_y, dst_y;
 202 #endif
 203
 204         /* turn to build zone */
 205         strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
 206         trajectory_a_abs(&mainboard.traj, COLOR_A(90));
 207         err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
 208         if (!TRAJ_SUCCESS(err))
 209                 return err;
 210
 211 #ifdef OLD_STYLE
 212         cur_y = position_get_y_s16(&mainboard.pos);
 213         dst_y = COLOR_Y(AREA_Y - (ROBOT_LENGTH/2) - 100);
 214         diff_y = ABS(cur_y - dst_y);
 215
 216         /* move forward to reach the build zone */
 217         strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_SLOW);
 218         i2c_mechboard_mode_prepare_build_both(level);
 219         trajectory_d_rel(&mainboard.traj, diff_y);
 220         err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
 221         if (err == END_BLOCKING) { /* not very good for z0 but... */
 222                 err = END_TRAJ;
 223         }
 224 #else
 225         /* move forward to reach the build zone */
 226         strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_SLOW);
 227         i2c_mechboard_mode_prepare_build_both(level);
 228         err = strat_calib(500, TRAJ_FLAGS_SMALL_DIST);
 229         if (err == END_BLOCKING) {
 230                 err = END_TRAJ;
 231         }
 232 #endif
 233
 234         DEBUG(E_USER_STRAT, "build zone reached");
 235         return err;
 236 }
 237
 238 /* Go to any build zone: disc, 1a or 1b. Doesn't work with zone 0 for
 239  * now... */
 240 uint8_t strat_goto_build_zone(struct build_zone *zone, uint8_t level)
 241 {
 242         uint8_t err = END_TRAJ;
 243         uint16_t old_spdd, old_spda;
 244         int16_t checkpoint_x, checkpoint_y;
 245         int16_t errx;
 246
 247         zone->last_try_time = time_get_s();
 248
 249         if (zone->flags & ZONE_F_DISC)
 250                 return strat_goto_disc(level);
 251
 252         DEBUG(E_USER_STRAT, "goto build zone x=%d", zone->checkpoint_x);
 253
 254         /* workaround for some static cols configurations */
 255         if (time_get_s() > 15)
 256                 i2c_mechboard_mode_loaded();
 257
 258         strat_get_speed(&old_spdd, &old_spda);
 259         strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
 260
 261         checkpoint_x = zone->checkpoint_x;
 262         checkpoint_y = COLOR_Y(zone->checkpoint_y);
 263         errx = position_get_x_s16(&mainboard.pos) - checkpoint_x;
 264
 265         /* goto checkpoint if we are too far from it, or if error on x
 266          * is too big. */
 267         if (distance_from_robot(checkpoint_x, checkpoint_y) > 300 ||
 268             ABS(errx) > 15) {
 269                 err = goto_and_avoid(checkpoint_x, checkpoint_y,
 270                                      TRAJ_FLAGS_STD,
 271                                      TRAJ_FLAGS_NO_NEAR);
 272                 if (!TRAJ_SUCCESS(err))
 273                         ERROUT(err);
 274         }
 275
 276         if (zone->flags & ZONE_F_ZONE1)
 277                 err = strat_goto_build_zone1_near(level);
 278         else if (zone->flags & ZONE_F_ZONE0)
 279                 err = strat_goto_build_zone0_near(level);
 280         if (!TRAJ_SUCCESS(err))
 281                 ERROUT(err);
 282
 283  end:
 284         strat_set_speed(old_spdd, old_spda);
 285         return err;
 286 }
 287
 288 /* return a free temple structure */
 289 struct temple *strat_get_free_temple(void)
 290 {
 291         uint8_t i;
 292
 293         for (i=0; i<MAX_TEMPLE; i++) {
 294                 if (!(strat_infos.temple_list[i].flags & TEMPLE_F_VALID))
 295                         return &strat_infos.temple_list[i];
 296         }
 297         return NULL;
 298 }
 299
 300 uint8_t strat_can_build_on_temple(struct temple *temple)
 301 {
 302         uint8_t col_l, col_r, max_col, lintel;
 303
 304         col_l = get_column_count_left();
 305         col_r = get_column_count_right();
 306         max_col = (col_l > col_r ? col_l : col_r);
 307         lintel = (get_lintel_count() > 0);
 308
 309         if (strat_infos.conf.flags & STRAT_CONF_ONLY_ONE_ON_DISC) {
 310                 if ((temple->level_r > 5) &&
 311                     (temple->flags & TEMPLE_F_ON_DISC))
 312                         return 0;
 313         }
 314
 315         /* return symetric temples only */
 316         if (temple->level_l != temple->level_r)
 317                 return 0;
 318
 319         if ((time_get_s() - temple->last_try_time) < TEMPLE_DISABLE_TIME)
 320                 return 0;
 321
 322         /* we could do better to work on non-symetric temples */
 323         if (temple->level_l + max_col + lintel > 9)
 324                 return 0;
 325
 326         if (temple->flags & TEMPLE_F_MONOCOL)
 327                 return 0;
 328
 329         /* XXX don't allow to build on opponent temple. For that we
 330          * must support the little back_mm. */
 331         if (temple->flags & TEMPLE_F_OPPONENT)
 332                 return 0;
 333
 334         return 1;
 335 }
 336
 337
 338 /* return the best existing temple for building */
 339 struct temple *strat_get_best_temple(void)
 340 {
 341         uint8_t i;
 342         struct temple *best = NULL;
 343         struct temple *temple = NULL;
 344
 345         for (i=0; i<MAX_TEMPLE; i++) {
 346                 temple = &strat_infos.temple_list[i];
 347
 348                 if (!(temple->flags & TEMPLE_F_VALID))
 349                         continue;
 350
 351                 if (strat_can_build_on_temple(temple) == 0)
 352                         continue;
 353
 354                 if (best == NULL) {
 355                         best = temple;
 356                         continue;
 357                 }
 358
 359                 /* take the higher temple between 'best' and 'temple' */
 360                 if (best->level_l < temple->level_l)
 361                         best = temple;
 362         }
 363
 364         DEBUG(E_USER_STRAT, "%s() return %p", __FUNCTION__, best);
 365         return best;
 366 }
 367
 368 /* return the temple we built on the disc if any. If valid == 1, the
 369  * temple must be buildable. */
 370 struct temple *strat_get_our_temple_on_disc(uint8_t valid)
 371 {
 372         uint8_t i;
 373         struct temple *temple = NULL;
 374
 375         if (strat_infos.conf.flags & STRAT_CONF_ONLY_ONE_ON_DISC) {
 376                 return NULL;
 377         }
 378
 379         for (i=0; i<MAX_TEMPLE; i++) {
 380                 temple = &strat_infos.temple_list[i];
 381
 382                 if (!(temple->flags & TEMPLE_F_VALID))
 383                         continue;
 384
 385                 if (valid == 1 && strat_can_build_on_temple(temple) == 0)
 386                         continue;
 387
 388                 if (temple->flags & TEMPLE_F_ON_DISC)
 389                         return temple;
 390         }
 391         return NULL;
 392 }
 393
 394 #define COL_MAX(x,y) (((x)>(y)) ? (x) : (y))
 395
 396 #define TIME_FOR_LINTEL 3000L
 397 #define TIME_FOR_BUILD  0L
 398 #define TIME_FOR_COL    800L
 399 #define TIME_MARGIN     2000L
 400
 401 #define CHECKPOINT_DISC_DIST 380
 402 #define CHECKPOINT_OTHER_DIST 200
 403 /* Grow a temple. It will update temple list. */
 404 uint8_t strat_grow_temple(struct temple *temple)
 405 {
 406         double checkpoint_x, checkpoint_y;
 407         uint8_t add_level = 0;
 408         uint8_t do_lintel = 1;
 409         uint8_t col_l, col_r, col_max;
 410         uint8_t err;
 411         uint16_t timeout;
 412
 413         /* XXX temple must be symetric */
 414         uint8_t level = temple->level_l;
 415
 416         DEBUG(E_USER_STRAT, "%s()", __FUNCTION__);
 417
 418         if (temple->level_l >= 9)
 419                 return END_ERROR;
 420
 421         if ( (temple->zone->flags & ZONE_F_ZONE1) ||
 422              (temple->zone->flags & ZONE_F_ZONE0) ) {
 423                 strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW);
 424                 trajectory_d_rel(&mainboard.traj, -17);
 425                 wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
 426         }
 427
 428         col_l = get_column_count_left();
 429         col_r = get_column_count_right();
 430
 431         if (time_get_s() < 75) {
 432                 /* make temple symetric: if we have 1 col on left and 2 cols
 433                  * on right, only build one on both sides. */
 434                 if (col_l > col_r) {
 435                         col_r = col_l;
 436                         do_lintel = 0;
 437                 }
 438                 if (col_r > col_l) {
 439                         col_r = col_l;
 440                         do_lintel = 0;
 441                 }
 442                 if (get_lintel_count() == 0)
 443                         do_lintel = 0;
 444         }
 445         else if (col_l != col_r)
 446                 do_lintel = 0;
 447
 448         if (col_l == 0 || col_r == 0) {
 449                 if (temple->flags & TEMPLE_F_LINTEL)
 450                         do_lintel = 0;
 451         }
 452
 453         if (col_l == 0 && col_r == 0 && do_lintel == 0) {
 454                 DEBUG(E_USER_STRAT, "nothing to do");
 455                 return END_ERROR;
 456         }
 457
 458         add_level = do_lintel + col_l;
 459         while (level + add_level > 9) {
 460                 if (do_lintel) {
 461                         do_lintel = 0;
 462                         add_level = do_lintel + col_l;
 463                         continue;
 464                 }
 465                 /* we know col_r and col_l are > 0 */
 466                 col_r--;
 467                 col_l--;
 468         }
 469
 470         col_max = COL_MAX(col_r, col_l);
 471
 472         /* Reduce nb elts if we don't have time */
 473         timeout = (!!col_max) * TIME_FOR_BUILD;
 474         timeout += col_max * TIME_FOR_COL;
 475         timeout += do_lintel * TIME_FOR_LINTEL;
 476         if ((timeout / 1000L) + time_get_s() > 89 && do_lintel) {
 477                 do_lintel = 0;
 478                 timeout -= TIME_FOR_LINTEL;
 479         }
 480         if ((timeout / 1000L) + time_get_s() > 89 && col_max) {
 481                 if (col_r > 0)
 482                         col_r--;
 483                 if (col_l > 0)
 484                         col_l--;
 485                 col_max--;
 486                 timeout -= TIME_FOR_COL;
 487         }
 488
 489         /* take a margin for timeout */
 490         timeout += (!!col_max) * TIME_MARGIN;
 491
 492         if (col_l == 0 && col_r == 0 && do_lintel == 0) {
 493                 DEBUG(E_USER_STRAT, "nothing to do (2)");
 494                 return END_ERROR;
 495         }
 496
 497         DEBUG(E_USER_STRAT, "Autobuild: left=%d,%d right=%d,%d lintel=%d",
 498               level, col_l, level, col_r, do_lintel);
 499
 500         i2c_mechboard_mode_autobuild(level, col_l, I2C_AUTOBUILD_DEFAULT_DIST,
 501                                      level, col_r, I2C_AUTOBUILD_DEFAULT_DIST,
 502                                      do_lintel);
 503         WAIT_COND_OR_TIMEOUT(get_mechboard_mode() ==
 504                              I2C_MECHBOARD_MODE_AUTOBUILD, 100);
 505         err = WAIT_COND_OR_TIMEOUT(get_mechboard_mode() !=
 506                                    I2C_MECHBOARD_MODE_AUTOBUILD, timeout);
 507         if (err == 0) {
 508                 DEBUG(E_USER_STRAT, "timeout building temple (timeout was %d)", timeout);
 509                 temple->flags = 0; /* remove temple from list */
 510                 return END_TRAJ;
 511         }
 512         else
 513                 DEBUG(E_USER_STRAT, "temple built");
 514
 515         /* position of the robot when build the new temple */
 516         temple->x = position_get_x_s16(&mainboard.pos);
 517         temple->y = position_get_y_s16(&mainboard.pos);
 518         temple->a = position_get_a_deg_s16(&mainboard.pos);
 519
 520         /* checkpoint is a bit behind us */
 521         if (temple->zone->flags & ZONE_F_DISC) {
 522                 rel_da_to_abs_xy(CHECKPOINT_DISC_DIST, M_PI,
 523                                  &checkpoint_x, &checkpoint_y);
 524         }
 525         else {
 526                 rel_da_to_abs_xy(CHECKPOINT_OTHER_DIST, M_PI,
 527                                  &checkpoint_x, &checkpoint_y);
 528         }
 529         temple->checkpoint_x = checkpoint_x;
 530         temple->checkpoint_y = checkpoint_y;
 531
 532         temple->level_l = level + add_level;
 533         temple->dist_l = 0;
 534         temple->angle_l = 0;
 535
 536         temple->level_r = level + add_level;
 537         temple->dist_r = 0;
 538         temple->angle_r = 0;
 539
 540         temple->flags = TEMPLE_F_VALID;
 541
 542         if (distance_from_robot(CENTER_X, CENTER_Y) < 400)
 543                 temple->flags |= TEMPLE_F_ON_DISC;
 544
 545         if (do_lintel)
 546                 temple->flags |= TEMPLE_F_LINTEL;
 547
 548         /* we must push the temple */
 549         if ( ((temple->zone->flags & ZONE_F_ZONE1) ||
 550               (temple->zone->flags & ZONE_F_ZONE0)) &&
 551              level <= 1) {
 552                 strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW);
 553                 trajectory_d_rel(&mainboard.traj, -100);
 554                 wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
 555                 i2c_mechboard_mode_push_temple(level);
 556                 time_wait_ms(400);
 557                 strat_set_speed(200, SPEED_ANGLE_SLOW);
 558                 trajectory_d_rel(&mainboard.traj, 100);
 559                 wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
 560         }
 561
 562         /* Special case for big 3 */
 563         if (strat_infos.col_in_boobs) {
 564                 uint16_t old_spdd, old_spda;
 565                 strat_get_speed(&old_spdd, &old_spda);
 566                 strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_SLOW);
 567                 DEBUG(E_USER_STRAT, "%s() big 3", __FUNCTION__);
 568                 strat_infos.col_in_boobs = 0;
 569                 trajectory_d_rel(&mainboard.traj, -120);
 570                 wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
 571                 i2c_mechboard_mode_prepare_pickup_next(I2C_AUTO_SIDE,
 572                                                        I2C_MECHBOARD_MODE_CLEAR);
 573                 WAIT_COND_OR_TIMEOUT(get_column_count() >= 2, 4000L);
 574                 i2c_mechboard_mode_prepare_build_both(level + add_level);
 575                 time_wait_ms(800);
 576                 strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW);
 577                 trajectory_d_rel(&mainboard.traj, 120);
 578                 wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
 579                 err = strat_grow_temple(temple);
 580                 strat_set_speed(old_spdd, old_spda);
 581                 return err;
 582         }
 583
 584         return END_TRAJ;
 585 }
 586
 587 #define COL_BACK_DIST 70
 588 #define COL_ANGLE     20
 589 #define COL_ARM_DIST  220
 590
 591 #define COL_BACK_DIST_ZONE1 35
 592 #define COL_ARM_DIST_ZONE1  230
 593 #define COL_ANGLE_ZONE1     19
 594
 595 static uint8_t try_build_col(uint8_t l, uint8_t r,
 596                              uint8_t lp, uint8_t rp,
 597                              uint8_t lvl)
 598 {
 599         uint8_t max_lvl = lvl + r + l;
 600
 601         if (l == 0 && r == 0)
 602                 return 0;
 603         if (lp - l == 2 && rp - r == 0)
 604                 return 0;
 605         if (lp - l == 0 && rp - r == 2)
 606                 return 0;
 607         if (max_lvl > 9)
 608                 return 0;
 609         if (max_lvl == 9 && rp == 2 && r == 1)
 610                 return 0;
 611         return max_lvl;
 612 }
 613
 614 /* Grow a temple by building a column on it. It will update temple
 615  * list. */
 616 uint8_t strat_grow_temple_column(struct temple *temple)
 617 {
 618         uint16_t old_spdd, old_spda;
 619         double checkpoint_x, checkpoint_y;
 620         uint8_t add_level = 0;
 621         uint8_t col_l, col_r;
 622         uint8_t col_l_before, col_r_before;
 623         uint8_t err;
 624         int16_t a_abs, a;
 625         uint8_t level = temple->level_l;
 626         uint8_t lvl_ok = 0, col_l_ok = 0, col_r_ok = 0;
 627         uint8_t tmp_lvl;
 628         int16_t col_arm_dist = COL_ARM_DIST;
 629         int16_t col_back_dist = COL_BACK_DIST;
 630         int16_t col_angle = COL_ANGLE;
 631
 632         DEBUG(E_USER_STRAT, "%s()", __FUNCTION__);
 633
 634         if (level >= 9)
 635                 return END_ERROR;
 636
 637         strat_get_speed(&old_spdd, &old_spda);
 638
 639         if ( (temple->zone->flags & ZONE_F_ZONE1) ||
 640              (temple->zone->flags & ZONE_F_ZONE0) ) {
 641                      if (level == 1)
 642                              col_arm_dist = COL_ARM_DIST_ZONE1;
 643                      col_back_dist = COL_BACK_DIST_ZONE1;
 644                      col_angle = COL_ANGLE_ZONE1;
 645         }
 646
 647         a_abs = position_get_a_deg_s16(&mainboard.pos);
 648
 649         col_l_before = get_column_count_left();
 650         col_r_before = get_column_count_right();
 651         col_l = col_l_before;
 652         col_r = col_r_before;
 653
 654         /* check number of cols */
 655         for (col_l = 0; col_l < col_l_before + 1; col_l++) {
 656                 for (col_r = 0; col_r < col_r_before + 1; col_r++) {
 657                         tmp_lvl = try_build_col(col_l, col_r,
 658                                                 col_l_before,
 659                                                 col_r_before, level);
 660                         if (tmp_lvl > lvl_ok) {
 661                                 lvl_ok = tmp_lvl;
 662                                 col_l_ok = col_l;
 663                                 col_r_ok = col_r;
 664                         }
 665                 }
 666         }
 667
 668         col_l = col_l_ok;
 669         col_r = col_r_ok;
 670         add_level = col_l + col_r;
 671
 672         strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW);
 673
 674         if (col_l == 0 && col_r == 0)
 675                 ERROUT(END_ERROR);
 676
 677         DEBUG(E_USER_STRAT, "Build col: left=%d right=%d",
 678               col_l, col_r);
 679
 680         i2c_mechboard_mode_prepare_inside_both(level);
 681         trajectory_d_rel(&mainboard.traj, -col_back_dist);
 682         err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
 683         if (!TRAJ_SUCCESS(err))
 684                 ERROUT(err);
 685
 686         /* build with left arm */
 687         if (col_l) {
 688                 a = a_abs - col_angle;
 689                 if (a < -180)
 690                         a += 360;
 691                 trajectory_a_abs(&mainboard.traj, a);
 692                 err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
 693                 if (!TRAJ_SUCCESS(err))
 694                         ERROUT(err);
 695
 696                 if (time_get_s() > 88)
 697                         return END_TIMER;
 698
 699                 if (level >= 7 && get_column_count_left() == 2)
 700                         i2c_mechboard_mode_prepare_build_select(level+1, -1);
 701                 else
 702                         i2c_mechboard_mode_prepare_build_select(level, -1);
 703                 time_wait_ms(200);
 704                 i2c_mechboard_mode_autobuild(level, col_l, col_arm_dist,
 705                                              0, 0, col_arm_dist, 0);
 706                 while (get_mechboard_mode() != I2C_MECHBOARD_MODE_AUTOBUILD);
 707                 while (get_mechboard_mode() == I2C_MECHBOARD_MODE_AUTOBUILD);
 708
 709                 if ((strat_infos.conf.flags & STRAT_CONF_PUSH_OPP_COLS) &&
 710                     (col_r == 0) &&
 711                     (temple->flags & TEMPLE_F_OPPONENT)) {
 712                         strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST);
 713                         trajectory_d_rel(&mainboard.traj, -100);
 714                         wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
 715                         i2c_mechboard_mode_push_temple_disc(I2C_LEFT_SIDE);
 716                         time_wait_ms(500);
 717                         trajectory_d_rel(&mainboard.traj, 100);
 718                         wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
 719                 }
 720                 else if ((level == 1 || level == 0) && (col_r == 0)) {
 721                         trajectory_d_rel(&mainboard.traj, -100);
 722                         wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
 723                         i2c_mechboard_mode_push_temple(level);
 724                         time_wait_ms(400);
 725                         strat_set_speed(200, SPEED_ANGLE_SLOW);
 726                         trajectory_d_rel(&mainboard.traj, 120);
 727                         wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
 728                 }
 729
 730                 i2c_mechboard_mode_prepare_inside_select(level+col_l, -1);
 731         }
 732
 733         /* build with right arm */
 734         if (col_r) {
 735                 a = a_abs + col_angle;
 736                 if (a > 180)
 737                         a -= 360;
 738                 trajectory_a_abs(&mainboard.traj, a);
 739                 err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
 740                 if (!TRAJ_SUCCESS(err))
 741                         ERROUT(err);
 742
 743                 if (time_get_s() > 88)
 744                         return END_TIMER;
 745
 746                 if ((level+col_l) >= 7 && get_column_count_right() == 2)
 747                         i2c_mechboard_mode_prepare_build_select(-1, level + col_l + 1);
 748                 else
 749                         i2c_mechboard_mode_prepare_build_select(-1, level + col_l);
 750                 time_wait_ms(200);
 751                 i2c_mechboard_mode_autobuild(0, 0, col_arm_dist,
 752                                              level + col_l, col_r,
 753                                              col_arm_dist, 0);
 754                 while (get_mechboard_mode() != I2C_MECHBOARD_MODE_AUTOBUILD);
 755                 while (get_mechboard_mode() == I2C_MECHBOARD_MODE_AUTOBUILD);
 756
 757                 if ((strat_infos.conf.flags & STRAT_CONF_PUSH_OPP_COLS) &&
 758                     (temple->flags & TEMPLE_F_OPPONENT)) {
 759                         strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST);
 760                         trajectory_d_rel(&mainboard.traj, -100);
 761                         wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
 762                         i2c_mechboard_mode_push_temple_disc(I2C_RIGHT_SIDE);
 763                         time_wait_ms(500);
 764                         trajectory_d_rel(&mainboard.traj, 100);
 765                         wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
 766                 }
 767                 else if (level == 1 || level == 0) {
 768                         trajectory_d_rel(&mainboard.traj, -100);
 769                         wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
 770                         i2c_mechboard_mode_push_temple(level);
 771                         time_wait_ms(400);
 772                         strat_set_speed(200, SPEED_ANGLE_SLOW);
 773                         trajectory_d_rel(&mainboard.traj, 120);
 774                         wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
 775                 }
 776
 777                 i2c_mechboard_mode_prepare_inside_select(-1, level + col_l + col_r);
 778
 779         }
 780
 781         trajectory_a_abs(&mainboard.traj, a_abs);
 782         err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
 783         if (!TRAJ_SUCCESS(err))
 784                 ERROUT(err);
 785
 786         /* position of the robot when build the new temple */
 787         temple->x = position_get_x_s16(&mainboard.pos);
 788         temple->y = position_get_y_s16(&mainboard.pos);
 789         temple->a = position_get_a_deg_s16(&mainboard.pos);
 790
 791         /* checkpoint is a bit behind us */
 792         if (temple->zone->flags | ZONE_F_DISC) {
 793                 rel_da_to_abs_xy(CHECKPOINT_DISC_DIST, M_PI,
 794                                  &checkpoint_x, &checkpoint_y);
 795         }
 796         else {
 797                 rel_da_to_abs_xy(CHECKPOINT_OTHER_DIST, M_PI,
 798                                  &checkpoint_x, &checkpoint_y);
 799         }
 800         temple->checkpoint_x = checkpoint_x;
 801         temple->checkpoint_y = checkpoint_y;
 802
 803         temple->level_l = level + add_level;
 804         temple->dist_l = 0; /* XXX */
 805         temple->angle_l = 0;
 806
 807         temple->level_r = level + add_level;
 808         temple->dist_r = 0;
 809         temple->angle_r = 0;
 810
 811         temple->flags = TEMPLE_F_VALID | TEMPLE_F_MONOCOL;
 812
 813         if (distance_from_robot(CENTER_X, CENTER_Y) < 400)
 814                 temple->flags |= TEMPLE_F_ON_DISC;
 815
 816         if ( (temple->zone->flags & ZONE_F_ZONE1) ||
 817              (temple->zone->flags & ZONE_F_ZONE0) ) {
 818
 819         }
 820         return END_TRAJ;
 821  end:
 822         strat_set_speed(old_spdd, old_spda);
 823         return err;
 824 }
 825
 826 uint8_t strat_build_new_temple(struct build_zone *zone)
 827 {
 828         struct temple *temple;
 829         uint8_t level = zone->level;
 830         uint8_t err;
 831
 832         /* create a dummy temple */
 833         temple = strat_get_free_temple();
 834         if (!temple)
 835                 return END_ERROR;
 836
 837         memset(temple, 0, sizeof(*temple));
 838         temple->level_l = level;
 839         temple->level_r = level;
 840         temple->flags = TEMPLE_F_VALID | TEMPLE_F_LINTEL;
 841         temple->zone = zone;
 842
 843         zone->flags |= ZONE_F_BUSY;
 844
 845         if (time_get_s() > 50 && time_get_s() < 85 &&
 846             get_lintel_count() == 0)
 847                 err = strat_grow_temple_column(temple);
 848         else
 849                 err = strat_grow_temple(temple);
 850
 851         if (!TRAJ_SUCCESS(err))
 852                 temple->flags = 0;
 853         return err;
 854 }
 855
 856 uint8_t strat_goto_temple(struct temple *temple)
 857 {
 858         uint16_t old_spdd, old_spda;
 859         uint8_t err;
 860
 861         DEBUG(E_USER_STRAT, "goto temple %p checkpoint=%d,%d",
 862               temple, temple->checkpoint_x, temple->checkpoint_y);
 863
 864         temple->last_try_time = time_get_s();
 865
 866         strat_get_speed(&old_spdd, &old_spda);
 867         strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
 868
 869         i2c_mechboard_mode_loaded();
 870
 871         err = goto_and_avoid(temple->checkpoint_x,
 872                              temple->checkpoint_y,
 873                              TRAJ_FLAGS_STD,
 874                              TRAJ_FLAGS_NO_NEAR);
 875         if (!TRAJ_SUCCESS(err))
 876                 ERROUT(err);
 877
 878         err = strat_goto_build_zone(temple->zone, temple->level_r);
 879         if (!TRAJ_SUCCESS(err))
 880                 ERROUT(err);
 881
 882         DEBUG(E_USER_STRAT, "zone reached");
 883         ERROUT(END_TRAJ);
 884
 885  end:
 886         strat_set_speed(old_spdd, old_spda);
 887         return err;
 888 }
 889
 890 /* return the best existing temple for building */
 891 struct build_zone *strat_get_best_zone(void)
 892 {
 893         uint8_t i;
 894         struct build_zone *zone = NULL;
 895
 896         for (i=0; i<MAX_ZONE; i++) {
 897                 zone = &strat_infos.zone_list[i];
 898
 899                 if (zone->flags & ZONE_F_BUSY)
 900                         continue;
 901                 if ((time_get_s() - zone->last_try_time) < ZONE_DISABLE_TIME)
 902                         continue;
 903
 904                 return zone;
 905         }
 906         return NULL;
 907 }