projects/microb2010/mechboard/state.c

   1 /*
   2  *  Copyright Droids Corporation (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: state.c,v 1.5 2009-11-08 17:25:00 zer0 Exp $
  19  *
  20  */
  21
  22 #include <math.h>
  23 #include <string.h>
  24
  25 #include <aversive.h>
  26 #include <aversive/wait.h>
  27 #include <aversive/error.h>
  28
  29 #include <ax12.h>
  30 #include <uart.h>
  31 #include <spi.h>
  32 #include <encoders_spi.h>
  33 #include <pwm_ng.h>
  34 #include <timer.h>
  35 #include <scheduler.h>
  36 #include <time.h>
  37
  38 #include <pid.h>
  39 #include <quadramp.h>
  40 #include <control_system_manager.h>
  41 #include <blocking_detection_manager.h>
  42
  43 #include <rdline.h>
  44 #include <vt100.h>
  45
  46 #include "../common/i2c_commands.h"
  47 #include "main.h"
  48 #include "cmdline.h"
  49 #include "sensor.h"
  50 #include "actuator.h"
  51 #include "arm_xy.h"
  52 #include "arm_highlevel.h"
  53 #include "state.h"
  54
  55 #define STMCH_DEBUG(args...) DEBUG(E_USER_ST_MACH, args)
  56 #define STMCH_NOTICE(args...) NOTICE(E_USER_ST_MACH, args)
  57 #define STMCH_ERROR(args...) ERROR(E_USER_ST_MACH, args)
  58
  59 /* shorter aliases for this file */
  60 #define MANUAL             I2C_MECHBOARD_MODE_MANUAL
  61 #define HARVEST            I2C_MECHBOARD_MODE_HARVEST
  62 #define PREPARE_PICKUP     I2C_MECHBOARD_MODE_PREPARE_PICKUP
  63 #define PICKUP             I2C_MECHBOARD_MODE_PICKUP
  64 #define PREPARE_BUILD      I2C_MECHBOARD_MODE_PREPARE_BUILD
  65 #define AUTOBUILD          I2C_MECHBOARD_MODE_AUTOBUILD
  66 #define WAIT               I2C_MECHBOARD_MODE_WAIT
  67 #define INIT               I2C_MECHBOARD_MODE_INIT
  68 #define PREPARE_GET_LINTEL I2C_MECHBOARD_MODE_PREPARE_GET_LINTEL
  69 #define GET_LINTEL         I2C_MECHBOARD_MODE_GET_LINTEL
  70 #define PUT_LINTEL         I2C_MECHBOARD_MODE_PUT_LINTEL
  71 #define PREPARE_EJECT      I2C_MECHBOARD_MODE_PREPARE_EJECT
  72 #define EJECT              I2C_MECHBOARD_MODE_EJECT
  73 #define CLEAR              I2C_MECHBOARD_MODE_CLEAR
  74 #define LAZY_HARVEST       I2C_MECHBOARD_MODE_LAZY_HARVEST
  75 #define LOADED             I2C_MECHBOARD_MODE_LOADED
  76 #define PREPARE_INSIDE     I2C_MECHBOARD_MODE_PREPARE_INSIDE
  77 #define STORE              I2C_MECHBOARD_MODE_STORE
  78 #define LAZY_PICKUP        I2C_MECHBOARD_MODE_LAZY_PICKUP
  79 #define MANIVELLE          I2C_MECHBOARD_MODE_MANIVELLE
  80 #define PUSH_TEMPLE        I2C_MECHBOARD_MODE_PUSH_TEMPLE
  81 #define PUSH_TEMPLE_DISC   I2C_MECHBOARD_MODE_PUSH_TEMPLE_DISC
  82 #define EXIT               I2C_MECHBOARD_MODE_EXIT
  83
  84 static void state_do_eject(uint8_t arm_num, uint8_t pump_num, uint8_t old_mode);
  85
  86 static struct i2c_cmd_mechboard_set_mode mainboard_command;
  87 static struct vt100 local_vt100;
  88 static volatile uint8_t prev_state;
  89 static uint8_t pickup_side;
  90 static volatile uint8_t changed = 0;
  91
  92 uint8_t state_debug = 0;
  93
  94 void state_dump_sensors(void)
  95 {
  96         uint16_t tmp = sensor_get_all();
  97         prog_char *front = PSTR("no_front");
  98         prog_char *left = PSTR("no_left");
  99         prog_char *right = PSTR("no_right");
 100
 101         if (tmp & _BV(S_FRONT))
 102                 front = PSTR("FRONT");
 103         if (tmp & _BV(S_LEFT)) {
 104                 if (tmp & _BV(S_COL_LEFT))
 105                         left = PSTR("LEFT(red)");
 106                 else
 107                         left = PSTR("LEFT(green)");
 108         }
 109         if (tmp & _BV(S_RIGHT)) {
 110                 if (tmp & _BV(S_COL_RIGHT))
 111                         right = PSTR("RIGHT(red)");
 112                 else
 113                         right = PSTR("RIGHT(green)");
 114         }
 115
 116         STMCH_DEBUG("sensors = %S %S %S", front, left, right);
 117 }
 118
 119 /* return 1 if column is there */
 120 uint8_t arm_get_sensor(uint8_t arm_num)
 121 {
 122         if (arm_num == ARM_LEFT_NUM) {
 123                 return sensor_get(S_LEFT);
 124         }
 125         else if (arm_num == ARM_RIGHT_NUM) {
 126                 return sensor_get(S_RIGHT);
 127         }
 128         return 0;
 129 }
 130
 131 /* return 0 if color is correct, else return -1 */
 132 int8_t arm_get_color_sensor(uint8_t arm_num)
 133 {
 134         uint8_t col = 0;
 135         if (arm_num == ARM_LEFT_NUM) {
 136                 col = sensor_get(S_COL_LEFT);
 137         }
 138         else if (arm_num == ARM_RIGHT_NUM) {
 139                 col = sensor_get(S_COL_RIGHT);
 140         }
 141
 142         /* if col != 0, column is red */
 143         if (col) {
 144                 if (mechboard.our_color == I2C_COLOR_RED)
 145                         return 0;
 146                 return -1;
 147         }
 148         else {
 149                 if (mechboard.our_color == I2C_COLOR_GREEN)
 150                         return 0;
 151                 return -1;
 152         }
 153 }
 154
 155 void state_debug_wait_key_pressed(void)
 156 {
 157         if (!state_debug)
 158                 return;
 159         printf_P(PSTR("press a key\r\n"));
 160         while(!cmdline_keypressed());
 161 }
 162
 163 /* set a new state, return 0 on success */
 164 int8_t state_set_mode(struct i2c_cmd_mechboard_set_mode *cmd)
 165 {
 166         changed = 1;
 167         prev_state = mainboard_command.mode;
 168         memcpy(&mainboard_command, cmd, sizeof(mainboard_command));
 169         STMCH_DEBUG("%s mode=%d", __FUNCTION__, mainboard_command.mode);
 170         return 0;
 171 }
 172
 173 /* check that state is the one in parameter and that state did not
 174  * changed */
 175 uint8_t state_check(uint8_t mode)
 176 {
 177         int16_t c;
 178         if (mode != mainboard_command.mode)
 179                 return 0;
 180
 181         if (changed)
 182                 return 0;
 183
 184         /* force quit when CTRL-C is typed */
 185         c = cmdline_getchar();
 186         if (c == -1)
 187                 return 1;
 188         if (vt100_parser(&local_vt100, c) == KEY_CTRL_C) {
 189                 mainboard_command.mode = EXIT;
 190                 return 0;
 191         }
 192         return 1;
 193 }
 194
 195 uint8_t state_get_mode(void)
 196 {
 197         return mainboard_command.mode;
 198 }
 199
 200 void pump_reset_all(void)
 201 {
 202         uint8_t i;
 203         for (i=0; i<4; i++) {
 204                 pump_set(i, PUMP_OFF);
 205                 pump_mark_free(i);
 206         }
 207 }
 208
 209 void pump_check_all(void)
 210 {
 211         if (pump_is_busy(PUMP_LEFT1_NUM) &&
 212             mechboard.pump_left1_current < I2C_MECHBOARD_CURRENT_COLUMN) {
 213                 STMCH_DEBUG("Mark l1 as free");
 214                 pump_mark_free(PUMP_LEFT1_NUM);
 215                 pump_set(PUMP_LEFT1_NUM, PUMP_OFF);
 216         }
 217
 218         if (pump_is_busy(PUMP_LEFT2_NUM) &&
 219             mechboard.pump_left2_current < I2C_MECHBOARD_CURRENT_COLUMN) {
 220                 STMCH_DEBUG("Mark l2 as free");
 221                 pump_mark_free(PUMP_LEFT2_NUM);
 222                 pump_set(PUMP_LEFT2_NUM, PUMP_OFF);
 223         }
 224
 225         if (pump_is_busy(PUMP_RIGHT1_NUM) &&
 226             sensor_get_adc(ADC_CSENSE3) < I2C_MECHBOARD_CURRENT_COLUMN) {
 227                 STMCH_DEBUG("Mark r1 as free");
 228                 pump_mark_free(PUMP_RIGHT1_NUM);
 229                 pump_set(PUMP_RIGHT1_NUM, PUMP_OFF);
 230         }
 231
 232         if (pump_is_busy(PUMP_RIGHT2_NUM) &&
 233             sensor_get_adc(ADC_CSENSE4) < I2C_MECHBOARD_CURRENT_COLUMN) {
 234                 STMCH_DEBUG("Mark r2 as free");
 235                 pump_mark_free(PUMP_RIGHT2_NUM);
 236                 pump_set(PUMP_RIGHT2_NUM, PUMP_OFF);
 237         }
 238 }
 239
 240 uint8_t get_free_pump_count(void)
 241 {
 242         uint8_t i, free_pump_count = 0;
 243         for (i=0; i<4; i++) {
 244                 if (pump_is_free(i))
 245                         free_pump_count++;
 246         }
 247         return free_pump_count;
 248 }
 249
 250 /* move finger if we are not in lazy harvest */
 251 void state_finger_goto(uint8_t mode, uint16_t position)
 252 {
 253         if (mode == LAZY_HARVEST)
 254                 return;
 255         finger_goto(position);
 256 }
 257
 258 void state_manivelle(int16_t step_deg)
 259 {
 260         double add_h = 0.;
 261         double add_d = 160.;
 262         double l = 70.;
 263         double step = RAD(step_deg);
 264         microseconds us;
 265         double al = RAD(0);
 266         double ar = RAD(180);
 267
 268         time_wait_ms(500);
 269
 270         us = time_get_us2();
 271         while (1) {
 272                 al += step;
 273                 ar += step;
 274                 arm_do_xy(&left_arm, add_d+l*sin(al), add_h+l*cos(al), 10);
 275                 arm_do_xy(&right_arm, add_d+l*sin(ar), add_h+l*cos(ar), 10);
 276                 time_wait_ms(25);
 277                 if (time_get_us2() - us > (4000L * 1000L))
 278                         break;
 279         }
 280 }
 281
 282 static void state_do_manivelle(void)
 283 {
 284         if (!state_check(MANIVELLE))
 285                 return;
 286         state_manivelle(30);
 287         while (state_check(MANIVELLE));
 288 }
 289
 290 /* common function for pickup/harvest */
 291 static void state_pickup_or_harvest(uint8_t mode)
 292 {
 293         int8_t arm_num, pump_num;
 294         int8_t other_arm_num, other_pump_num;
 295         struct arm *arm;
 296         microseconds us;
 297         uint8_t flags, bad_color = 0, have_2cols = 0;
 298
 299         pump_check_all();
 300
 301         /* get arm num */
 302         if (pickup_side == I2C_LEFT_SIDE) {
 303                 arm_num = ARM_LEFT_NUM;
 304                 other_arm_num = ARM_RIGHT_NUM;
 305         }
 306         else {
 307                 arm_num = ARM_RIGHT_NUM;
 308                 other_arm_num = ARM_LEFT_NUM;
 309         }
 310
 311         pump_num = arm_get_free_pump(arm_num);
 312         other_pump_num = arm_get_free_pump(other_arm_num);
 313
 314         /* pump is not free... skip to other arm */
 315         if (mode == HARVEST && pump_num == -1) {
 316                 STMCH_DEBUG("%s no free pump", __FUNCTION__);
 317                 if (arm_num == ARM_RIGHT_NUM) {
 318                         state_finger_goto(mode, FINGER_CENTER_RIGHT);
 319                         pickup_side = I2C_LEFT_SIDE;
 320                 }
 321                 else {
 322                         state_finger_goto(mode, FINGER_CENTER_LEFT);
 323                         pickup_side = I2C_RIGHT_SIDE;
 324                 }
 325                 return;
 326         }
 327         else if (mode == PICKUP && pump_num == -1) {
 328                 /* or exit when we are in pickup mode */
 329                 IRQ_LOCK(flags);
 330                 if (mainboard_command.mode == mode)
 331                         mainboard_command.mode = WAIT;
 332                 IRQ_UNLOCK(flags);
 333         }
 334
 335         us = time_get_us2();
 336         /* wait front sensor */
 337         if (mode == HARVEST || mode == LAZY_HARVEST) {
 338                 STMCH_DEBUG("%s wait front", __FUNCTION__);
 339
 340                 while (1) {
 341                         if (sensor_get(S_FRONT))
 342                                 break;
 343                         if (state_check(mode) == 0)
 344                                 return;
 345                         /* wait 500ms before reading other
 346                            sensors */
 347                         if (time_get_us2() - us < (500 * 1000L))
 348                                 continue;
 349                         if (arm_get_sensor(arm_num))
 350                                 break;
 351                         if (arm_get_sensor(other_arm_num)) {
 352                                 uint8_t tmp;
 353                                 tmp = arm_num;
 354                                 arm_num = other_arm_num;
 355                                 other_arm_num = tmp;
 356                                 pump_num = arm_get_free_pump(arm_num);
 357                                 other_pump_num = arm_get_free_pump(other_arm_num);
 358                                 if (other_pump_num == -1)
 359                                         return; // XXX
 360                                 break;
 361                         }
 362                 }
 363         }
 364
 365
 366         STMCH_DEBUG("%s arm_num=%d pump_num=%d",
 367                     __FUNCTION__, arm_num, pump_num);
 368
 369         /* when ready, move finger */
 370         if (arm_num == ARM_RIGHT_NUM)
 371                 state_finger_goto(mode, FINGER_RIGHT);
 372         else
 373                 state_finger_goto(mode, FINGER_LEFT);
 374
 375         state_debug_wait_key_pressed();
 376
 377
 378         arm = arm_num2ptr(arm_num);
 379
 380         /* prepare arm, should be already done */
 381         arm_goto_prepare_get(arm_num, pump_num);
 382         while (arm_test_traj_end(arm, ARM_TRAJ_ALL) &&
 383                state_check(mode));
 384
 385         STMCH_DEBUG("%s arm pos ok", __FUNCTION__);
 386
 387         state_debug_wait_key_pressed();
 388
 389         /* wait to see the column on the sensor */
 390         us = time_get_us2();
 391         while (1) {
 392                 if (arm_get_sensor(arm_num))
 393                         break;
 394                 if (state_check(mode) == 0)
 395                         return;
 396                 if (mode == PICKUP) /* no timeout in pickup */
 397                         continue;
 398                 /* 500ms timeout in harvest, go back */
 399                 if (time_get_us2() - us > 500*1000L) {
 400                         STMCH_DEBUG("%s timeout", __FUNCTION__);
 401
 402                         if (arm_num == ARM_RIGHT_NUM)
 403                                 state_finger_goto(mode, FINGER_LEFT);
 404                         else
 405                                 state_finger_goto(mode, FINGER_RIGHT);
 406
 407                         if (sensor_get(S_FRONT))
 408                                 time_wait_ms(500);
 409
 410                         pump_set(pump_num, PUMP_OFF);
 411                         return;
 412                 }
 413         }
 414
 415         state_dump_sensors();
 416
 417         pump_set(pump_num, PUMP_ON);
 418         /* bad color */
 419         if (arm_get_color_sensor(arm_num) == -1) {
 420                 bad_color = 1;
 421                 STMCH_DEBUG("%s prepare eject", __FUNCTION__);
 422                 mainboard_command.mode = PREPARE_EJECT;
 423                 state_do_eject(arm_num, pump_num, mode);
 424                 return;
 425         }
 426
 427         STMCH_DEBUG("%s sensor ok", __FUNCTION__);
 428
 429         /* by the way, prepare the other arm */
 430         if (other_pump_num != -1)
 431                 arm_goto_prepare_get(other_arm_num, other_pump_num);
 432
 433         /* get the column */
 434         arm_goto_get_column(arm_num, pump_num);
 435
 436         us = time_get_us2();
 437         while (1) {
 438                 /* wait 50 ms */
 439                 if (time_get_us2() - us > 50*1000L)
 440                         break;
 441                 if (mode != HARVEST)
 442                         continue;
 443                 /* if we still see the front sensor, it's because
 444                  * there are 2 columns instead of one or because there
 445                  * is another column, so send the arm on other
 446                  * side. */
 447                 if (sensor_get(S_FRONT) && have_2cols == 0) {
 448                         STMCH_DEBUG("%s 2 columns, release finger", __FUNCTION__);
 449                         have_2cols = 1;
 450                         if (finger_get_side() == I2C_LEFT_SIDE)
 451                                 state_finger_goto(mode, FINGER_RIGHT);
 452                         else
 453                                 state_finger_goto(mode, FINGER_LEFT);
 454                 }
 455         }
 456
 457         if (mode == HARVEST && have_2cols == 0) {
 458                 /* just release a bit of effort */
 459                 if (finger_get_side() == I2C_LEFT_SIDE) {
 460                         state_finger_goto(mode, FINGER_LEFT_RELAX);
 461                 }
 462                 else {
 463                         state_finger_goto(mode, FINGER_RIGHT_RELAX);
 464                 }
 465         }
 466         else if (mode == PICKUP) {
 467                 /* no free pump on other arm */
 468                 if (other_pump_num == -1) {
 469                         if (finger_get_side() == I2C_LEFT_SIDE) {
 470                                 state_finger_goto(mode, FINGER_LEFT_RELAX);
 471                         }
 472                         else {
 473                                 state_finger_goto(mode, FINGER_RIGHT_RELAX);
 474                         }
 475                 }
 476                 /* else send finger on the other side */
 477                 else {
 478                         if (finger_get_side() == I2C_LEFT_SIDE) {
 479                                 state_finger_goto(mode, FINGER_RIGHT);
 480                         }
 481                         else {
 482                                 state_finger_goto(mode, FINGER_LEFT);
 483                         }
 484                 }
 485         }
 486
 487         us = time_get_us2();
 488         while (1) {
 489                 /* wait 100 ms */
 490                 if (time_get_us2() - us > 100*1000L)
 491                         break;
 492                 if (mode != HARVEST)
 493                         continue;
 494                 /* if we still see the front sensor, it's because
 495                  * there are 2 columns instead of one or because there
 496                  * is another column, so send the arm on other
 497                  * side. */
 498                 if (sensor_get(S_FRONT) && have_2cols == 0) {
 499                         STMCH_DEBUG("%s 2 columns, release finger", __FUNCTION__);
 500                         have_2cols = 1;
 501                         if (finger_get_side() == I2C_LEFT_SIDE)
 502                                 state_finger_goto(mode, FINGER_RIGHT);
 503                         else
 504                                 state_finger_goto(mode, FINGER_LEFT);
 505                 }
 506         }
 507
 508         /* consider the column as taken */
 509         pump_mark_busy(pump_num);
 510
 511         state_debug_wait_key_pressed();
 512
 513         arm_goto_intermediate_get(arm_num, pump_num);
 514         arm_wait_traj_end(arm, ARM_TRAJ_ALL_NEAR);
 515
 516         /* prepare next */
 517         pump_num = arm_get_free_pump(arm_num);
 518         if (pump_num == -1)
 519                 arm_goto_loaded(arm_num);
 520         else
 521                 arm_goto_intermediate_get(arm_num, pump_num);
 522
 523         state_debug_wait_key_pressed();
 524
 525         /* switch to wait state */
 526         if (get_free_pump_count() == 0) {
 527                 IRQ_LOCK(flags);
 528                 if (mainboard_command.mode == mode)
 529                         mainboard_command.mode = WAIT;
 530                 IRQ_UNLOCK(flags);
 531         }
 532
 533         /* next pickup/harvest will be on the other side */
 534         if (pickup_side == I2C_LEFT_SIDE)
 535                 pickup_side = I2C_RIGHT_SIDE;
 536         else
 537                 pickup_side = I2C_LEFT_SIDE;
 538 }
 539
 540
 541 /* manual mode, arm position is sent from mainboard */
 542 static void state_do_manual(void)
 543 {
 544         if (!state_check(MANUAL))
 545                 return;
 546         STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode());
 547         while (state_check(MANUAL));
 548 }
 549
 550 /* wait mode */
 551 static void state_do_wait(void)
 552 {
 553         if (!state_check(WAIT))
 554                 return;
 555         STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode());
 556         while (state_check(WAIT));
 557 }
 558
 559 /* init mode */
 560 static void state_do_init(void)
 561 {
 562         if (!state_check(INIT))
 563                 return;
 564         state_init();
 565         STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode());
 566         while (state_check(INIT));
 567 }
 568
 569 /* harvest columns elts from area */
 570 static void state_do_harvest(void)
 571 {
 572         if (!state_check(HARVEST))
 573                 return;
 574
 575         if (get_free_pump_count() == 0) {
 576                 mainboard_command.mode = WAIT;
 577                 return;
 578         }
 579
 580         STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode());
 581
 582         state_pickup_or_harvest(HARVEST);
 583 }
 584
 585 /* harvest columns elts from area without moving finger */
 586 static void state_do_lazy_harvest(void)
 587 {
 588         if (!state_check(LAZY_HARVEST))
 589                 return;
 590
 591         if (get_free_pump_count() == 0) {
 592                 mainboard_command.mode = WAIT;
 593                 return;
 594         }
 595
 596         STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode());
 597
 598         state_pickup_or_harvest(LAZY_HARVEST);
 599 }
 600
 601 /* eject a column. always called from pickup mode. */
 602 static void state_do_eject(uint8_t arm_num, uint8_t pump_num, uint8_t old_mode)
 603 {
 604         struct arm *arm;
 605         arm = arm_num2ptr(arm_num);
 606
 607         if (finger_get_side() == I2C_LEFT_SIDE) {
 608                 state_finger_goto(old_mode, FINGER_LEFT_RELAX);
 609         }
 610         else {
 611                 state_finger_goto(old_mode, FINGER_RIGHT_RELAX);
 612         }
 613
 614         /* wait mainboard to eject */
 615         while (state_check(PREPARE_EJECT));
 616
 617         if (finger_get_side() == I2C_LEFT_SIDE) {
 618                 state_finger_goto(old_mode, FINGER_CENTER_LEFT);
 619         }
 620         else {
 621                 state_finger_goto(old_mode, FINGER_CENTER_RIGHT);
 622         }
 623
 624         arm_goto_get_column(arm_num, pump_num);
 625         arm_wait_traj_end(arm, ARM_TRAJ_ALL);
 626         time_wait_ms(150);
 627
 628         state_debug_wait_key_pressed();
 629
 630         arm_goto_prepare_eject(arm_num, pump_num);
 631         arm_wait_traj_end(arm, ARM_TRAJ_ALL);
 632
 633         state_debug_wait_key_pressed();
 634
 635         if (finger_get_side() == I2C_LEFT_SIDE) {
 636                 state_finger_goto(old_mode, FINGER_LEFT_RELAX);
 637         }
 638         else {
 639                 state_finger_goto(old_mode, FINGER_RIGHT_RELAX);
 640         }
 641
 642         state_debug_wait_key_pressed();
 643
 644         time_wait_ms(300);
 645         arm_goto_eject(arm_num, pump_num);
 646         time_wait_ms(200);
 647         pump_set(pump_num, PUMP_REVERSE);
 648         arm_wait_traj_end(arm, ARM_TRAJ_ALL);
 649
 650         arm_goto_intermediate_get(arm_num, pump_num);
 651         pump_set(pump_num, PUMP_OFF);
 652 }
 653
 654
 655 /* prepare pickup in a dispenser, or harvest */
 656 static void state_do_prepare_pickup(void)
 657 {
 658         uint8_t left_count = 0, right_count = 0;
 659         int8_t pump_l, pump_r;
 660
 661         if (!state_check(PREPARE_PICKUP))
 662                 return;
 663         STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode());
 664
 665         pump_check_all();
 666
 667         pump_l = arm_get_free_pump(ARM_LEFT_NUM);
 668         if (pump_l == -1) {
 669                 arm_goto_loaded(ARM_LEFT_NUM);
 670         }
 671         else {
 672                 arm_goto_intermediate_front_get(ARM_LEFT_NUM, pump_l);
 673         }
 674
 675         pump_r = arm_get_free_pump(ARM_RIGHT_NUM);
 676         if (pump_r == -1) {
 677                 arm_goto_loaded(ARM_RIGHT_NUM);
 678         }
 679         else {
 680                 arm_goto_intermediate_front_get(ARM_RIGHT_NUM, pump_r);
 681         }
 682
 683         arm_wait_both(ARM_TRAJ_ALL);
 684
 685         if (pump_l != -1)
 686                 arm_goto_prepare_get(ARM_LEFT_NUM, pump_l);
 687         if (pump_r != -1)
 688                 arm_goto_prepare_get(ARM_RIGHT_NUM, pump_r);
 689
 690         if (mainboard_command.prep_pickup.side == I2C_AUTO_SIDE) {
 691                 left_count += pump_is_busy(PUMP_LEFT1_NUM);
 692                 left_count += pump_is_busy(PUMP_LEFT2_NUM);
 693                 right_count += pump_is_busy(PUMP_RIGHT1_NUM);
 694                 right_count += pump_is_busy(PUMP_RIGHT2_NUM);
 695                 if (left_count < right_count)
 696                         finger_goto(FINGER_RIGHT);
 697                 else
 698                         finger_goto(FINGER_LEFT);
 699         }
 700         else if (mainboard_command.prep_pickup.side == I2C_LEFT_SIDE)
 701                 finger_goto(FINGER_LEFT);
 702         else if (mainboard_command.prep_pickup.side == I2C_RIGHT_SIDE)
 703                 finger_goto(FINGER_RIGHT);
 704         else if (mainboard_command.prep_pickup.side == I2C_CENTER_SIDE)
 705                 finger_goto(FINGER_CENTER_LEFT);
 706
 707         /* try to know on which side we have to pickup */
 708         if (finger_get_side() == I2C_RIGHT_SIDE) {
 709                 pickup_side = I2C_LEFT_SIDE;
 710         }
 711         else {
 712                 pickup_side = I2C_RIGHT_SIDE;
 713         }
 714
 715         arm_prepare_free_pumps();
 716
 717         mainboard_command.mode = mainboard_command.prep_pickup.next_mode;
 718
 719         while (state_check(PREPARE_PICKUP));
 720 }
 721
 722 /* clear pickup zone, will switch to harvest if needed */
 723 static void state_do_clear(void)
 724 {
 725         uint8_t flags, err;
 726
 727         if (!state_check(CLEAR))
 728                 return;
 729
 730         if (get_free_pump_count() == 0) {
 731                 mainboard_command.mode = WAIT;
 732                 return;
 733         }
 734
 735         STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode());
 736
 737         finger_goto(FINGER_LEFT);
 738         err = WAIT_COND_OR_TIMEOUT(sensor_get(S_LEFT), 500);
 739         if (err) {
 740                 IRQ_LOCK(flags);
 741                 if (mainboard_command.mode == CLEAR)
 742                         mainboard_command.mode = I2C_MECHBOARD_MODE_HARVEST;
 743                 IRQ_UNLOCK(flags);
 744                 pickup_side = I2C_LEFT_SIDE;
 745                 return;
 746         }
 747
 748         finger_goto(FINGER_RIGHT);
 749         err = WAIT_COND_OR_TIMEOUT(sensor_get(S_RIGHT), 500);
 750         if (err) {
 751                 IRQ_LOCK(flags);
 752                 if (mainboard_command.mode == CLEAR)
 753                         mainboard_command.mode = I2C_MECHBOARD_MODE_HARVEST;
 754                 IRQ_UNLOCK(flags);
 755                 pickup_side = I2C_RIGHT_SIDE;
 756                 return;
 757         }
 758
 759         IRQ_LOCK(flags);
 760         if (mainboard_command.mode == CLEAR)
 761                 mainboard_command.mode = I2C_MECHBOARD_MODE_HARVEST;
 762         IRQ_UNLOCK(flags);
 763 }
 764
 765 /* do a lazy pickup */
 766 static void state_do_lazy_pickup(void)
 767 {
 768         int8_t flags, arm_num, pump_num;
 769         uint32_t us;
 770
 771         if (!state_check(LAZY_PICKUP))
 772                 return;
 773
 774         STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode());
 775
 776         if (arm_get_sensor(ARM_LEFT_NUM) &&
 777             arm_get_sensor(ARM_RIGHT_NUM)) {
 778                 IRQ_LOCK(flags);
 779                 if (mainboard_command.mode == LAZY_PICKUP) {
 780                         mainboard_command.mode = WAIT;
 781                 }
 782                 IRQ_UNLOCK(flags);
 783                 return;
 784         }
 785
 786         if (finger_get_side() == I2C_RIGHT_SIDE) {
 787                 finger_goto(FINGER_LEFT);
 788                 arm_num = ARM_LEFT_NUM;
 789         }
 790         else {
 791                 finger_goto(FINGER_RIGHT);
 792                 arm_num = ARM_RIGHT_NUM;
 793         }
 794
 795         us = time_get_us2();
 796         while(1) {
 797                 if (state_check(LAZY_PICKUP) == 0)
 798                         return;
 799                 if (arm_get_sensor(arm_num))
 800                         break;
 801                 if (time_get_us2() - us > 500*1000L) {
 802                         if (finger_get_side() == I2C_RIGHT_SIDE)
 803                                 finger_goto(FINGER_LEFT);
 804                         else
 805                                 finger_goto(FINGER_RIGHT);
 806                         return;
 807                 }
 808         }
 809
 810         if (arm_get_color_sensor(arm_num) == -1) {
 811                 pump_num = arm_get_free_pump(arm_num);
 812                 if (pump_num == -1)
 813                         return; /* XXX */
 814                 pump_set(pump_num, PUMP_ON);
 815                 STMCH_DEBUG("%s prepare eject", __FUNCTION__);
 816                 mainboard_command.mode = PREPARE_EJECT;
 817                 state_do_eject(arm_num, pump_num, LAZY_PICKUP);
 818         }
 819 }
 820
 821 /* pickup from a dispenser automatically */
 822 static void state_do_pickup(void)
 823 {
 824         if (!state_check(PICKUP))
 825                 return;
 826
 827         if (get_free_pump_count() == 0) {
 828                 mainboard_command.mode = WAIT;
 829                 return;
 830         }
 831
 832         /* XXX check that finger is at correct place */
 833
 834         STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode());
 835
 836         state_pickup_or_harvest(PICKUP);
 837 }
 838
 839 /* store columns without using arms */
 840 static void state_do_store(void)
 841 {
 842         int8_t arm_num;
 843         int8_t other_arm_num;
 844         microseconds us;
 845
 846         if (!state_check(STORE))
 847                 return;
 848
 849         if (get_free_pump_count() == 0) {
 850                 mainboard_command.mode = WAIT;
 851                 return;
 852         }
 853
 854         STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode());
 855
 856         /* get arm num */
 857         if (pickup_side == I2C_LEFT_SIDE) {
 858                 arm_num = ARM_LEFT_NUM;
 859                 other_arm_num = ARM_RIGHT_NUM;
 860         }
 861         else {
 862                 arm_num = ARM_RIGHT_NUM;
 863                 other_arm_num = ARM_LEFT_NUM;
 864         }
 865
 866         while (1) {
 867                 if (sensor_get(S_FRONT))
 868                         break;
 869                 if (state_check(STORE) == 0)
 870                         return;
 871         }
 872
 873         /* when ready, move finger */
 874         if (arm_num == ARM_RIGHT_NUM)
 875                 finger_goto(FINGER_RIGHT);
 876         else
 877                 finger_goto(FINGER_LEFT);
 878
 879         /* wait to see the column on the sensor */
 880         us = time_get_us2();
 881         while (1) {
 882                 if (arm_get_sensor(arm_num))
 883                         break;
 884                 if (state_check(STORE) == 0)
 885                         return;
 886                 /* 500ms timeout in harvest, go back */
 887                 if (time_get_us2() - us > 500*1000L) {
 888                         STMCH_DEBUG("%s timeout", __FUNCTION__);
 889
 890                         if (arm_num == ARM_RIGHT_NUM)
 891                                 finger_goto(FINGER_LEFT);
 892                         else
 893                                 finger_goto(FINGER_RIGHT);
 894                         return;
 895                 }
 896         }
 897
 898         if (arm_get_sensor(arm_num) && arm_get_sensor(other_arm_num)) {
 899                 STMCH_DEBUG("%s full", __FUNCTION__);
 900                 while (state_check(STORE));
 901                 return;
 902         }
 903
 904         /* next store will be on the other side */
 905         if (pickup_side == I2C_LEFT_SIDE)
 906                 pickup_side = I2C_RIGHT_SIDE;
 907         else
 908                 pickup_side = I2C_LEFT_SIDE;
 909 }
 910
 911 /* prepare the building of a temple */
 912 static void state_do_prepare_build(void)
 913 {
 914         int8_t pump_num, level;
 915         if (!state_check(PREPARE_BUILD))
 916                 return;
 917         STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode());
 918
 919         pump_check_all();
 920
 921         if (finger_get_side() == I2C_LEFT_SIDE)
 922                 finger_goto(FINGER_LEFT);
 923         else
 924                 finger_goto(FINGER_RIGHT);
 925
 926         pump_num = arm_get_busy_pump(ARM_LEFT_NUM);
 927         level = mainboard_command.prep_build.level_l;
 928         if (pump_num != -1 && level != -1)
 929                 arm_goto_prepare_autobuild_outside(ARM_LEFT_NUM, pump_num,
 930                                                    level, I2C_AUTOBUILD_DEFAULT_DIST);
 931
 932         pump_num = arm_get_busy_pump(ARM_RIGHT_NUM);
 933         level = mainboard_command.prep_build.level_r;
 934         if (pump_num != -1 && level != -1)
 935                 arm_goto_prepare_autobuild_outside(ARM_RIGHT_NUM, pump_num,
 936                                                    level, I2C_AUTOBUILD_DEFAULT_DIST);
 937
 938         while (state_check(PREPARE_BUILD));
 939 }
 940
 941 /* prepare the building of a temple */
 942 static void state_do_push_temple(void)
 943 {
 944         uint8_t level;
 945
 946         level = mainboard_command.push_temple.level;
 947
 948         if (!state_check(PUSH_TEMPLE))
 949                 return;
 950         STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode());
 951
 952         if (finger_get_side() == I2C_LEFT_SIDE)
 953                 finger_goto(FINGER_LEFT);
 954         else
 955                 finger_goto(FINGER_RIGHT);
 956
 957         arm_goto_prepare_push_temple(ARM_LEFT_NUM);
 958         arm_goto_prepare_push_temple(ARM_RIGHT_NUM);
 959         arm_wait_both(ARM_TRAJ_ALL);
 960
 961         arm_goto_push_temple(ARM_LEFT_NUM, level);
 962         arm_goto_push_temple(ARM_RIGHT_NUM, level);
 963
 964         while (state_check(PUSH_TEMPLE));
 965 }
 966
 967 /* prepare the building of a temple */
 968 static void state_do_push_temple_disc(void)
 969 {
 970         uint8_t side;
 971         struct arm *arm;
 972
 973         if (!state_check(PUSH_TEMPLE_DISC))
 974                 return;
 975         STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode());
 976
 977         side = mainboard_command.push_temple_disc.side;
 978
 979         if (side == I2C_LEFT_SIDE) {
 980                 arm = arm_num2ptr(ARM_LEFT_NUM);
 981                 arm_goto_prepare_push_temple_disc(ARM_LEFT_NUM);
 982                 arm_wait_traj_end(arm, ARM_TRAJ_ALL);
 983                 arm_goto_push_temple_disc(ARM_LEFT_NUM);
 984         }
 985         else {
 986                 arm = arm_num2ptr(ARM_RIGHT_NUM);
 987                 arm_goto_prepare_push_temple_disc(ARM_RIGHT_NUM);
 988                 arm_wait_traj_end(arm, ARM_TRAJ_ALL);
 989                 arm_goto_push_temple_disc(ARM_RIGHT_NUM);
 990         }
 991
 992         while (state_check(PUSH_TEMPLE_DISC));
 993 }
 994
 995 /* prepare the building of a temple (mainly for columns) */
 996 static void state_do_prepare_inside(void)
 997 {
 998         int8_t pump_num, level_l, level_r;
 999         if (!state_check(PREPARE_INSIDE))
1000                 return;
1001
1002         level_l = mainboard_command.prep_inside.level_l;
1003         level_r = mainboard_command.prep_inside.level_r;
1004         STMCH_DEBUG("%s mode=%d level_l=%d, level_r=%d", __FUNCTION__,
1005                     state_get_mode(), level_l, level_r);
1006
1007         pump_check_all();
1008
1009         if (finger_get_side() == I2C_LEFT_SIDE)
1010                 finger_goto(FINGER_LEFT);
1011         else
1012                 finger_goto(FINGER_RIGHT);
1013
1014         pump_num = arm_get_busy_pump(ARM_LEFT_NUM);
1015         if (pump_num == -1)
1016                 pump_num = PUMP_LEFT1_NUM;
1017         if (level_l != -1)
1018                 arm_goto_prepare_build_inside(ARM_LEFT_NUM, pump_num,
1019                                                   level_l);
1020
1021         pump_num = arm_get_busy_pump(ARM_RIGHT_NUM);
1022         if (pump_num == -1)
1023                 pump_num = PUMP_RIGHT1_NUM;
1024         if (level_r != -1)
1025                 arm_goto_prepare_build_inside(ARM_RIGHT_NUM, pump_num,
1026                                                   level_r);
1027
1028         while (state_check(PREPARE_INSIDE));
1029 }
1030
1031 /* moving position */
1032 static void state_do_loaded(void)
1033 {
1034         if (!state_check(LOADED))
1035                 return;
1036         STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode());
1037
1038         pump_check_all();
1039
1040         if (finger_get_side() == I2C_LEFT_SIDE)
1041                 finger_goto(FINGER_LEFT);
1042         else
1043                 finger_goto(FINGER_RIGHT);
1044
1045         arm_goto_loaded(ARM_LEFT_NUM);
1046         arm_goto_loaded(ARM_RIGHT_NUM);
1047
1048         while (state_check(LOADED));
1049 }
1050
1051 static void state_do_build_lintel(uint8_t level)
1052 {
1053         STMCH_DEBUG("%s() level=%d have_lintel=%d",
1054                     __FUNCTION__, level, mechboard.lintel_count);
1055
1056         servo_lintel_out();
1057
1058         arm_goto_prepare_get_lintel_inside1();
1059         arm_wait_both(ARM_TRAJ_ALL);
1060         state_debug_wait_key_pressed();
1061
1062         pump_set(PUMP_LEFT1_NUM, PUMP_REVERSE);
1063         pump_set(PUMP_RIGHT1_NUM, PUMP_REVERSE);
1064         arm_goto_prepare_get_lintel_inside2(mechboard.lintel_count);
1065         arm_wait_both(ARM_TRAJ_ALL);
1066         state_debug_wait_key_pressed();
1067
1068         arm_goto_get_lintel_inside(mechboard.lintel_count);
1069         arm_wait_both(ARM_TRAJ_ALL);
1070         state_debug_wait_key_pressed();
1071
1072         time_wait_ms(150);
1073         arm_goto_prepare_build_lintel1();
1074         arm_wait_both(ARM_TRAJ_ALL);
1075         state_debug_wait_key_pressed();
1076
1077         arm_goto_prepare_build_lintel2(level);
1078         arm_wait_both(ARM_TRAJ_ALL);
1079         state_debug_wait_key_pressed();
1080
1081         arm_goto_prepare_build_lintel3(level);
1082         arm_wait_both(ARM_TRAJ_ALL);
1083         state_debug_wait_key_pressed();
1084
1085         if (mechboard.lintel_count == 1)
1086                 servo_lintel_1lin();
1087         else
1088                 servo_lintel_2lin();
1089
1090         arm_goto_build_lintel(level);
1091         arm_wait_both(ARM_TRAJ_ALL);
1092         time_wait_ms(170);
1093         pump_set(PUMP_LEFT1_NUM, PUMP_ON);
1094         time_wait_ms(50); /* right arm a bit after */
1095         pump_set(PUMP_RIGHT1_NUM, PUMP_ON);
1096         time_wait_ms(130);
1097         pump_set(PUMP_LEFT1_NUM, PUMP_OFF);
1098         pump_set(PUMP_RIGHT1_NUM, PUMP_OFF);
1099
1100         mechboard.lintel_count --;
1101 }
1102
1103 /* Build one level of column. If pump_r or pump_l is -1, don't build
1104  * with this arm. */
1105 static void state_do_build_column(uint8_t level_l, int8_t pump_l,
1106                                   uint8_t dist_l,
1107                                   uint8_t level_r, int8_t pump_r,
1108                                   uint8_t dist_r)
1109 {
1110         STMCH_DEBUG("%s() level_l=%d pump_l=%d level_r=%d pump_r=%d",
1111                     __FUNCTION__, level_l, pump_l, level_r, pump_r);
1112
1113         /* nothing to do */
1114         if (pump_l == -1 && pump_r == -1)
1115                 return;
1116
1117         /* go above the selected level */
1118         if (pump_l != -1)
1119                 arm_goto_prepare_autobuild_outside(ARM_LEFT_NUM, pump_l, level_l, dist_l);
1120         if (pump_r != -1)
1121                 arm_goto_prepare_autobuild_outside(ARM_RIGHT_NUM, pump_r, level_r, dist_r);
1122         STMCH_DEBUG("l=%d r=%d", arm_test_traj_end(&left_arm, ARM_TRAJ_ALL),
1123                     arm_test_traj_end(&right_arm, ARM_TRAJ_ALL));
1124         arm_wait_select(pump_l != -1, pump_r != -1, ARM_TRAJ_ALL);
1125         STMCH_DEBUG("l=%d r=%d", arm_test_traj_end(&left_arm, ARM_TRAJ_ALL),
1126                     arm_test_traj_end(&right_arm, ARM_TRAJ_ALL));
1127
1128         state_debug_wait_key_pressed();
1129
1130         /* drop columns of P2 */
1131         if (pump_l != -1)
1132                 arm_goto_autobuild(ARM_LEFT_NUM, pump_l, level_l, dist_l);
1133         if (pump_r != -1)
1134                 arm_goto_autobuild(ARM_RIGHT_NUM, pump_r, level_r, dist_r);
1135         arm_wait_select(pump_l != -1, pump_r != -1, ARM_TRAJ_ALL);
1136
1137         state_debug_wait_key_pressed();
1138
1139         time_wait_ms(150);
1140         if (pump_l != -1)
1141                 pump_set(pump_l, PUMP_REVERSE);
1142         if (pump_r != -1)
1143                 pump_set(pump_r, PUMP_REVERSE);
1144         time_wait_ms(150);
1145         if (pump_l != -1) {
1146                 pump_set(pump_l, PUMP_OFF);
1147                 pump_mark_free(pump_l);
1148         }
1149         if (pump_r != -1) {
1150                 pump_set(pump_r, PUMP_OFF);
1151                 pump_mark_free(pump_r);
1152         }
1153
1154         state_debug_wait_key_pressed();
1155 }
1156
1157 /* autobuild columns elts from area */
1158 /* check level to avoid bad things ? */
1159 /* check if enough cols ? */
1160 static void state_do_autobuild(void)
1161 {
1162         int8_t pump_l, pump_r;
1163         /* copy command into local data */
1164         int8_t level_l = mainboard_command.autobuild.level_left;
1165         int8_t level_r = mainboard_command.autobuild.level_right;
1166         uint8_t count_l = mainboard_command.autobuild.count_left;
1167         uint8_t count_r = mainboard_command.autobuild.count_right;
1168         uint8_t dist_l = mainboard_command.autobuild.distance_left;
1169         uint8_t dist_r = mainboard_command.autobuild.distance_right;
1170         uint8_t do_lintel = mainboard_command.autobuild.do_lintel;
1171         int8_t max_level = level_l;
1172
1173
1174         if (!state_check(AUTOBUILD))
1175                 return;
1176
1177         STMCH_DEBUG("%s mode=%d do_lintel=%d", __FUNCTION__,
1178                     state_get_mode(), do_lintel);
1179         STMCH_DEBUG("  left: level=%d count=%d", level_l, count_l);
1180         STMCH_DEBUG("  right: level=%d count=%d", level_r, count_r);
1181
1182         /*
1183          * build the first level of column if needed
1184          */
1185
1186         /* don't build with this arm if no pump or if we don't ask to */
1187         pump_l = arm_get_busy_pump(ARM_LEFT_NUM);
1188         if (count_l == 0)
1189                 pump_l = -1;
1190         pump_r = arm_get_busy_pump(ARM_RIGHT_NUM);
1191         if (count_r == 0)
1192                 pump_r = -1;
1193
1194         if  (pump_l == -1 && pump_r == -1)
1195                 goto lintel_only;
1196
1197         state_do_build_column(level_l, pump_l, dist_l,
1198                               level_r, pump_r, dist_r);
1199
1200         /* one level up */
1201         if (pump_l != -1) {
1202                 count_l --;
1203                 level_l ++;
1204                 max_level = level_l;
1205         }
1206         if (pump_r != -1) {
1207                 count_r --;
1208                 level_r ++;
1209                 if (level_r > max_level)
1210                         max_level = level_r;
1211         }
1212
1213         /*
1214          * build the second level of column if needed
1215          */
1216
1217         /* don't build with this arm if no pump or if we don't ask to */
1218         pump_l = arm_get_busy_pump(ARM_LEFT_NUM);
1219         if (count_l == 0)
1220                 pump_l = -1;
1221         pump_r = arm_get_busy_pump(ARM_RIGHT_NUM);
1222         if (count_r == 0)
1223                 pump_r = -1;
1224
1225         state_do_build_column(level_l, pump_l, dist_l,
1226                               level_r, pump_r, dist_r);
1227
1228         /* one level up */
1229         if (pump_l != -1) {
1230                 count_l --;
1231                 level_l ++;
1232                 max_level = level_l;
1233         }
1234         if (pump_r != -1) {
1235                 count_r --;
1236                 level_r ++;
1237                 if (level_r > max_level)
1238                         max_level = level_r;
1239         }
1240
1241         state_debug_wait_key_pressed();
1242
1243         if (mechboard.lintel_count != 0 && do_lintel != 0) {
1244                 arm_goto_prepare_autobuild_outside(ARM_LEFT_NUM,
1245                                                    PUMP_LEFT1_NUM,
1246                                                    max_level,
1247                                                    I2C_AUTOBUILD_DEFAULT_DIST);
1248                 arm_goto_prepare_autobuild_outside(ARM_RIGHT_NUM,
1249                                                    PUMP_RIGHT1_NUM,
1250                                                    max_level,
1251                                                    I2C_AUTOBUILD_DEFAULT_DIST);
1252                 arm_wait_both(ARM_TRAJ_ALL_NEAR);
1253                 state_debug_wait_key_pressed();
1254
1255                 arm_goto_prepare_autobuild_inside(ARM_LEFT_NUM,
1256                                                   PUMP_LEFT1_NUM,
1257                                                   max_level);
1258                 arm_goto_prepare_autobuild_inside(ARM_RIGHT_NUM,
1259                                                   PUMP_RIGHT1_NUM,
1260                                                   max_level);
1261                 arm_wait_both(ARM_TRAJ_ALL_NEAR);
1262                 state_debug_wait_key_pressed();
1263         }
1264
1265  lintel_only:
1266         if (mechboard.lintel_count == 0 || do_lintel == 0) {
1267                 mainboard_command.mode = WAIT;
1268                 return;
1269         }
1270
1271         state_do_build_lintel(max_level);
1272         mainboard_command.mode = WAIT;
1273 }
1274
1275 /* prepare to get the lintel */
1276 static void state_do_prepare_get_lintel(void)
1277 {
1278         if (!state_check(PREPARE_GET_LINTEL))
1279                 return;
1280         STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode());
1281
1282         arm_goto_prepare_get_lintel_disp();
1283         arm_wait_both(ARM_TRAJ_ALL);
1284
1285         pump_set(PUMP_LEFT1_NUM, PUMP_OFF);
1286         pump_set(PUMP_RIGHT1_NUM, PUMP_OFF);
1287
1288         /* go fully left or right */
1289         if (finger_get_side() == I2C_LEFT_SIDE)
1290                 finger_goto(FINGER_LEFT);
1291         else
1292                 finger_goto(FINGER_RIGHT);
1293
1294         while (state_check(PREPARE_GET_LINTEL));
1295 }
1296
1297 /* get the lintel from the dispenser */
1298 static void state_do_get_lintel(void)
1299 {
1300         if (!state_check(GET_LINTEL))
1301                 return;
1302         STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode());
1303
1304         pump_set(PUMP_LEFT1_NUM, PUMP_REVERSE);
1305         pump_set(PUMP_RIGHT1_NUM, PUMP_REVERSE);
1306
1307         arm_goto_get_lintel_disp();
1308         arm_wait_both(ARM_TRAJ_ALL_NEAR);
1309
1310         time_wait_ms(200);
1311
1312         STMCH_DEBUG("%s left1=%d left2=%d", __FUNCTION__,
1313                     mechboard.pump_left1_current,
1314                     sensor_get_adc(ADC_CSENSE3));
1315
1316         while (state_check(GET_LINTEL));
1317
1318         /* mainboard asked to release lintel, so release pump first */
1319         if (state_get_mode() == PREPARE_GET_LINTEL) {
1320                 pump_set(PUMP_LEFT1_NUM, PUMP_ON);
1321                 pump_set(PUMP_RIGHT1_NUM, PUMP_ON);
1322                 time_wait_ms(200);
1323                 pump_set(PUMP_LEFT1_NUM, PUMP_OFF);
1324                 pump_set(PUMP_RIGHT1_NUM, PUMP_OFF);
1325         }
1326 }
1327
1328 /* put the lintel inside the robot */
1329 static void state_do_put_lintel(void)
1330 {
1331         uint8_t prev_lin_count;
1332
1333         if (!state_check(PUT_LINTEL))
1334                 return;
1335
1336         STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode());
1337         prev_lin_count = mechboard.lintel_count;
1338         mechboard.lintel_count ++;
1339
1340         arm_goto_prepare_get_lintel_disp();
1341         arm_wait_both(ARM_TRAJ_ALL);
1342
1343         servo_lintel_out();
1344
1345         arm_goto_prepare_put_lintel();
1346         arm_wait_both(ARM_TRAJ_ALL_NEAR);
1347
1348         arm_goto_put_lintel(prev_lin_count);
1349         arm_wait_both(ARM_TRAJ_ALL);
1350
1351         pump_set(PUMP_LEFT1_NUM, PUMP_ON);
1352         pump_set(PUMP_RIGHT1_NUM, PUMP_ON);
1353
1354         if (mechboard.lintel_count == 1)
1355                 servo_lintel_1lin();
1356         else
1357                 servo_lintel_2lin();
1358
1359         time_wait_ms(300);
1360
1361         pump_set(PUMP_LEFT1_NUM, PUMP_OFF);
1362         pump_set(PUMP_RIGHT1_NUM, PUMP_OFF);
1363
1364         arm_goto_prepare_put_lintel();
1365         arm_wait_both(ARM_TRAJ_ALL_NEAR);
1366
1367         while (state_check(PUT_LINTEL));
1368 }
1369
1370 /* main state machine */
1371 void state_machine(void)
1372 {
1373         while (state_get_mode() != EXIT) {
1374                 changed = 0;
1375                 state_do_init();
1376                 state_do_manual();
1377                 state_do_harvest();
1378                 state_do_lazy_harvest();
1379                 state_do_prepare_pickup();
1380                 state_do_pickup();
1381                 state_do_prepare_inside();
1382                 state_do_prepare_build();
1383                 state_do_autobuild();
1384                 state_do_prepare_get_lintel();
1385                 state_do_get_lintel();
1386                 state_do_put_lintel();
1387                 state_do_loaded();
1388                 state_do_clear();
1389                 state_do_lazy_pickup();
1390                 state_do_wait();
1391                 state_do_store();
1392                 state_do_manivelle();
1393                 state_do_push_temple();
1394                 state_do_push_temple_disc();
1395         }
1396 }
1397
1398 void state_init(void)
1399 {
1400         vt100_init(&local_vt100);
1401         mainboard_command.mode = WAIT;
1402         pump_reset_all();
1403         mechboard.lintel_count = 1;
1404         mechboard.column_flags = 0;
1405         servo_lintel_1lin();
1406         finger_goto(FINGER_LEFT);
1407 }