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