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