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