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_utils.h>
  45 #include <vect_base.h>
  46 #include <lines.h>
  47 #include <polygon.h>
  48 #include <obstacle_avoidance.h>
  49 #include <blocking_detection_manager.h>
  50 #include <robot_system.h>
  51 #include <position_manager.h>
  52
  53 #include <rdline.h>
  54 #include <parse.h>
  55 #include <parse_string.h>
  56 #include <parse_num.h>
  57
  58 #include "../common/i2c_commands.h"
  59 #include "../common/eeprom_mapping.h"
  60
  61 #include "main.h"
  62 #include "robotsim.h"
  63 #include "sensor.h"
  64 #include "cmdline.h"
  65 #include "strat.h"
  66 #include "strat_utils.h"
  67 #include "strat_base.h"
  68 #include "i2c_protocol.h"
  69 #include "actuator.h"
  70
  71 struct cmd_event_result {
  72         fixed_string_t arg0;
  73         fixed_string_t arg1;
  74         fixed_string_t arg2;
  75 };
  76
  77
  78 /* function called when cmd_event is parsed successfully */
  79 static void cmd_event_parsed(void *parsed_result, void *data)
  80 {
  81         u08 bit=0;
  82
  83         struct cmd_event_result * res = parsed_result;
  84
  85         if (!strcmp_P(res->arg1, PSTR("all"))) {
  86                 bit = DO_ENCODERS | DO_CS | DO_RS | DO_POS |
  87                         DO_BD | DO_TIMER | DO_POWER;
  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                 }
 108                 return;
 109         }
 110
 111         if (!strcmp_P(res->arg1, PSTR("encoders")))
 112                 bit = DO_ENCODERS;
 113         else if (!strcmp_P(res->arg1, PSTR("cs"))) {
 114                 strat_hardstop();
 115                 bit = DO_CS;
 116         }
 117         else if (!strcmp_P(res->arg1, PSTR("rs")))
 118                 bit = DO_RS;
 119         else if (!strcmp_P(res->arg1, PSTR("pos")))
 120                 bit = DO_POS;
 121         else if (!strcmp_P(res->arg1, PSTR("bd")))
 122                 bit = DO_BD;
 123         else if (!strcmp_P(res->arg1, PSTR("timer"))) {
 124                 time_reset();
 125                 bit = DO_TIMER;
 126         }
 127         else if (!strcmp_P(res->arg1, PSTR("power")))
 128                 bit = DO_POWER;
 129
 130         if (!strcmp_P(res->arg2, PSTR("on")))
 131                 mainboard.flags |= bit;
 132         else if (!strcmp_P(res->arg2, PSTR("off"))) {
 133                 if (!strcmp_P(res->arg1, PSTR("cs"))) {
 134 #ifdef HOST_VERSION
 135                         robotsim_pwm(LEFT_PWM, 0);
 136                         robotsim_pwm(RIGHT_PWM, 0);
 137 #else
 138                         pwm_ng_set(LEFT_PWM, 0);
 139                         pwm_ng_set(RIGHT_PWM, 0);
 140 #endif
 141                 }
 142                 mainboard.flags &= (~bit);
 143         }
 144         printf_P(PSTR("%s is %s\r\n"), res->arg1,
 145                       (bit & mainboard.flags) ? "on":"off");
 146 }
 147
 148 prog_char str_event_arg0[] = "event";
 149 parse_pgm_token_string_t cmd_event_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg0, str_event_arg0);
 150 prog_char str_event_arg1[] = "all#encoders#cs#rs#pos#bd#timer#power";
 151 parse_pgm_token_string_t cmd_event_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg1, str_event_arg1);
 152 prog_char str_event_arg2[] = "on#off#show";
 153 parse_pgm_token_string_t cmd_event_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg2, str_event_arg2);
 154
 155 prog_char help_event[] = "Enable/disable events";
 156 parse_pgm_inst_t cmd_event = {
 157         .f = cmd_event_parsed,  /* function to call */
 158         .data = NULL,      /* 2nd arg of func */
 159         .help_str = help_event,
 160         .tokens = {        /* token list, NULL terminated */
 161                 (prog_void *)&cmd_event_arg0,
 162                 (prog_void *)&cmd_event_arg1,
 163                 (prog_void *)&cmd_event_arg2,
 164                 NULL,
 165         },
 166 };
 167
 168
 169 /**********************************************************/
 170 /* Spi_Test */
 171
 172 /* this structure is filled when cmd_spi_test is parsed successfully */
 173 struct cmd_spi_test_result {
 174         fixed_string_t arg0;
 175 };
 176
 177 /* function called when cmd_spi_test is parsed successfully */
 178 static void cmd_spi_test_parsed(void * parsed_result, void * data)
 179 {
 180 #ifdef HOST_VERSION
 181         printf("not implemented\n");
 182 #else
 183         uint16_t i = 0, ret = 0, ret2 = 0;
 184
 185         if (mainboard.flags & DO_ENCODERS) {
 186                 printf_P(PSTR("Disable encoder event first\r\n"));
 187                 return;
 188         }
 189
 190         do {
 191                 spi_slave_select(0);
 192                 ret = spi_send_and_receive_byte(i);
 193                 ret2 = spi_send_and_receive_byte(i);
 194                 spi_slave_deselect(0);
 195
 196                 if ((i & 0x7ff) == 0)
 197                         printf_P(PSTR("Sent %.4x twice, received %x %x\r\n"),
 198                                  i, ret, ret2);
 199
 200                 i++;
 201         } while(!cmdline_keypressed());
 202 #endif
 203 }
 204
 205 prog_char str_spi_test_arg0[] = "spi_test";
 206 parse_pgm_token_string_t cmd_spi_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_spi_test_result, arg0, str_spi_test_arg0);
 207
 208 prog_char help_spi_test[] = "Test the SPI";
 209 parse_pgm_inst_t cmd_spi_test = {
 210         .f = cmd_spi_test_parsed,  /* function to call */
 211         .data = NULL,      /* 2nd arg of func */
 212         .help_str = help_spi_test,
 213         .tokens = {        /* token list, NULL terminated */
 214                 (prog_void *)&cmd_spi_test_arg0,
 215                 NULL,
 216         },
 217 };
 218
 219
 220
 221 /**********************************************************/
 222 /* Opponent tests */
 223
 224 /* this structure is filled when cmd_opponent is parsed successfully */
 225 struct cmd_opponent_result {
 226         fixed_string_t arg0;
 227         fixed_string_t arg1;
 228         int32_t arg2;
 229         int32_t arg3;
 230 };
 231
 232 /* function called when cmd_opponent is parsed successfully */
 233 static void cmd_opponent_parsed(void *parsed_result, void *data)
 234 {
 235         int16_t x,y,d,a;
 236
 237         if (get_opponent_xyda(&x, &y, &d, &a))
 238                 printf_P(PSTR("No opponent\r\n"));
 239         else
 240                 printf_P(PSTR("x=%d y=%d, d=%d a=%d\r\n"), x, y, d, a);
 241 }
 242
 243 prog_char str_opponent_arg0[] = "opponent";
 244 parse_pgm_token_string_t cmd_opponent_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_opponent_result, arg0, str_opponent_arg0);
 245 prog_char str_opponent_arg1[] = "show";
 246 parse_pgm_token_string_t cmd_opponent_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_opponent_result, arg1, str_opponent_arg1);
 247
 248 prog_char help_opponent[] = "Show (x,y) opponent";
 249 parse_pgm_inst_t cmd_opponent = {
 250         .f = cmd_opponent_parsed,  /* function to call */
 251         .data = NULL,      /* 2nd arg of func */
 252         .help_str = help_opponent,
 253         .tokens = {        /* token list, NULL terminated */
 254                 (prog_void *)&cmd_opponent_arg0,
 255                 (prog_void *)&cmd_opponent_arg1,
 256                 NULL,
 257         },
 258 };
 259
 260
 261 prog_char str_opponent_arg1_set[] = "set";
 262 parse_pgm_token_string_t cmd_opponent_arg1_set = TOKEN_STRING_INITIALIZER(struct cmd_opponent_result, arg1, str_opponent_arg1_set);
 263 parse_pgm_token_num_t cmd_opponent_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_opponent_result, arg2, INT32);
 264 parse_pgm_token_num_t cmd_opponent_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_opponent_result, arg3, INT32);
 265
 266 prog_char help_opponent_set[] = "Set (x,y) opponent";
 267 parse_pgm_inst_t cmd_opponent_set = {
 268         .f = cmd_opponent_parsed,  /* function to call */
 269         .data = NULL,      /* 2nd arg of func */
 270         .help_str = help_opponent_set,
 271         .tokens = {        /* token list, NULL terminated */
 272                 (prog_void *)&cmd_opponent_arg0,
 273                 (prog_void *)&cmd_opponent_arg1_set,
 274                 (prog_void *)&cmd_opponent_arg2,
 275                 (prog_void *)&cmd_opponent_arg3,
 276                 NULL,
 277         },
 278 };
 279
 280
 281 /**********************************************************/
 282 /* Start */
 283
 284 /* this structure is filled when cmd_start is parsed successfully */
 285 struct cmd_start_result {
 286         fixed_string_t arg0;
 287         fixed_string_t color;
 288         fixed_string_t debug;
 289 };
 290
 291 /* function called when cmd_start is parsed successfully */
 292 static void cmd_start_parsed(void *parsed_result, void *data)
 293 {
 294 #ifdef HOST_VERSION
 295         printf("not implemented\n");
 296 #else
 297         struct cmd_start_result *res = parsed_result;
 298         uint8_t old_level = gen.log_level;
 299
 300         gen.logs[NB_LOGS] = E_USER_STRAT;
 301         if (!strcmp_P(res->debug, PSTR("debug"))) {
 302                 strat_infos.dump_enabled = 1;
 303                 gen.log_level = 5;
 304         }
 305         else {
 306                 strat_infos.dump_enabled = 0;
 307                 gen.log_level = 0;
 308         }
 309
 310         if (!strcmp_P(res->color, PSTR("yellow"))) {
 311                 mainboard.our_color = I2C_COLOR_YELLOW;
 312                 i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_YELLOW);
 313                 i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_YELLOW);
 314         }
 315         else if (!strcmp_P(res->color, PSTR("blue"))) {
 316                 mainboard.our_color = I2C_COLOR_BLUE;
 317                 i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_BLUE);
 318                 i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_BLUE);
 319         }
 320
 321         strat_start();
 322
 323         gen.logs[NB_LOGS] = 0;
 324         gen.log_level = old_level;
 325 #endif
 326 }
 327
 328 prog_char str_start_arg0[] = "start";
 329 parse_pgm_token_string_t cmd_start_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_start_result, arg0, str_start_arg0);
 330 prog_char str_start_color[] = "blue#yellow";
 331 parse_pgm_token_string_t cmd_start_color = TOKEN_STRING_INITIALIZER(struct cmd_start_result, color, str_start_color);
 332 prog_char str_start_debug[] = "debug#match";
 333 parse_pgm_token_string_t cmd_start_debug = TOKEN_STRING_INITIALIZER(struct cmd_start_result, debug, str_start_debug);
 334
 335 prog_char help_start[] = "Start the robot";
 336 parse_pgm_inst_t cmd_start = {
 337         .f = cmd_start_parsed,  /* function to call */
 338         .data = NULL,      /* 2nd arg of func */
 339         .help_str = help_start,
 340         .tokens = {        /* token list, NULL terminated */
 341                 (prog_void *)&cmd_start_arg0,
 342                 (prog_void *)&cmd_start_color,
 343                 (prog_void *)&cmd_start_debug,
 344                 NULL,
 345         },
 346 };
 347
 348
 349
 350 /**********************************************************/
 351 /* Interact */
 352
 353 /* this structure is filled when cmd_interact is parsed successfully */
 354 struct cmd_interact_result {
 355         fixed_string_t arg0;
 356 };
 357
 358 static void print_cs(void)
 359 {
 360         printf_P(PSTR("cons_d=% .8"PRIi32" cons_a=% .8"PRIi32" fil_d=% .8"PRIi32" fil_a=% .8"PRIi32" "
 361                       "err_d=% .8"PRIi32" err_a=% .8"PRIi32" out_d=% .8"PRIi32" out_a=% .8"PRIi32"\r\n"),
 362                  cs_get_consign(&mainboard.distance.cs),
 363                  cs_get_consign(&mainboard.angle.cs),
 364                  cs_get_filtered_consign(&mainboard.distance.cs),
 365                  cs_get_filtered_consign(&mainboard.angle.cs),
 366                  cs_get_error(&mainboard.distance.cs),
 367                  cs_get_error(&mainboard.angle.cs),
 368                  cs_get_out(&mainboard.distance.cs),
 369                  cs_get_out(&mainboard.angle.cs));
 370 }
 371
 372 static void print_pos(void)
 373 {
 374         printf_P(PSTR("x=% .8d y=% .8d a=% .8d\r\n"),
 375                  position_get_x_s16(&mainboard.pos),
 376                  position_get_y_s16(&mainboard.pos),
 377                  position_get_a_deg_s16(&mainboard.pos));
 378 }
 379
 380 static void print_time(void)
 381 {
 382         printf_P(PSTR("time %d\r\n"),time_get_s());
 383 }
 384
 385
 386 static void print_sensors(void)
 387 {
 388 #ifdef notyet
 389         if (sensor_start_switch())
 390                 printf_P(PSTR("Start switch | "));
 391         else
 392                 printf_P(PSTR("             | "));
 393
 394         if (IR_DISP_SENSOR())
 395                 printf_P(PSTR("IR disp | "));
 396         else
 397                 printf_P(PSTR("        | "));
 398
 399         printf_P(PSTR("\r\n"));
 400 #endif
 401 }
 402
 403 static void print_pid(void)
 404 {
 405         printf_P(PSTR("P=% .8"PRIi32" I=% .8"PRIi32" D=% .8"PRIi32" out=% .8"PRIi32" | "
 406                       "P=% .8"PRIi32" I=% .8"PRIi32" D=% .8"PRIi32" out=% .8"PRIi32"\r\n"),
 407                  pid_get_value_in(&mainboard.distance.pid) * pid_get_gain_P(&mainboard.distance.pid),
 408                  pid_get_value_I(&mainboard.distance.pid) * pid_get_gain_I(&mainboard.distance.pid),
 409                  pid_get_value_D(&mainboard.distance.pid) * pid_get_gain_D(&mainboard.distance.pid),
 410                  pid_get_value_out(&mainboard.distance.pid),
 411                  pid_get_value_in(&mainboard.angle.pid) * pid_get_gain_P(&mainboard.angle.pid),
 412                  pid_get_value_I(&mainboard.angle.pid) * pid_get_gain_I(&mainboard.angle.pid),
 413                  pid_get_value_D(&mainboard.angle.pid) * pid_get_gain_D(&mainboard.angle.pid),
 414                  pid_get_value_out(&mainboard.angle.pid));
 415 }
 416
 417 #define PRINT_POS       (1<<0)
 418 #define PRINT_PID       (1<<1)
 419 #define PRINT_CS        (1<<2)
 420 #define PRINT_SENSORS   (1<<3)
 421 #define PRINT_TIME      (1<<4)
 422 #define PRINT_BLOCKING  (1<<5)
 423
 424 static void cmd_interact_parsed(void * parsed_result, void * data)
 425 {
 426         int c;
 427         int8_t cmd;
 428         uint8_t print = 0;
 429         struct vt100 vt100;
 430
 431         vt100_init(&vt100);
 432
 433         printf_P(PSTR("Display debugs:\r\n"
 434                       "  1:pos\r\n"
 435                       "  2:pid\r\n"
 436                       "  3:cs\r\n"
 437                       "  4:sensors\r\n"
 438                       "  5:time\r\n"
 439                       /* "  6:blocking\r\n" */
 440                       "Commands:\r\n"
 441                       "  arrows:move\r\n"
 442                       "  space:stop\r\n"
 443                       "  q:quit\r\n"));
 444
 445         /* stop motors */
 446         mainboard.flags &= (~DO_CS);
 447         pwm_set_and_save(LEFT_PWM, 0);
 448         pwm_set_and_save(RIGHT_PWM, 0);
 449
 450         while(1) {
 451                 if (print & PRINT_POS) {
 452                         print_pos();
 453                 }
 454
 455                 if (print & PRINT_PID) {
 456                         print_pid();
 457                 }
 458
 459                 if (print & PRINT_CS) {
 460                         print_cs();
 461                 }
 462
 463                 if (print & PRINT_SENSORS) {
 464                         print_sensors();
 465                 }
 466
 467                 if (print & PRINT_TIME) {
 468                         print_time();
 469                 }
 470 /*              if (print & PRINT_BLOCKING) { */
 471 /*                      printf_P(PSTR("%s %s blocking=%d\r\n"),  */
 472 /*                               mainboard.blocking ? "BLOCK1":"      ", */
 473 /*                               rs_is_blocked(&mainboard.rs) ? "BLOCK2":"      ", */
 474 /*                               rs_get_blocking(&mainboard.rs)); */
 475 /*              } */
 476
 477                 c = cmdline_getchar();
 478                 if (c == -1) {
 479                         wait_ms(10);
 480                         continue;
 481                 }
 482                 cmd = vt100_parser(&vt100, c);
 483                 if (cmd == -2) {
 484                         wait_ms(10);
 485                         continue;
 486                 }
 487
 488                 if (cmd == -1) {
 489                         switch(c) {
 490                         case '1': print ^= PRINT_POS; break;
 491                         case '2': print ^= PRINT_PID; break;
 492                         case '3': print ^= PRINT_CS; break;
 493                         case '4': print ^= PRINT_SENSORS; break;
 494                         case '5': print ^= PRINT_TIME; break;
 495                         case '6': print ^= PRINT_BLOCKING; break;
 496
 497                         case 'q':
 498                                 if (mainboard.flags & DO_CS)
 499                                         strat_hardstop();
 500                                 pwm_set_and_save(LEFT_PWM, 0);
 501                                 pwm_set_and_save(RIGHT_PWM, 0);
 502                                 return;
 503                         case ' ':
 504                                 pwm_set_and_save(LEFT_PWM, 0);
 505                                 pwm_set_and_save(RIGHT_PWM, 0);
 506                                 break;
 507                         default:
 508                                 break;
 509                         }
 510                 }
 511                 else {
 512                         switch(cmd) {
 513                         case KEY_UP_ARR:
 514                                 pwm_set_and_save(LEFT_PWM, 1200);
 515                                 pwm_set_and_save(RIGHT_PWM, 1200);
 516                                 break;
 517                         case KEY_LEFT_ARR:
 518                                 pwm_set_and_save(LEFT_PWM, -1200);
 519                                 pwm_set_and_save(RIGHT_PWM, 1200);
 520                                 break;
 521                         case KEY_DOWN_ARR:
 522                                 pwm_set_and_save(LEFT_PWM, -1200);
 523                                 pwm_set_and_save(RIGHT_PWM, -1200);
 524                                 break;
 525                         case KEY_RIGHT_ARR:
 526                                 pwm_set_and_save(LEFT_PWM, 1200);
 527                                 pwm_set_and_save(RIGHT_PWM, -1200);
 528                                 break;
 529                         }
 530                 }
 531                 wait_ms(10);
 532         }
 533 }
 534
 535 prog_char str_interact_arg0[] = "interact";
 536 parse_pgm_token_string_t cmd_interact_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_interact_result, arg0, str_interact_arg0);
 537
 538 prog_char help_interact[] = "Interactive mode";
 539 parse_pgm_inst_t cmd_interact = {
 540         .f = cmd_interact_parsed,  /* function to call */
 541         .data = NULL,      /* 2nd arg of func */
 542         .help_str = help_interact,
 543         .tokens = {        /* token list, NULL terminated */
 544                 (prog_void *)&cmd_interact_arg0,
 545                 NULL,
 546         },
 547 };
 548
 549
 550 /**********************************************************/
 551 /* Color */
 552
 553 /* this structure is filled when cmd_color is parsed successfully */
 554 struct cmd_color_result {
 555         fixed_string_t arg0;
 556         fixed_string_t color;
 557 };
 558
 559 /* function called when cmd_color is parsed successfully */
 560 static void cmd_color_parsed(void *parsed_result, void *data)
 561 {
 562 #ifdef HOST_VERSION
 563         printf("not implemented\n");
 564 #else
 565         struct cmd_color_result *res = (struct cmd_color_result *) parsed_result;
 566         if (!strcmp_P(res->color, PSTR("yellow"))) {
 567                 mainboard.our_color = I2C_COLOR_YELLOW;
 568                 i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_YELLOW);
 569                 i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_YELLOW);
 570         }
 571         else if (!strcmp_P(res->color, PSTR("blue"))) {
 572                 mainboard.our_color = I2C_COLOR_BLUE;
 573                 i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_BLUE);
 574                 i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_BLUE);
 575         }
 576         printf_P(PSTR("Done\r\n"));
 577 #endif
 578 }
 579
 580 prog_char str_color_arg0[] = "color";
 581 parse_pgm_token_string_t cmd_color_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_color_result, arg0, str_color_arg0);
 582 prog_char str_color_color[] = "blue#yellow";
 583 parse_pgm_token_string_t cmd_color_color = TOKEN_STRING_INITIALIZER(struct cmd_color_result, color, str_color_color);
 584
 585 prog_char help_color[] = "Set our color";
 586 parse_pgm_inst_t cmd_color = {
 587         .f = cmd_color_parsed,  /* function to call */
 588         .data = NULL,      /* 2nd arg of func */
 589         .help_str = help_color,
 590         .tokens = {        /* token list, NULL terminated */
 591                 (prog_void *)&cmd_color_arg0,
 592                 (prog_void *)&cmd_color_color,
 593                 NULL,
 594         },
 595 };
 596
 597
 598 /**********************************************************/
 599 /* Rs tests */
 600
 601 /* this structure is filled when cmd_rs is parsed successfully */
 602 struct cmd_rs_result {
 603         fixed_string_t arg0;
 604         fixed_string_t arg1;
 605 };
 606
 607 /* function called when cmd_rs is parsed successfully */
 608 static void cmd_rs_parsed(void *parsed_result, void *data)
 609 {
 610         //      struct cmd_rs_result *res = parsed_result;
 611         do {
 612                 printf_P(PSTR("angle cons=% .6"PRIi32" in=% .6"PRIi32" out=% .6"PRIi32" / "),
 613                          cs_get_consign(&mainboard.angle.cs),
 614                          cs_get_filtered_feedback(&mainboard.angle.cs),
 615                          cs_get_out(&mainboard.angle.cs));
 616                 printf_P(PSTR("distance cons=% .6"PRIi32" in=% .6"PRIi32" out=% .6"PRIi32" / "),
 617                          cs_get_consign(&mainboard.distance.cs),
 618                          cs_get_filtered_feedback(&mainboard.distance.cs),
 619                          cs_get_out(&mainboard.distance.cs));
 620                 printf_P(PSTR("l=% .4"PRIi32" r=% .4"PRIi32"\r\n"), mainboard.pwm_l,
 621                          mainboard.pwm_r);
 622                 wait_ms(100);
 623         } while(!cmdline_keypressed());
 624 }
 625
 626 prog_char str_rs_arg0[] = "rs";
 627 parse_pgm_token_string_t cmd_rs_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_rs_result, arg0, str_rs_arg0);
 628 prog_char str_rs_arg1[] = "show";
 629 parse_pgm_token_string_t cmd_rs_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_rs_result, arg1, str_rs_arg1);
 630
 631 prog_char help_rs[] = "Show rs (robot system) values";
 632 parse_pgm_inst_t cmd_rs = {
 633         .f = cmd_rs_parsed,  /* function to call */
 634         .data = NULL,      /* 2nd arg of func */
 635         .help_str = help_rs,
 636         .tokens = {        /* token list, NULL terminated */
 637                 (prog_void *)&cmd_rs_arg0,
 638                 (prog_void *)&cmd_rs_arg1,
 639                 NULL,
 640         },
 641 };
 642
 643 /**********************************************************/
 644 /* I2cdebug */
 645
 646 /* this structure is filled when cmd_i2cdebug is parsed successfully */
 647 struct cmd_i2cdebug_result {
 648         fixed_string_t arg0;
 649 };
 650
 651 /* function called when cmd_i2cdebug is parsed successfully */
 652 static void cmd_i2cdebug_parsed(void * parsed_result, void * data)
 653 {
 654 #ifdef HOST_VERSION
 655         printf("not implemented\n");
 656 #else
 657         i2c_debug();
 658         i2c_protocol_debug();
 659 #endif
 660 }
 661
 662 prog_char str_i2cdebug_arg0[] = "i2cdebug";
 663 parse_pgm_token_string_t cmd_i2cdebug_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_i2cdebug_result, arg0, str_i2cdebug_arg0);
 664
 665 prog_char help_i2cdebug[] = "I2c debug infos";
 666 parse_pgm_inst_t cmd_i2cdebug = {
 667         .f = cmd_i2cdebug_parsed,  /* function to call */
 668         .data = NULL,      /* 2nd arg of func */
 669         .help_str = help_i2cdebug,
 670         .tokens = {        /* token list, NULL terminated */
 671                 (prog_void *)&cmd_i2cdebug_arg0,
 672                 NULL,
 673         },
 674 };
 675
 676 /**********************************************************/
 677 /* Cobboard_Show */
 678
 679 /* this structure is filled when cmd_cobboard_show is parsed successfully */
 680 struct cmd_cobboard_show_result {
 681         fixed_string_t arg0;
 682         fixed_string_t arg1;
 683 };
 684
 685 /* function called when cmd_cobboard_show is parsed successfully */
 686 static void cmd_cobboard_show_parsed(void * parsed_result, void * data)
 687 {
 688 #ifdef HOST_VERSION
 689         printf("not implemented\n");
 690 #else
 691         printf_P(PSTR("mode = %x\r\n"), cobboard.mode);
 692         printf_P(PSTR("status = %x\r\n"), cobboard.status);
 693         printf_P(PSTR("cob_count = %x\r\n"), cobboard.cob_count);
 694         printf_P(PSTR("left_cobroller_speed = %d\r\n"), cobboard.left_cobroller_speed);
 695         printf_P(PSTR("right_cobroller_speed = %d\r\n"), cobboard.right_cobroller_speed);
 696 #endif
 697 }
 698
 699 prog_char str_cobboard_show_arg0[] = "cobboard";
 700 parse_pgm_token_string_t cmd_cobboard_show_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_show_result, arg0, str_cobboard_show_arg0);
 701 prog_char str_cobboard_show_arg1[] = "show";
 702 parse_pgm_token_string_t cmd_cobboard_show_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_show_result, arg1, str_cobboard_show_arg1);
 703
 704 prog_char help_cobboard_show[] = "show cobboard status";
 705 parse_pgm_inst_t cmd_cobboard_show = {
 706         .f = cmd_cobboard_show_parsed,  /* function to call */
 707         .data = NULL,      /* 2nd arg of func */
 708         .help_str = help_cobboard_show,
 709         .tokens = {        /* token list, NULL terminated */
 710                 (prog_void *)&cmd_cobboard_show_arg0,
 711                 (prog_void *)&cmd_cobboard_show_arg1,
 712                 NULL,
 713         },
 714 };
 715
 716 /**********************************************************/
 717 /* Cobboard_Setmode1 */
 718
 719 /* this structure is filled when cmd_cobboard_setmode1 is parsed successfully */
 720 struct cmd_cobboard_setmode1_result {
 721         fixed_string_t arg0;
 722         fixed_string_t arg1;
 723 };
 724
 725 /* function called when cmd_cobboard_setmode1 is parsed successfully */
 726 static void cmd_cobboard_setmode1_parsed(void *parsed_result, void *data)
 727 {
 728 #ifdef HOST_VERSION
 729         printf("not implemented\n");
 730 #else
 731         struct cmd_cobboard_setmode1_result *res = parsed_result;
 732
 733         if (!strcmp_P(res->arg1, PSTR("init")))
 734                 i2c_cobboard_mode_init();
 735         else if (!strcmp_P(res->arg1, PSTR("eject")))
 736                 i2c_cobboard_mode_eject();
 737 #endif
 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 #ifdef HOST_VERSION
 771         printf("not implemented\n");
 772 #else
 773         struct cmd_cobboard_setmode2_result *res = parsed_result;
 774         uint8_t side = I2C_LEFT_SIDE;
 775
 776         if (!strcmp_P(res->arg2, PSTR("left")))
 777                 side = I2C_LEFT_SIDE;
 778         else if (!strcmp_P(res->arg2, PSTR("right")))
 779                 side = I2C_RIGHT_SIDE;
 780
 781         if (!strcmp_P(res->arg1, PSTR("deploy")))
 782                 i2c_cobboard_mode_deploy(side);
 783         else if (!strcmp_P(res->arg1, PSTR("harvest")))
 784                 i2c_cobboard_mode_harvest(side);
 785         else if (!strcmp_P(res->arg1, PSTR("pack")))
 786                 i2c_cobboard_mode_pack(side);
 787 #endif
 788 }
 789
 790 prog_char str_cobboard_setmode2_arg0[] = "cobboard";
 791 parse_pgm_token_string_t cmd_cobboard_setmode2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode2_result, arg0, str_cobboard_setmode2_arg0);
 792 prog_char str_cobboard_setmode2_arg1[] = "harvest#deploy#pack";
 793 parse_pgm_token_string_t cmd_cobboard_setmode2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode2_result, arg1, str_cobboard_setmode2_arg1);
 794 prog_char str_cobboard_setmode2_arg2[] = "left#right";
 795 parse_pgm_token_string_t cmd_cobboard_setmode2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode2_result, arg2, str_cobboard_setmode2_arg2);
 796
 797 prog_char help_cobboard_setmode2[] = "set cobboard mode (mode, side)";
 798 parse_pgm_inst_t cmd_cobboard_setmode2 = {
 799         .f = cmd_cobboard_setmode2_parsed,  /* function to call */
 800         .data = NULL,      /* 2nd arg of func */
 801         .help_str = help_cobboard_setmode2,
 802         .tokens = {        /* token list, NULL terminated */
 803                 (prog_void *)&cmd_cobboard_setmode2_arg0,
 804                 (prog_void *)&cmd_cobboard_setmode2_arg1,
 805                 (prog_void *)&cmd_cobboard_setmode2_arg2,
 806                 NULL,
 807         },
 808 };
 809
 810 /**********************************************************/
 811 /* Cobboard_Setmode3 */
 812
 813 /* this structure is filled when cmd_cobboard_setmode3 is parsed successfully */
 814 struct cmd_cobboard_setmode3_result {
 815         fixed_string_t arg0;
 816         fixed_string_t arg1;
 817         uint8_t level;
 818 };
 819
 820 /* function called when cmd_cobboard_setmode3 is parsed successfully */
 821 static void cmd_cobboard_setmode3_parsed(void *parsed_result, void *data)
 822 {
 823 #ifdef HOST_VERSION
 824         printf("not implemented\n");
 825 #else
 826         struct cmd_cobboard_setmode3_result *res = parsed_result;
 827         if (!strcmp_P(res->arg1, PSTR("xxx")))
 828                 printf("faux\r\n");
 829 #endif
 830 }
 831
 832 prog_char str_cobboard_setmode3_arg0[] = "cobboard";
 833 parse_pgm_token_string_t cmd_cobboard_setmode3_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode3_result, arg0, str_cobboard_setmode3_arg0);
 834 prog_char str_cobboard_setmode3_arg1[] = "xxx";
 835 parse_pgm_token_string_t cmd_cobboard_setmode3_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode3_result, arg1, str_cobboard_setmode3_arg1);
 836 parse_pgm_token_num_t cmd_cobboard_setmode3_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_cobboard_setmode3_result, level, UINT8);
 837
 838 prog_char help_cobboard_setmode3[] = "set cobboard mode (mode, level)";
 839 parse_pgm_inst_t cmd_cobboard_setmode3 = {
 840         .f = cmd_cobboard_setmode3_parsed,  /* function to call */
 841         .data = NULL,      /* 2nd arg of func */
 842         .help_str = help_cobboard_setmode3,
 843         .tokens = {        /* token list, NULL terminated */
 844                 (prog_void *)&cmd_cobboard_setmode3_arg0,
 845                 (prog_void *)&cmd_cobboard_setmode3_arg1,
 846                 (prog_void *)&cmd_cobboard_setmode3_arg2,
 847                 NULL,
 848         },
 849 };
 850
 851 /**********************************************************/
 852 /* Ballboard_Show */
 853
 854 /* this structure is filled when cmd_ballboard_show is parsed successfully */
 855 struct cmd_ballboard_show_result {
 856         fixed_string_t arg0;
 857         fixed_string_t arg1;
 858 };
 859
 860 /* function called when cmd_ballboard_show is parsed successfully */
 861 static void cmd_ballboard_show_parsed(void * parsed_result, void * data)
 862 {
 863 #ifdef HOST_VERSION
 864         printf("not implemented\n");
 865 #else
 866         printf_P(PSTR("mode = %x\r\n"), ballboard.mode);
 867         printf_P(PSTR("status = %x\r\n"), ballboard.status);
 868         printf_P(PSTR("ball_count = %d\r\n"), ballboard.ball_count);
 869 #endif
 870 }
 871
 872 prog_char str_ballboard_show_arg0[] = "ballboard";
 873 parse_pgm_token_string_t cmd_ballboard_show_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_show_result, arg0, str_ballboard_show_arg0);
 874 prog_char str_ballboard_show_arg1[] = "show";
 875 parse_pgm_token_string_t cmd_ballboard_show_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_show_result, arg1, str_ballboard_show_arg1);
 876
 877 prog_char help_ballboard_show[] = "show ballboard status";
 878 parse_pgm_inst_t cmd_ballboard_show = {
 879         .f = cmd_ballboard_show_parsed,  /* function to call */
 880         .data = NULL,      /* 2nd arg of func */
 881         .help_str = help_ballboard_show,
 882         .tokens = {        /* token list, NULL terminated */
 883                 (prog_void *)&cmd_ballboard_show_arg0,
 884                 (prog_void *)&cmd_ballboard_show_arg1,
 885                 NULL,
 886         },
 887 };
 888
 889 /**********************************************************/
 890 /* Ballboard_Setmode1 */
 891
 892 /* this structure is filled when cmd_ballboard_setmode1 is parsed successfully */
 893 struct cmd_ballboard_setmode1_result {
 894         fixed_string_t arg0;
 895         fixed_string_t arg1;
 896 };
 897
 898 /* function called when cmd_ballboard_setmode1 is parsed successfully */
 899 static void cmd_ballboard_setmode1_parsed(void *parsed_result, void *data)
 900 {
 901 #ifdef HOST_VERSION
 902         printf("not implemented\n");
 903 #else
 904         struct cmd_ballboard_setmode1_result *res = parsed_result;
 905
 906         if (!strcmp_P(res->arg1, PSTR("init")))
 907                 i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_INIT);
 908         else if (!strcmp_P(res->arg1, PSTR("off")))
 909                 i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_OFF);
 910         else if (!strcmp_P(res->arg1, PSTR("eject")))
 911                 i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_EJECT);
 912         else if (!strcmp_P(res->arg1, PSTR("harvest")))
 913                 i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_HARVEST);
 914
 915         /* other commands */
 916 #endif
 917 }
 918
 919 prog_char str_ballboard_setmode1_arg0[] = "ballboard";
 920 parse_pgm_token_string_t cmd_ballboard_setmode1_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode1_result, arg0, str_ballboard_setmode1_arg0);
 921 prog_char str_ballboard_setmode1_arg1[] = "init#eject#harvest#off";
 922 parse_pgm_token_string_t cmd_ballboard_setmode1_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode1_result, arg1, str_ballboard_setmode1_arg1);
 923
 924 prog_char help_ballboard_setmode1[] = "set ballboard mode (mode)";
 925 parse_pgm_inst_t cmd_ballboard_setmode1 = {
 926         .f = cmd_ballboard_setmode1_parsed,  /* function to call */
 927         .data = NULL,      /* 2nd arg of func */
 928         .help_str = help_ballboard_setmode1,
 929         .tokens = {        /* token list, NULL terminated */
 930                 (prog_void *)&cmd_ballboard_setmode1_arg0,
 931                 (prog_void *)&cmd_ballboard_setmode1_arg1,
 932                 NULL,
 933         },
 934 };
 935
 936 /**********************************************************/
 937 /* Ballboard_Setmode2 */
 938
 939 /* this structure is filled when cmd_ballboard_setmode2 is parsed successfully */
 940 struct cmd_ballboard_setmode2_result {
 941         fixed_string_t arg0;
 942         fixed_string_t arg1;
 943         fixed_string_t arg2;
 944 };
 945
 946 /* function called when cmd_ballboard_setmode2 is parsed successfully */
 947 static void cmd_ballboard_setmode2_parsed(void * parsed_result, void * data)
 948 {
 949 #ifdef HOST_VERSION
 950         printf("not implemented\n");
 951 #else
 952         struct cmd_ballboard_setmode2_result *res = parsed_result;
 953         uint8_t mode = I2C_BALLBOARD_MODE_INIT;
 954
 955         if (!strcmp_P(res->arg2, PSTR("left"))) {
 956                 if (!strcmp_P(res->arg1, PSTR("prepare")))
 957                         mode = I2C_BALLBOARD_MODE_PREP_L_FORK;
 958                 else if (!strcmp_P(res->arg1, PSTR("take")))
 959                         mode = I2C_BALLBOARD_MODE_TAKE_L_FORK;
 960         }
 961         else {
 962                 if (!strcmp_P(res->arg1, PSTR("prepare")))
 963                         mode = I2C_BALLBOARD_MODE_PREP_R_FORK;
 964                 else if (!strcmp_P(res->arg1, PSTR("take")))
 965                         mode = I2C_BALLBOARD_MODE_TAKE_R_FORK;
 966         }
 967         i2c_ballboard_set_mode(mode);
 968 #endif
 969 }
 970
 971 prog_char str_ballboard_setmode2_arg0[] = "ballboard";
 972 parse_pgm_token_string_t cmd_ballboard_setmode2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode2_result, arg0, str_ballboard_setmode2_arg0);
 973 prog_char str_ballboard_setmode2_arg1[] = "prepare#take";
 974 parse_pgm_token_string_t cmd_ballboard_setmode2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode2_result, arg1, str_ballboard_setmode2_arg1);
 975 prog_char str_ballboard_setmode2_arg2[] = "left#right";
 976 parse_pgm_token_string_t cmd_ballboard_setmode2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode2_result, arg2, str_ballboard_setmode2_arg2);
 977
 978 prog_char help_ballboard_setmode2[] = "set ballboard mode (mode, side)";
 979 parse_pgm_inst_t cmd_ballboard_setmode2 = {
 980         .f = cmd_ballboard_setmode2_parsed,  /* function to call */
 981         .data = NULL,      /* 2nd arg of func */
 982         .help_str = help_ballboard_setmode2,
 983         .tokens = {        /* token list, NULL terminated */
 984                 (prog_void *)&cmd_ballboard_setmode2_arg0,
 985                 (prog_void *)&cmd_ballboard_setmode2_arg1,
 986                 (prog_void *)&cmd_ballboard_setmode2_arg2,
 987                 NULL,
 988         },
 989 };
 990
 991 /**********************************************************/
 992 /* Ballboard_Setmode3 */
 993
 994 /* this structure is filled when cmd_ballboard_setmode3 is parsed successfully */
 995 struct cmd_ballboard_setmode3_result {
 996         fixed_string_t arg0;
 997         fixed_string_t arg1;
 998         uint8_t level;
 999 };
1000
1001 /* function called when cmd_ballboard_setmode3 is parsed successfully */
1002 static void cmd_ballboard_setmode3_parsed(void *parsed_result, void *data)
1003 {
1004 #ifdef HOST_VERSION
1005         printf("not implemented\n");
1006 #else
1007         struct cmd_ballboard_setmode3_result *res = parsed_result;
1008         if (!strcmp_P(res->arg1, PSTR("xxx")))
1009                 printf("faux\r\n");
1010 #endif
1011 }
1012
1013 prog_char str_ballboard_setmode3_arg0[] = "ballboard";
1014 parse_pgm_token_string_t cmd_ballboard_setmode3_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode3_result, arg0, str_ballboard_setmode3_arg0);
1015 prog_char str_ballboard_setmode3_arg1[] = "xxx";
1016 parse_pgm_token_string_t cmd_ballboard_setmode3_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode3_result, arg1, str_ballboard_setmode3_arg1);
1017 parse_pgm_token_num_t cmd_ballboard_setmode3_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_ballboard_setmode3_result, level, UINT8);
1018
1019 prog_char help_ballboard_setmode3[] = "set ballboard mode (mode, level)";
1020 parse_pgm_inst_t cmd_ballboard_setmode3 = {
1021         .f = cmd_ballboard_setmode3_parsed,  /* function to call */
1022         .data = NULL,      /* 2nd arg of func */
1023         .help_str = help_ballboard_setmode3,
1024         .tokens = {        /* token list, NULL terminated */
1025                 (prog_void *)&cmd_ballboard_setmode3_arg0,
1026                 (prog_void *)&cmd_ballboard_setmode3_arg1,
1027                 (prog_void *)&cmd_ballboard_setmode3_arg2,
1028                 NULL,
1029         },
1030 };
1031
1032 /**********************************************************/
1033 /* Servo_Balls */
1034
1035 /* this structure is filled when cmd_servo_balls is parsed successfully */
1036 struct cmd_servo_balls_result {
1037         fixed_string_t arg0;
1038         fixed_string_t arg1;
1039 };
1040
1041 /* function called when cmd_servo_balls is parsed successfully */
1042 static void cmd_servo_balls_parsed(void *parsed_result,
1043                                    __attribute__((unused)) void *data)
1044 {
1045         struct cmd_servo_balls_result *res = parsed_result;
1046
1047         if (!strcmp_P(res->arg1, PSTR("deploy")))
1048                 support_balls_deploy();
1049         else if (!strcmp_P(res->arg1, PSTR("pack")))
1050                 support_balls_pack();
1051 }
1052
1053 prog_char str_servo_balls_arg0[] = "support_balls";
1054 parse_pgm_token_string_t cmd_servo_balls_arg0 =
1055         TOKEN_STRING_INITIALIZER(struct cmd_servo_balls_result, arg0, str_servo_balls_arg0);
1056 prog_char str_servo_balls_arg1[] = "deploy#pack";
1057 parse_pgm_token_string_t cmd_servo_balls_arg1 =
1058         TOKEN_STRING_INITIALIZER(struct cmd_servo_balls_result, arg1, str_servo_balls_arg1);
1059
1060 prog_char help_servo_balls[] = "control support balls";
1061 parse_pgm_inst_t cmd_servo_balls = {
1062         .f = cmd_servo_balls_parsed,  /* function to call */
1063         .data = NULL,      /* 2nd arg of func */
1064         .help_str = help_servo_balls,
1065         .tokens = {        /* token list, NULL terminated */
1066                 (prog_void *)&cmd_servo_balls_arg0,
1067                 (prog_void *)&cmd_servo_balls_arg1,
1068                 NULL,
1069         },
1070 };
1071
1072 /**********************************************************/
1073 /* Clitoid */
1074
1075 /* this structure is filled when cmd_clitoid is parsed successfully */
1076 struct cmd_clitoid_result {
1077         fixed_string_t arg0;
1078         float alpha_deg;
1079         float beta_deg;
1080         float R_mm;
1081         float Vd;
1082         float Amax;
1083         float d_inter_mm;
1084 };
1085
1086 /**
1087  * do a superb curve joining line1 to line2 which is composed of:
1088  *   - a clothoid starting from line1
1089  *   - a circle
1090  *   - another clothoid up to line2
1091  *
1092  * the function assumes that the initial linear speed is Vd and
1093  * angular speed is 0.
1094  *
1095  * - alpha: total angle
1096  * - beta: circular part of angle (lower than alpha)
1097  * - R: the radius of the circle (must be != 0)
1098  * - Vd: linear speed to use (in imp per cs period)
1099  * - Amax: maximum angular acceleration
1100  * - d_inter: distance in mm until the intersection of the
1101  *            2 lines
1102  *
1103  * return 0 on success: in this case these parameters are filled:
1104  * - Aa_out: the angular acceleration to configure in quadramp
1105  * - remain_d_mm_out: remaining distance before start to turn
1106  */
1107 uint8_t clitoid(double alpha_deg, double beta_deg, double R_mm,
1108                 double Vd, double Amax, double d_inter_mm)
1109 {
1110         double Vd_mm_s;
1111         double Va, Va_rd_s;
1112         double t, d_mm, alpha_rad, beta_rad;
1113         double remain_d_mm;
1114         double Aa, Aa_rd_s2;
1115         line_t line1, line2;
1116         double x, y, a_rad;
1117         point_t robot, intersect, pt2, center, proj;
1118         vect_t v;
1119
1120         /* param check */
1121         if (fabs(alpha_deg) <= fabs(beta_deg)) {
1122                 DEBUG(E_USER_STRAT, "alpha is smaller than beta");
1123                 return END_ERROR;
1124         }
1125
1126         /* get angular speed Va */
1127         Vd_mm_s = Vd * (CS_HZ/DIST_IMP_MM);
1128         DEBUG(E_USER_STRAT, "Vd_mm_s=%2.2f", Vd_mm_s);
1129         Va_rd_s = Vd_mm_s / R_mm;
1130         Va = Va_rd_s * (DIST_IMP_MM * EXT_TRACK_MM / (2 * CS_HZ));
1131         DEBUG(E_USER_STRAT, "Va_rd_s=%2.2f Va=%2.2f", Va_rd_s, Va);
1132
1133         /* process 't', the time in seconds that we will take to do
1134          * the first clothoid */
1135         alpha_rad = RAD(alpha_deg);
1136         beta_rad = RAD(beta_deg);
1137         t = fabs(((alpha_rad - beta_rad) * R_mm) / Vd_mm_s);
1138         DEBUG(E_USER_STRAT, "R_mm=%2.2f alpha_rad=%2.2f beta_rad=%2.2f t=%2.2f",
1139               R_mm, alpha_rad, beta_rad, t);
1140
1141         /* process the angular acceleration */
1142         Aa_rd_s2 = Va_rd_s / t;
1143         Aa = Aa_rd_s2 * (DIST_IMP_MM * EXT_TRACK_MM /
1144                          (2 * CS_HZ * CS_HZ));
1145         DEBUG(E_USER_STRAT, "Aa_rd_s2=%2.2f Aa=%2.2f", Aa_rd_s2, Aa);
1146
1147         /* exit if the robot cannot physically do it */
1148         if (Aa > Amax) {
1149                 DEBUG(E_USER_STRAT, "greater than max acceleration");
1150                 return END_ERROR;
1151         }
1152
1153         /* the robot position */
1154         x = position_get_x_double(&mainboard.pos);
1155         y = position_get_y_double(&mainboard.pos);
1156         a_rad = position_get_a_rad_double(&mainboard.pos);
1157
1158         /* define line1 and line2 */
1159         robot.x = x;
1160         robot.y = y;
1161         intersect.x = x + cos(a_rad) * d_inter_mm;
1162         intersect.y = y + sin(a_rad) * d_inter_mm;
1163         pts2line(&robot, &intersect, &line1);
1164         pt2.x = intersect.x + cos(a_rad + alpha_rad);
1165         pt2.y = intersect.y + sin(a_rad + alpha_rad);
1166         pts2line(&intersect, &pt2, &line2);
1167         DEBUG(E_USER_STRAT, "intersect=(%2.2f, %2.2f)",
1168               intersect.x, intersect.y);
1169
1170         /* the center of the circle is at (d_mm, d_mm) when we have to
1171          * start the clothoid */
1172         d_mm = R_mm * sqrt(fabs(alpha_rad - beta_rad)) *
1173                 sqrt(M_PI) / 2.;
1174         DEBUG(E_USER_STRAT, "d_mm=%2.2f", d_mm);
1175
1176         /* translate line1 */
1177         v.x = intersect.x - robot.x;
1178         v.y = intersect.y - robot.y;
1179         if (a_rad > 0)
1180                 vect_rot_trigo(&v);
1181         else
1182                 vect_rot_retro(&v);
1183         vect_resize(&v, d_mm);
1184         line_translate(&line1, &v);
1185
1186         /* translate line2 */
1187         v.x = intersect.x - pt2.x;
1188         v.y = intersect.y - pt2.y;
1189         if (a_rad > 0)
1190                 vect_rot_trigo(&v);
1191         else
1192                 vect_rot_retro(&v);
1193         vect_resize(&v, d_mm);
1194         line_translate(&line2, &v);
1195
1196         /* find the center of the circle, at the intersection of the
1197          * new translated lines */
1198         if (intersect_line(&line1, &line2, &center) != 1) {
1199                 DEBUG(E_USER_STRAT, "cannot find circle center");
1200                 return END_ERROR;
1201         }
1202         DEBUG(E_USER_STRAT, "center=(%2.2f,%2.2f)", center.x, center.y);
1203
1204         /* project center of circle on line1 */
1205         proj_pt_line(&center, &line1, &proj);
1206         DEBUG(E_USER_STRAT, "proj=(%2.2f,%2.2f)", proj.x, proj.y);
1207
1208         /* process remaining distance before start turning */
1209         remain_d_mm = d_inter_mm - (pt_norm(&proj, &intersect) + d_mm);
1210         DEBUG(E_USER_STRAT, "remain_d=%2.2f", remain_d_mm);
1211         if (remain_d_mm < 0) {
1212                 DEBUG(E_USER_STRAT, "too late, cannot turn");
1213                 return END_ERROR;
1214         }
1215
1216         return END_TRAJ;
1217 }
1218
1219 /* function called when cmd_test is parsed successfully */
1220 static void cmd_clitoid_parsed(void *parsed_result, void *data)
1221 {
1222         struct cmd_clitoid_result *res = parsed_result;
1223         clitoid(res->alpha_deg, res->beta_deg, res->R_mm,
1224                 res->Vd, res->Amax, res->d_inter_mm);
1225 }
1226
1227 prog_char str_clitoid_arg0[] = "clitoid";
1228 parse_pgm_token_string_t cmd_clitoid_arg0 =
1229         TOKEN_STRING_INITIALIZER(struct cmd_clitoid_result,
1230                                  arg0, str_clitoid_arg0);
1231 parse_pgm_token_num_t cmd_clitoid_alpha_deg =
1232         TOKEN_NUM_INITIALIZER(struct cmd_clitoid_result,
1233                               alpha_deg, FLOAT);
1234 parse_pgm_token_num_t cmd_clitoid_beta_deg =
1235         TOKEN_NUM_INITIALIZER(struct cmd_clitoid_result,
1236                               beta_deg, FLOAT);
1237 parse_pgm_token_num_t cmd_clitoid_R_mm =
1238         TOKEN_NUM_INITIALIZER(struct cmd_clitoid_result,
1239                               R_mm, FLOAT);
1240 parse_pgm_token_num_t cmd_clitoid_Vd =
1241         TOKEN_NUM_INITIALIZER(struct cmd_clitoid_result,
1242                               Vd, FLOAT);
1243 parse_pgm_token_num_t cmd_clitoid_Amax =
1244         TOKEN_NUM_INITIALIZER(struct cmd_clitoid_result,
1245                               Amax, FLOAT);
1246 parse_pgm_token_num_t cmd_clitoid_d_inter_mm =
1247         TOKEN_NUM_INITIALIZER(struct cmd_clitoid_result,
1248                               d_inter_mm, FLOAT);
1249
1250 prog_char help_clitoid[] = "do a clitoid (alpha, beta, R, Vd, Amax, d_inter)";
1251 parse_pgm_inst_t cmd_clitoid = {
1252         .f = cmd_clitoid_parsed,  /* function to call */
1253         .data = NULL,      /* 2nd arg of func */
1254         .help_str = help_clitoid,
1255         .tokens = {        /* token list, NULL terminated */
1256                 (prog_void *)&cmd_clitoid_arg0,
1257                 (prog_void *)&cmd_clitoid_alpha_deg,
1258                 (prog_void *)&cmd_clitoid_beta_deg,
1259                 (prog_void *)&cmd_clitoid_R_mm,
1260                 (prog_void *)&cmd_clitoid_Vd,
1261                 (prog_void *)&cmd_clitoid_Amax,
1262                 (prog_void *)&cmd_clitoid_d_inter_mm,
1263                 NULL,
1264         },
1265 };
1266
1267 //////////////////////
1268
1269 // 500 -- 5
1270 // 400 -- 3
1271 #define TEST_SPEED 400
1272 #define TEST_ACC 3
1273
1274 static void line2line(double line1x1, double line1y1,
1275                       double line1x2, double line1y2,
1276                       double line2x1, double line2y1,
1277                       double line2x2, double line2y2,
1278                       double radius, double dist)
1279 {
1280         uint8_t err;
1281         double speed_d, speed_a;
1282         double distance, angle;
1283         double line1_angle = atan2(line1y2-line1y1, line1x2-line1x1);
1284         double line2_angle = atan2(line2y2-line2y1, line2x2-line2x1);
1285
1286         printf_P(PSTR("%s()\r\n"), __FUNCTION__);
1287
1288         strat_set_speed(TEST_SPEED, TEST_SPEED);
1289         quadramp_set_2nd_order_vars(&mainboard.angle.qr, TEST_ACC, TEST_ACC);
1290
1291         circle_get_da_speed_from_radius(&mainboard.traj, radius,
1292                                         &speed_d, &speed_a);
1293         trajectory_line_abs(&mainboard.traj,
1294                             line1x1, line1y1,
1295                             line1x2, line1y2, 150.);
1296         err = WAIT_COND_OR_TRAJ_END(distance_from_robot(line1x2, line1y2) <
1297                                     dist, TRAJ_FLAGS_NO_NEAR);
1298         /* circle */
1299         strat_set_speed(speed_d, speed_a);
1300         angle = line2_angle - line1_angle;
1301         distance = angle * radius;
1302         if (distance < 0)
1303                 distance = -distance;
1304         angle = simple_modulo_2pi(angle);
1305         angle = DEG(angle);
1306         printf_P(PSTR("(%d,%d,%d) "),
1307                  position_get_x_s16(&mainboard.pos),
1308                  position_get_y_s16(&mainboard.pos),
1309                  position_get_a_deg_s16(&mainboard.pos));
1310         printf_P(PSTR("circle distance=%2.2f angle=%2.2f\r\n"),
1311                  distance, angle);
1312
1313         /* take some margin on dist to avoid deceleration */
1314         trajectory_d_a_rel(&mainboard.traj, distance + 250, angle);
1315
1316         /* circle exit condition */
1317         err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj),
1318                                     TRAJ_FLAGS_NO_NEAR);
1319
1320         strat_set_speed(500, 500);
1321         printf_P(PSTR("(%d,%d,%d) "),
1322                  position_get_x_s16(&mainboard.pos),
1323                  position_get_y_s16(&mainboard.pos),
1324                  position_get_a_deg_s16(&mainboard.pos));
1325         printf_P(PSTR("line\r\n"));
1326         trajectory_line_abs(&mainboard.traj,
1327                             line2x1, line2y1,
1328                             line2x2, line2y2, 150.);
1329 }
1330
1331 static void halfturn(double line1x1, double line1y1,
1332                      double line1x2, double line1y2,
1333                      double line2x1, double line2y1,
1334                      double line2x2, double line2y2,
1335                      double radius, double dist, double dir)
1336 {
1337         uint8_t err;
1338         double speed_d, speed_a;
1339         double distance, angle;
1340
1341         printf_P(PSTR("%s()\r\n"), __FUNCTION__);
1342
1343         strat_set_speed(TEST_SPEED, TEST_SPEED);
1344         quadramp_set_2nd_order_vars(&mainboard.angle.qr, TEST_ACC, TEST_ACC);
1345
1346         circle_get_da_speed_from_radius(&mainboard.traj, radius,
1347                                         &speed_d, &speed_a);
1348         trajectory_line_abs(&mainboard.traj,
1349                             line1x1, line1y1,
1350                             line1x2, line1y2, 150.);
1351         err = WAIT_COND_OR_TRAJ_END(distance_from_robot(line1x2, line1y2) <
1352                                     dist, TRAJ_FLAGS_NO_NEAR);
1353         /* circle */
1354         strat_set_speed(speed_d, speed_a);
1355         angle = dir * M_PI/2.;
1356         distance = angle * radius;
1357         if (distance < 0)
1358                 distance = -distance;
1359         angle = simple_modulo_2pi(angle);
1360         angle = DEG(angle);
1361
1362         /* take some margin on dist to avoid deceleration */
1363         DEBUG(E_USER_STRAT, "circle1 distance=%2.2f angle=%2.2f",
1364               distance, angle);
1365         trajectory_d_a_rel(&mainboard.traj, distance + 500, angle);
1366
1367         /* circle exit condition */
1368         err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj),
1369                                     TRAJ_FLAGS_NO_NEAR);
1370
1371         DEBUG(E_USER_STRAT, "miniline");
1372         err = WAIT_COND_OR_TRAJ_END(distance_from_robot(line2x1, line2y1) <
1373                                     dist, TRAJ_FLAGS_NO_NEAR);
1374         DEBUG(E_USER_STRAT, "circle2");
1375         /* take some margin on dist to avoid deceleration */
1376         trajectory_d_a_rel(&mainboard.traj, distance + 500, angle);
1377
1378         err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj),
1379                                     TRAJ_FLAGS_NO_NEAR);
1380
1381         strat_set_speed(500, 500);
1382         DEBUG(E_USER_STRAT, "line");
1383         trajectory_line_abs(&mainboard.traj,
1384                             line2x1, line2y1,
1385                             line2x2, line2y2, 150.);
1386 }
1387
1388 /**********************************************************/
1389 /* Test */
1390
1391 /* this structure is filled when cmd_test is parsed successfully */
1392 struct cmd_test_result {
1393         fixed_string_t arg0;
1394         int32_t radius;
1395         int32_t dist;
1396 };
1397
1398 /* function called when cmd_test is parsed successfully */
1399 static void cmd_test_parsed(void *parsed_result, void *data)
1400 {
1401         //      struct cmd_test_result *res = parsed_result;
1402 #ifdef HOST_VERSION
1403         strat_reset_pos(400, 400, 90);
1404         mainboard.angle.on = 1;
1405         mainboard.distance.on = 1;
1406 #endif
1407         printf_P(PSTR("%s()\r\n"), __FUNCTION__);
1408         while (!cmdline_keypressed()) {
1409                 /****** PASS1 */
1410
1411 #define DIST_HARD_TURN   260
1412 #define RADIUS_HARD_TURN 100
1413 #define DIST_EASY_TURN   190
1414 #define RADIUS_EASY_TURN 190
1415 #define DIST_HALF_TURN   225
1416 #define RADIUS_HALF_TURN 130
1417
1418                 /* hard turn */
1419                 line2line(375, 597, 375, 1847,
1420                           375, 1847, 1050, 1472,
1421                           RADIUS_HARD_TURN, DIST_HARD_TURN);
1422
1423                 /* easy left and easy right !*/
1424                 line2line(825, 1596, 1050, 1472,
1425                           1050, 1472, 1500, 1722,
1426                           RADIUS_EASY_TURN, DIST_EASY_TURN);
1427                 line2line(1050, 1472, 1500, 1722,
1428                           1500, 1722, 2175, 1347,
1429                           RADIUS_EASY_TURN, DIST_EASY_TURN);
1430                 line2line(1500, 1722, 2175, 1347,
1431                           2175, 1347, 2175, 847,
1432                           RADIUS_EASY_TURN, DIST_EASY_TURN);
1433
1434                 /* half turns */
1435                 halfturn(2175, 1347, 2175, 722,
1436                          2625, 722, 2625, 1597,
1437                          RADIUS_HALF_TURN, DIST_HALF_TURN, 1.);
1438                 halfturn(2625, 847, 2625, 1722,
1439                           2175, 1722, 2175, 1097,
1440                           RADIUS_HALF_TURN, DIST_HALF_TURN, 1.);
1441
1442                 /* easy turns */
1443                 line2line(2175, 1597, 2175, 1097,
1444                           2175, 1097, 1500, 722,
1445                           RADIUS_EASY_TURN, DIST_EASY_TURN);
1446                 line2line(2175, 1097, 1500, 722,
1447                           1500, 722, 1050, 972,
1448                           RADIUS_EASY_TURN, DIST_EASY_TURN);
1449                 line2line(1500, 722, 1050, 972,
1450                           1050, 972, 375, 597,
1451                           RADIUS_EASY_TURN, DIST_EASY_TURN);
1452
1453                 /* hard turn */
1454                 line2line(1050, 972, 375, 597,
1455                           375, 597, 375, 1097,
1456                           RADIUS_HARD_TURN, DIST_HARD_TURN);
1457
1458                 /****** PASS2 */
1459
1460                 /* easy turn */
1461                 line2line(375, 597, 375, 1097,
1462                           375, 1097, 1050, 1472,
1463                           RADIUS_EASY_TURN, DIST_EASY_TURN);
1464
1465                 /* hard turn */
1466                 line2line(375, 1097, 1050, 1472,
1467                           1050, 1472, 375, 1847,
1468                           RADIUS_HARD_TURN, DIST_HARD_TURN);
1469
1470                 /* hard turn */
1471                 line2line(1050, 1472, 375, 1847,
1472                           375, 1847, 375, 1347,
1473                           RADIUS_HARD_TURN, DIST_HARD_TURN);
1474
1475                 /* easy turn */
1476                 line2line(375, 1847, 375, 1347,
1477                           375, 1347, 1050, 972,
1478                           RADIUS_EASY_TURN, DIST_EASY_TURN);
1479
1480                 /* hard turn */
1481                 line2line(375, 1347, 1050, 972,
1482                           1050, 972, 375, 597,
1483                           RADIUS_HARD_TURN, DIST_HARD_TURN);
1484
1485                 /* hard turn */
1486                 line2line(1050, 972, 375, 597,
1487                           375, 597, 375, 1847,
1488                           RADIUS_HARD_TURN, DIST_HARD_TURN);
1489
1490         }
1491         trajectory_hardstop(&mainboard.traj);
1492 }
1493
1494 prog_char str_test_arg0[] = "test";
1495 parse_pgm_token_string_t cmd_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_test_result, arg0, str_test_arg0);
1496 parse_pgm_token_num_t cmd_test_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_test_result, radius, INT32);
1497 parse_pgm_token_num_t cmd_test_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_test_result, dist, INT32);
1498
1499 prog_char help_test[] = "Test function";
1500 parse_pgm_inst_t cmd_test = {
1501         .f = cmd_test_parsed,  /* function to call */
1502         .data = NULL,      /* 2nd arg of func */
1503         .help_str = help_test,
1504         .tokens = {        /* token list, NULL terminated */
1505                 (prog_void *)&cmd_test_arg0,
1506                 NULL,
1507         },
1508 };