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