projects/microb2010/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 #include <math.h>
  26
  27 #include <hostsim.h>
  28 #include <aversive/pgmspace.h>
  29 #include <aversive/wait.h>
  30 #include <aversive/error.h>
  31 #include <aversive/eeprom.h>
  32
  33 #include <ax12.h>
  34 #include <uart.h>
  35 #include <pwm_ng.h>
  36 #include <clock_time.h>
  37 #include <spi.h>
  38 #include <i2c.h>
  39
  40 #include <pid.h>
  41 #include <quadramp.h>
  42 #include <control_system_manager.h>
  43 #include <trajectory_manager.h>
  44 #include <trajectory_manager_core.h>
  45 #include <trajectory_manager_utils.h>
  46 #include <vect_base.h>
  47 #include <lines.h>
  48 #include <polygon.h>
  49 #include <obstacle_avoidance.h>
  50 #include <blocking_detection_manager.h>
  51 #include <robot_system.h>
  52 #include <position_manager.h>
  53
  54 #include <rdline.h>
  55 #include <parse.h>
  56 #include <parse_string.h>
  57 #include <parse_num.h>
  58
  59 #include "../common/i2c_commands.h"
  60 #include "../common/eeprom_mapping.h"
  61
  62 #include "main.h"
  63 #include "robotsim.h"
  64 #include "sensor.h"
  65 #include "cmdline.h"
  66 #include "strat.h"
  67 #include "strat_utils.h"
  68 #include "strat_base.h"
  69 #include "strat_db.h"
  70 #include "strat_corn.h"
  71 #include "i2c_protocol.h"
  72 #include "actuator.h"
  73 #include "beacon.h"
  74
  75 struct cmd_event_result {
  76         fixed_string_t arg0;
  77         fixed_string_t arg1;
  78         fixed_string_t arg2;
  79 };
  80
  81
  82 /* function called when cmd_event is parsed successfully */
  83 static void cmd_event_parsed(void *parsed_result, void *data)
  84 {
  85         u08 bit=0;
  86
  87         struct cmd_event_result * res = parsed_result;
  88
  89         if (!strcmp_P(res->arg1, PSTR("all"))) {
  90                 bit = 0xFF;
  91                 if (!strcmp_P(res->arg2, PSTR("on")))
  92                         mainboard.flags |= bit;
  93                 else if (!strcmp_P(res->arg2, PSTR("off")))
  94                         mainboard.flags &= bit;
  95                 else { /* show */
  96                         printf_P(PSTR("encoders is %s\r\n"),
  97                                  (DO_ENCODERS & mainboard.flags) ? "on":"off");
  98                         printf_P(PSTR("cs is %s\r\n"),
  99                                  (DO_CS & mainboard.flags) ? "on":"off");
 100                         printf_P(PSTR("rs is %s\r\n"),
 101                                  (DO_RS & mainboard.flags) ? "on":"off");
 102                         printf_P(PSTR("pos is %s\r\n"),
 103                                  (DO_POS & mainboard.flags) ? "on":"off");
 104                         printf_P(PSTR("bd is %s\r\n"),
 105                                  (DO_BD & mainboard.flags) ? "on":"off");
 106                         printf_P(PSTR("timer is %s\r\n"),
 107                                  (DO_TIMER & mainboard.flags) ? "on":"off");
 108                         printf_P(PSTR("power is %s\r\n"),
 109                                  (DO_POWER & mainboard.flags) ? "on":"off");
 110                         printf_P(PSTR("errblock is %s\r\n"),
 111                                  (DO_ERRBLOCKING & mainboard.flags) ? "on":"off");
 112                 }
 113                 return;
 114         }
 115
 116         if (!strcmp_P(res->arg1, PSTR("encoders")))
 117                 bit = DO_ENCODERS;
 118         else if (!strcmp_P(res->arg1, PSTR("cs"))) {
 119                 strat_hardstop();
 120                 bit = DO_CS;
 121         }
 122         else if (!strcmp_P(res->arg1, PSTR("rs")))
 123                 bit = DO_RS;
 124         else if (!strcmp_P(res->arg1, PSTR("pos")))
 125                 bit = DO_POS;
 126         else if (!strcmp_P(res->arg1, PSTR("bd")))
 127                 bit = DO_BD;
 128         else if (!strcmp_P(res->arg1, PSTR("timer"))) {
 129                 time_reset();
 130                 bit = DO_TIMER;
 131         }
 132         else if (!strcmp_P(res->arg1, PSTR("power")))
 133                 bit = DO_POWER;
 134         else if (!strcmp_P(res->arg1, PSTR("errblock")))
 135                 bit = DO_ERRBLOCKING;
 136
 137         if (!strcmp_P(res->arg2, PSTR("on")))
 138                 mainboard.flags |= bit;
 139         else if (!strcmp_P(res->arg2, PSTR("off"))) {
 140                 if (!strcmp_P(res->arg1, PSTR("cs"))) {
 141 #ifdef HOST_VERSION
 142                         robotsim_pwm(LEFT_PWM, 0);
 143                         robotsim_pwm(RIGHT_PWM, 0);
 144 #else
 145                         pwm_ng_set(LEFT_PWM, 0);
 146                         pwm_ng_set(RIGHT_PWM, 0);
 147 #endif
 148                 }
 149                 mainboard.flags &= (~bit);
 150         }
 151         printf_P(PSTR("%s is %s\r\n"), res->arg1,
 152                       (bit & mainboard.flags) ? "on":"off");
 153 }
 154
 155 prog_char str_event_arg0[] = "event";
 156 parse_pgm_token_string_t cmd_event_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg0, str_event_arg0);
 157 prog_char str_event_arg1[] = "all#encoders#cs#rs#pos#bd#timer#power#errblock";
 158 parse_pgm_token_string_t cmd_event_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg1, str_event_arg1);
 159 prog_char str_event_arg2[] = "on#off#show";
 160 parse_pgm_token_string_t cmd_event_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg2, str_event_arg2);
 161
 162 prog_char help_event[] = "Enable/disable events";
 163 parse_pgm_inst_t cmd_event = {
 164         .f = cmd_event_parsed,  /* function to call */
 165         .data = NULL,      /* 2nd arg of func */
 166         .help_str = help_event,
 167         .tokens = {        /* token list, NULL terminated */
 168                 (prog_void *)&cmd_event_arg0,
 169                 (prog_void *)&cmd_event_arg1,
 170                 (prog_void *)&cmd_event_arg2,
 171                 NULL,
 172         },
 173 };
 174
 175
 176 /**********************************************************/
 177 /* Spi_Test */
 178
 179 /* this structure is filled when cmd_spi_test is parsed successfully */
 180 struct cmd_spi_test_result {
 181         fixed_string_t arg0;
 182 };
 183
 184 /* function called when cmd_spi_test is parsed successfully */
 185 static void cmd_spi_test_parsed(void * parsed_result, void * data)
 186 {
 187 #ifdef HOST_VERSION
 188         printf("not implemented\n");
 189 #else
 190         uint16_t i = 0, ret = 0, ret2 = 0;
 191
 192         if (mainboard.flags & DO_ENCODERS) {
 193                 printf_P(PSTR("Disable encoder event first\r\n"));
 194                 return;
 195         }
 196
 197         do {
 198                 spi_slave_select(0);
 199                 ret = spi_send_and_receive_byte(i);
 200                 ret2 = spi_send_and_receive_byte(i);
 201                 spi_slave_deselect(0);
 202
 203                 if ((i & 0x7ff) == 0)
 204                         printf_P(PSTR("Sent %.4x twice, received %x %x\r\n"),
 205                                  i, ret, ret2);
 206
 207                 i++;
 208         } while(!cmdline_keypressed());
 209 #endif
 210 }
 211
 212 prog_char str_spi_test_arg0[] = "spi_test";
 213 parse_pgm_token_string_t cmd_spi_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_spi_test_result, arg0, str_spi_test_arg0);
 214
 215 prog_char help_spi_test[] = "Test the SPI";
 216 parse_pgm_inst_t cmd_spi_test = {
 217         .f = cmd_spi_test_parsed,  /* function to call */
 218         .data = NULL,      /* 2nd arg of func */
 219         .help_str = help_spi_test,
 220         .tokens = {        /* token list, NULL terminated */
 221                 (prog_void *)&cmd_spi_test_arg0,
 222                 NULL,
 223         },
 224 };
 225
 226
 227
 228 /**********************************************************/
 229 /* Opponent tests */
 230
 231 /* this structure is filled when cmd_opponent is parsed successfully */
 232 struct cmd_opponent_result {
 233         fixed_string_t arg0;
 234         fixed_string_t arg1;
 235         int32_t arg2;
 236         int32_t arg3;
 237 };
 238
 239 /* function called when cmd_opponent is parsed successfully */
 240 static void cmd_opponent_parsed(void *parsed_result, void *data)
 241 {
 242         int16_t x,y,d,a;
 243
 244         if (get_opponent_xyda(&x, &y, &d, &a))
 245                 printf_P(PSTR("No opponent\r\n"));
 246         else
 247                 printf_P(PSTR("x=%d y=%d, d=%d a=%d\r\n"), x, y, d, a);
 248 }
 249
 250 prog_char str_opponent_arg0[] = "opponent";
 251 parse_pgm_token_string_t cmd_opponent_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_opponent_result, arg0, str_opponent_arg0);
 252 prog_char str_opponent_arg1[] = "show";
 253 parse_pgm_token_string_t cmd_opponent_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_opponent_result, arg1, str_opponent_arg1);
 254
 255 prog_char help_opponent[] = "Show (x,y) opponent";
 256 parse_pgm_inst_t cmd_opponent = {
 257         .f = cmd_opponent_parsed,  /* function to call */
 258         .data = NULL,      /* 2nd arg of func */
 259         .help_str = help_opponent,
 260         .tokens = {        /* token list, NULL terminated */
 261                 (prog_void *)&cmd_opponent_arg0,
 262                 (prog_void *)&cmd_opponent_arg1,
 263                 NULL,
 264         },
 265 };
 266
 267
 268 prog_char str_opponent_arg1_set[] = "set";
 269 parse_pgm_token_string_t cmd_opponent_arg1_set = TOKEN_STRING_INITIALIZER(struct cmd_opponent_result, arg1, str_opponent_arg1_set);
 270 parse_pgm_token_num_t cmd_opponent_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_opponent_result, arg2, INT32);
 271 parse_pgm_token_num_t cmd_opponent_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_opponent_result, arg3, INT32);
 272
 273 prog_char help_opponent_set[] = "Set (x,y) opponent";
 274 parse_pgm_inst_t cmd_opponent_set = {
 275         .f = cmd_opponent_parsed,  /* function to call */
 276         .data = NULL,      /* 2nd arg of func */
 277         .help_str = help_opponent_set,
 278         .tokens = {        /* token list, NULL terminated */
 279                 (prog_void *)&cmd_opponent_arg0,
 280                 (prog_void *)&cmd_opponent_arg1_set,
 281                 (prog_void *)&cmd_opponent_arg2,
 282                 (prog_void *)&cmd_opponent_arg3,
 283                 NULL,
 284         },
 285 };
 286
 287 /**********************************************************/
 288 /* Beacon tests */
 289
 290 /* this structure is filled when cmd_beacon is parsed successfully */
 291 struct cmd_beacon_result {
 292         fixed_string_t arg0;
 293         fixed_string_t arg1;
 294 };
 295
 296 /* function called when cmd_beacon is parsed successfully */
 297 static void cmd_beacon_parsed(void *parsed_result, void *data)
 298 {
 299         int16_t x, y;
 300         double a;
 301
 302         if (beacon_get_pos(&x, &y, &a) < 0)
 303                 printf_P(PSTR("No position from beacon\r\n"));
 304         else
 305                 printf_P(PSTR("x=%d y=%d a=%2.2f\r\n"), x, y, a);
 306 }
 307
 308 prog_char str_beacon_arg0[] = "beacon";
 309 parse_pgm_token_string_t cmd_beacon_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_beacon_result, arg0, str_beacon_arg0);
 310 prog_char str_beacon_arg1[] = "show";
 311 parse_pgm_token_string_t cmd_beacon_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_beacon_result, arg1, str_beacon_arg1);
 312
 313 prog_char help_beacon[] = "Show (x,y) beacon";
 314 parse_pgm_inst_t cmd_beacon = {
 315         .f = cmd_beacon_parsed,  /* function to call */
 316         .data = NULL,      /* 2nd arg of func */
 317         .help_str = help_beacon,
 318         .tokens = {        /* token list, NULL terminated */
 319                 (prog_void *)&cmd_beacon_arg0,
 320                 (prog_void *)&cmd_beacon_arg1,
 321                 NULL,
 322         },
 323 };
 324
 325 /**********************************************************/
 326 /* Start */
 327
 328 /* this structure is filled when cmd_start is parsed successfully */
 329 struct cmd_start_result {
 330         fixed_string_t arg0;
 331         fixed_string_t color;
 332         fixed_string_t debug;
 333 };
 334
 335 /* function called when cmd_start is parsed successfully */
 336 static void cmd_start_parsed(void *parsed_result, void *data)
 337 {
 338         struct cmd_start_result *res = parsed_result;
 339         uint8_t old_level = gen.log_level;
 340
 341         gen.logs[NB_LOGS] = E_USER_STRAT;
 342         if (!strcmp_P(res->debug, PSTR("debug"))) {
 343                 strat_db.dump_enabled = 1;
 344                 gen.log_level = 5;
 345         }
 346         else {
 347                 strat_db.dump_enabled = 0;
 348                 gen.log_level = 0;
 349         }
 350
 351         if (!strcmp_P(res->color, PSTR("yellow"))) {
 352                 mainboard.our_color = I2C_COLOR_YELLOW;
 353                 i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_YELLOW);
 354                 i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_YELLOW);
 355                 beacon_set_color(I2C_COLOR_YELLOW);
 356         }
 357         else if (!strcmp_P(res->color, PSTR("blue"))) {
 358                 mainboard.our_color = I2C_COLOR_BLUE;
 359                 i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_BLUE);
 360                 i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_BLUE);
 361                 beacon_set_color(I2C_COLOR_BLUE);
 362         }
 363
 364         strat_start();
 365
 366         gen.logs[NB_LOGS] = 0;
 367         gen.log_level = old_level;
 368 }
 369
 370 prog_char str_start_arg0[] = "start";
 371 parse_pgm_token_string_t cmd_start_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_start_result, arg0, str_start_arg0);
 372 prog_char str_start_color[] = "blue#yellow";
 373 parse_pgm_token_string_t cmd_start_color = TOKEN_STRING_INITIALIZER(struct cmd_start_result, color, str_start_color);
 374 prog_char str_start_debug[] = "debug#match";
 375 parse_pgm_token_string_t cmd_start_debug = TOKEN_STRING_INITIALIZER(struct cmd_start_result, debug, str_start_debug);
 376
 377 prog_char help_start[] = "Start the robot";
 378 parse_pgm_inst_t cmd_start = {
 379         .f = cmd_start_parsed,  /* function to call */
 380         .data = NULL,      /* 2nd arg of func */
 381         .help_str = help_start,
 382         .tokens = {        /* token list, NULL terminated */
 383                 (prog_void *)&cmd_start_arg0,
 384                 (prog_void *)&cmd_start_color,
 385                 (prog_void *)&cmd_start_debug,
 386                 NULL,
 387         },
 388 };
 389
 390
 391
 392 /**********************************************************/
 393 /* Interact */
 394
 395 /* this structure is filled when cmd_interact is parsed successfully */
 396 struct cmd_interact_result {
 397         fixed_string_t arg0;
 398 };
 399
 400 static void print_cs(void)
 401 {
 402         printf_P(PSTR("cons_d=% .8"PRIi32" cons_a=% .8"PRIi32" fil_d=% .8"PRIi32" fil_a=% .8"PRIi32" "
 403                       "err_d=% .8"PRIi32" err_a=% .8"PRIi32" out_d=% .8"PRIi32" out_a=% .8"PRIi32"\r\n"),
 404                  cs_get_consign(&mainboard.distance.cs),
 405                  cs_get_consign(&mainboard.angle.cs),
 406                  cs_get_filtered_consign(&mainboard.distance.cs),
 407                  cs_get_filtered_consign(&mainboard.angle.cs),
 408                  cs_get_error(&mainboard.distance.cs),
 409                  cs_get_error(&mainboard.angle.cs),
 410                  cs_get_out(&mainboard.distance.cs),
 411                  cs_get_out(&mainboard.angle.cs));
 412 }
 413
 414 static void print_pos(void)
 415 {
 416         printf_P(PSTR("x=% .8d y=% .8d a=% .8d\r\n"),
 417                  position_get_x_s16(&mainboard.pos),
 418                  position_get_y_s16(&mainboard.pos),
 419                  position_get_a_deg_s16(&mainboard.pos));
 420 }
 421
 422 static void print_time(void)
 423 {
 424         printf_P(PSTR("time %d\r\n"),time_get_s());
 425 }
 426
 427
 428 static void print_sensors(void)
 429 {
 430 #ifdef notyet
 431         if (sensor_start_switch())
 432                 printf_P(PSTR("Start switch | "));
 433         else
 434                 printf_P(PSTR("             | "));
 435
 436         if (IR_DISP_SENSOR())
 437                 printf_P(PSTR("IR disp | "));
 438         else
 439                 printf_P(PSTR("        | "));
 440
 441         printf_P(PSTR("\r\n"));
 442 #endif
 443 }
 444
 445 static void print_pid(void)
 446 {
 447         printf_P(PSTR("P=% .8"PRIi32" I=% .8"PRIi32" D=% .8"PRIi32" out=% .8"PRIi32" | "
 448                       "P=% .8"PRIi32" I=% .8"PRIi32" D=% .8"PRIi32" out=% .8"PRIi32"\r\n"),
 449                  pid_get_value_in(&mainboard.distance.pid) * pid_get_gain_P(&mainboard.distance.pid),
 450                  pid_get_value_I(&mainboard.distance.pid) * pid_get_gain_I(&mainboard.distance.pid),
 451                  pid_get_value_D(&mainboard.distance.pid) * pid_get_gain_D(&mainboard.distance.pid),
 452                  pid_get_value_out(&mainboard.distance.pid),
 453                  pid_get_value_in(&mainboard.angle.pid) * pid_get_gain_P(&mainboard.angle.pid),
 454                  pid_get_value_I(&mainboard.angle.pid) * pid_get_gain_I(&mainboard.angle.pid),
 455                  pid_get_value_D(&mainboard.angle.pid) * pid_get_gain_D(&mainboard.angle.pid),
 456                  pid_get_value_out(&mainboard.angle.pid));
 457 }
 458
 459 #define PRINT_POS       (1<<0)
 460 #define PRINT_PID       (1<<1)
 461 #define PRINT_CS        (1<<2)
 462 #define PRINT_SENSORS   (1<<3)
 463 #define PRINT_TIME      (1<<4)
 464 #define PRINT_BLOCKING  (1<<5)
 465
 466 static void cmd_interact_parsed(void * parsed_result, void * data)
 467 {
 468         int c;
 469         int8_t cmd;
 470         uint8_t print = 0;
 471         struct vt100 vt100;
 472
 473         vt100_init(&vt100);
 474
 475         printf_P(PSTR("Display debugs:\r\n"
 476                       "  1:pos\r\n"
 477                       "  2:pid\r\n"
 478                       "  3:cs\r\n"
 479                       "  4:sensors\r\n"
 480                       "  5:time\r\n"
 481                       /* "  6:blocking\r\n" */
 482                       "Commands:\r\n"
 483                       "  arrows:move\r\n"
 484                       "  space:stop\r\n"
 485                       "  q:quit\r\n"));
 486
 487         /* stop motors */
 488         mainboard.flags &= (~DO_CS);
 489         pwm_set_and_save(LEFT_PWM, 0);
 490         pwm_set_and_save(RIGHT_PWM, 0);
 491
 492         while(1) {
 493                 if (print & PRINT_POS) {
 494                         print_pos();
 495                 }
 496
 497                 if (print & PRINT_PID) {
 498                         print_pid();
 499                 }
 500
 501                 if (print & PRINT_CS) {
 502                         print_cs();
 503                 }
 504
 505                 if (print & PRINT_SENSORS) {
 506                         print_sensors();
 507                 }
 508
 509                 if (print & PRINT_TIME) {
 510                         print_time();
 511                 }
 512 /*              if (print & PRINT_BLOCKING) { */
 513 /*                      printf_P(PSTR("%s %s blocking=%d\r\n"),  */
 514 /*                               mainboard.blocking ? "BLOCK1":"      ", */
 515 /*                               rs_is_blocked(&mainboard.rs) ? "BLOCK2":"      ", */
 516 /*                               rs_get_blocking(&mainboard.rs)); */
 517 /*              } */
 518
 519                 c = cmdline_getchar();
 520                 if (c == -1) {
 521                         wait_ms(10);
 522                         continue;
 523                 }
 524                 cmd = vt100_parser(&vt100, c);
 525                 if (cmd == -2) {
 526                         wait_ms(10);
 527                         continue;
 528                 }
 529
 530                 if (cmd == -1) {
 531                         switch(c) {
 532                         case '1': print ^= PRINT_POS; break;
 533                         case '2': print ^= PRINT_PID; break;
 534                         case '3': print ^= PRINT_CS; break;
 535                         case '4': print ^= PRINT_SENSORS; break;
 536                         case '5': print ^= PRINT_TIME; break;
 537                         case '6': print ^= PRINT_BLOCKING; break;
 538
 539                         case 'q':
 540                                 if (mainboard.flags & DO_CS)
 541                                         strat_hardstop();
 542                                 pwm_set_and_save(LEFT_PWM, 0);
 543                                 pwm_set_and_save(RIGHT_PWM, 0);
 544                                 return;
 545                         case ' ':
 546                                 pwm_set_and_save(LEFT_PWM, 0);
 547                                 pwm_set_and_save(RIGHT_PWM, 0);
 548                                 break;
 549                         default:
 550                                 break;
 551                         }
 552                 }
 553                 else {
 554 #ifdef HOST_VERSION
 555 #define PWM_INTERACT 1200
 556 #else
 557 #define PWM_INTERACT 1200
 558 #endif
 559                         switch(cmd) {
 560                         case KEY_UP_ARR:
 561                                 pwm_set_and_save(LEFT_PWM, PWM_INTERACT);
 562                                 pwm_set_and_save(RIGHT_PWM, PWM_INTERACT);
 563                                 break;
 564                         case KEY_LEFT_ARR:
 565                                 pwm_set_and_save(LEFT_PWM, -PWM_INTERACT);
 566                                 pwm_set_and_save(RIGHT_PWM, PWM_INTERACT);
 567                                 break;
 568                         case KEY_DOWN_ARR:
 569                                 pwm_set_and_save(LEFT_PWM, -PWM_INTERACT);
 570                                 pwm_set_and_save(RIGHT_PWM, -PWM_INTERACT);
 571                                 break;
 572                         case KEY_RIGHT_ARR:
 573                                 pwm_set_and_save(LEFT_PWM, PWM_INTERACT);
 574                                 pwm_set_and_save(RIGHT_PWM, -PWM_INTERACT);
 575                                 break;
 576                         }
 577                 }
 578                 wait_ms(10);
 579         }
 580 }
 581
 582 prog_char str_interact_arg0[] = "interact";
 583 parse_pgm_token_string_t cmd_interact_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_interact_result, arg0, str_interact_arg0);
 584
 585 prog_char help_interact[] = "Interactive mode";
 586 parse_pgm_inst_t cmd_interact = {
 587         .f = cmd_interact_parsed,  /* function to call */
 588         .data = NULL,      /* 2nd arg of func */
 589         .help_str = help_interact,
 590         .tokens = {        /* token list, NULL terminated */
 591                 (prog_void *)&cmd_interact_arg0,
 592                 NULL,
 593         },
 594 };
 595
 596
 597 /**********************************************************/
 598 /* Color */
 599
 600 /* this structure is filled when cmd_color is parsed successfully */
 601 struct cmd_color_result {
 602         fixed_string_t arg0;
 603         fixed_string_t color;
 604 };
 605
 606 /* function called when cmd_color is parsed successfully */
 607 static void cmd_color_parsed(void *parsed_result, void *data)
 608 {
 609 #ifdef HOST_VERSION
 610         printf("not implemented\n");
 611 #else
 612         struct cmd_color_result *res = (struct cmd_color_result *) parsed_result;
 613         if (!strcmp_P(res->color, PSTR("yellow"))) {
 614                 mainboard.our_color = I2C_COLOR_YELLOW;
 615                 i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_YELLOW);
 616                 i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_YELLOW);
 617                 beacon_set_color(I2C_COLOR_YELLOW);
 618         }
 619         else if (!strcmp_P(res->color, PSTR("blue"))) {
 620                 mainboard.our_color = I2C_COLOR_BLUE;
 621                 i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_BLUE);
 622                 i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_BLUE);
 623                 beacon_set_color(I2C_COLOR_BLUE);
 624         }
 625         printf_P(PSTR("Done\r\n"));
 626 #endif
 627 }
 628
 629 prog_char str_color_arg0[] = "color";
 630 parse_pgm_token_string_t cmd_color_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_color_result, arg0, str_color_arg0);
 631 prog_char str_color_color[] = "blue#yellow";
 632 parse_pgm_token_string_t cmd_color_color = TOKEN_STRING_INITIALIZER(struct cmd_color_result, color, str_color_color);
 633
 634 prog_char help_color[] = "Set our color";
 635 parse_pgm_inst_t cmd_color = {
 636         .f = cmd_color_parsed,  /* function to call */
 637         .data = NULL,      /* 2nd arg of func */
 638         .help_str = help_color,
 639         .tokens = {        /* token list, NULL terminated */
 640                 (prog_void *)&cmd_color_arg0,
 641                 (prog_void *)&cmd_color_color,
 642                 NULL,
 643         },
 644 };
 645
 646
 647 /**********************************************************/
 648 /* Rs tests */
 649
 650 /* this structure is filled when cmd_rs is parsed successfully */
 651 struct cmd_rs_result {
 652         fixed_string_t arg0;
 653         fixed_string_t arg1;
 654 };
 655
 656 /* function called when cmd_rs is parsed successfully */
 657 static void cmd_rs_parsed(void *parsed_result, void *data)
 658 {
 659         //      struct cmd_rs_result *res = parsed_result;
 660         do {
 661                 printf_P(PSTR("angle cons=% .6"PRIi32" in=% .6"PRIi32" out=% .6"PRIi32" / "),
 662                          cs_get_consign(&mainboard.angle.cs),
 663                          cs_get_filtered_feedback(&mainboard.angle.cs),
 664                          cs_get_out(&mainboard.angle.cs));
 665                 printf_P(PSTR("distance cons=% .6"PRIi32" in=% .6"PRIi32" out=% .6"PRIi32" / "),
 666                          cs_get_consign(&mainboard.distance.cs),
 667                          cs_get_filtered_feedback(&mainboard.distance.cs),
 668                          cs_get_out(&mainboard.distance.cs));
 669                 printf_P(PSTR("l=% .4"PRIi32" r=% .4"PRIi32"\r\n"), mainboard.pwm_l,
 670                          mainboard.pwm_r);
 671                 wait_ms(100);
 672         } while(!cmdline_keypressed());
 673 }
 674
 675 prog_char str_rs_arg0[] = "rs";
 676 parse_pgm_token_string_t cmd_rs_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_rs_result, arg0, str_rs_arg0);
 677 prog_char str_rs_arg1[] = "show";
 678 parse_pgm_token_string_t cmd_rs_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_rs_result, arg1, str_rs_arg1);
 679
 680 prog_char help_rs[] = "Show rs (robot system) values";
 681 parse_pgm_inst_t cmd_rs = {
 682         .f = cmd_rs_parsed,  /* function to call */
 683         .data = NULL,      /* 2nd arg of func */
 684         .help_str = help_rs,
 685         .tokens = {        /* token list, NULL terminated */
 686                 (prog_void *)&cmd_rs_arg0,
 687                 (prog_void *)&cmd_rs_arg1,
 688                 NULL,
 689         },
 690 };
 691
 692 /**********************************************************/
 693 /* I2cdebug */
 694
 695 /* this structure is filled when cmd_i2cdebug is parsed successfully */
 696 struct cmd_i2cdebug_result {
 697         fixed_string_t arg0;
 698 };
 699
 700 /* function called when cmd_i2cdebug is parsed successfully */
 701 static void cmd_i2cdebug_parsed(void * parsed_result, void * data)
 702 {
 703 #ifdef HOST_VERSION
 704         printf("not implemented\n");
 705 #else
 706         i2c_debug();
 707         i2c_protocol_debug();
 708 #endif
 709 }
 710
 711 prog_char str_i2cdebug_arg0[] = "i2cdebug";
 712 parse_pgm_token_string_t cmd_i2cdebug_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_i2cdebug_result, arg0, str_i2cdebug_arg0);
 713
 714 prog_char help_i2cdebug[] = "I2c debug infos";
 715 parse_pgm_inst_t cmd_i2cdebug = {
 716         .f = cmd_i2cdebug_parsed,  /* function to call */
 717         .data = NULL,      /* 2nd arg of func */
 718         .help_str = help_i2cdebug,
 719         .tokens = {        /* token list, NULL terminated */
 720                 (prog_void *)&cmd_i2cdebug_arg0,
 721                 NULL,
 722         },
 723 };
 724
 725 /**********************************************************/
 726 /* Cobboard_Show */
 727
 728 /* this structure is filled when cmd_cobboard_show is parsed successfully */
 729 struct cmd_cobboard_show_result {
 730         fixed_string_t arg0;
 731         fixed_string_t arg1;
 732 };
 733
 734 /* function called when cmd_cobboard_show is parsed successfully */
 735 static void cmd_cobboard_show_parsed(void * parsed_result, void * data)
 736 {
 737 #ifdef HOST_VERSION
 738         printf("not implemented\n");
 739 #else
 740         printf_P(PSTR("mode = %x\r\n"), cobboard.mode);
 741         printf_P(PSTR("status = %x\r\n"), cobboard.status);
 742         printf_P(PSTR("cob_count = %x\r\n"), cobboard.cob_count);
 743         printf_P(PSTR("left_cobroller_speed = %d\r\n"), cobboard.left_cobroller_speed);
 744         printf_P(PSTR("right_cobroller_speed = %d\r\n"), cobboard.right_cobroller_speed);
 745 #endif
 746 }
 747
 748 prog_char str_cobboard_show_arg0[] = "cobboard";
 749 parse_pgm_token_string_t cmd_cobboard_show_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_show_result, arg0, str_cobboard_show_arg0);
 750 prog_char str_cobboard_show_arg1[] = "show";
 751 parse_pgm_token_string_t cmd_cobboard_show_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_show_result, arg1, str_cobboard_show_arg1);
 752
 753 prog_char help_cobboard_show[] = "show cobboard status";
 754 parse_pgm_inst_t cmd_cobboard_show = {
 755         .f = cmd_cobboard_show_parsed,  /* function to call */
 756         .data = NULL,      /* 2nd arg of func */
 757         .help_str = help_cobboard_show,
 758         .tokens = {        /* token list, NULL terminated */
 759                 (prog_void *)&cmd_cobboard_show_arg0,
 760                 (prog_void *)&cmd_cobboard_show_arg1,
 761                 NULL,
 762         },
 763 };
 764
 765 /**********************************************************/
 766 /* Cobboard_Setmode1 */
 767
 768 /* this structure is filled when cmd_cobboard_setmode1 is parsed successfully */
 769 struct cmd_cobboard_setmode1_result {
 770         fixed_string_t arg0;
 771         fixed_string_t arg1;
 772 };
 773
 774 /* function called when cmd_cobboard_setmode1 is parsed successfully */
 775 static void cmd_cobboard_setmode1_parsed(void *parsed_result, void *data)
 776 {
 777         struct cmd_cobboard_setmode1_result *res = parsed_result;
 778
 779         if (!strcmp_P(res->arg1, PSTR("init")))
 780                 i2c_cobboard_set_mode(I2C_COBBOARD_MODE_INIT);
 781         else if (!strcmp_P(res->arg1, PSTR("eject")))
 782                 i2c_cobboard_set_mode(I2C_COBBOARD_MODE_EJECT);
 783         else if (!strcmp_P(res->arg1, PSTR("kickstand_up")))
 784                 i2c_cobboard_set_mode(I2C_COBBOARD_MODE_KICKSTAND_UP);
 785         else if (!strcmp_P(res->arg1, PSTR("kickstand_down")))
 786                 i2c_cobboard_set_mode(I2C_COBBOARD_MODE_KICKSTAND_DOWN);
 787 }
 788
 789 prog_char str_cobboard_setmode1_arg0[] = "cobboard";
 790 parse_pgm_token_string_t cmd_cobboard_setmode1_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode1_result, arg0, str_cobboard_setmode1_arg0);
 791 prog_char str_cobboard_setmode1_arg1[] = "init#eject#kickstand_up#kickstand_down";
 792 parse_pgm_token_string_t cmd_cobboard_setmode1_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode1_result, arg1, str_cobboard_setmode1_arg1);
 793
 794 prog_char help_cobboard_setmode1[] = "set cobboard mode (mode)";
 795 parse_pgm_inst_t cmd_cobboard_setmode1 = {
 796         .f = cmd_cobboard_setmode1_parsed,  /* function to call */
 797         .data = NULL,      /* 2nd arg of func */
 798         .help_str = help_cobboard_setmode1,
 799         .tokens = {        /* token list, NULL terminated */
 800                 (prog_void *)&cmd_cobboard_setmode1_arg0,
 801                 (prog_void *)&cmd_cobboard_setmode1_arg1,
 802                 NULL,
 803         },
 804 };
 805
 806 /**********************************************************/
 807 /* Cobboard_Setmode2 */
 808
 809 /* this structure is filled when cmd_cobboard_setmode2 is parsed successfully */
 810 struct cmd_cobboard_setmode2_result {
 811         fixed_string_t arg0;
 812         fixed_string_t arg1;
 813         fixed_string_t arg2;
 814 };
 815
 816 /* function called when cmd_cobboard_setmode2 is parsed successfully */
 817 static void cmd_cobboard_setmode2_parsed(void *parsed_result, void *data)
 818 {
 819         struct cmd_cobboard_setmode2_result *res = parsed_result;
 820         uint8_t side = I2C_LEFT_SIDE;
 821
 822         if (!strcmp_P(res->arg2, PSTR("left")))
 823                 side = I2C_LEFT_SIDE;
 824         else if (!strcmp_P(res->arg2, PSTR("right")))
 825                 side = I2C_RIGHT_SIDE;
 826
 827         if (!strcmp_P(res->arg1, PSTR("deploy"))) {
 828                 i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST);
 829                 i2c_cobboard_deploy(side);
 830         }
 831         else if (!strcmp_P(res->arg1, PSTR("harvest"))) {
 832                 i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST);
 833                 i2c_cobboard_autoharvest(side);
 834         }
 835         else if (!strcmp_P(res->arg1, PSTR("pack"))) {
 836                 i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST);
 837                 i2c_cobboard_pack(side);
 838         }
 839         else if (!strcmp_P(res->arg1, PSTR("weak_pack"))) {
 840                 i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST);
 841                 i2c_cobboard_pack_weak(side);
 842         }
 843         else if (!strcmp_P(res->arg1, PSTR("deploy_nomove"))) {
 844                 i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST);
 845                 i2c_cobboard_deploy_nomove(side);
 846         }
 847         else if (!strcmp_P(res->arg1, PSTR("harvest_nomove"))) {
 848                 i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST);
 849                 i2c_cobboard_autoharvest_nomove(side);
 850         }
 851 }
 852
 853 prog_char str_cobboard_setmode2_arg0[] = "cobboard";
 854 parse_pgm_token_string_t cmd_cobboard_setmode2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode2_result, arg0, str_cobboard_setmode2_arg0);
 855 prog_char str_cobboard_setmode2_arg1[] = "harvest#deploy#pack#weak_pack#harvest_nomove#deploy_nomove";
 856 parse_pgm_token_string_t cmd_cobboard_setmode2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode2_result, arg1, str_cobboard_setmode2_arg1);
 857 prog_char str_cobboard_setmode2_arg2[] = "left#right";
 858 parse_pgm_token_string_t cmd_cobboard_setmode2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode2_result, arg2, str_cobboard_setmode2_arg2);
 859
 860 prog_char help_cobboard_setmode2[] = "set cobboard mode (mode, side)";
 861 parse_pgm_inst_t cmd_cobboard_setmode2 = {
 862         .f = cmd_cobboard_setmode2_parsed,  /* function to call */
 863         .data = NULL,      /* 2nd arg of func */
 864         .help_str = help_cobboard_setmode2,
 865         .tokens = {        /* token list, NULL terminated */
 866                 (prog_void *)&cmd_cobboard_setmode2_arg0,
 867                 (prog_void *)&cmd_cobboard_setmode2_arg1,
 868                 (prog_void *)&cmd_cobboard_setmode2_arg2,
 869                 NULL,
 870         },
 871 };
 872
 873 /**********************************************************/
 874 /* Cobboard_Setmode3 */
 875
 876 /* this structure is filled when cmd_cobboard_setmode3 is parsed successfully */
 877 struct cmd_cobboard_setmode3_result {
 878         fixed_string_t arg0;
 879         fixed_string_t arg1;
 880         uint8_t level;
 881 };
 882
 883 /* function called when cmd_cobboard_setmode3 is parsed successfully */
 884 static void cmd_cobboard_setmode3_parsed(void *parsed_result, void *data)
 885 {
 886         struct cmd_cobboard_setmode3_result *res = parsed_result;
 887         if (!strcmp_P(res->arg1, PSTR("xxx")))
 888                 printf("faux\r\n");
 889 }
 890
 891 prog_char str_cobboard_setmode3_arg0[] = "cobboard";
 892 parse_pgm_token_string_t cmd_cobboard_setmode3_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode3_result, arg0, str_cobboard_setmode3_arg0);
 893 prog_char str_cobboard_setmode3_arg1[] = "xxx";
 894 parse_pgm_token_string_t cmd_cobboard_setmode3_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode3_result, arg1, str_cobboard_setmode3_arg1);
 895 parse_pgm_token_num_t cmd_cobboard_setmode3_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_cobboard_setmode3_result, level, UINT8);
 896
 897 prog_char help_cobboard_setmode3[] = "set cobboard mode (mode, level)";
 898 parse_pgm_inst_t cmd_cobboard_setmode3 = {
 899         .f = cmd_cobboard_setmode3_parsed,  /* function to call */
 900         .data = NULL,      /* 2nd arg of func */
 901         .help_str = help_cobboard_setmode3,
 902         .tokens = {        /* token list, NULL terminated */
 903                 (prog_void *)&cmd_cobboard_setmode3_arg0,
 904                 (prog_void *)&cmd_cobboard_setmode3_arg1,
 905                 (prog_void *)&cmd_cobboard_setmode3_arg2,
 906                 NULL,
 907         },
 908 };
 909
 910 /**********************************************************/
 911 /* Ballboard_Show */
 912
 913 /* this structure is filled when cmd_ballboard_show is parsed successfully */
 914 struct cmd_ballboard_show_result {
 915         fixed_string_t arg0;
 916         fixed_string_t arg1;
 917 };
 918
 919 /* function called when cmd_ballboard_show is parsed successfully */
 920 static void cmd_ballboard_show_parsed(void * parsed_result, void * data)
 921 {
 922 #ifdef HOST_VERSION
 923         printf("not implemented\n");
 924 #else
 925         printf_P(PSTR("mode = %x\r\n"), ballboard.mode);
 926         printf_P(PSTR("status = %x\r\n"), ballboard.status);
 927         printf_P(PSTR("ball_count = %d\r\n"), ballboard.ball_count);
 928         printf_P(PSTR("lcob = %d\r\n"), ballboard.lcob);
 929         printf_P(PSTR("rcob = %d\r\n"), ballboard.rcob);
 930 #endif
 931 }
 932
 933 prog_char str_ballboard_show_arg0[] = "ballboard";
 934 parse_pgm_token_string_t cmd_ballboard_show_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_show_result, arg0, str_ballboard_show_arg0);
 935 prog_char str_ballboard_show_arg1[] = "show";
 936 parse_pgm_token_string_t cmd_ballboard_show_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_show_result, arg1, str_ballboard_show_arg1);
 937
 938 prog_char help_ballboard_show[] = "show ballboard status";
 939 parse_pgm_inst_t cmd_ballboard_show = {
 940         .f = cmd_ballboard_show_parsed,  /* function to call */
 941         .data = NULL,      /* 2nd arg of func */
 942         .help_str = help_ballboard_show,
 943         .tokens = {        /* token list, NULL terminated */
 944                 (prog_void *)&cmd_ballboard_show_arg0,
 945                 (prog_void *)&cmd_ballboard_show_arg1,
 946                 NULL,
 947         },
 948 };
 949
 950 /**********************************************************/
 951 /* Ballboard_Setmode1 */
 952
 953 /* this structure is filled when cmd_ballboard_setmode1 is parsed successfully */
 954 struct cmd_ballboard_setmode1_result {
 955         fixed_string_t arg0;
 956         fixed_string_t arg1;
 957 };
 958
 959 /* function called when cmd_ballboard_setmode1 is parsed successfully */
 960 static void cmd_ballboard_setmode1_parsed(void *parsed_result, void *data)
 961 {
 962         struct cmd_ballboard_setmode1_result *res = parsed_result;
 963
 964         if (!strcmp_P(res->arg1, PSTR("init")))
 965                 i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_INIT);
 966         else if (!strcmp_P(res->arg1, PSTR("off")))
 967                 i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_OFF);
 968         else if (!strcmp_P(res->arg1, PSTR("eject")))
 969                 i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_EJECT);
 970         else if (!strcmp_P(res->arg1, PSTR("harvest")))
 971                 i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_HARVEST);
 972         else if (!strcmp_P(res->arg1, PSTR("prepare")))
 973                 i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_PREP_FORK);
 974         else if (!strcmp_P(res->arg1, PSTR("take")))
 975                 i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_TAKE_FORK);
 976
 977         /* other commands */
 978 }
 979
 980 prog_char str_ballboard_setmode1_arg0[] = "ballboard";
 981 parse_pgm_token_string_t cmd_ballboard_setmode1_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode1_result, arg0, str_ballboard_setmode1_arg0);
 982 prog_char str_ballboard_setmode1_arg1[] = "init#eject#harvest#off#take#prepare";
 983 parse_pgm_token_string_t cmd_ballboard_setmode1_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode1_result, arg1, str_ballboard_setmode1_arg1);
 984
 985 prog_char help_ballboard_setmode1[] = "set ballboard mode (mode)";
 986 parse_pgm_inst_t cmd_ballboard_setmode1 = {
 987         .f = cmd_ballboard_setmode1_parsed,  /* function to call */
 988         .data = NULL,      /* 2nd arg of func */
 989         .help_str = help_ballboard_setmode1,
 990         .tokens = {        /* token list, NULL terminated */
 991                 (prog_void *)&cmd_ballboard_setmode1_arg0,
 992                 (prog_void *)&cmd_ballboard_setmode1_arg1,
 993                 NULL,
 994         },
 995 };
 996
 997 /**********************************************************/
 998 /* Ballboard_Setmode2 */
 999
1000 /* this structure is filled when cmd_ballboard_setmode2 is parsed successfully */
1001 struct cmd_ballboard_setmode2_result {
1002         fixed_string_t arg0;
1003         fixed_string_t arg1;
1004         fixed_string_t arg2;
1005 };
1006
1007 /* function called when cmd_ballboard_setmode2 is parsed successfully */
1008 static void cmd_ballboard_setmode2_parsed(void * parsed_result, void * data)
1009 {
1010 }
1011
1012 prog_char str_ballboard_setmode2_arg0[] = "ballboard";
1013 parse_pgm_token_string_t cmd_ballboard_setmode2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode2_result, arg0, str_ballboard_setmode2_arg0);
1014 prog_char str_ballboard_setmode2_arg1[] = "xxx";
1015 parse_pgm_token_string_t cmd_ballboard_setmode2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode2_result, arg1, str_ballboard_setmode2_arg1);
1016 prog_char str_ballboard_setmode2_arg2[] = "left#right";
1017 parse_pgm_token_string_t cmd_ballboard_setmode2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode2_result, arg2, str_ballboard_setmode2_arg2);
1018
1019 prog_char help_ballboard_setmode2[] = "set ballboard mode (mode, side)";
1020 parse_pgm_inst_t cmd_ballboard_setmode2 = {
1021         .f = cmd_ballboard_setmode2_parsed,  /* function to call */
1022         .data = NULL,      /* 2nd arg of func */
1023         .help_str = help_ballboard_setmode2,
1024         .tokens = {        /* token list, NULL terminated */
1025                 (prog_void *)&cmd_ballboard_setmode2_arg0,
1026                 (prog_void *)&cmd_ballboard_setmode2_arg1,
1027                 (prog_void *)&cmd_ballboard_setmode2_arg2,
1028                 NULL,
1029         },
1030 };
1031
1032 /**********************************************************/
1033 /* Ballboard_Setmode3 */
1034
1035 /* this structure is filled when cmd_ballboard_setmode3 is parsed successfully */
1036 struct cmd_ballboard_setmode3_result {
1037         fixed_string_t arg0;
1038         fixed_string_t arg1;
1039         uint8_t level;
1040 };
1041
1042 /* function called when cmd_ballboard_setmode3 is parsed successfully */
1043 static void cmd_ballboard_setmode3_parsed(void *parsed_result, void *data)
1044 {
1045         struct cmd_ballboard_setmode3_result *res = parsed_result;
1046         if (!strcmp_P(res->arg1, PSTR("xxx")))
1047                 printf("faux\r\n");
1048 }
1049
1050 prog_char str_ballboard_setmode3_arg0[] = "ballboard";
1051 parse_pgm_token_string_t cmd_ballboard_setmode3_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode3_result, arg0, str_ballboard_setmode3_arg0);
1052 prog_char str_ballboard_setmode3_arg1[] = "xxx";
1053 parse_pgm_token_string_t cmd_ballboard_setmode3_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode3_result, arg1, str_ballboard_setmode3_arg1);
1054 parse_pgm_token_num_t cmd_ballboard_setmode3_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_ballboard_setmode3_result, level, UINT8);
1055
1056 prog_char help_ballboard_setmode3[] = "set ballboard mode (mode, level)";
1057 parse_pgm_inst_t cmd_ballboard_setmode3 = {
1058         .f = cmd_ballboard_setmode3_parsed,  /* function to call */
1059         .data = NULL,      /* 2nd arg of func */
1060         .help_str = help_ballboard_setmode3,
1061         .tokens = {        /* token list, NULL terminated */
1062                 (prog_void *)&cmd_ballboard_setmode3_arg0,
1063                 (prog_void *)&cmd_ballboard_setmode3_arg1,
1064                 (prog_void *)&cmd_ballboard_setmode3_arg2,
1065                 NULL,
1066         },
1067 };
1068
1069 /**********************************************************/
1070 /* Servo_Balls */
1071
1072 /* this structure is filled when cmd_servo_balls is parsed successfully */
1073 struct cmd_servo_balls_result {
1074         fixed_string_t arg0;
1075         fixed_string_t arg1;
1076 };
1077
1078 /* function called when cmd_servo_balls is parsed successfully */
1079 static void cmd_servo_balls_parsed(void *parsed_result,
1080                                    __attribute__((unused)) void *data)
1081 {
1082         struct cmd_servo_balls_result *res = parsed_result;
1083
1084         if (!strcmp_P(res->arg1, PSTR("deploy")))
1085                 support_balls_deploy();
1086         else if (!strcmp_P(res->arg1, PSTR("pack")))
1087                 support_balls_pack();
1088 }
1089
1090 prog_char str_servo_balls_arg0[] = "support_balls";
1091 parse_pgm_token_string_t cmd_servo_balls_arg0 =
1092         TOKEN_STRING_INITIALIZER(struct cmd_servo_balls_result, arg0, str_servo_balls_arg0);
1093 prog_char str_servo_balls_arg1[] = "deploy#pack";
1094 parse_pgm_token_string_t cmd_servo_balls_arg1 =
1095         TOKEN_STRING_INITIALIZER(struct cmd_servo_balls_result, arg1, str_servo_balls_arg1);
1096
1097 prog_char help_servo_balls[] = "control support balls";
1098 parse_pgm_inst_t cmd_servo_balls = {
1099         .f = cmd_servo_balls_parsed,  /* function to call */
1100         .data = NULL,      /* 2nd arg of func */
1101         .help_str = help_servo_balls,
1102         .tokens = {        /* token list, NULL terminated */
1103                 (prog_void *)&cmd_servo_balls_arg0,
1104                 (prog_void *)&cmd_servo_balls_arg1,
1105                 NULL,
1106         },
1107 };
1108
1109 /**********************************************************/
1110 /* Clitoid */
1111
1112 /* this structure is filled when cmd_clitoid is parsed successfully */
1113 struct cmd_clitoid_result {
1114         fixed_string_t arg0;
1115         float alpha_deg;
1116         float beta_deg;
1117         float R_mm;
1118         float Vd;
1119         float Amax;
1120         float d_inter_mm;
1121 };
1122
1123 /* function called when cmd_test is parsed successfully */
1124 static void cmd_clitoid_parsed(void *parsed_result, void *data)
1125 {
1126         struct cmd_clitoid_result *res = parsed_result;
1127 /*      clitoid(res->alpha_deg, res->beta_deg, res->R_mm, */
1128 /*              res->Vd, res->Amax, res->d_inter_mm); */
1129         double x = position_get_x_double(&mainboard.pos);
1130         double y = position_get_y_double(&mainboard.pos);
1131         double a = position_get_a_rad_double(&mainboard.pos);
1132
1133         strat_set_speed(res->Vd, SPEED_ANGLE_FAST);
1134         trajectory_clitoid(&mainboard.traj, x, y, a, 150.,
1135                            res->alpha_deg, res->beta_deg, res->R_mm,
1136                            res->d_inter_mm);
1137 }
1138
1139 prog_char str_clitoid_arg0[] = "clitoid";
1140 parse_pgm_token_string_t cmd_clitoid_arg0 =
1141         TOKEN_STRING_INITIALIZER(struct cmd_clitoid_result,
1142                                  arg0, str_clitoid_arg0);
1143 parse_pgm_token_num_t cmd_clitoid_alpha_deg =
1144         TOKEN_NUM_INITIALIZER(struct cmd_clitoid_result,
1145                               alpha_deg, FLOAT);
1146 parse_pgm_token_num_t cmd_clitoid_beta_deg =
1147         TOKEN_NUM_INITIALIZER(struct cmd_clitoid_result,
1148                               beta_deg, FLOAT);
1149 parse_pgm_token_num_t cmd_clitoid_R_mm =
1150         TOKEN_NUM_INITIALIZER(struct cmd_clitoid_result,
1151                               R_mm, FLOAT);
1152 parse_pgm_token_num_t cmd_clitoid_Vd =
1153         TOKEN_NUM_INITIALIZER(struct cmd_clitoid_result,
1154                               Vd, FLOAT);
1155 parse_pgm_token_num_t cmd_clitoid_Amax =
1156         TOKEN_NUM_INITIALIZER(struct cmd_clitoid_result,
1157                               Amax, FLOAT);
1158 parse_pgm_token_num_t cmd_clitoid_d_inter_mm =
1159         TOKEN_NUM_INITIALIZER(struct cmd_clitoid_result,
1160                               d_inter_mm, FLOAT);
1161
1162 prog_char help_clitoid[] = "do a clitoid (alpha, beta, R, Vd, Amax, d_inter)";
1163 parse_pgm_inst_t cmd_clitoid = {
1164         .f = cmd_clitoid_parsed,  /* function to call */
1165         .data = NULL,      /* 2nd arg of func */
1166         .help_str = help_clitoid,
1167         .tokens = {        /* token list, NULL terminated */
1168                 (prog_void *)&cmd_clitoid_arg0,
1169                 (prog_void *)&cmd_clitoid_alpha_deg,
1170                 (prog_void *)&cmd_clitoid_beta_deg,
1171                 (prog_void *)&cmd_clitoid_R_mm,
1172                 (prog_void *)&cmd_clitoid_Vd,
1173                 (prog_void *)&cmd_clitoid_Amax,
1174                 (prog_void *)&cmd_clitoid_d_inter_mm,
1175                 NULL,
1176         },
1177 };
1178
1179 /**********************************************************/
1180 /* Time_Monitor */
1181
1182 /* this structure is filled when cmd_time_monitor is parsed successfully */
1183 struct cmd_time_monitor_result {
1184         fixed_string_t arg0;
1185         fixed_string_t arg1;
1186 };
1187
1188 /* function called when cmd_time_monitor is parsed successfully */
1189 static void cmd_time_monitor_parsed(void *parsed_result, void *data)
1190 {
1191 #ifndef HOST_VERSION
1192         struct cmd_time_monitor_result *res = parsed_result;
1193         uint16_t seconds;
1194
1195         if (!strcmp_P(res->arg1, PSTR("reset"))) {
1196                 eeprom_write_word(EEPROM_TIME_ADDRESS, 0);
1197         }
1198         seconds = eeprom_read_word(EEPROM_TIME_ADDRESS);
1199         printf_P(PSTR("Running since %d mn %d\r\n"), seconds/60, seconds%60);
1200 #endif
1201 }
1202
1203 prog_char str_time_monitor_arg0[] = "time_monitor";
1204 parse_pgm_token_string_t cmd_time_monitor_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_time_monitor_result, arg0, str_time_monitor_arg0);
1205 prog_char str_time_monitor_arg1[] = "show#reset";
1206 parse_pgm_token_string_t cmd_time_monitor_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_time_monitor_result, arg1, str_time_monitor_arg1);
1207
1208 prog_char help_time_monitor[] = "Show since how long we are running";
1209 parse_pgm_inst_t cmd_time_monitor = {
1210         .f = cmd_time_monitor_parsed,  /* function to call */
1211         .data = NULL,      /* 2nd arg of func */
1212         .help_str = help_time_monitor,
1213         .tokens = {        /* token list, NULL terminated */
1214                 (prog_void *)&cmd_time_monitor_arg0,
1215                 (prog_void *)&cmd_time_monitor_arg1,
1216                 NULL,
1217         },
1218 };
1219
1220
1221 /**********************************************************/
1222 /* Strat_Event */
1223
1224 /* this structure is filled when cmd_strat_event is parsed successfully */
1225 struct cmd_strat_event_result {
1226         fixed_string_t arg0;
1227         fixed_string_t arg1;
1228 };
1229
1230 /* function called when cmd_strat_event is parsed successfully */
1231 static void cmd_strat_event_parsed(void *parsed_result, void *data)
1232 {
1233         struct cmd_strat_event_result *res = parsed_result;
1234
1235         if (!strcmp_P(res->arg1, PSTR("on")))
1236                 strat_event_enable();
1237         else
1238                 strat_event_disable();
1239 }
1240
1241 prog_char str_strat_event_arg0[] = "strat_event";
1242 parse_pgm_token_string_t cmd_strat_event_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_event_result, arg0, str_strat_event_arg0);
1243 prog_char str_strat_event_arg1[] = "on#off";
1244 parse_pgm_token_string_t cmd_strat_event_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_event_result, arg1, str_strat_event_arg1);
1245
1246 prog_char help_strat_event[] = "Enable/disable strat_event callback";
1247 parse_pgm_inst_t cmd_strat_event = {
1248         .f = cmd_strat_event_parsed,  /* function to call */
1249         .data = NULL,      /* 2nd arg of func */
1250         .help_str = help_strat_event,
1251         .tokens = {        /* token list, NULL terminated */
1252                 (prog_void *)&cmd_strat_event_arg0,
1253                 (prog_void *)&cmd_strat_event_arg1,
1254                 NULL,
1255         },
1256 };
1257
1258 /**********************************************************/
1259 /* Sleep */
1260
1261 /* this structure is filled when cmd_sleep is parsed successfully */
1262 struct cmd_sleep_result {
1263         fixed_string_t arg0;
1264         uint32_t ms;
1265 };
1266
1267 /* function called when cmd_sleep is parsed successfully */
1268 static void cmd_sleep_parsed(void *parsed_result, void *data)
1269 {
1270         struct cmd_sleep_result *res = parsed_result;
1271         time_wait_ms(res->ms);
1272 }
1273
1274 prog_char str_sleep_arg0[] = "sleep";
1275 parse_pgm_token_string_t cmd_sleep_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_sleep_result, arg0, str_sleep_arg0);
1276 parse_pgm_token_num_t cmd_sleep_ms = TOKEN_NUM_INITIALIZER(struct cmd_sleep_result, ms, UINT32);
1277
1278 prog_char help_sleep[] = "Sleep during some miliseconds";
1279 parse_pgm_inst_t cmd_sleep = {
1280         .f = cmd_sleep_parsed,  /* function to call */
1281         .data = NULL,      /* 2nd arg of func */
1282         .help_str = help_sleep,
1283         .tokens = {        /* token list, NULL terminated */
1284                 (prog_void *)&cmd_sleep_arg0,
1285                 (prog_void *)&cmd_sleep_ms,
1286                 NULL,
1287         },
1288 };
1289
1290
1291 /**********************************************************/
1292 /* Test */
1293
1294 /* this structure is filled when cmd_test is parsed successfully */
1295 struct cmd_test_result {
1296         fixed_string_t arg0;
1297         int32_t radius;
1298         int32_t dist;
1299 };
1300
1301 /* function called when cmd_test is parsed successfully */
1302 static void cmd_test_parsed(void *parsed_result, void *data)
1303 {
1304         strat_db.dump_enabled = 1;
1305         strat_db_dump(__FUNCTION__);
1306
1307         corn_set_color(strat_db.corn_table[0], I2C_COB_BLACK);
1308         strat_db_dump(__FUNCTION__);
1309
1310         corn_set_color(strat_db.corn_table[3], I2C_COB_WHITE);
1311         strat_db_dump(__FUNCTION__);
1312         corn_set_color(strat_db.corn_table[4], I2C_COB_WHITE);
1313         strat_db_dump(__FUNCTION__);
1314         corn_set_color(strat_db.corn_table[5], I2C_COB_WHITE);
1315         strat_db_dump(__FUNCTION__);
1316 }
1317
1318 prog_char str_test_arg0[] = "test";
1319 parse_pgm_token_string_t cmd_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_test_result, arg0, str_test_arg0);
1320 parse_pgm_token_num_t cmd_test_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_test_result, radius, INT32);
1321 parse_pgm_token_num_t cmd_test_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_test_result, dist, INT32);
1322
1323 prog_char help_test[] = "Test function";
1324 parse_pgm_inst_t cmd_test = {
1325         .f = cmd_test_parsed,  /* function to call */
1326         .data = NULL,      /* 2nd arg of func */
1327         .help_str = help_test,
1328         .tokens = {        /* token list, NULL terminated */
1329                 (prog_void *)&cmd_test_arg0,
1330                 NULL,
1331         },
1332 };