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