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