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