projects/microb2010/tests/hostsim/commands_mainboard.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: commands_mainboard.c,v 1.9 2009-11-08 17:24:33 zer0 Exp $
  19  *
  20  *  Olivier MATZ <zer0@droids-corp.org>
  21  */
  22
  23 #include <stdio.h>
  24 #include <string.h>
  25
  26 #include <hostsim.h>
  27 #include <aversive/pgmspace.h>
  28 #include <aversive/wait.h>
  29 #include <aversive/error.h>
  30 #include <aversive/eeprom.h>
  31
  32 #include <uart.h>
  33 #include <clock_time.h>
  34
  35 #include <pid.h>
  36 #include <quadramp.h>
  37 #include <control_system_manager.h>
  38 #include <trajectory_manager.h>
  39 #include <vect_base.h>
  40 #include <lines.h>
  41 #include <polygon.h>
  42 #include <obstacle_avoidance.h>
  43 #include <blocking_detection_manager.h>
  44 #include <robot_system.h>
  45 #include <position_manager.h>
  46
  47 #include <rdline.h>
  48 #include <parse.h>
  49 #include <parse_string.h>
  50 #include <parse_num.h>
  51
  52 #include "main.h"
  53 #include "actuator.h"
  54 #include "robotsim.h"
  55 #include "cmdline.h"
  56 #include "strat.h"
  57 #include "strat_base.h"
  58 #include "strat_utils.h"
  59
  60 struct cmd_event_result {
  61         fixed_string_t arg0;
  62         fixed_string_t arg1;
  63         fixed_string_t arg2;
  64 };
  65
  66
  67 /* function called when cmd_event is parsed successfully */
  68 static void cmd_event_parsed(void *parsed_result, void *data)
  69 {
  70         u08 bit=0;
  71
  72         struct cmd_event_result * res = parsed_result;
  73
  74         if (!strcmp_P(res->arg1, PSTR("all"))) {
  75                 bit = DO_ENCODERS | DO_CS | DO_RS | DO_POS |
  76                         DO_BD | DO_TIMER | DO_POWER;
  77                 if (!strcmp_P(res->arg2, PSTR("on")))
  78                         mainboard.flags |= bit;
  79                 else if (!strcmp_P(res->arg2, PSTR("off")))
  80                         mainboard.flags &= bit;
  81                 else { /* show */
  82                         printf_P(PSTR("encoders is %s\r\n"),
  83                                  (DO_ENCODERS & mainboard.flags) ? "on":"off");
  84                         printf_P(PSTR("cs is %s\r\n"),
  85                                  (DO_CS & mainboard.flags) ? "on":"off");
  86                         printf_P(PSTR("rs is %s\r\n"),
  87                                  (DO_RS & mainboard.flags) ? "on":"off");
  88                         printf_P(PSTR("pos is %s\r\n"),
  89                                  (DO_POS & mainboard.flags) ? "on":"off");
  90                         printf_P(PSTR("bd is %s\r\n"),
  91                                  (DO_BD & mainboard.flags) ? "on":"off");
  92                         printf_P(PSTR("timer is %s\r\n"),
  93                                  (DO_TIMER & mainboard.flags) ? "on":"off");
  94                         printf_P(PSTR("power is %s\r\n"),
  95                                  (DO_POWER & mainboard.flags) ? "on":"off");
  96                 }
  97                 return;
  98         }
  99
 100         if (!strcmp_P(res->arg1, PSTR("encoders")))
 101                 bit = DO_ENCODERS;
 102         else if (!strcmp_P(res->arg1, PSTR("cs"))) {
 103                 strat_hardstop();
 104                 bit = DO_CS;
 105         }
 106         else if (!strcmp_P(res->arg1, PSTR("rs")))
 107                 bit = DO_RS;
 108         else if (!strcmp_P(res->arg1, PSTR("pos")))
 109                 bit = DO_POS;
 110         else if (!strcmp_P(res->arg1, PSTR("bd")))
 111                 bit = DO_BD;
 112         else if (!strcmp_P(res->arg1, PSTR("timer"))) {
 113                 time_reset();
 114                 bit = DO_TIMER;
 115         }
 116         else if (!strcmp_P(res->arg1, PSTR("power")))
 117                 bit = DO_POWER;
 118
 119         if (!strcmp_P(res->arg2, PSTR("on")))
 120                 mainboard.flags |= bit;
 121         else if (!strcmp_P(res->arg2, PSTR("off"))) {
 122                 if (!strcmp_P(res->arg1, PSTR("cs"))) {
 123 #ifdef HOST_VERSION
 124                         robotsim_pwm(LEFT_PWM, 0);
 125                         robotsim_pwm(RIGHT_PWM, 0);
 126 #else
 127                         pwm_ng_set(LEFT_PWM, 0);
 128                         pwm_ng_set(RIGHT_PWM, 0);
 129 #endif
 130                 }
 131                 mainboard.flags &= (~bit);
 132         }
 133         printf_P(PSTR("%s is %s\r\n"), res->arg1,
 134                       (bit & mainboard.flags) ? "on":"off");
 135 }
 136
 137 prog_char str_event_arg0[] = "event";
 138 parse_pgm_token_string_t cmd_event_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg0, str_event_arg0);
 139 prog_char str_event_arg1[] = "all#encoders#cs#rs#pos#bd#timer#power";
 140 parse_pgm_token_string_t cmd_event_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg1, str_event_arg1);
 141 prog_char str_event_arg2[] = "on#off#show";
 142 parse_pgm_token_string_t cmd_event_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg2, str_event_arg2);
 143
 144 prog_char help_event[] = "Enable/disable events";
 145 parse_pgm_inst_t cmd_event = {
 146         .f = cmd_event_parsed,  /* function to call */
 147         .data = NULL,      /* 2nd arg of func */
 148         .help_str = help_event,
 149         .tokens = {        /* token list, NULL terminated */
 150                 (prog_void *)&cmd_event_arg0,
 151                 (prog_void *)&cmd_event_arg1,
 152                 (prog_void *)&cmd_event_arg2,
 153                 NULL,
 154         },
 155 };
 156
 157
 158 /**********************************************************/
 159 /* Spi_Test */
 160
 161 /* this structure is filled when cmd_spi_test is parsed successfully */
 162 struct cmd_spi_test_result {
 163         fixed_string_t arg0;
 164 };
 165
 166 /* function called when cmd_spi_test is parsed successfully */
 167 static void cmd_spi_test_parsed(void * parsed_result, void * data)
 168 {
 169 #ifdef HOST_VERSION
 170         printf("not implemented\n");
 171 #else
 172         uint16_t i = 0, ret = 0, ret2 = 0;
 173
 174         if (mainboard.flags & DO_ENCODERS) {
 175                 printf_P(PSTR("Disable encoder event first\r\n"));
 176                 return;
 177         }
 178
 179         do {
 180                 spi_slave_select(0);
 181                 ret = spi_send_and_receive_byte(i);
 182                 ret2 = spi_send_and_receive_byte(i);
 183                 spi_slave_deselect(0);
 184
 185                 if ((i & 0x7ff) == 0)
 186                         printf_P(PSTR("Sent %.4x twice, received %x %x\r\n"),
 187                                  i, ret, ret2);
 188
 189                 i++;
 190         } while(!cmdline_keypressed());
 191 #endif
 192 }
 193
 194 prog_char str_spi_test_arg0[] = "spi_test";
 195 parse_pgm_token_string_t cmd_spi_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_spi_test_result, arg0, str_spi_test_arg0);
 196
 197 prog_char help_spi_test[] = "Test the SPI";
 198 parse_pgm_inst_t cmd_spi_test = {
 199         .f = cmd_spi_test_parsed,  /* function to call */
 200         .data = NULL,      /* 2nd arg of func */
 201         .help_str = help_spi_test,
 202         .tokens = {        /* token list, NULL terminated */
 203                 (prog_void *)&cmd_spi_test_arg0,
 204                 NULL,
 205         },
 206 };
 207
 208
 209
 210 /**********************************************************/
 211 /* Opponent tests */
 212
 213 /* this structure is filled when cmd_opponent is parsed successfully */
 214 struct cmd_opponent_result {
 215         fixed_string_t arg0;
 216         fixed_string_t arg1;
 217         int32_t arg2;
 218         int32_t arg3;
 219 };
 220
 221 /* function called when cmd_opponent is parsed successfully */
 222 static void cmd_opponent_parsed(void *parsed_result, void *data)
 223 {
 224         int16_t x,y,d,a;
 225
 226         if (get_opponent_xyda(&x, &y, &d, &a))
 227                 printf_P(PSTR("No opponent\r\n"));
 228         else
 229                 printf_P(PSTR("x=%d y=%d, d=%d a=%d\r\n"), x, y, d, a);
 230 }
 231
 232 prog_char str_opponent_arg0[] = "opponent";
 233 parse_pgm_token_string_t cmd_opponent_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_opponent_result, arg0, str_opponent_arg0);
 234 prog_char str_opponent_arg1[] = "show";
 235 parse_pgm_token_string_t cmd_opponent_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_opponent_result, arg1, str_opponent_arg1);
 236
 237 prog_char help_opponent[] = "Show (x,y) opponent";
 238 parse_pgm_inst_t cmd_opponent = {
 239         .f = cmd_opponent_parsed,  /* function to call */
 240         .data = NULL,      /* 2nd arg of func */
 241         .help_str = help_opponent,
 242         .tokens = {        /* token list, NULL terminated */
 243                 (prog_void *)&cmd_opponent_arg0,
 244                 (prog_void *)&cmd_opponent_arg1,
 245                 NULL,
 246         },
 247 };
 248
 249
 250 prog_char str_opponent_arg1_set[] = "set";
 251 parse_pgm_token_string_t cmd_opponent_arg1_set = TOKEN_STRING_INITIALIZER(struct cmd_opponent_result, arg1, str_opponent_arg1_set);
 252 parse_pgm_token_num_t cmd_opponent_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_opponent_result, arg2, INT32);
 253 parse_pgm_token_num_t cmd_opponent_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_opponent_result, arg3, INT32);
 254
 255 prog_char help_opponent_set[] = "Set (x,y) opponent";
 256 parse_pgm_inst_t cmd_opponent_set = {
 257         .f = cmd_opponent_parsed,  /* function to call */
 258         .data = NULL,      /* 2nd arg of func */
 259         .help_str = help_opponent_set,
 260         .tokens = {        /* token list, NULL terminated */
 261                 (prog_void *)&cmd_opponent_arg0,
 262                 (prog_void *)&cmd_opponent_arg1_set,
 263                 (prog_void *)&cmd_opponent_arg2,
 264                 (prog_void *)&cmd_opponent_arg3,
 265                 NULL,
 266         },
 267 };
 268
 269
 270 /**********************************************************/
 271 /* Start */
 272
 273 /* this structure is filled when cmd_start is parsed successfully */
 274 struct cmd_start_result {
 275         fixed_string_t arg0;
 276         fixed_string_t color;
 277         fixed_string_t debug;
 278 };
 279
 280 /* function called when cmd_start is parsed successfully */
 281 static void cmd_start_parsed(void *parsed_result, void *data)
 282 {
 283 #ifdef HOST_VERSION
 284         printf("not implemented\n");
 285 #else
 286         struct cmd_start_result *res = parsed_result;
 287         uint8_t old_level = gen.log_level;
 288
 289         gen.logs[NB_LOGS] = E_USER_STRAT;
 290         if (!strcmp_P(res->debug, PSTR("debug"))) {
 291                 strat_infos.dump_enabled = 1;
 292                 gen.log_level = 5;
 293         }
 294         else {
 295                 strat_infos.dump_enabled = 0;
 296                 gen.log_level = 0;
 297         }
 298
 299         if (!strcmp_P(res->color, PSTR("red"))) {
 300                 mainboard.our_color = I2C_COLOR_RED;
 301                 i2c_set_color(I2C_MECHBOARD_ADDR, I2C_COLOR_RED);
 302                 i2c_set_color(I2C_SENSORBOARD_ADDR, I2C_COLOR_RED);
 303         }
 304         else if (!strcmp_P(res->color, PSTR("green"))) {
 305                 mainboard.our_color = I2C_COLOR_GREEN;
 306                 i2c_set_color(I2C_MECHBOARD_ADDR, I2C_COLOR_GREEN);
 307                 i2c_set_color(I2C_SENSORBOARD_ADDR, I2C_COLOR_GREEN);
 308         }
 309
 310         printf_P(PSTR("Check that lintel is loaded\r\n"));
 311         while(!cmdline_keypressed());
 312
 313         printf_P(PSTR("Press a key when beacon ready\r\n"));
 314         i2c_sensorboard_set_beacon(0);
 315         while(!cmdline_keypressed());
 316         i2c_sensorboard_set_beacon(1);
 317
 318         strat_start();
 319
 320         gen.logs[NB_LOGS] = 0;
 321         gen.log_level = old_level;
 322 #endif
 323 }
 324
 325 prog_char str_start_arg0[] = "start";
 326 parse_pgm_token_string_t cmd_start_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_start_result, arg0, str_start_arg0);
 327 prog_char str_start_color[] = "green#red";
 328 parse_pgm_token_string_t cmd_start_color = TOKEN_STRING_INITIALIZER(struct cmd_start_result, color, str_start_color);
 329 prog_char str_start_debug[] = "debug#match";
 330 parse_pgm_token_string_t cmd_start_debug = TOKEN_STRING_INITIALIZER(struct cmd_start_result, debug, str_start_debug);
 331
 332 prog_char help_start[] = "Start the robot";
 333 parse_pgm_inst_t cmd_start = {
 334         .f = cmd_start_parsed,  /* function to call */
 335         .data = NULL,      /* 2nd arg of func */
 336         .help_str = help_start,
 337         .tokens = {        /* token list, NULL terminated */
 338                 (prog_void *)&cmd_start_arg0,
 339                 (prog_void *)&cmd_start_color,
 340                 (prog_void *)&cmd_start_debug,
 341                 NULL,
 342         },
 343 };
 344
 345
 346
 347 /**********************************************************/
 348 /* Interact */
 349
 350 /* this structure is filled when cmd_interact is parsed successfully */
 351 struct cmd_interact_result {
 352         fixed_string_t arg0;
 353 };
 354
 355 static void print_cs(void)
 356 {
 357         printf_P(PSTR("cons_d=% .8"PRIi32" cons_a=% .8"PRIi32" fil_d=% .8"PRIi32" fil_a=% .8"PRIi32" "
 358                       "err_d=% .8"PRIi32" err_a=% .8"PRIi32" out_d=% .8"PRIi32" out_a=% .8"PRIi32"\r\n"),
 359                  cs_get_consign(&mainboard.distance.cs),
 360                  cs_get_consign(&mainboard.angle.cs),
 361                  cs_get_filtered_consign(&mainboard.distance.cs),
 362                  cs_get_filtered_consign(&mainboard.angle.cs),
 363                  cs_get_error(&mainboard.distance.cs),
 364                  cs_get_error(&mainboard.angle.cs),
 365                  cs_get_out(&mainboard.distance.cs),
 366                  cs_get_out(&mainboard.angle.cs));
 367 }
 368
 369 static void print_pos(void)
 370 {
 371         printf_P(PSTR("x=% .8d y=% .8d a=% .8d\r\n"),
 372                  position_get_x_s16(&mainboard.pos),
 373                  position_get_y_s16(&mainboard.pos),
 374                  position_get_a_deg_s16(&mainboard.pos));
 375 }
 376
 377 static void print_time(void)
 378 {
 379         printf_P(PSTR("time %d\r\n"),time_get_s());
 380 }
 381
 382
 383 static void print_sensors(void)
 384 {
 385 #ifdef notyet
 386         if (sensor_start_switch())
 387                 printf_P(PSTR("Start switch | "));
 388         else
 389                 printf_P(PSTR("             | "));
 390
 391         if (IR_DISP_SENSOR())
 392                 printf_P(PSTR("IR disp | "));
 393         else
 394                 printf_P(PSTR("        | "));
 395
 396         printf_P(PSTR("\r\n"));
 397 #endif
 398 }
 399
 400 static void print_pid(void)
 401 {
 402         printf_P(PSTR("P=% .8"PRIi32" I=% .8"PRIi32" D=% .8"PRIi32" out=% .8"PRIi32" | "
 403                       "P=% .8"PRIi32" I=% .8"PRIi32" D=% .8"PRIi32" out=% .8"PRIi32"\r\n"),
 404                  pid_get_value_in(&mainboard.distance.pid) * pid_get_gain_P(&mainboard.distance.pid),
 405                  pid_get_value_I(&mainboard.distance.pid) * pid_get_gain_I(&mainboard.distance.pid),
 406                  pid_get_value_D(&mainboard.distance.pid) * pid_get_gain_D(&mainboard.distance.pid),
 407                  pid_get_value_out(&mainboard.distance.pid),
 408                  pid_get_value_in(&mainboard.angle.pid) * pid_get_gain_P(&mainboard.angle.pid),
 409                  pid_get_value_I(&mainboard.angle.pid) * pid_get_gain_I(&mainboard.angle.pid),
 410                  pid_get_value_D(&mainboard.angle.pid) * pid_get_gain_D(&mainboard.angle.pid),
 411                  pid_get_value_out(&mainboard.angle.pid));
 412 }
 413
 414 #define PRINT_POS       (1<<0)
 415 #define PRINT_PID       (1<<1)
 416 #define PRINT_CS        (1<<2)
 417 #define PRINT_SENSORS   (1<<3)
 418 #define PRINT_TIME      (1<<4)
 419 #define PRINT_BLOCKING  (1<<5)
 420
 421 static void cmd_interact_parsed(void * parsed_result, void * data)
 422 {
 423         int c;
 424         int8_t cmd;
 425         uint8_t print = 0;
 426         struct vt100 vt100;
 427
 428         vt100_init(&vt100);
 429
 430         printf_P(PSTR("Display debugs:\r\n"
 431                       "  1:pos\r\n"
 432                       "  2:pid\r\n"
 433                       "  3:cs\r\n"
 434                       "  4:sensors\r\n"
 435                       "  5:time\r\n"
 436                       /* "  6:blocking\r\n" */
 437                       "Commands:\r\n"
 438                       "  arrows:move\r\n"
 439                       "  space:stop\r\n"
 440                       "  q:quit\r\n"));
 441
 442         /* stop motors */
 443         mainboard.flags &= (~DO_CS);
 444         pwm_set_and_save(LEFT_PWM, 0);
 445         pwm_set_and_save(RIGHT_PWM, 0);
 446
 447         while(1) {
 448                 if (print & PRINT_POS) {
 449                         print_pos();
 450                 }
 451
 452                 if (print & PRINT_PID) {
 453                         print_pid();
 454                 }
 455
 456                 if (print & PRINT_CS) {
 457                         print_cs();
 458                 }
 459
 460                 if (print & PRINT_SENSORS) {
 461                         print_sensors();
 462                 }
 463
 464                 if (print & PRINT_TIME) {
 465                         print_time();
 466                 }
 467 /*              if (print & PRINT_BLOCKING) { */
 468 /*                      printf_P(PSTR("%s %s blocking=%d\r\n"),  */
 469 /*                               mainboard.blocking ? "BLOCK1":"      ", */
 470 /*                               rs_is_blocked(&mainboard.rs) ? "BLOCK2":"      ", */
 471 /*                               rs_get_blocking(&mainboard.rs)); */
 472 /*              } */
 473
 474                 c = cmdline_getchar();
 475                 if (c == -1) {
 476                         wait_ms(10);
 477                         continue;
 478                 }
 479                 cmd = vt100_parser(&vt100, c);
 480                 if (cmd == -2) {
 481                         wait_ms(10);
 482                         continue;
 483                 }
 484
 485                 if (cmd == -1) {
 486                         switch(c) {
 487                         case '1': print ^= PRINT_POS; break;
 488                         case '2': print ^= PRINT_PID; break;
 489                         case '3': print ^= PRINT_CS; break;
 490                         case '4': print ^= PRINT_SENSORS; break;
 491                         case '5': print ^= PRINT_TIME; break;
 492                         case '6': print ^= PRINT_BLOCKING; break;
 493
 494                         case 'q':
 495                                 if (mainboard.flags & DO_CS)
 496                                         strat_hardstop();
 497                                 pwm_set_and_save(LEFT_PWM, 0);
 498                                 pwm_set_and_save(RIGHT_PWM, 0);
 499                                 return;
 500                         case ' ':
 501                                 pwm_set_and_save(LEFT_PWM, 0);
 502                                 pwm_set_and_save(RIGHT_PWM, 0);
 503                                 break;
 504                         default:
 505                                 break;
 506                         }
 507                 }
 508                 else {
 509                         switch(cmd) {
 510                         case KEY_UP_ARR:
 511                                 pwm_set_and_save(LEFT_PWM, 1200);
 512                                 pwm_set_and_save(RIGHT_PWM, 1200);
 513                                 break;
 514                         case KEY_LEFT_ARR:
 515                                 pwm_set_and_save(LEFT_PWM, -1200);
 516                                 pwm_set_and_save(RIGHT_PWM, 1200);
 517                                 break;
 518                         case KEY_DOWN_ARR:
 519                                 pwm_set_and_save(LEFT_PWM, -1200);
 520                                 pwm_set_and_save(RIGHT_PWM, -1200);
 521                                 break;
 522                         case KEY_RIGHT_ARR:
 523                                 pwm_set_and_save(LEFT_PWM, 1200);
 524                                 pwm_set_and_save(RIGHT_PWM, -1200);
 525                                 break;
 526                         }
 527                 }
 528                 wait_ms(10);
 529         }
 530 }
 531
 532 prog_char str_interact_arg0[] = "interact";
 533 parse_pgm_token_string_t cmd_interact_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_interact_result, arg0, str_interact_arg0);
 534
 535 prog_char help_interact[] = "Interactive mode";
 536 parse_pgm_inst_t cmd_interact = {
 537         .f = cmd_interact_parsed,  /* function to call */
 538         .data = NULL,      /* 2nd arg of func */
 539         .help_str = help_interact,
 540         .tokens = {        /* token list, NULL terminated */
 541                 (prog_void *)&cmd_interact_arg0,
 542                 NULL,
 543         },
 544 };
 545
 546
 547 /**********************************************************/
 548 /* Color */
 549
 550 /* this structure is filled when cmd_color is parsed successfully */
 551 struct cmd_color_result {
 552         fixed_string_t arg0;
 553         fixed_string_t color;
 554 };
 555
 556 /* function called when cmd_color is parsed successfully */
 557 static void cmd_color_parsed(void *parsed_result, void *data)
 558 {
 559 #ifdef HOST_VERSION
 560         printf("not implemented\n");
 561 #else
 562         struct cmd_color_result *res = (struct cmd_color_result *) parsed_result;
 563         if (!strcmp_P(res->color, PSTR("red"))) {
 564                 mainboard.our_color = I2C_COLOR_RED;
 565                 i2c_set_color(I2C_MECHBOARD_ADDR, I2C_COLOR_RED);
 566                 i2c_set_color(I2C_SENSORBOARD_ADDR, I2C_COLOR_RED);
 567         }
 568         else if (!strcmp_P(res->color, PSTR("green"))) {
 569                 mainboard.our_color = I2C_COLOR_GREEN;
 570                 i2c_set_color(I2C_MECHBOARD_ADDR, I2C_COLOR_GREEN);
 571                 i2c_set_color(I2C_SENSORBOARD_ADDR, I2C_COLOR_GREEN);
 572         }
 573         printf_P(PSTR("Done\r\n"));
 574 #endif
 575 }
 576
 577 prog_char str_color_arg0[] = "color";
 578 parse_pgm_token_string_t cmd_color_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_color_result, arg0, str_color_arg0);
 579 prog_char str_color_color[] = "green#red";
 580 parse_pgm_token_string_t cmd_color_color = TOKEN_STRING_INITIALIZER(struct cmd_color_result, color, str_color_color);
 581
 582 prog_char help_color[] = "Set our color";
 583 parse_pgm_inst_t cmd_color = {
 584         .f = cmd_color_parsed,  /* function to call */
 585         .data = NULL,      /* 2nd arg of func */
 586         .help_str = help_color,
 587         .tokens = {        /* token list, NULL terminated */
 588                 (prog_void *)&cmd_color_arg0,
 589                 (prog_void *)&cmd_color_color,
 590                 NULL,
 591         },
 592 };
 593
 594
 595 /**********************************************************/
 596 /* Rs tests */
 597
 598 /* this structure is filled when cmd_rs is parsed successfully */
 599 struct cmd_rs_result {
 600         fixed_string_t arg0;
 601         fixed_string_t arg1;
 602 };
 603
 604 /* function called when cmd_rs is parsed successfully */
 605 static void cmd_rs_parsed(void *parsed_result, void *data)
 606 {
 607         //      struct cmd_rs_result *res = parsed_result;
 608         do {
 609                 printf_P(PSTR("angle cons=% .6"PRIi32" in=% .6"PRIi32" out=% .6"PRIi32" / "),
 610                          cs_get_consign(&mainboard.angle.cs),
 611                          cs_get_filtered_feedback(&mainboard.angle.cs),
 612                          cs_get_out(&mainboard.angle.cs));
 613                 printf_P(PSTR("distance cons=% .6"PRIi32" in=% .6"PRIi32" out=% .6"PRIi32" / "),
 614                          cs_get_consign(&mainboard.distance.cs),
 615                          cs_get_filtered_feedback(&mainboard.distance.cs),
 616                          cs_get_out(&mainboard.distance.cs));
 617                 printf_P(PSTR("l=% .4"PRIi32" r=% .4"PRIi32"\r\n"), mainboard.pwm_l,
 618                          mainboard.pwm_r);
 619                 wait_ms(100);
 620         } while(!cmdline_keypressed());
 621 }
 622
 623 prog_char str_rs_arg0[] = "rs";
 624 parse_pgm_token_string_t cmd_rs_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_rs_result, arg0, str_rs_arg0);
 625 prog_char str_rs_arg1[] = "show";
 626 parse_pgm_token_string_t cmd_rs_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_rs_result, arg1, str_rs_arg1);
 627
 628 prog_char help_rs[] = "Show rs (robot system) values";
 629 parse_pgm_inst_t cmd_rs = {
 630         .f = cmd_rs_parsed,  /* function to call */
 631         .data = NULL,      /* 2nd arg of func */
 632         .help_str = help_rs,
 633         .tokens = {        /* token list, NULL terminated */
 634                 (prog_void *)&cmd_rs_arg0,
 635                 (prog_void *)&cmd_rs_arg1,
 636                 NULL,
 637         },
 638 };
 639
 640 /**********************************************************/
 641 /* I2cdebug */
 642
 643 /* this structure is filled when cmd_i2cdebug is parsed successfully */
 644 struct cmd_i2cdebug_result {
 645         fixed_string_t arg0;
 646 };
 647
 648 /* function called when cmd_i2cdebug is parsed successfully */
 649 static void cmd_i2cdebug_parsed(void * parsed_result, void * data)
 650 {
 651 #ifdef HOST_VERSION
 652         printf("not implemented\n");
 653 #else
 654         i2c_debug();
 655         i2c_protocol_debug();
 656 #endif
 657 }
 658
 659 prog_char str_i2cdebug_arg0[] = "i2cdebug";
 660 parse_pgm_token_string_t cmd_i2cdebug_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_i2cdebug_result, arg0, str_i2cdebug_arg0);
 661
 662 prog_char help_i2cdebug[] = "I2c debug infos";
 663 parse_pgm_inst_t cmd_i2cdebug = {
 664         .f = cmd_i2cdebug_parsed,  /* function to call */
 665         .data = NULL,      /* 2nd arg of func */
 666         .help_str = help_i2cdebug,
 667         .tokens = {        /* token list, NULL terminated */
 668                 (prog_void *)&cmd_i2cdebug_arg0,
 669                 NULL,
 670         },
 671 };
 672
 673 /**********************************************************/
 674 /* Mechboard_Show */
 675
 676 /* this structure is filled when cmd_mechboard_show is parsed successfully */
 677 struct cmd_mechboard_show_result {
 678         fixed_string_t arg0;
 679         fixed_string_t arg1;
 680 };
 681
 682 /* function called when cmd_mechboard_show is parsed successfully */
 683 static void cmd_mechboard_show_parsed(void * parsed_result, void * data)
 684 {
 685 #ifdef HOST_VERSION
 686         printf("not implemented\n");
 687 #else
 688         printf_P(PSTR("mode = %x\r\n"), mechboard.mode);
 689         printf_P(PSTR("status = %x\r\n"), mechboard.status);
 690         printf_P(PSTR("lintel_count = %d\r\n"), mechboard.lintel_count);
 691
 692         printf_P(PSTR("column_count = %d\r\n"), get_column_count());
 693         printf_P(PSTR("left1=%d left2=%d right1=%d right2=%d\r\n"),
 694                  pump_left1_is_full(), pump_left2_is_full(),
 695                  pump_right1_is_full(), pump_right2_is_full());
 696
 697         printf_P(PSTR("pump_left1 = %d\r\n"), mechboard.pump_left1);
 698         printf_P(PSTR("pump_left2 = %d\r\n"), mechboard.pump_left2);
 699         printf_P(PSTR("pump_right1 = %d\r\n"), mechboard.pump_right1);
 700         printf_P(PSTR("pump_right2 = %d\r\n"), mechboard.pump_right2);
 701
 702         printf_P(PSTR("servo_lintel_left = %d\r\n"), mechboard.servo_lintel_left);
 703         printf_P(PSTR("servo_lintel_right = %d\r\n"), mechboard.servo_lintel_right);
 704
 705 #endif
 706 }
 707
 708 prog_char str_mechboard_show_arg0[] = "mechboard";
 709 parse_pgm_token_string_t cmd_mechboard_show_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_show_result, arg0, str_mechboard_show_arg0);
 710 prog_char str_mechboard_show_arg1[] = "show";
 711 parse_pgm_token_string_t cmd_mechboard_show_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_show_result, arg1, str_mechboard_show_arg1);
 712
 713 prog_char help_mechboard_show[] = "show mechboard status";
 714 parse_pgm_inst_t cmd_mechboard_show = {
 715         .f = cmd_mechboard_show_parsed,  /* function to call */
 716         .data = NULL,      /* 2nd arg of func */
 717         .help_str = help_mechboard_show,
 718         .tokens = {        /* token list, NULL terminated */
 719                 (prog_void *)&cmd_mechboard_show_arg0,
 720                 (prog_void *)&cmd_mechboard_show_arg1,
 721                 NULL,
 722         },
 723 };
 724
 725 /**********************************************************/
 726 /* Mechboard_Setmode1 */
 727
 728 /* this structure is filled when cmd_mechboard_setmode1 is parsed successfully */
 729 struct cmd_mechboard_setmode1_result {
 730         fixed_string_t arg0;
 731         fixed_string_t arg1;
 732 };
 733
 734 /* function called when cmd_mechboard_setmode1 is parsed successfully */
 735 static void cmd_mechboard_setmode1_parsed(void *parsed_result, void *data)
 736 {
 737 #ifdef HOST_VERSION
 738         printf("not implemented\n");
 739 #else
 740         struct cmd_mechboard_setmode1_result *res = parsed_result;
 741
 742         if (!strcmp_P(res->arg1, PSTR("init")))
 743                 i2c_mechboard_mode_init();
 744         else if (!strcmp_P(res->arg1, PSTR("manual")))
 745                 i2c_mechboard_mode_manual();
 746         else if (!strcmp_P(res->arg1, PSTR("pickup")))
 747                 i2c_mechboard_mode_pickup();
 748         else if (!strcmp_P(res->arg1, PSTR("lazy_harvest")))
 749                 i2c_mechboard_mode_lazy_harvest();
 750         else if (!strcmp_P(res->arg1, PSTR("harvest")))
 751                 i2c_mechboard_mode_harvest();
 752         else if (!strcmp_P(res->arg1, PSTR("prepare_get_lintel")))
 753                 i2c_mechboard_mode_prepare_get_lintel();
 754         else if (!strcmp_P(res->arg1, PSTR("get_lintel")))
 755                 i2c_mechboard_mode_get_lintel();
 756         else if (!strcmp_P(res->arg1, PSTR("put_lintel")))
 757                 i2c_mechboard_mode_put_lintel();
 758         else if (!strcmp_P(res->arg1, PSTR("init")))
 759                 i2c_mechboard_mode_init();
 760         else if (!strcmp_P(res->arg1, PSTR("eject")))
 761                 i2c_mechboard_mode_init();
 762         else if (!strcmp_P(res->arg1, PSTR("clear")))
 763                 i2c_mechboard_mode_clear();
 764         else if (!strcmp_P(res->arg1, PSTR("loaded")))
 765                 i2c_mechboard_mode_loaded();
 766         else if (!strcmp_P(res->arg1, PSTR("store")))
 767                 i2c_mechboard_mode_store();
 768         else if (!strcmp_P(res->arg1, PSTR("manivelle")))
 769                 i2c_mechboard_mode_manivelle();
 770         else if (!strcmp_P(res->arg1, PSTR("lazy_pickup")))
 771                 i2c_mechboard_mode_lazy_pickup();
 772 #endif
 773 }
 774
 775 prog_char str_mechboard_setmode1_arg0[] = "mechboard";
 776 parse_pgm_token_string_t cmd_mechboard_setmode1_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode1_result, arg0, str_mechboard_setmode1_arg0);
 777 prog_char str_mechboard_setmode1_arg1[] = "manivelle#init#manual#pickup#prepare_get_lintel#get_lintel#put_lintel1#eject#clear#harvest#lazy_harvest#store#lazy_pickup";
 778 parse_pgm_token_string_t cmd_mechboard_setmode1_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode1_result, arg1, str_mechboard_setmode1_arg1);
 779
 780 prog_char help_mechboard_setmode1[] = "set mechboard mode (mode)";
 781 parse_pgm_inst_t cmd_mechboard_setmode1 = {
 782         .f = cmd_mechboard_setmode1_parsed,  /* function to call */
 783         .data = NULL,      /* 2nd arg of func */
 784         .help_str = help_mechboard_setmode1,
 785         .tokens = {        /* token list, NULL terminated */
 786                 (prog_void *)&cmd_mechboard_setmode1_arg0,
 787                 (prog_void *)&cmd_mechboard_setmode1_arg1,
 788                 NULL,
 789         },
 790 };
 791
 792 /**********************************************************/
 793 /* Mechboard_Setmode2 */
 794
 795 /* this structure is filled when cmd_mechboard_setmode2 is parsed successfully */
 796 struct cmd_mechboard_setmode2_result {
 797         fixed_string_t arg0;
 798         fixed_string_t arg1;
 799         fixed_string_t arg2;
 800 };
 801
 802 /* function called when cmd_mechboard_setmode2 is parsed successfully */
 803 static void cmd_mechboard_setmode2_parsed(void * parsed_result, void * data)
 804 {
 805 #ifdef HOST_VERSION
 806         printf("not implemented\n");
 807 #else
 808         struct cmd_mechboard_setmode2_result *res = parsed_result;
 809         uint8_t side = I2C_LEFT_SIDE;
 810
 811         if (!strcmp_P(res->arg2, PSTR("left")))
 812                 side = I2C_LEFT_SIDE;
 813         else if (!strcmp_P(res->arg2, PSTR("right")))
 814                 side = I2C_RIGHT_SIDE;
 815         else if (!strcmp_P(res->arg2, PSTR("center")))
 816                 side = I2C_CENTER_SIDE;
 817         else if (!strcmp_P(res->arg2, PSTR("auto")))
 818                 side = I2C_AUTO_SIDE;
 819
 820         if (!strcmp_P(res->arg1, PSTR("prepare_pickup")))
 821                 i2c_mechboard_mode_prepare_pickup(side);
 822         else if (!strcmp_P(res->arg1, PSTR("push_temple_disc")))
 823                 i2c_mechboard_mode_push_temple_disc(side);
 824 #endif
 825 }
 826
 827 prog_char str_mechboard_setmode2_arg0[] = "mechboard";
 828 parse_pgm_token_string_t cmd_mechboard_setmode2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode2_result, arg0, str_mechboard_setmode2_arg0);
 829 prog_char str_mechboard_setmode2_arg1[] = "prepare_pickup#push_temple_disc";
 830 parse_pgm_token_string_t cmd_mechboard_setmode2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode2_result, arg1, str_mechboard_setmode2_arg1);
 831 prog_char str_mechboard_setmode2_arg2[] = "left#right#auto#center";
 832 parse_pgm_token_string_t cmd_mechboard_setmode2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode2_result, arg2, str_mechboard_setmode2_arg2);
 833
 834 prog_char help_mechboard_setmode2[] = "set mechboard mode (more, side)";
 835 parse_pgm_inst_t cmd_mechboard_setmode2 = {
 836         .f = cmd_mechboard_setmode2_parsed,  /* function to call */
 837         .data = NULL,      /* 2nd arg of func */
 838         .help_str = help_mechboard_setmode2,
 839         .tokens = {        /* token list, NULL terminated */
 840                 (prog_void *)&cmd_mechboard_setmode2_arg0,
 841                 (prog_void *)&cmd_mechboard_setmode2_arg1,
 842                 (prog_void *)&cmd_mechboard_setmode2_arg2,
 843                 NULL,
 844         },
 845 };
 846
 847 /**********************************************************/
 848 /* Mechboard_Setmode3 */
 849
 850 /* this structure is filled when cmd_mechboard_setmode3 is parsed successfully */
 851 struct cmd_mechboard_setmode3_result {
 852         fixed_string_t arg0;
 853         fixed_string_t arg1;
 854         uint8_t level;
 855 };
 856
 857 /* function called when cmd_mechboard_setmode3 is parsed successfully */
 858 static void cmd_mechboard_setmode3_parsed(void *parsed_result, void *data)
 859 {
 860 #ifdef HOST_VERSION
 861         printf("not implemented\n");
 862 #else
 863         struct cmd_mechboard_setmode3_result *res = parsed_result;
 864         if (!strcmp_P(res->arg1, PSTR("autobuild")))
 865                 i2c_mechboard_mode_simple_autobuild(res->level);
 866         else if (!strcmp_P(res->arg1, PSTR("prepare_build")))
 867                 i2c_mechboard_mode_prepare_build_both(res->level);
 868         else if (!strcmp_P(res->arg1, PSTR("prepare_inside")))
 869                 i2c_mechboard_mode_prepare_inside_both(res->level);
 870         else if (!strcmp_P(res->arg1, PSTR("push_temple")))
 871                 i2c_mechboard_mode_push_temple(res->level);
 872 #endif
 873 }
 874
 875 prog_char str_mechboard_setmode3_arg0[] = "mechboard";
 876 parse_pgm_token_string_t cmd_mechboard_setmode3_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode3_result, arg0, str_mechboard_setmode3_arg0);
 877 prog_char str_mechboard_setmode3_arg1[] = "autobuild#prepare_build#prepare_inside#push_temple";
 878 parse_pgm_token_string_t cmd_mechboard_setmode3_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode3_result, arg1, str_mechboard_setmode3_arg1);
 879 parse_pgm_token_num_t cmd_mechboard_setmode3_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_mechboard_setmode3_result, level, UINT8);
 880
 881 prog_char help_mechboard_setmode3[] = "set mechboard mode (mode, level)";
 882 parse_pgm_inst_t cmd_mechboard_setmode3 = {
 883         .f = cmd_mechboard_setmode3_parsed,  /* function to call */
 884         .data = NULL,      /* 2nd arg of func */
 885         .help_str = help_mechboard_setmode3,
 886         .tokens = {        /* token list, NULL terminated */
 887                 (prog_void *)&cmd_mechboard_setmode3_arg0,
 888                 (prog_void *)&cmd_mechboard_setmode3_arg1,
 889                 (prog_void *)&cmd_mechboard_setmode3_arg2,
 890                 NULL,
 891         },
 892 };
 893
 894 /**********************************************************/
 895 /* Mechboard_Setmode4 */
 896
 897 /* this structure is filled when cmd_mechboard_setmode4 is parsed successfully */
 898 struct cmd_mechboard_setmode4_result {
 899         fixed_string_t arg0;
 900         fixed_string_t arg1;
 901         uint8_t level_l;
 902         uint8_t count_l;
 903         uint8_t dist_l;
 904         uint8_t level_r;
 905         uint8_t count_r;
 906         uint8_t dist_r;
 907         uint8_t do_lintel;
 908 };
 909
 910 /* function called when cmd_mechboard_setmode4 is parsed successfully */
 911 static void cmd_mechboard_setmode4_parsed(void *parsed_result, void *data)
 912 {
 913 #ifdef HOST_VERSION
 914         printf("not implemented\n");
 915 #else
 916         struct cmd_mechboard_setmode4_result *res = parsed_result;
 917         i2c_mechboard_mode_autobuild(res->level_l, res->count_l, res->dist_l,
 918                                      res->level_r, res->count_r, res->dist_r,
 919                                      res->do_lintel);
 920 #endif
 921 }
 922
 923 prog_char str_mechboard_setmode4_arg0[] = "mechboard";
 924 parse_pgm_token_string_t cmd_mechboard_setmode4_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode4_result, arg0, str_mechboard_setmode4_arg0);
 925 prog_char str_mechboard_setmode4_arg1[] = "autobuild";
 926 parse_pgm_token_string_t cmd_mechboard_setmode4_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode4_result, arg1, str_mechboard_setmode4_arg1);
 927 parse_pgm_token_num_t cmd_mechboard_setmode4_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_mechboard_setmode4_result, level_l, UINT8);
 928 parse_pgm_token_num_t cmd_mechboard_setmode4_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_mechboard_setmode4_result, count_l, UINT8);
 929 parse_pgm_token_num_t cmd_mechboard_setmode4_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_mechboard_setmode4_result, dist_l, UINT8);
 930 parse_pgm_token_num_t cmd_mechboard_setmode4_arg5 = TOKEN_NUM_INITIALIZER(struct cmd_mechboard_setmode4_result, level_r, UINT8);
 931 parse_pgm_token_num_t cmd_mechboard_setmode4_arg6 = TOKEN_NUM_INITIALIZER(struct cmd_mechboard_setmode4_result, count_r, UINT8);
 932 parse_pgm_token_num_t cmd_mechboard_setmode4_arg7 = TOKEN_NUM_INITIALIZER(struct cmd_mechboard_setmode4_result, dist_r, UINT8);
 933 parse_pgm_token_num_t cmd_mechboard_setmode4_arg8 = TOKEN_NUM_INITIALIZER(struct cmd_mechboard_setmode4_result, do_lintel, UINT8);
 934
 935 prog_char help_mechboard_setmode4[] = "set mechboard mode (autobuild level_l count_l dist_l level_r count_r dist_r lintel)";
 936 parse_pgm_inst_t cmd_mechboard_setmode4 = {
 937         .f = cmd_mechboard_setmode4_parsed,  /* function to call */
 938         .data = NULL,      /* 2nd arg of func */
 939         .help_str = help_mechboard_setmode4,
 940         .tokens = {        /* token list, NULL terminated */
 941                 (prog_void *)&cmd_mechboard_setmode4_arg0,
 942                 (prog_void *)&cmd_mechboard_setmode4_arg1,
 943                 (prog_void *)&cmd_mechboard_setmode4_arg2,
 944                 (prog_void *)&cmd_mechboard_setmode4_arg3,
 945                 (prog_void *)&cmd_mechboard_setmode4_arg4,
 946                 (prog_void *)&cmd_mechboard_setmode4_arg5,
 947                 (prog_void *)&cmd_mechboard_setmode4_arg6,
 948                 (prog_void *)&cmd_mechboard_setmode4_arg7,
 949                 (prog_void *)&cmd_mechboard_setmode4_arg8,
 950                 NULL,
 951         },
 952 };
 953
 954 /**********************************************************/
 955 /* Mechboard_Setmode5 */
 956
 957 /* this structure is filled when cmd_mechboard_setmode5 is parsed successfully */
 958 struct cmd_mechboard_setmode5_result {
 959         fixed_string_t arg0;
 960         fixed_string_t arg1;
 961         fixed_string_t arg2;
 962         fixed_string_t arg3;
 963 };
 964
 965 /* function called when cmd_mechboard_setmode5 is parsed successfully */
 966 static void cmd_mechboard_setmode5_parsed(void *parsed_result, void * data)
 967 {
 968 #ifdef HOST_VERSION
 969         printf("not implemented\n");
 970 #else
 971         struct cmd_mechboard_setmode5_result *res = parsed_result;
 972         uint8_t side = I2C_LEFT_SIDE, next_mode = I2C_MECHBOARD_MODE_PREPARE_PICKUP;
 973
 974         if (!strcmp_P(res->arg2, PSTR("left")))
 975                 side = I2C_LEFT_SIDE;
 976         else if (!strcmp_P(res->arg2, PSTR("right")))
 977                 side = I2C_RIGHT_SIDE;
 978         else if (!strcmp_P(res->arg2, PSTR("center")))
 979                 side = I2C_CENTER_SIDE;
 980         else if (!strcmp_P(res->arg2, PSTR("auto")))
 981                 side = I2C_AUTO_SIDE;
 982
 983         if (!strcmp_P(res->arg3, PSTR("harvest")))
 984                 next_mode = I2C_MECHBOARD_MODE_HARVEST;
 985         else if (!strcmp_P(res->arg3, PSTR("lazy_harvest")))
 986                 next_mode = I2C_MECHBOARD_MODE_LAZY_HARVEST;
 987         else if (!strcmp_P(res->arg3, PSTR("pickup")))
 988                 next_mode = I2C_MECHBOARD_MODE_PICKUP;
 989         else if (!strcmp_P(res->arg3, PSTR("clear")))
 990                 next_mode = I2C_MECHBOARD_MODE_CLEAR;
 991         else if (!strcmp_P(res->arg3, PSTR("store")))
 992                 next_mode = I2C_MECHBOARD_MODE_STORE;
 993         else if (!strcmp_P(res->arg3, PSTR("lazy_pickup")))
 994                 next_mode = I2C_MECHBOARD_MODE_LAZY_PICKUP;
 995
 996         if (!strcmp_P(res->arg1, PSTR("prepare_pickup")))
 997                 i2c_mechboard_mode_prepare_pickup_next(side, next_mode);
 998 #endif
 999 }
1000
1001 prog_char str_mechboard_setmode5_arg0[] = "mechboard";
1002 parse_pgm_token_string_t cmd_mechboard_setmode5_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode5_result, arg0, str_mechboard_setmode5_arg0);
1003 prog_char str_mechboard_setmode5_arg1[] = "prepare_pickup";
1004 parse_pgm_token_string_t cmd_mechboard_setmode5_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode5_result, arg1, str_mechboard_setmode5_arg1);
1005 prog_char str_mechboard_setmode5_arg2[] = "left#right#auto#center";
1006 parse_pgm_token_string_t cmd_mechboard_setmode5_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode5_result, arg2, str_mechboard_setmode5_arg2);
1007 prog_char str_mechboard_setmode5_arg3[] = "harvest#pickup#store#lazy_harvest#lazy_pickup#clear";
1008 parse_pgm_token_string_t cmd_mechboard_setmode5_arg3 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode5_result, arg3, str_mechboard_setmode5_arg3);
1009
1010 prog_char help_mechboard_setmode5[] = "set mechboard mode (more, side)";
1011 parse_pgm_inst_t cmd_mechboard_setmode5 = {
1012         .f = cmd_mechboard_setmode5_parsed,  /* function to call */
1013         .data = NULL,      /* 2nd arg of func */
1014         .help_str = help_mechboard_setmode5,
1015         .tokens = {        /* token list, NULL terminated */
1016                 (prog_void *)&cmd_mechboard_setmode5_arg0,
1017                 (prog_void *)&cmd_mechboard_setmode5_arg1,
1018                 (prog_void *)&cmd_mechboard_setmode5_arg2,
1019                 (prog_void *)&cmd_mechboard_setmode5_arg3,
1020                 NULL,
1021         },
1022 };
1023
1024 /**********************************************************/
1025 /* pickup wheels */
1026
1027 /* this structure is filled when cmd_pickup_wheels is parsed successfully */
1028 struct cmd_pickup_wheels_result {
1029         fixed_string_t arg0;
1030         fixed_string_t arg1;
1031 };
1032
1033 /* function called when cmd_pickup_wheels is parsed successfully */
1034 static void cmd_pickup_wheels_parsed(void *parsed_result, void *data)
1035 {
1036         struct cmd_pickup_wheels_result *res = parsed_result;
1037
1038         if (!strcmp_P(res->arg1, PSTR("on")))
1039                 pickup_wheels_on();
1040         else
1041                 pickup_wheels_off();
1042 }
1043
1044 prog_char str_pickup_wheels_arg0[] = "pickup_wheels";
1045 parse_pgm_token_string_t cmd_pickup_wheels_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_pickup_wheels_result, arg0, str_pickup_wheels_arg0);
1046 prog_char str_pickup_wheels_arg1[] = "on#off";
1047 parse_pgm_token_string_t cmd_pickup_wheels_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_pickup_wheels_result, arg1, str_pickup_wheels_arg1);
1048
1049 prog_char help_pickup_wheels[] = "Enable/disable pickup wheels";
1050 parse_pgm_inst_t cmd_pickup_wheels = {
1051         .f = cmd_pickup_wheels_parsed,  /* function to call */
1052         .data = NULL,      /* 2nd arg of func */
1053         .help_str = help_pickup_wheels,
1054         .tokens = {        /* token list, NULL terminated */
1055                 (prog_void *)&cmd_pickup_wheels_arg0,
1056                 (prog_void *)&cmd_pickup_wheels_arg1,
1057                 NULL,
1058         },
1059 };
1060
1061 /**********************************************************/
1062 /* Beacon_Start */
1063
1064 /* this structure is filled when cmd_beacon_start is parsed successfully */
1065 struct cmd_beacon_start_result {
1066         fixed_string_t arg0;
1067         fixed_string_t arg1;
1068 };
1069
1070 /* function called when cmd_beacon_start is parsed successfully */
1071 static void cmd_beacon_start_parsed(void *parsed_result, void *data)
1072 {
1073 #ifdef HOST_VERSION
1074         printf("not implemented\n");
1075 #else
1076         struct cmd_beacon_start_result *res = parsed_result;
1077
1078         if (!strcmp_P(res->arg1, PSTR("start")))
1079                 i2c_sensorboard_set_beacon(1);
1080         else
1081                 i2c_sensorboard_set_beacon(0);
1082 #endif
1083 }
1084
1085 prog_char str_beacon_start_arg0[] = "beacon";
1086 parse_pgm_token_string_t cmd_beacon_start_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_beacon_start_result, arg0, str_beacon_start_arg0);
1087 prog_char str_beacon_start_arg1[] = "start#stop";
1088 parse_pgm_token_string_t cmd_beacon_start_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_beacon_start_result, arg1, str_beacon_start_arg1);
1089
1090 prog_char help_beacon_start[] = "Beacon enabled/disable";
1091 parse_pgm_inst_t cmd_beacon_start = {
1092         .f = cmd_beacon_start_parsed,  /* function to call */
1093         .data = NULL,      /* 2nd arg of func */
1094         .help_str = help_beacon_start,
1095         .tokens = {        /* token list, NULL terminated */
1096                 (prog_void *)&cmd_beacon_start_arg0,
1097                 (prog_void *)&cmd_beacon_start_arg1,
1098                 NULL,
1099         },
1100 };
1101
1102 /**********************************************************/
1103 /* Pump_Current */
1104
1105 /* this structure is filled when cmd_pump_current is parsed successfully */
1106 struct cmd_pump_current_result {
1107         fixed_string_t arg0;
1108         fixed_string_t arg1;
1109 };
1110
1111 /* function called when cmd_pump_current is parsed successfully */
1112 static void cmd_pump_current_parsed(__attribute__((unused)) void *parsed_result,
1113                                     __attribute__((unused)) void *data)
1114 {
1115 #ifdef HOST_VERSION
1116         printf("not implemented\n");
1117 #else
1118         printf_P(PSTR("l1=%d l2=%d r1=%d r2=%d\r\n"),
1119                  sensor_get_adc(ADC_CSENSE3), sensor_get_adc(ADC_CSENSE4),
1120                  mechboard.pump_right1_current, mechboard.pump_right2_current);
1121 #endif
1122 }
1123
1124 prog_char str_pump_current_arg0[] = "pump_current";
1125 parse_pgm_token_string_t cmd_pump_current_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_pump_current_result, arg0, str_pump_current_arg0);
1126 prog_char str_pump_current_arg1[] = "show";
1127 parse_pgm_token_string_t cmd_pump_current_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_pump_current_result, arg1, str_pump_current_arg1);
1128
1129 prog_char help_pump_current[] = "dump pump current";
1130 parse_pgm_inst_t cmd_pump_current = {
1131         .f = cmd_pump_current_parsed,  /* function to call */
1132         .data = NULL,      /* 2nd arg of func */
1133         .help_str = help_pump_current,
1134         .tokens = {        /* token list, NULL terminated */
1135                 (prog_void *)&cmd_pump_current_arg0,
1136                 (prog_void *)&cmd_pump_current_arg1,
1137                 NULL,
1138         },
1139 };
1140
1141 /**********************************************************/
1142 /* Build_Test */
1143
1144 /* this structure is filled when cmd_build_test is parsed successfully */
1145 struct cmd_build_test_result {
1146         fixed_string_t arg0;
1147 };
1148
1149 /* function called when cmd_build_test is parsed successfully */
1150 static void cmd_build_test_parsed(void *parsed_result, void *data)
1151 {
1152 #ifdef HOST_VERSION
1153         printf("not implemented\n");
1154 #else
1155         //struct cmd_build_test_result *res = parsed_result;
1156
1157         printf_P(PSTR("lintel must be there\r\n"));
1158         i2c_mechboard_mode_prepare_pickup_next(I2C_LEFT_SIDE,
1159                                                I2C_MECHBOARD_MODE_HARVEST);
1160         wait_ms(500);
1161
1162         printf_P(PSTR("Insert 4 colums\r\n"));
1163         while (get_column_count() != 4);
1164
1165         i2c_mechboard_mode_prepare_build_both(0);
1166         trajectory_d_rel(&mainboard.traj, 200);
1167         wait_traj_end(TRAJ_FLAGS_NO_NEAR);
1168         wait_ms(500);
1169
1170         i2c_mechboard_mode_simple_autobuild(0);
1171         wait_ms(100);
1172         while (get_mechboard_mode() == I2C_MECHBOARD_MODE_AUTOBUILD);
1173
1174         trajectory_d_rel(&mainboard.traj, -200);
1175         wait_traj_end(TRAJ_FLAGS_NO_NEAR);
1176         i2c_mechboard_mode_prepare_pickup_next(I2C_LEFT_SIDE,
1177                                                I2C_MECHBOARD_MODE_HARVEST);
1178
1179         while (get_column_count() != 3);
1180
1181         i2c_mechboard_mode_prepare_build_both(3);
1182         trajectory_d_rel(&mainboard.traj, 200);
1183         wait_traj_end(TRAJ_FLAGS_NO_NEAR);
1184         wait_ms(500);
1185
1186         i2c_mechboard_mode_autobuild(3, 1, I2C_AUTOBUILD_DEFAULT_DIST,
1187                                      3, 2,I2C_AUTOBUILD_DEFAULT_DIST, 0);
1188         i2cproto_wait_update();
1189         while (get_mechboard_mode() == I2C_MECHBOARD_MODE_AUTOBUILD);
1190
1191         trajectory_d_rel(&mainboard.traj, -200);
1192         wait_traj_end(TRAJ_FLAGS_NO_NEAR);
1193         i2c_mechboard_mode_prepare_pickup(I2C_RIGHT_SIDE);
1194         wait_ms(500);
1195
1196         i2c_mechboard_mode_harvest();
1197         while (get_column_count() != 3);
1198
1199         i2c_mechboard_mode_prepare_build_both(5);
1200         trajectory_d_rel(&mainboard.traj, 200);
1201         wait_traj_end(TRAJ_FLAGS_NO_NEAR);
1202         wait_ms(1000);
1203
1204         i2c_mechboard_mode_autobuild(4, 2, I2C_AUTOBUILD_DEFAULT_DIST,
1205                                      5, 1, I2C_AUTOBUILD_DEFAULT_DIST, 0);
1206         i2cproto_wait_update();
1207         while (get_mechboard_mode() == I2C_MECHBOARD_MODE_AUTOBUILD);
1208
1209         trajectory_d_rel(&mainboard.traj, -200);
1210 #endif
1211 }
1212
1213 prog_char str_build_test_arg0[] = "build_test";
1214 parse_pgm_token_string_t cmd_build_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_build_test_result, arg0, str_build_test_arg0);
1215
1216 prog_char help_build_test[] = "Build_Test function";
1217 parse_pgm_inst_t cmd_build_test = {
1218         .f = cmd_build_test_parsed,  /* function to call */
1219         .data = NULL,      /* 2nd arg of func */
1220         .help_str = help_build_test,
1221         .tokens = {        /* token list, NULL terminated */
1222                 (prog_void *)&cmd_build_test_arg0,
1223                 NULL,
1224         },
1225 };
1226
1227
1228 /**********************************************************/
1229 /* Column_Test */
1230
1231 /* this structure is filled when cmd_column_test is parsed successfully */
1232 struct cmd_column_test_result {
1233         fixed_string_t arg0;
1234         uint8_t level;
1235         int16_t dist;
1236         int8_t a1;
1237         int8_t a2;
1238         int8_t a3;
1239         int16_t arm_dist;
1240         int8_t nb_col;
1241 };
1242
1243 /* function called when cmd_column_test is parsed successfully */
1244 static void cmd_column_test_parsed(void *parsed_result, void *data)
1245 {
1246 #ifdef HOST_VERSION
1247         printf("not implemented\n");
1248 #else
1249         struct cmd_column_test_result *res = parsed_result;
1250         uint8_t level = res->level, debug = 0;
1251         uint8_t c, push = 0;
1252
1253         /* default conf */
1254         if (data) {
1255                 res->dist = 70;
1256                 res->a1 = -20;
1257                 res->a2 =  40;
1258                 res->a3 = -20;
1259                 res->arm_dist = 220;
1260                 res->nb_col = 2;
1261         }
1262
1263         if (!strcmp_P(res->arg0, PSTR("column_test_debug")))
1264                 debug = 1;
1265         if (!strcmp_P(res->arg0, PSTR("column_test_push")))
1266                 push = 1;
1267
1268         strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_VERY_SLOW);
1269
1270         /* Go to disc */
1271
1272         trajectory_d_rel(&mainboard.traj, 200);
1273         wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
1274
1275         /* go back, insert colums */
1276
1277         trajectory_d_rel(&mainboard.traj, -200);
1278         wait_traj_end(TRAJ_FLAGS_NO_NEAR);
1279
1280         i2c_mechboard_mode_prepare_pickup_next(I2C_LEFT_SIDE,
1281                                                I2C_MECHBOARD_MODE_HARVEST);
1282         printf_P(PSTR("Insert 4 colums\r\n"));
1283         while (get_column_count() != 4);
1284
1285         /* build with left arm */
1286
1287         i2c_mechboard_mode_prepare_inside_both(level);
1288         trajectory_d_rel(&mainboard.traj, 200-(res->dist));
1289         wait_traj_end(TRAJ_FLAGS_NO_NEAR);
1290
1291         if (debug)
1292                 c = cmdline_getchar_wait();
1293
1294         trajectory_a_rel(&mainboard.traj, res->a1);
1295         wait_traj_end(TRAJ_FLAGS_NO_NEAR);
1296
1297         if (debug)
1298                 c = cmdline_getchar_wait();
1299
1300         i2c_mechboard_mode_prepare_build_select(level, -1);
1301         time_wait_ms(200);
1302         if (debug)
1303                 c = cmdline_getchar_wait();
1304         i2c_mechboard_mode_autobuild(level, res->nb_col, res->arm_dist,
1305                                      0, 0, res->arm_dist, 0);
1306         while (get_mechboard_mode() != I2C_MECHBOARD_MODE_AUTOBUILD);
1307         while (get_mechboard_mode() == I2C_MECHBOARD_MODE_AUTOBUILD);
1308
1309         if (debug)
1310                 c = cmdline_getchar_wait();
1311         i2c_mechboard_mode_prepare_inside_select(level+res->nb_col, -1);
1312
1313         if (debug)
1314                 c = cmdline_getchar_wait();
1315         /* build with right arm */
1316
1317         trajectory_a_rel(&mainboard.traj, res->a2);
1318         wait_traj_end(TRAJ_FLAGS_NO_NEAR);
1319
1320         if (debug)
1321                 c = cmdline_getchar_wait();
1322         /* only ok for nb_col == 2 */
1323         if ((level + res->nb_col) >= 7)
1324                 i2c_mechboard_mode_prepare_build_select(-1, level + res->nb_col + 1);
1325         else
1326                 i2c_mechboard_mode_prepare_build_select(-1, level + res->nb_col);
1327         time_wait_ms(200);
1328         if (debug)
1329                 c = cmdline_getchar_wait();
1330         i2c_mechboard_mode_autobuild(0, 0, res->arm_dist,
1331                                      level + res->nb_col, res->nb_col,
1332                                      res->arm_dist, 0);
1333         while (get_mechboard_mode() != I2C_MECHBOARD_MODE_AUTOBUILD);
1334         while (get_mechboard_mode() == I2C_MECHBOARD_MODE_AUTOBUILD);
1335
1336
1337         if (push) {
1338                 strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST);
1339                 trajectory_d_rel(&mainboard.traj, -100);
1340                 wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
1341                 i2c_mechboard_mode_push_temple_disc(I2C_RIGHT_SIDE);
1342                 time_wait_ms(500);
1343                 trajectory_d_rel(&mainboard.traj, 100);
1344                 wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
1345         }
1346         else if (level == 1 || level == 0) {
1347                 trajectory_d_rel(&mainboard.traj, -100);
1348                 wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
1349                 i2c_mechboard_mode_push_temple(level);
1350                 time_wait_ms(400);
1351                 strat_set_speed(200, SPEED_ANGLE_SLOW);
1352                 trajectory_d_rel(&mainboard.traj, 120);
1353                 wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
1354         }
1355
1356         strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
1357
1358         if (debug)
1359                 c = cmdline_getchar_wait();
1360         i2c_mechboard_mode_prepare_inside_select(-1, level+res->nb_col*2);
1361
1362         if (debug)
1363                 c = cmdline_getchar_wait();
1364
1365         trajectory_a_rel(&mainboard.traj, res->a3);
1366         wait_traj_end(TRAJ_FLAGS_NO_NEAR);
1367
1368         if (debug)
1369                 c = cmdline_getchar_wait();
1370         /* go back, insert colums */
1371
1372         trajectory_d_rel(&mainboard.traj, -100);
1373
1374         return;
1375 #endif
1376 }
1377
1378 prog_char str_column_test_arg0[] = "column_test#column_test_debug#column_test_push";
1379 parse_pgm_token_string_t cmd_column_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_column_test_result, arg0, str_column_test_arg0);
1380 parse_pgm_token_num_t cmd_column_test_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_column_test_result, level, UINT8);
1381
1382 prog_char help_column_test[] = "Column_Test function (level)";
1383 parse_pgm_inst_t cmd_column_test = {
1384         .f = cmd_column_test_parsed,  /* function to call */
1385         .data = (void *)1,      /* 2nd arg of func */
1386         .help_str = help_column_test,
1387         .tokens = {        /* token list, NULL terminated */
1388                 (prog_void *)&cmd_column_test_arg0,
1389                 (prog_void *)&cmd_column_test_arg1,
1390                 NULL,
1391         },
1392 };
1393
1394 parse_pgm_token_num_t cmd_column_test_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_column_test_result, dist, INT16);
1395 parse_pgm_token_num_t cmd_column_test_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_column_test_result, a1, INT8);
1396 parse_pgm_token_num_t cmd_column_test_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_column_test_result, a2, INT8);
1397 parse_pgm_token_num_t cmd_column_test_arg5 = TOKEN_NUM_INITIALIZER(struct cmd_column_test_result, a3, INT8);
1398 parse_pgm_token_num_t cmd_column_test_arg6 = TOKEN_NUM_INITIALIZER(struct cmd_column_test_result, arm_dist, INT16);
1399 parse_pgm_token_num_t cmd_column_test_arg7 = TOKEN_NUM_INITIALIZER(struct cmd_column_test_result, nb_col, INT8);
1400
1401 prog_char help_column_test2[] = "Column_Test function (level, dist, a1, a2, a3, arm_dist, nb_col)";
1402 parse_pgm_inst_t cmd_column_test2 = {
1403         .f = cmd_column_test_parsed,  /* function to call */
1404         .data = NULL,      /* 2nd arg of func */
1405         .help_str = help_column_test2,
1406         .tokens = {        /* token list, NULL terminated */
1407                 (prog_void *)&cmd_column_test_arg0,
1408                 (prog_void *)&cmd_column_test_arg1,
1409                 (prog_void *)&cmd_column_test_arg2,
1410                 (prog_void *)&cmd_column_test_arg3,
1411                 (prog_void *)&cmd_column_test_arg4,
1412                 (prog_void *)&cmd_column_test_arg5,
1413                 (prog_void *)&cmd_column_test_arg6,
1414                 (prog_void *)&cmd_column_test_arg7,
1415                 NULL,
1416         },
1417 };
1418
1419
1420 /**********************************************************/
1421 /* Pickup_Test */
1422
1423 /* this structure is filled when cmd_pickup_test is parsed successfully */
1424 struct cmd_pickup_test_result {
1425         fixed_string_t arg0;
1426         fixed_string_t arg1;
1427         int16_t dist;
1428 };
1429
1430 /* return red or green sensor */
1431 #define COLOR_IR_SENSOR()                                               \
1432         ({                                                              \
1433                 uint8_t __ret = 0;                                      \
1434                 if (side == I2C_RIGHT_SIDE)                             \
1435                         __ret = sensor_get(S_DISP_RIGHT);               \
1436                 else                                                    \
1437                         __ret = sensor_get(S_DISP_LEFT);                \
1438                 __ret;                                                  \
1439         })                                                              \
1440 /* column dispensers */
1441 #define COL_SCAN_MARGIN 200
1442 /* distance between the wheel axis and the IR sensor */
1443
1444 /* function called when cmd_pickup_test is parsed successfully */
1445 static void cmd_pickup_test_parsed(void *parsed_result, void *data)
1446 {
1447 #ifdef HOST_VERSION
1448         printf("not implemented\n");
1449 #else
1450         uint8_t err, side, first_try = 1;
1451         int8_t cols_count_before, cols_count_after, cols;
1452         struct cmd_pickup_test_result *res = parsed_result;
1453         int16_t pos1, pos2, pos;
1454         microseconds us;
1455         int16_t dist = res->dist;
1456         uint8_t timeout = 0;
1457
1458         if (!strcmp_P(res->arg1, PSTR("left")))
1459                 side = I2C_LEFT_SIDE;
1460         else
1461                 side = I2C_RIGHT_SIDE;
1462
1463         i2c_mechboard_mode_prepare_pickup(I2C_AUTO_SIDE);
1464         cols_count_before = get_column_count();
1465         position_set(&mainboard.pos, 0, 0, 0);
1466
1467         strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_FAST);
1468         trajectory_d_rel(&mainboard.traj, -1000);
1469         err = WAIT_COND_OR_TRAJ_END(!COLOR_IR_SENSOR(), TRAJ_FLAGS_NO_NEAR);
1470         if (err) /* we should not reach end */
1471                 goto fail;
1472         pos1 = position_get_x_s16(&mainboard.pos);
1473         printf_P(PSTR("pos1 = %d\r\n"), pos1);
1474
1475         err = WAIT_COND_OR_TRAJ_END(COLOR_IR_SENSOR(), TRAJ_FLAGS_NO_NEAR);
1476         if (err)
1477                 goto fail;
1478         pos2 = position_get_x_s16(&mainboard.pos);
1479         printf_P(PSTR("pos2 = %d\r\n"), pos2);
1480
1481         pos = ABS(pos1 - pos2);
1482         printf_P(PSTR("pos = %d\r\n"), pos);
1483
1484         trajectory_d_rel(&mainboard.traj, -dist + pos/2);
1485         err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
1486
1487         if (side == I2C_LEFT_SIDE)
1488                 trajectory_a_rel(&mainboard.traj, 90);
1489         else
1490                 trajectory_a_rel(&mainboard.traj, -90);
1491         err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
1492
1493         pickup_wheels_on();
1494  retry:
1495         if (first_try)
1496                 i2c_mechboard_mode_lazy_harvest();
1497         else
1498                 i2c_mechboard_mode_prepare_pickup(I2C_AUTO_SIDE);
1499         first_try = 0;
1500
1501         strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_FAST);
1502         trajectory_d_rel(&mainboard.traj, 300);
1503         err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST | END_NEAR);
1504         strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST);
1505         err = strat_calib(600, TRAJ_FLAGS_SMALL_DIST);
1506
1507         trajectory_d_rel(&mainboard.traj, -DIST_BACK_DISPENSER);
1508         err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
1509         if (!TRAJ_SUCCESS(err))
1510                 goto fail;
1511
1512         position_set(&mainboard.pos, 0, 0, 0);
1513         if (get_mechboard_mode() == I2C_MECHBOARD_MODE_PREPARE_EJECT) {
1514                 strat_eject_col(90, 0);
1515                 goto retry;
1516         }
1517
1518         /* start to pickup with finger / arms */
1519
1520         printf_P(PSTR("%s pickup now\r\n"), __FUNCTION__);
1521         i2c_mechboard_mode_pickup();
1522         WAIT_COND_OR_TIMEOUT(get_mechboard_mode() ==
1523                              I2C_MECHBOARD_MODE_PICKUP, 100);
1524         us = time_get_us2();
1525         cols = get_column_count();
1526         while (get_mechboard_mode() == I2C_MECHBOARD_MODE_PICKUP) {
1527                 if (get_column_count() != cols) {
1528                         cols = get_column_count();
1529                         us = time_get_us2();
1530                 }
1531                 if ((get_column_count() - cols_count_before) >= 4) {
1532                         printf_P(PSTR("%s no more cols in disp\r\n"), __FUNCTION__);
1533                         break;
1534                 }
1535                 /* 1 second timeout */
1536                 if (time_get_us2() - us > 1500000L) {
1537                         printf_P(PSTR("%s timeout\r\n"), __FUNCTION__);
1538                         timeout = 1;
1539                         break;
1540                 }
1541         }
1542
1543         /* eject if we found a bad color column */
1544
1545         if (get_mechboard_mode() == I2C_MECHBOARD_MODE_PREPARE_EJECT) {
1546                 strat_eject_col(90, 0);
1547                 goto retry;
1548         }
1549
1550         strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
1551         trajectory_d_rel(&mainboard.traj, -250);
1552         wait_traj_end(TRAJ_FLAGS_SMALL_DIST | END_NEAR);
1553
1554         cols_count_after = get_column_count();
1555         cols = cols_count_after - cols_count_before;
1556         DEBUG(E_USER_STRAT, "%s we got %d cols", __FUNCTION__, cols);
1557
1558         pickup_wheels_off();
1559         i2c_mechboard_mode_clear();
1560
1561         wait_ms(1000);
1562         return;
1563  fail:
1564         printf_P(PSTR("failed\r\n"));
1565         strat_hardstop();
1566 #endif
1567 }
1568
1569 prog_char str_pickup_test_arg0[] = "pickup_test";
1570 parse_pgm_token_string_t cmd_pickup_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_pickup_test_result, arg0, str_pickup_test_arg0);
1571 prog_char str_pickup_test_arg1[] = "left#right";
1572 parse_pgm_token_string_t cmd_pickup_test_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_pickup_test_result, arg1, str_pickup_test_arg1);
1573 parse_pgm_token_num_t cmd_pickup_test_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_pickup_test_result, dist, INT16);
1574
1575 prog_char help_pickup_test[] = "Pickup_Test function";
1576 parse_pgm_inst_t cmd_pickup_test = {
1577         .f = cmd_pickup_test_parsed,  /* function to call */
1578         .data = NULL,      /* 2nd arg of func */
1579         .help_str = help_pickup_test,
1580         .tokens = {        /* token list, NULL terminated */
1581                 (prog_void *)&cmd_pickup_test_arg0,
1582                 (prog_void *)&cmd_pickup_test_arg1,
1583                 (prog_void *)&cmd_pickup_test_arg2,
1584                 NULL,
1585         },
1586 };
1587
1588 /**********************************************************/
1589 /* Lintel_Test */
1590
1591 /* this structure is filled when cmd_lintel_test is parsed successfully */
1592 struct cmd_lintel_test_result {
1593         fixed_string_t arg0;
1594 };
1595
1596 /* function called when cmd_lintel_test is parsed successfully */
1597 static void cmd_lintel_test_parsed(void *parsed_result, void *data)
1598 {
1599 #ifdef HOST_VERSION
1600         printf("not implemented\n");
1601 #else
1602         uint8_t err, first_try = 1, right_ok, left_ok;
1603         int16_t left_cur, right_cur;
1604
1605         i2c_mechboard_mode_prepare_get_lintel();
1606         time_wait_ms(500);
1607  retry:
1608         strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST);
1609         trajectory_d_rel(&mainboard.traj, 500);
1610         err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
1611         if (!TRAJ_SUCCESS(err) && err != END_BLOCKING)
1612                 goto fail;
1613
1614         i2c_mechboard_mode_get_lintel();
1615         time_wait_ms(500);
1616
1617         left_cur = sensor_get_adc(ADC_CSENSE3);
1618         left_ok = (left_cur > I2C_MECHBOARD_CURRENT_COLUMN);
1619         right_cur = mechboard.pump_right1_current;
1620         right_ok = (right_cur > I2C_MECHBOARD_CURRENT_COLUMN);
1621
1622         printf_P(PSTR("left_ok=%d (%d), right_ok=%d (%d)\r\n"),
1623                  left_ok, left_cur, right_ok, right_cur);
1624         if (first_try) {
1625                 if (!right_ok && !left_ok) {
1626                         i2c_mechboard_mode_prepare_get_lintel();
1627                         time_wait_ms(300);
1628                 }
1629                 else if (right_ok && !left_ok) {
1630                         i2c_mechboard_mode_prepare_get_lintel();
1631                         time_wait_ms(300);
1632                         strat_set_speed(500, 500);
1633                         trajectory_d_a_rel(&mainboard.traj, -150, 30);
1634                         err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
1635                         trajectory_d_a_rel(&mainboard.traj, -140, -30);
1636                         err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
1637                         first_try = 0;
1638                         goto retry;
1639                 }
1640                 else if (!right_ok && left_ok) {
1641                         i2c_mechboard_mode_prepare_get_lintel();
1642                         time_wait_ms(300);
1643                         strat_set_speed(500, 500);
1644                         trajectory_d_a_rel(&mainboard.traj, -150, -30);
1645                         err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
1646                         trajectory_d_a_rel(&mainboard.traj, -140, 30);
1647                         err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
1648                         first_try = 0;
1649                         goto retry;
1650                 }
1651                 /* else, lintel is ok */
1652                 else {
1653                         i2c_mechboard_mode_put_lintel();
1654                 }
1655         }
1656         else {
1657                 if (right_ok && left_ok) {
1658                         /* lintel is ok */
1659                         i2c_mechboard_mode_put_lintel();
1660                 }
1661                 else {
1662                         i2c_mechboard_mode_prepare_get_lintel();
1663                         time_wait_ms(300);
1664                 }
1665         }
1666
1667         strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
1668         trajectory_d_rel(&mainboard.traj, -250);
1669         err = wait_traj_end(TRAJ_FLAGS_STD);
1670         return;
1671
1672 fail:
1673         printf_P(PSTR("fail\r\n"));
1674         return;
1675 #endif
1676 }
1677
1678 prog_char str_lintel_test_arg0[] = "lintel_test";
1679 parse_pgm_token_string_t cmd_lintel_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_lintel_test_result, arg0, str_lintel_test_arg0);
1680
1681 prog_char help_lintel_test[] = "Lintel_Test function";
1682 parse_pgm_inst_t cmd_lintel_test = {
1683         .f = cmd_lintel_test_parsed,  /* function to call */
1684         .data = NULL,      /* 2nd arg of func */
1685         .help_str = help_lintel_test,
1686         .tokens = {        /* token list, NULL terminated */
1687                 (prog_void *)&cmd_lintel_test_arg0,
1688                 NULL,
1689         },
1690 };
1691
1692 /**********************************************************/
1693 /* Scan_Test */
1694
1695 /* this structure is filled when cmd_scan_test is parsed successfully */
1696 struct cmd_scan_test_result {
1697         fixed_string_t arg0;
1698         fixed_string_t arg1;
1699         int16_t start_dist;
1700         int16_t scan_dist;
1701         int16_t scan_speed;
1702         int16_t center_x;
1703         int16_t center_y;
1704         uint8_t level;
1705 };
1706
1707 #define SCAN_MODE_CHECK_TEMPLE 0
1708 #define SCAN_MODE_SCAN_COL     1
1709 #define SCAN_MODE_SCAN_TEMPLE  2
1710 #define SCAN_MODE_TRAJ_ONLY    3
1711
1712 /* function called when cmd_scan_test is parsed successfully */
1713 static void cmd_scan_test_parsed(void *parsed_result, void *data)
1714 {
1715 #ifdef HOST_VERSION
1716         printf("not implemented\n");
1717 #else
1718         uint8_t err, mode=0, c;
1719         int16_t pos1x, pos1y, dist;
1720         struct cmd_scan_test_result *res = parsed_result;
1721         int16_t back_mm = 0;
1722
1723         int16_t ckpt_rel_x = 0, ckpt_rel_y = 0;
1724
1725         double center_abs_x, center_abs_y;
1726         double ckpt_rel_d, ckpt_rel_a;
1727         double ckpt_abs_x, ckpt_abs_y;
1728
1729         if (!strcmp_P(res->arg1, PSTR("traj_only")))
1730                 mode = SCAN_MODE_TRAJ_ONLY;
1731         else if (!strcmp_P(res->arg1, PSTR("check_temple")))
1732                 mode = SCAN_MODE_CHECK_TEMPLE;
1733         else if (!strcmp_P(res->arg1, PSTR("scan_col")))
1734                 mode = SCAN_MODE_SCAN_COL;
1735         else if (!strcmp_P(res->arg1, PSTR("scan_temple")))
1736                 mode = SCAN_MODE_SCAN_TEMPLE;
1737
1738         /* go to disc */
1739         strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_VERY_SLOW);
1740         trajectory_d_rel(&mainboard.traj, 400);
1741         err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
1742         if (err != END_BLOCKING)
1743                 return;
1744
1745         /* save absolute position of disc */
1746         rel_da_to_abs_xy(265, 0, &center_abs_x, &center_abs_y);
1747
1748         /* go back and prepare to scan */
1749         strat_set_speed(1000, 1000);
1750         trajectory_d_a_rel(&mainboard.traj, -140, 130);
1751         err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
1752         if (!TRAJ_SUCCESS(err))
1753                 return;
1754
1755         /* prepare scanner arm */
1756         if (mode != SCAN_MODE_TRAJ_ONLY)
1757                 i2c_sensorboard_scanner_prepare();
1758         time_wait_ms(250);
1759
1760         strat_set_speed(res->scan_speed, 1000);
1761
1762         pos1x = position_get_x_s16(&mainboard.pos);
1763         pos1y = position_get_y_s16(&mainboard.pos);
1764         trajectory_d_rel(&mainboard.traj, -res->scan_dist);
1765
1766         while (1) {
1767                 err = test_traj_end(TRAJ_FLAGS_SMALL_DIST);
1768                 if (err != 0)
1769                         break;
1770
1771                 dist = distance_from_robot(pos1x, pos1y);
1772
1773                 if (dist > res->start_dist)
1774                         break;
1775
1776                 if (get_scanner_status() & I2C_SCAN_MAX_COLUMN) {
1777                         err = END_ERROR;
1778                         break;
1779                 }
1780         }
1781
1782         if (err) {
1783                 if (TRAJ_SUCCESS(err))
1784                         err = END_ERROR; /* should not reach end */
1785                 strat_hardstop();
1786                 trajectory_goto_xy_abs(&mainboard.traj, pos1x, pos1y);
1787                 wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
1788                 if (mode != SCAN_MODE_TRAJ_ONLY)
1789                         i2c_sensorboard_scanner_stop();
1790                 return;
1791         }
1792
1793         /* start the scanner */
1794
1795         if (mode != SCAN_MODE_TRAJ_ONLY)
1796                 i2c_sensorboard_scanner_start();
1797
1798         err = WAIT_COND_OR_TRAJ_END(get_scanner_status() & I2C_SCAN_MAX_COLUMN,
1799                                     TRAJ_FLAGS_NO_NEAR);
1800         if (err == 0)
1801                 err = END_ERROR;
1802         if (!TRAJ_SUCCESS(err)) {
1803                 strat_hardstop();
1804                 trajectory_goto_xy_abs(&mainboard.traj, pos1x, pos1y);
1805                 wait_traj_end(TRAJ_FLAGS_NO_NEAR);
1806                 if (mode != SCAN_MODE_TRAJ_ONLY)
1807                         i2c_sensorboard_scanner_stop();
1808                 return;
1809         }
1810
1811         if (mode == SCAN_MODE_TRAJ_ONLY)
1812                 return;
1813
1814         wait_scan_done(10000);
1815
1816         i2c_sensorboard_scanner_stop();
1817
1818         if (mode == SCAN_MODE_CHECK_TEMPLE) {
1819                 i2c_sensorboard_scanner_algo_check(res->level,
1820                                                    res->center_x, res->center_y);
1821                 i2cproto_wait_update();
1822                 wait_scan_done(10000);
1823                 scanner_dump_state();
1824
1825                 if (sensorboard.dropzone_h == -1) {
1826                         printf_P(PSTR("-- try to build a temple\r\n"));
1827                         res->center_x = 15;
1828                         res->center_y = 13;
1829                         mode = SCAN_MODE_SCAN_TEMPLE;
1830                 }
1831         }
1832
1833         if (mode == SCAN_MODE_SCAN_TEMPLE) {
1834                 i2c_sensorboard_scanner_algo_temple(I2C_SCANNER_ZONE_DISC,
1835                                                     res->center_x,
1836                                                     res->center_y);
1837                 i2cproto_wait_update();
1838                 wait_scan_done(10000);
1839                 scanner_dump_state();
1840
1841                 if (sensorboard.dropzone_h == -1 ||
1842                     strat_scan_get_checkpoint(mode, &ckpt_rel_x,
1843                                               &ckpt_rel_y, &back_mm)) {
1844                         printf_P(PSTR("-- try to build a column\r\n"));
1845                         mode = SCAN_MODE_SCAN_COL;
1846                 }
1847         }
1848
1849         if (mode == SCAN_MODE_SCAN_COL) {
1850                 i2c_sensorboard_scanner_algo_column(I2C_SCANNER_ZONE_DISC,
1851                                                     res->center_x, res->center_y);
1852                 i2cproto_wait_update();
1853                 wait_scan_done(10000);
1854                 scanner_dump_state();
1855
1856                 if (sensorboard.dropzone_h == -1 ||
1857                     strat_scan_get_checkpoint(mode, &ckpt_rel_x,
1858                                               &ckpt_rel_y, &back_mm)) {
1859                         return;
1860                 }
1861         }
1862
1863         if (sensorboard.dropzone_h == -1)
1864                 return;
1865
1866         if (mode == SCAN_MODE_CHECK_TEMPLE) {
1867                 ckpt_rel_x = 220;
1868                 ckpt_rel_y = 100;
1869         }
1870
1871
1872         printf_P(PSTR("rel xy for ckpt is %d,%d\r\n"), ckpt_rel_x, ckpt_rel_y);
1873
1874         rel_xy_to_abs_xy(ckpt_rel_x, ckpt_rel_y, &ckpt_abs_x, &ckpt_abs_y);
1875         abs_xy_to_rel_da(ckpt_abs_x, ckpt_abs_y, &ckpt_rel_d, &ckpt_rel_a);
1876
1877         printf_P(PSTR("abs ckpt is %2.2f,%2.2f\r\n"), ckpt_abs_x, ckpt_abs_y);
1878
1879         printf_P(PSTR("ok ? (y/n)\r\n"));
1880
1881         c = cmdline_getchar_wait();
1882
1883         if (c != 'y')
1884                 return;
1885
1886         strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
1887
1888         /* intermediate checkpoint for some positions */
1889         if ( (DEG(ckpt_rel_a) < 0 && DEG(ckpt_rel_a) > -90) ) {
1890                 trajectory_goto_xy_rel(&mainboard.traj, 200, 100);
1891                 err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
1892                 if (!TRAJ_SUCCESS(err))
1893                         return;
1894         }
1895
1896         trajectory_goto_xy_abs(&mainboard.traj, ckpt_abs_x, ckpt_abs_y);
1897         err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
1898         if (!TRAJ_SUCCESS(err))
1899                 return;
1900
1901         trajectory_turnto_xy(&mainboard.traj, center_abs_x, center_abs_y);
1902         err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
1903         if (!TRAJ_SUCCESS(err))
1904                 return;
1905
1906         c = cmdline_getchar_wait();
1907
1908         pos1x = position_get_x_s16(&mainboard.pos);
1909         pos1y = position_get_y_s16(&mainboard.pos);
1910
1911         strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW);
1912         trajectory_d_rel(&mainboard.traj, 200);
1913         err = WAIT_COND_OR_TRAJ_END(distance_from_robot(pos1x, pos1y) > 200,
1914                                     TRAJ_FLAGS_SMALL_DIST);
1915         if (err == 0) {
1916                 strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_VERY_SLOW);
1917                 trajectory_d_rel(&mainboard.traj, 400);
1918                 err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
1919         }
1920         if (err != END_BLOCKING)
1921                 return;
1922
1923         if (back_mm) {
1924                 trajectory_d_rel(&mainboard.traj, -back_mm);
1925                 wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
1926         }
1927 #endif
1928 }
1929
1930 prog_char str_scan_test_arg0[] = "scan_test";
1931 parse_pgm_token_string_t cmd_scan_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_scan_test_result, arg0, str_scan_test_arg0);
1932 prog_char str_scan_test_arg1[] = "traj_only#scan_col#scan_temple";
1933 parse_pgm_token_string_t cmd_scan_test_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_scan_test_result, arg1, str_scan_test_arg1);
1934 parse_pgm_token_num_t cmd_scan_test_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_scan_test_result, start_dist, INT16);
1935 parse_pgm_token_num_t cmd_scan_test_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_scan_test_result, scan_dist, INT16);
1936 parse_pgm_token_num_t cmd_scan_test_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_scan_test_result, scan_speed, INT16);
1937 parse_pgm_token_num_t cmd_scan_test_arg5 = TOKEN_NUM_INITIALIZER(struct cmd_scan_test_result, center_x, INT16);
1938 parse_pgm_token_num_t cmd_scan_test_arg6 = TOKEN_NUM_INITIALIZER(struct cmd_scan_test_result, center_y, INT16);
1939
1940 prog_char help_scan_test[] = "Scan_Test function (start_dist, scan_dist, speed_dist, centerx, centery)";
1941 parse_pgm_inst_t cmd_scan_test = {
1942         .f = cmd_scan_test_parsed,  /* function to call */
1943         .data = NULL,      /* 2nd arg of func */
1944         .help_str = help_scan_test,
1945         .tokens = {        /* token list, NULL terminated */
1946                 (prog_void *)&cmd_scan_test_arg0,
1947                 (prog_void *)&cmd_scan_test_arg1,
1948                 (prog_void *)&cmd_scan_test_arg2,
1949                 (prog_void *)&cmd_scan_test_arg3,
1950                 (prog_void *)&cmd_scan_test_arg4,
1951                 (prog_void *)&cmd_scan_test_arg5,
1952                 (prog_void *)&cmd_scan_test_arg6,
1953                 NULL,
1954         },
1955 };
1956
1957 prog_char str_scan_test_arg1b[] = "check_temple";
1958 parse_pgm_token_string_t cmd_scan_test_arg1b = TOKEN_STRING_INITIALIZER(struct cmd_scan_test_result, arg1, str_scan_test_arg1b);
1959 parse_pgm_token_num_t cmd_scan_test_arg7 = TOKEN_NUM_INITIALIZER(struct cmd_scan_test_result, level, UINT8);
1960
1961 prog_char help_scan_test2[] = "Scan_Test function (start_dist, scan_dist, speed_dist, templex, templey, level)";
1962 parse_pgm_inst_t cmd_scan_test2 = {
1963         .f = cmd_scan_test_parsed,  /* function to call */
1964         .data = NULL,      /* 2nd arg of func */
1965         .help_str = help_scan_test,
1966         .tokens = {        /* token list, NULL terminated */
1967                 (prog_void *)&cmd_scan_test_arg0,
1968                 (prog_void *)&cmd_scan_test_arg1b,
1969                 (prog_void *)&cmd_scan_test_arg2,
1970                 (prog_void *)&cmd_scan_test_arg3,
1971                 (prog_void *)&cmd_scan_test_arg4,
1972                 (prog_void *)&cmd_scan_test_arg5,
1973                 (prog_void *)&cmd_scan_test_arg6,
1974                 (prog_void *)&cmd_scan_test_arg7,
1975                 NULL,
1976         },
1977 };
1978
1979 /**********************************************************/
1980 /* Time_Monitor */
1981
1982 /* this structure is filled when cmd_time_monitor is parsed successfully */
1983 struct cmd_time_monitor_result {
1984         fixed_string_t arg0;
1985         fixed_string_t arg1;
1986 };
1987
1988 /* function called when cmd_time_monitor is parsed successfully */
1989 static void cmd_time_monitor_parsed(void *parsed_result, void *data)
1990 {
1991 #ifdef HOST_VERSION
1992         printf("not implemented\n");
1993 #else
1994         struct cmd_time_monitor_result *res = parsed_result;
1995         uint16_t seconds;
1996
1997         if (!strcmp_P(res->arg1, PSTR("reset"))) {
1998                 eeprom_write_word(EEPROM_TIME_ADDRESS, 0);
1999         }
2000         seconds = eeprom_read_word(EEPROM_TIME_ADDRESS);
2001         printf_P(PSTR("Running since %d mn %d\r\n"), seconds/60, seconds%60);
2002 #endif
2003 }
2004
2005 prog_char str_time_monitor_arg0[] = "time_monitor";
2006 parse_pgm_token_string_t cmd_time_monitor_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_time_monitor_result, arg0, str_time_monitor_arg0);
2007 prog_char str_time_monitor_arg1[] = "show#reset";
2008 parse_pgm_token_string_t cmd_time_monitor_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_time_monitor_result, arg1, str_time_monitor_arg1);
2009
2010 prog_char help_time_monitor[] = "Show since how long we are running";
2011 parse_pgm_inst_t cmd_time_monitor = {
2012         .f = cmd_time_monitor_parsed,  /* function to call */
2013         .data = NULL,      /* 2nd arg of func */
2014         .help_str = help_time_monitor,
2015         .tokens = {        /* token list, NULL terminated */
2016                 (prog_void *)&cmd_time_monitor_arg0,
2017                 (prog_void *)&cmd_time_monitor_arg1,
2018                 NULL,
2019         },
2020 };
2021
2022
2023 /**********************************************************/
2024 /* Scanner */
2025
2026 /* this structure is filled when cmd_scanner is parsed successfully */
2027 struct cmd_scanner_result {
2028         fixed_string_t arg0;
2029         fixed_string_t arg1;
2030 };
2031
2032 /* function called when cmd_scanner is parsed successfully */
2033 static void cmd_scanner_parsed(void *parsed_result, void *data)
2034 {
2035 #ifdef HOST_VERSION
2036         printf("not implemented\n");
2037 #else
2038         struct cmd_scanner_result *res = parsed_result;
2039
2040         if (!strcmp_P(res->arg1, PSTR("prepare"))) {
2041                 i2c_sensorboard_scanner_prepare();
2042         }
2043         else if (!strcmp_P(res->arg1, PSTR("stop"))) {
2044                 i2c_sensorboard_scanner_stop();
2045         }
2046         else if (!strcmp_P(res->arg1, PSTR("start"))) {
2047                 i2c_sensorboard_scanner_start();
2048         }
2049         else if (!strcmp_P(res->arg1, PSTR("algo_col"))) {
2050                 i2c_sensorboard_scanner_algo_column(I2C_SCANNER_ZONE_DISC,
2051                                                      15, 15);
2052         }
2053         else if (!strcmp_P(res->arg1, PSTR("algo_check"))) {
2054                 i2c_sensorboard_scanner_algo_check(2, 15, 15); // XXX
2055         }
2056         else if (!strcmp_P(res->arg1, PSTR("calib"))) {
2057                 i2c_sensorboard_scanner_calib();
2058         }
2059         else if (!strcmp_P(res->arg1, PSTR("show"))) {
2060                 scanner_dump_state();
2061         }
2062 #endif
2063 }
2064
2065 prog_char str_scanner_arg0[] = "scanner";
2066 parse_pgm_token_string_t cmd_scanner_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_scanner_result, arg0, str_scanner_arg0);
2067 prog_char str_scanner_arg1[] = "prepare#start#algo_col#algo_check#stop#show#calib";
2068 parse_pgm_token_string_t cmd_scanner_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_scanner_result, arg1, str_scanner_arg1);
2069
2070 prog_char help_scanner[] = "send commands to scanner";
2071 parse_pgm_inst_t cmd_scanner = {
2072         .f = cmd_scanner_parsed,  /* function to call */
2073         .data = NULL,      /* 2nd arg of func */
2074         .help_str = help_scanner,
2075         .tokens = {        /* token list, NULL terminated */
2076                 (prog_void *)&cmd_scanner_arg0,
2077                 (prog_void *)&cmd_scanner_arg1,
2078                 NULL,
2079         },
2080 };
2081
2082 /**********************************************************/
2083 /* Build_Z1 */
2084
2085 /* this structure is filled when cmd_build_z1 is parsed successfully */
2086 struct cmd_build_z1_result {
2087         fixed_string_t arg0;
2088         uint8_t level;
2089         int16_t d1;
2090         int16_t d2;
2091         int16_t d3;
2092 };
2093
2094 /* function called when cmd_build_z1 is parsed successfully */
2095 static void cmd_build_z1_parsed(void *parsed_result, void *data)
2096 {
2097 #ifdef HOST_VERSION
2098         printf("not implemented\n");
2099 #else
2100         struct cmd_build_z1_result *res = parsed_result;
2101
2102         strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_SLOW);
2103         trajectory_d_rel(&mainboard.traj, 400);
2104         wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
2105
2106         trajectory_d_rel(&mainboard.traj, -200);
2107         wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
2108
2109         i2c_mechboard_mode_prepare_pickup_next(I2C_LEFT_SIDE,
2110                                                I2C_MECHBOARD_MODE_HARVEST);
2111
2112         while (get_column_count() != 4);
2113
2114         i2c_mechboard_mode_prepare_build_both(res->level);
2115         time_wait_ms(500);
2116
2117         trajectory_d_rel(&mainboard.traj, 400);
2118         wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
2119
2120         strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW);
2121         trajectory_d_rel(&mainboard.traj, -res->d1);
2122         wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
2123         i2c_mechboard_mode_autobuild(res->level, 2, I2C_AUTOBUILD_DEFAULT_DIST,
2124                                      res->level, 2, I2C_AUTOBUILD_DEFAULT_DIST,
2125                                      1);
2126         WAIT_COND_OR_TIMEOUT(get_mechboard_mode() ==
2127                              I2C_MECHBOARD_MODE_AUTOBUILD, 100);
2128         WAIT_COND_OR_TIMEOUT(get_mechboard_mode() !=
2129                              I2C_MECHBOARD_MODE_AUTOBUILD, 10000);
2130
2131         strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW);
2132         trajectory_d_rel(&mainboard.traj, -res->d2);
2133         wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
2134         i2c_mechboard_mode_push_temple(1);
2135         time_wait_ms(400);
2136         strat_set_speed(200, SPEED_ANGLE_SLOW);
2137         trajectory_d_rel(&mainboard.traj, res->d3);
2138         wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
2139 #endif
2140 }
2141
2142 prog_char str_build_z1_arg0[] = "build_z1";
2143 parse_pgm_token_string_t cmd_build_z1_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_build_z1_result, arg0, str_build_z1_arg0);
2144 parse_pgm_token_num_t cmd_build_z1_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_build_z1_result, level, UINT8);
2145 parse_pgm_token_num_t cmd_build_z1_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_build_z1_result, d1, INT16);
2146 parse_pgm_token_num_t cmd_build_z1_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_build_z1_result, d2, INT16);
2147 parse_pgm_token_num_t cmd_build_z1_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_build_z1_result, d3, INT16);
2148
2149 prog_char help_build_z1[] = "Build_Z1 function (level, d1, d2, d3)";
2150 parse_pgm_inst_t cmd_build_z1 = {
2151         .f = cmd_build_z1_parsed,  /* function to call */
2152         .data = NULL,      /* 2nd arg of func */
2153         .help_str = help_build_z1,
2154         .tokens = {        /* token list, NULL terminated */
2155                 (prog_void *)&cmd_build_z1_arg0,
2156                 (prog_void *)&cmd_build_z1_arg1,
2157                 (prog_void *)&cmd_build_z1_arg2,
2158                 (prog_void *)&cmd_build_z1_arg3,
2159                 (prog_void *)&cmd_build_z1_arg4,
2160                 NULL,
2161         },
2162 };
2163
2164 #ifdef TEST_BEACON
2165 /**********************************************************/
2166 /* Beacon_Opp_Dump */
2167
2168 /* this structure is filled when cmd_beacon_opp_dump is parsed successfully */
2169 struct cmd_beacon_opp_dump_result {
2170         fixed_string_t arg0;
2171 };
2172
2173 void beacon_dump_samples(void);
2174
2175 /* function called when cmd_beacon_opp_dump is parsed successfully */
2176 static void cmd_beacon_opp_dump_parsed(void *parsed_result, void *data)
2177 {
2178 #ifdef HOST_VERSION
2179         printf("not implemented\n");
2180 #else
2181         beacon_dump_samples();
2182 #endif
2183 }
2184
2185 prog_char str_beacon_opp_dump_arg0[] = "beacon_opp_dump";
2186 parse_pgm_token_string_t cmd_beacon_opp_dump_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_beacon_opp_dump_result, arg0, str_beacon_opp_dump_arg0);
2187
2188 prog_char help_beacon_opp_dump[] = "Dump beacon samples";
2189 parse_pgm_inst_t cmd_beacon_opp_dump = {
2190         .f = cmd_beacon_opp_dump_parsed,  /* function to call */
2191         .data = NULL,      /* 2nd arg of func */
2192         .help_str = help_beacon_opp_dump,
2193         .tokens = {        /* token list, NULL terminated */
2194                 (prog_void *)&cmd_beacon_opp_dump_arg0,
2195                 NULL,
2196         },
2197 };
2198 #endif
2199
2200 /**********************************************************/
2201 /* Circle_Radius */
2202
2203 /* this structure is filled when cmd_circle_radius is parsed successfully */
2204 struct cmd_circle_radius_result {
2205         fixed_string_t arg0;
2206         int32_t radius;
2207 };
2208 void circle_get_da_speed_from_radius(struct trajectory *traj,
2209                                 double radius_mm,
2210                                 double *speed_d,
2211                                 double *speed_a);
2212 /* function called when cmd_circle_radius is parsed successfully */
2213 static void cmd_circle_radius_parsed(void *parsed_result, void *data)
2214 {
2215         struct cmd_circle_radius_result *res = parsed_result;
2216         double d,a;
2217         strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW);
2218         circle_get_da_speed_from_radius(&mainboard.traj, res->radius, &d, &a);
2219         printf_P(PSTR("d=%2.2f a=%2.2f\r\n"), d, a);
2220 }
2221
2222 prog_char str_circle_radius_arg0[] = "circle_radius";
2223 parse_pgm_token_string_t cmd_circle_radius_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_circle_radius_result, arg0, str_circle_radius_arg0);
2224 parse_pgm_token_num_t cmd_circle_radius_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_circle_radius_result, radius, INT32);
2225
2226 prog_char help_circle_radius[] = "Circle_Radius function";
2227 parse_pgm_inst_t cmd_circle_radius = {
2228         .f = cmd_circle_radius_parsed,  /* function to call */
2229         .data = NULL,      /* 2nd arg of func */
2230         .help_str = help_circle_radius,
2231         .tokens = {        /* token list, NULL terminated */
2232                 (prog_void *)&cmd_circle_radius_arg0,
2233                 (prog_void *)&cmd_circle_radius_arg1,
2234                 NULL,
2235         },
2236 };
2237
2238 /**********************************************************/
2239 /* Test */
2240
2241 /* this structure is filled when cmd_test is parsed successfully */
2242 struct cmd_test_result {
2243         fixed_string_t arg0;
2244         int32_t radius;
2245 };
2246 void circle_get_da_speed_from_radius(struct trajectory *traj,
2247                                 double radius_mm,
2248                                 double *speed_d,
2249                                 double *speed_a);
2250 /* function called when cmd_test is parsed successfully */
2251 static void cmd_test_parsed(void *parsed_result, void *data)
2252 {
2253         struct cmd_test_result *res = parsed_result;
2254         double d,a;
2255         uint8_t err;
2256
2257         strat_reset_pos(1000, 500, 0);
2258         strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW);
2259         circle_get_da_speed_from_radius(&mainboard.traj, res->radius, &d, &a);
2260         trajectory_d_rel(&mainboard.traj, 1000);
2261         err = WAIT_COND_OR_TRAJ_END(position_get_x_double(&mainboard.pos) > 1500, 0xFF);
2262         if (err)
2263                 return;
2264         strat_set_speed(d, a);
2265         trajectory_d_a_rel(&mainboard.traj, 10000, 1000);
2266 }
2267
2268 prog_char str_test_arg0[] = "test";
2269 parse_pgm_token_string_t cmd_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_test_result, arg0, str_test_arg0);
2270 parse_pgm_token_num_t cmd_test_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_test_result, radius, INT32);
2271
2272 prog_char help_test[] = "Test function";
2273 parse_pgm_inst_t cmd_test = {
2274         .f = cmd_test_parsed,  /* function to call */
2275         .data = NULL,      /* 2nd arg of func */
2276         .help_str = help_test,
2277         .tokens = {        /* token list, NULL terminated */
2278                 (prog_void *)&cmd_test_arg0,
2279                 (prog_void *)&cmd_test_arg1,
2280                 NULL,
2281         },
2282 };