projects/microb2010/cobboard/commands_cobboard.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_cobboard.c,v 1.6 2009-11-08 17:25:00 zer0 Exp $
  19  *
  20  *  Olivier MATZ <zer0@droids-corp.org>
  21  */
  22
  23 #include <stdio.h>
  24 #include <string.h>
  25
  26 #include <aversive/pgmspace.h>
  27 #include <aversive/wait.h>
  28 #include <aversive/error.h>
  29
  30 #include <ax12.h>
  31 #include <uart.h>
  32 #include <pwm_ng.h>
  33 #include <clock_time.h>
  34 #include <spi.h>
  35
  36 #include <pid.h>
  37 #include <quadramp.h>
  38 #include <control_system_manager.h>
  39 #include <blocking_detection_manager.h>
  40
  41 #include <rdline.h>
  42 #include <parse.h>
  43 #include <parse_string.h>
  44 #include <parse_num.h>
  45
  46 #include "../common/i2c_commands.h"
  47 #include "main.h"
  48 #include "sensor.h"
  49 #include "cmdline.h"
  50 #include "state.h"
  51 #include "i2c_protocol.h"
  52 #include "actuator.h"
  53 #include "spickle.h"
  54 #include "shovel.h"
  55
  56 struct cmd_event_result {
  57         fixed_string_t arg0;
  58         fixed_string_t arg1;
  59         fixed_string_t arg2;
  60 };
  61
  62
  63 /* function called when cmd_event is parsed successfully */
  64 static void cmd_event_parsed(void *parsed_result, __attribute__((unused)) void *data)
  65 {
  66         u08 bit=0;
  67
  68         struct cmd_event_result * res = parsed_result;
  69
  70         if (!strcmp_P(res->arg1, PSTR("all"))) {
  71                 bit = DO_ENCODERS | DO_CS | DO_BD | DO_POWER;
  72                 if (!strcmp_P(res->arg2, PSTR("on")))
  73                         cobboard.flags |= bit;
  74                 else if (!strcmp_P(res->arg2, PSTR("off")))
  75                         cobboard.flags &= bit;
  76                 else { /* show */
  77                         printf_P(PSTR("encoders is %s\r\n"),
  78                                  (DO_ENCODERS & cobboard.flags) ? "on":"off");
  79                         printf_P(PSTR("cs is %s\r\n"),
  80                                  (DO_CS & cobboard.flags) ? "on":"off");
  81                         printf_P(PSTR("bd is %s\r\n"),
  82                                  (DO_BD & cobboard.flags) ? "on":"off");
  83                         printf_P(PSTR("power is %s\r\n"),
  84                                  (DO_POWER & cobboard.flags) ? "on":"off");
  85                 }
  86                 return;
  87         }
  88
  89         if (!strcmp_P(res->arg1, PSTR("encoders")))
  90                 bit = DO_ENCODERS;
  91         else if (!strcmp_P(res->arg1, PSTR("cs"))) {
  92                 bit = DO_CS;
  93         }
  94         else if (!strcmp_P(res->arg1, PSTR("bd")))
  95                 bit = DO_BD;
  96         else if (!strcmp_P(res->arg1, PSTR("power")))
  97                 bit = DO_POWER;
  98
  99
 100         if (!strcmp_P(res->arg2, PSTR("on")))
 101                 cobboard.flags |= bit;
 102         else if (!strcmp_P(res->arg2, PSTR("off"))) {
 103                 if (!strcmp_P(res->arg1, PSTR("cs"))) {
 104                         pwm_ng_set(LEFT_SPICKLE_PWM, 0);
 105                         pwm_ng_set(RIGHT_SPICKLE_PWM, 0);
 106                         pwm_ng_set(SHOVEL_PWM, 0);
 107                 }
 108                 cobboard.flags &= (~bit);
 109         }
 110         printf_P(PSTR("%s is %s\r\n"), res->arg1,
 111                       (bit & cobboard.flags) ? "on":"off");
 112 }
 113
 114 prog_char str_event_arg0[] = "event";
 115 parse_pgm_token_string_t cmd_event_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg0, str_event_arg0);
 116 prog_char str_event_arg1[] = "all#encoders#cs#bd#power";
 117 parse_pgm_token_string_t cmd_event_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg1, str_event_arg1);
 118 prog_char str_event_arg2[] = "on#off#show";
 119 parse_pgm_token_string_t cmd_event_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg2, str_event_arg2);
 120
 121 prog_char help_event[] = "Enable/disable events";
 122 parse_pgm_inst_t cmd_event = {
 123         .f = cmd_event_parsed,  /* function to call */
 124         .data = NULL,      /* 2nd arg of func */
 125         .help_str = help_event,
 126         .tokens = {        /* token list, NULL terminated */
 127                 (prog_void *)&cmd_event_arg0,
 128                 (prog_void *)&cmd_event_arg1,
 129                 (prog_void *)&cmd_event_arg2,
 130                 NULL,
 131         },
 132 };
 133
 134 /**********************************************************/
 135 /* Color */
 136
 137 /* this structure is filled when cmd_color is parsed successfully */
 138 struct cmd_color_result {
 139         fixed_string_t arg0;
 140         fixed_string_t color;
 141 };
 142
 143 /* function called when cmd_color is parsed successfully */
 144 static void cmd_color_parsed(void *parsed_result, __attribute__((unused)) void *data)
 145 {
 146         struct cmd_color_result *res = (struct cmd_color_result *) parsed_result;
 147         if (!strcmp_P(res->color, PSTR("yellow"))) {
 148                 cobboard.our_color = I2C_COLOR_YELLOW;
 149         }
 150         else if (!strcmp_P(res->color, PSTR("blue"))) {
 151                 cobboard.our_color = I2C_COLOR_BLUE;
 152         }
 153         printf_P(PSTR("Done\r\n"));
 154 }
 155
 156 prog_char str_color_arg0[] = "color";
 157 parse_pgm_token_string_t cmd_color_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_color_result, arg0, str_color_arg0);
 158 prog_char str_color_color[] = "blue#yellow";
 159 parse_pgm_token_string_t cmd_color_color = TOKEN_STRING_INITIALIZER(struct cmd_color_result, color, str_color_color);
 160
 161 prog_char help_color[] = "Set our color";
 162 parse_pgm_inst_t cmd_color = {
 163         .f = cmd_color_parsed,  /* function to call */
 164         .data = NULL,      /* 2nd arg of func */
 165         .help_str = help_color,
 166         .tokens = {        /* token list, NULL terminated */
 167                 (prog_void *)&cmd_color_arg0,
 168                 (prog_void *)&cmd_color_color,
 169                 NULL,
 170         },
 171 };
 172
 173 /**********************************************************/
 174 /* State1 */
 175
 176 /* this structure is filled when cmd_state1 is parsed successfully */
 177 struct cmd_state1_result {
 178         fixed_string_t arg0;
 179         fixed_string_t arg1;
 180 };
 181
 182 /* function called when cmd_state1 is parsed successfully */
 183 static void cmd_state1_parsed(void *parsed_result,
 184                               __attribute__((unused)) void *data)
 185 {
 186         struct cmd_state1_result *res = parsed_result;
 187
 188         if (!strcmp_P(res->arg1, PSTR("init")))
 189                 state_init();
 190         else if (!strcmp_P(res->arg1, PSTR("eject")))
 191                 state_set_mode(I2C_COBBOARD_MODE_EJECT);
 192
 193         /* other commands */
 194 }
 195
 196 prog_char str_state1_arg0[] = "cobboard";
 197 parse_pgm_token_string_t cmd_state1_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_state1_result, arg0, str_state1_arg0);
 198 prog_char str_state1_arg1[] = "init#eject";
 199 parse_pgm_token_string_t cmd_state1_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_state1_result, arg1, str_state1_arg1);
 200
 201 prog_char help_state1[] = "set cobboard mode";
 202 parse_pgm_inst_t cmd_state1 = {
 203         .f = cmd_state1_parsed,  /* function to call */
 204         .data = NULL,      /* 2nd arg of func */
 205         .help_str = help_state1,
 206         .tokens = {        /* token list, NULL terminated */
 207                 (prog_void *)&cmd_state1_arg0,
 208                 (prog_void *)&cmd_state1_arg1,
 209                 NULL,
 210         },
 211 };
 212
 213 /**********************************************************/
 214 /* State2 */
 215
 216 /* this structure is filled when cmd_state2 is parsed successfully */
 217 struct cmd_state2_result {
 218         fixed_string_t arg0;
 219         fixed_string_t arg1;
 220         fixed_string_t arg2;
 221 };
 222
 223 /* function called when cmd_state2 is parsed successfully */
 224 static void cmd_state2_parsed(void *parsed_result,
 225                               __attribute__((unused)) void *data)
 226 {
 227         struct cmd_state2_result *res = parsed_result;
 228         uint8_t side, mode = state_get_mode();
 229
 230         if (!strcmp_P(res->arg2, PSTR("left"))) {
 231                 side = I2C_LEFT_SIDE;
 232                 mode &= ~(I2C_COBBOARD_MODE_L_DEPLOY | I2C_COBBOARD_MODE_L_HARVEST);
 233         }
 234         else {
 235                 side = I2C_RIGHT_SIDE;
 236                 mode &= ~(I2C_COBBOARD_MODE_R_DEPLOY | I2C_COBBOARD_MODE_R_HARVEST);
 237         }
 238
 239         if (!strcmp_P(res->arg1, PSTR("pack"))) {
 240                 /* nothing to do */
 241         }
 242         else if (!strcmp_P(res->arg1, PSTR("deploy"))) {
 243                 if (side == I2C_LEFT_SIDE)
 244                         mode |= I2C_COBBOARD_MODE_L_DEPLOY;
 245                 else
 246                         mode |= I2C_COBBOARD_MODE_R_DEPLOY;
 247         }
 248         else if (!strcmp_P(res->arg1, PSTR("harvest"))) {
 249                 if (side == I2C_LEFT_SIDE) {
 250                         mode |= I2C_COBBOARD_MODE_L_DEPLOY;
 251                         mode |= I2C_COBBOARD_MODE_L_HARVEST;
 252                 }
 253                 else {
 254                         mode |= I2C_COBBOARD_MODE_R_DEPLOY;
 255                         mode |= I2C_COBBOARD_MODE_R_HARVEST;
 256                 }
 257         }
 258
 259         state_set_mode(mode);
 260 }
 261
 262 prog_char str_state2_arg0[] = "cobboard";
 263 parse_pgm_token_string_t cmd_state2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_state2_result, arg0, str_state2_arg0);
 264 prog_char str_state2_arg1[] = "harvest#deploy#pack";
 265 parse_pgm_token_string_t cmd_state2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_state2_result, arg1, str_state2_arg1);
 266 prog_char str_state2_arg2[] = "left#right";
 267 parse_pgm_token_string_t cmd_state2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_state2_result, arg2, str_state2_arg2);
 268
 269 prog_char help_state2[] = "set cobboard mode";
 270 parse_pgm_inst_t cmd_state2 = {
 271         .f = cmd_state2_parsed,  /* function to call */
 272         .data = NULL,      /* 2nd arg of func */
 273         .help_str = help_state2,
 274         .tokens = {        /* token list, NULL terminated */
 275                 (prog_void *)&cmd_state2_arg0,
 276                 (prog_void *)&cmd_state2_arg1,
 277                 (prog_void *)&cmd_state2_arg2,
 278                 NULL,
 279         },
 280 };
 281
 282 /**********************************************************/
 283 /* State3 */
 284
 285 /* this structure is filled when cmd_state3 is parsed successfully */
 286 struct cmd_state3_result {
 287         fixed_string_t arg0;
 288         fixed_string_t arg1;
 289         uint8_t arg2;
 290 };
 291
 292 /* function called when cmd_state3 is parsed successfully */
 293 static void cmd_state3_parsed(void *parsed_result,
 294                               __attribute__((unused)) void *data)
 295 {
 296         struct cmd_state3_result *res = parsed_result;
 297
 298         if (!strcmp_P(res->arg1, PSTR("xxx"))) {
 299                 /* xxx = res->arg2 */
 300         }
 301         else if (!strcmp_P(res->arg1, PSTR("yyy"))) {
 302         }
 303         state_set_mode(0);
 304 }
 305
 306 prog_char str_state3_arg0[] = "cobboard";
 307 parse_pgm_token_string_t cmd_state3_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_state3_result, arg0, str_state3_arg0);
 308 prog_char str_state3_arg1[] = "xxx";
 309 parse_pgm_token_string_t cmd_state3_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_state3_result, arg1, str_state3_arg1);
 310 parse_pgm_token_num_t cmd_state3_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_state3_result, arg2, UINT8);
 311
 312 prog_char help_state3[] = "set cobboard mode";
 313 parse_pgm_inst_t cmd_state3 = {
 314         .f = cmd_state3_parsed,  /* function to call */
 315         .data = NULL,      /* 2nd arg of func */
 316         .help_str = help_state3,
 317         .tokens = {        /* token list, NULL terminated */
 318                 (prog_void *)&cmd_state3_arg0,
 319                 (prog_void *)&cmd_state3_arg1,
 320                 (prog_void *)&cmd_state3_arg2,
 321                 NULL,
 322         },
 323 };
 324
 325 /**********************************************************/
 326 /* State_Machine */
 327
 328 /* this structure is filled when cmd_state_machine is parsed successfully */
 329 struct cmd_state_machine_result {
 330         fixed_string_t arg0;
 331 };
 332
 333 /* function called when cmd_state_machine is parsed successfully */
 334 static void cmd_state_machine_parsed(__attribute__((unused)) void *parsed_result,
 335                                      __attribute__((unused)) void *data)
 336 {
 337         state_machine();
 338 }
 339
 340 prog_char str_state_machine_arg0[] = "state_machine";
 341 parse_pgm_token_string_t cmd_state_machine_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_state_machine_result, arg0, str_state_machine_arg0);
 342
 343 prog_char help_state_machine[] = "launch state machine";
 344 parse_pgm_inst_t cmd_state_machine = {
 345         .f = cmd_state_machine_parsed,  /* function to call */
 346         .data = NULL,      /* 2nd arg of func */
 347         .help_str = help_state_machine,
 348         .tokens = {        /* token list, NULL terminated */
 349                 (prog_void *)&cmd_state_machine_arg0,
 350                 NULL,
 351         },
 352 };
 353
 354 /**********************************************************/
 355 /* State_Debug */
 356
 357 /* this structure is filled when cmd_state_debug is parsed successfully */
 358 struct cmd_state_debug_result {
 359         fixed_string_t arg0;
 360         uint8_t on;
 361 };
 362
 363 /* function called when cmd_state_debug is parsed successfully */
 364 static void cmd_state_debug_parsed(void *parsed_result,
 365                                    __attribute__((unused)) void *data)
 366 {
 367         struct cmd_state_debug_result *res = parsed_result;
 368         state_debug = res->on;
 369 }
 370
 371 prog_char str_state_debug_arg0[] = "state_debug";
 372 parse_pgm_token_string_t cmd_state_debug_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_state_debug_result, arg0, str_state_debug_arg0);
 373 parse_pgm_token_num_t cmd_state_debug_on = TOKEN_NUM_INITIALIZER(struct cmd_state_debug_result, on, UINT8);
 374
 375 prog_char help_state_debug[] = "Set debug for state machine";
 376 parse_pgm_inst_t cmd_state_debug = {
 377         .f = cmd_state_debug_parsed,  /* function to call */
 378         .data = NULL,      /* 2nd arg of func */
 379         .help_str = help_state_debug,
 380         .tokens = {        /* token list, NULL terminated */
 381                 (prog_void *)&cmd_state_debug_arg0,
 382                 (prog_void *)&cmd_state_debug_on,
 383                 NULL,
 384         },
 385 };
 386
 387 /**********************************************************/
 388 /* Servo_Door */
 389
 390 /* this structure is filled when cmd_servo_door is parsed successfully */
 391 struct cmd_servo_door_result {
 392         fixed_string_t arg0;
 393         fixed_string_t arg1;
 394 };
 395
 396 /* function called when cmd_servo_door is parsed successfully */
 397 static void cmd_servo_door_parsed(void *parsed_result,
 398                                     __attribute__((unused)) void *data)
 399 {
 400         struct cmd_servo_door_result *res = parsed_result;
 401         if (!strcmp_P(res->arg1, PSTR("open")))
 402                 servo_door_open();
 403         else if (!strcmp_P(res->arg1, PSTR("closed")))
 404                 servo_door_close();
 405         else if (!strcmp_P(res->arg1, PSTR("block")))
 406                 servo_door_close();
 407 }
 408
 409 prog_char str_servo_door_arg0[] = "door";
 410 parse_pgm_token_string_t cmd_servo_door_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_servo_door_result, arg0, str_servo_door_arg0);
 411 prog_char str_servo_door_arg1[] = "open#closed#block";
 412 parse_pgm_token_string_t cmd_servo_door_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_servo_door_result, arg1, str_servo_door_arg1);
 413
 414 prog_char help_servo_door[] = "Servo door function";
 415 parse_pgm_inst_t cmd_servo_door = {
 416         .f = cmd_servo_door_parsed,  /* function to call */
 417         .data = NULL,      /* 2nd arg of func */
 418         .help_str = help_servo_door,
 419         .tokens = {        /* token list, NULL terminated */
 420                 (prog_void *)&cmd_servo_door_arg0,
 421                 (prog_void *)&cmd_servo_door_arg1,
 422                 NULL,
 423         },
 424 };
 425
 426 /**********************************************************/
 427 /* cobroller */
 428
 429 /* this structure is filled when cmd_cobroller is parsed successfully */
 430 struct cmd_cobroller_result {
 431         fixed_string_t arg0;
 432         fixed_string_t arg1;
 433         fixed_string_t arg2;
 434 };
 435
 436 /* function called when cmd_cobroller is parsed successfully */
 437 static void cmd_cobroller_parsed(void *parsed_result,
 438                                     __attribute__((unused)) void *data)
 439 {
 440         struct cmd_cobroller_result *res = parsed_result;
 441         uint8_t side;
 442
 443         if (!strcmp_P(res->arg1, PSTR("left")))
 444                 side = I2C_LEFT_SIDE;
 445         else
 446                 side = I2C_RIGHT_SIDE;
 447
 448         if (!strcmp_P(res->arg2, PSTR("on")))
 449                 cobroller_on(side);
 450         else if (!strcmp_P(res->arg2, PSTR("off")))
 451                 cobroller_off(side);
 452 }
 453
 454 prog_char str_cobroller_arg0[] = "cobroller";
 455 parse_pgm_token_string_t cmd_cobroller_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_cobroller_result, arg0, str_cobroller_arg0);
 456 prog_char str_cobroller_arg1[] = "left#right";
 457 parse_pgm_token_string_t cmd_cobroller_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_cobroller_result, arg1, str_cobroller_arg1);
 458 prog_char str_cobroller_arg2[] = "on#off";
 459 parse_pgm_token_string_t cmd_cobroller_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_cobroller_result, arg2, str_cobroller_arg2);
 460
 461 prog_char help_cobroller[] = "Servo door function";
 462 parse_pgm_inst_t cmd_cobroller = {
 463         .f = cmd_cobroller_parsed,  /* function to call */
 464         .data = NULL,      /* 2nd arg of func */
 465         .help_str = help_cobroller,
 466         .tokens = {        /* token list, NULL terminated */
 467                 (prog_void *)&cmd_cobroller_arg0,
 468                 (prog_void *)&cmd_cobroller_arg1,
 469                 (prog_void *)&cmd_cobroller_arg2,
 470                 NULL,
 471         },
 472 };
 473
 474 /**********************************************************/
 475 /* shovel */
 476
 477 /* this structure is filled when cmd_shovel is parsed successfully */
 478 struct cmd_shovel_result {
 479         fixed_string_t arg0;
 480         fixed_string_t arg1;
 481 };
 482
 483 /* function called when cmd_shovel is parsed successfully */
 484 static void cmd_shovel_parsed(void *parsed_result,
 485                               __attribute__((unused)) void *data)
 486 {
 487         struct cmd_shovel_result *res = parsed_result;
 488         if (!strcmp_P(res->arg1, PSTR("down")))
 489                 shovel_down();
 490         else if (!strcmp_P(res->arg1, PSTR("up")))
 491                 shovel_up();
 492         else if (!strcmp_P(res->arg1, PSTR("mid")))
 493                 shovel_mid();
 494 }
 495
 496 prog_char str_shovel_arg0[] = "shovel";
 497 parse_pgm_token_string_t cmd_shovel_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_shovel_result, arg0, str_shovel_arg0);
 498 prog_char str_shovel_arg1[] = "down#up#mid";
 499 parse_pgm_token_string_t cmd_shovel_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_shovel_result, arg1, str_shovel_arg1);
 500
 501 prog_char help_shovel[] = "Servo shovel function";
 502 parse_pgm_inst_t cmd_shovel = {
 503         .f = cmd_shovel_parsed,  /* function to call */
 504         .data = NULL,      /* 2nd arg of func */
 505         .help_str = help_shovel,
 506         .tokens = {        /* token list, NULL terminated */
 507                 (prog_void *)&cmd_shovel_arg0,
 508                 (prog_void *)&cmd_shovel_arg1,
 509                 NULL,
 510         },
 511 };
 512
 513 /**********************************************************/
 514 /* Servo_Carry */
 515
 516 /* this structure is filled when cmd_servo_carry is parsed successfully */
 517 struct cmd_servo_carry_result {
 518         fixed_string_t arg0;
 519         fixed_string_t arg1;
 520 };
 521
 522 /* function called when cmd_servo_carry is parsed successfully */
 523 static void cmd_servo_carry_parsed(void *parsed_result,
 524                                     __attribute__((unused)) void *data)
 525 {
 526         struct cmd_servo_carry_result *res = parsed_result;
 527         if (!strcmp_P(res->arg1, PSTR("open")))
 528                 servo_carry_open();
 529         else if (!strcmp_P(res->arg1, PSTR("closed")))
 530                 servo_carry_close();
 531 }
 532
 533 prog_char str_servo_carry_arg0[] = "carry";
 534 parse_pgm_token_string_t cmd_servo_carry_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_servo_carry_result, arg0, str_servo_carry_arg0);
 535 prog_char str_servo_carry_arg1[] = "open#closed";
 536 parse_pgm_token_string_t cmd_servo_carry_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_servo_carry_result, arg1, str_servo_carry_arg1);
 537
 538 prog_char help_servo_carry[] = "Servo carry function";
 539 parse_pgm_inst_t cmd_servo_carry = {
 540         .f = cmd_servo_carry_parsed,  /* function to call */
 541         .data = NULL,      /* 2nd arg of func */
 542         .help_str = help_servo_carry,
 543         .tokens = {        /* token list, NULL terminated */
 544                 (prog_void *)&cmd_servo_carry_arg0,
 545                 (prog_void *)&cmd_servo_carry_arg1,
 546                 NULL,
 547         },
 548 };
 549
 550 /**********************************************************/
 551 /* Spickle tests */
 552
 553 /* this structure is filled when cmd_spickle is parsed successfully */
 554 struct cmd_spickle_result {
 555         fixed_string_t arg0;
 556         fixed_string_t arg1;
 557         fixed_string_t arg2;
 558 };
 559
 560 /* function called when cmd_spickle is parsed successfully */
 561 static void cmd_spickle_parsed(void * parsed_result,
 562                                __attribute__((unused)) void *data)
 563 {
 564         struct cmd_spickle_result * res = parsed_result;
 565         uint8_t side;
 566
 567         if (!strcmp_P(res->arg1, PSTR("left")))
 568                 side = I2C_LEFT_SIDE;
 569         else
 570                 side = I2C_RIGHT_SIDE;
 571
 572         if (!strcmp_P(res->arg2, PSTR("deploy"))) {
 573                 spickle_deploy(side);
 574         }
 575         else if (!strcmp_P(res->arg2, PSTR("pack"))) {
 576                 spickle_pack(side);
 577         }
 578         else if (!strcmp_P(res->arg2, PSTR("mid"))) {
 579                 spickle_mid(side);
 580         }
 581         printf_P(PSTR("done\r\n"));
 582 }
 583
 584 prog_char str_spickle_arg0[] = "spickle";
 585 parse_pgm_token_string_t cmd_spickle_arg0 =
 586         TOKEN_STRING_INITIALIZER(struct cmd_spickle_result, arg0, str_spickle_arg0);
 587 prog_char str_spickle_arg1[] = "left#right";
 588 parse_pgm_token_string_t cmd_spickle_arg1 =
 589         TOKEN_STRING_INITIALIZER(struct cmd_spickle_result, arg1, str_spickle_arg1);
 590 prog_char str_spickle_arg2[] = "deploy#pack#mid";
 591 parse_pgm_token_string_t cmd_spickle_arg2 =
 592         TOKEN_STRING_INITIALIZER(struct cmd_spickle_result, arg2, str_spickle_arg2);
 593
 594 prog_char help_spickle[] = "move spickle";
 595 parse_pgm_inst_t cmd_spickle = {
 596         .f = cmd_spickle_parsed,  /* function to call */
 597         .data = NULL,      /* 2nd arg of func */
 598         .help_str = help_spickle,
 599         .tokens = {        /* token list, NULL terminated */
 600                 (prog_void *)&cmd_spickle_arg0,
 601                 (prog_void *)&cmd_spickle_arg1,
 602                 (prog_void *)&cmd_spickle_arg2,
 603                 NULL,
 604         },
 605 };
 606
 607 /**********************************************************/
 608 /* Set Spickle Params */
 609
 610 /* this structure is filled when cmd_spickle_params is parsed successfully */
 611 struct cmd_spickle_params_result {
 612         fixed_string_t arg0;
 613         fixed_string_t arg1;
 614         fixed_string_t arg2;
 615         int32_t arg3;
 616         int32_t arg4;
 617         int32_t arg5;
 618 };
 619
 620 /* function called when cmd_spickle_params is parsed successfully */
 621 static void cmd_spickle_params_parsed(void *parsed_result,
 622                                       __attribute__((unused)) void *data)
 623 {
 624         struct cmd_spickle_params_result * res = parsed_result;
 625         uint8_t side;
 626
 627         if (!strcmp_P(res->arg1, PSTR("show"))) {
 628                 spickle_dump_params();
 629                 return;
 630         }
 631
 632         if (!strcmp_P(res->arg1, PSTR("left")))
 633                 side = I2C_LEFT_SIDE;
 634         else
 635                 side = I2C_RIGHT_SIDE;
 636
 637         if (!strcmp_P(res->arg2, PSTR("pos")))
 638                 spickle_set_pos(side, res->arg3, res->arg4, res->arg5);
 639 }
 640
 641 prog_char str_spickle_params_arg0[] = "spickle_params";
 642 parse_pgm_token_string_t cmd_spickle_params_arg0 =
 643         TOKEN_STRING_INITIALIZER(struct cmd_spickle_params_result, arg0, str_spickle_params_arg0);
 644 prog_char str_spickle_params_arg1[] = "left#right";
 645 parse_pgm_token_string_t cmd_spickle_params_arg1 =
 646         TOKEN_STRING_INITIALIZER(struct cmd_spickle_params_result, arg1, str_spickle_params_arg1);
 647 prog_char str_spickle_params_arg2[] = "pos";
 648 parse_pgm_token_string_t cmd_spickle_params_arg2 =
 649         TOKEN_STRING_INITIALIZER(struct cmd_spickle_params_result, arg2, str_spickle_params_arg2);
 650 parse_pgm_token_num_t cmd_spickle_params_arg3 =
 651         TOKEN_NUM_INITIALIZER(struct cmd_spickle_params_result, arg3, INT32);
 652 parse_pgm_token_num_t cmd_spickle_params_arg4 =
 653         TOKEN_NUM_INITIALIZER(struct cmd_spickle_params_result, arg4, INT32);
 654 parse_pgm_token_num_t cmd_spickle_params_arg5 =
 655         TOKEN_NUM_INITIALIZER(struct cmd_spickle_params_result, arg5, INT32);
 656
 657 prog_char help_spickle_params[] = "Set spickle pos values: left|right pos INTPACK INTMID INTDEPL";
 658 parse_pgm_inst_t cmd_spickle_params = {
 659         .f = cmd_spickle_params_parsed,  /* function to call */
 660         .data = NULL,      /* 2nd arg of func */
 661         .help_str = help_spickle_params,
 662         .tokens = {        /* token list, NULL terminated */
 663                 (prog_void *)&cmd_spickle_params_arg0,
 664                 (prog_void *)&cmd_spickle_params_arg1,
 665                 (prog_void *)&cmd_spickle_params_arg2,
 666                 (prog_void *)&cmd_spickle_params_arg3,
 667                 (prog_void *)&cmd_spickle_params_arg4,
 668                 (prog_void *)&cmd_spickle_params_arg5,
 669                 NULL,
 670         },
 671 };
 672
 673 prog_char str_spickle_params_arg1_show[] = "show";
 674 parse_pgm_token_string_t cmd_spickle_params_arg1_show =
 675         TOKEN_STRING_INITIALIZER(struct cmd_spickle_params_result, arg1, str_spickle_params_arg1_show);
 676
 677 prog_char help_spickle_params_show[] = "show spickle params";
 678 parse_pgm_inst_t cmd_spickle_params_show = {
 679         .f = cmd_spickle_params_parsed,  /* function to call */
 680         .data = NULL,      /* 2nd arg of func */
 681         .help_str = help_spickle_params_show,
 682         .tokens = {        /* token list, NULL terminated */
 683                 (prog_void *)&cmd_spickle_params_arg0,
 684                 (prog_void *)&cmd_spickle_params_arg1_show,
 685                 NULL,
 686         },
 687 };
 688
 689 /**********************************************************/
 690 /* Set Spickle Params */
 691
 692 /* this structure is filled when cmd_spickle_params2 is parsed successfully */
 693 struct cmd_spickle_params2_result {
 694         fixed_string_t arg0;
 695         fixed_string_t arg1;
 696         int32_t arg2;
 697         int32_t arg3;
 698 };
 699
 700 /* function called when cmd_spickle_params2 is parsed successfully */
 701 static void cmd_spickle_params2_parsed(void *parsed_result,
 702                                       __attribute__((unused)) void *data)
 703 {
 704         struct cmd_spickle_params2_result * res = parsed_result;
 705
 706         if (!strcmp_P(res->arg1, PSTR("coef"))) {
 707                 spickle_set_coefs(res->arg2, res->arg3);
 708         }
 709
 710         /* else show */
 711         spickle_dump_params();
 712 }
 713
 714 prog_char str_spickle_params2_arg0[] = "spickle_params2";
 715 parse_pgm_token_string_t cmd_spickle_params2_arg0 =
 716         TOKEN_STRING_INITIALIZER(struct cmd_spickle_params2_result, arg0, str_spickle_params2_arg0);
 717 prog_char str_spickle_params2_arg1[] = "coef";
 718 parse_pgm_token_string_t cmd_spickle_params2_arg1 =
 719         TOKEN_STRING_INITIALIZER(struct cmd_spickle_params2_result, arg1, str_spickle_params2_arg1);
 720 parse_pgm_token_num_t cmd_spickle_params2_arg2 =
 721         TOKEN_NUM_INITIALIZER(struct cmd_spickle_params2_result, arg2, INT32);
 722 parse_pgm_token_num_t cmd_spickle_params2_arg3 =
 723         TOKEN_NUM_INITIALIZER(struct cmd_spickle_params2_result, arg3, INT32);
 724
 725 prog_char help_spickle_params2[] = "Set spickle_params2 values";
 726 parse_pgm_inst_t cmd_spickle_params2 = {
 727         .f = cmd_spickle_params2_parsed,  /* function to call */
 728         .data = NULL,      /* 2nd arg of func */
 729         .help_str = help_spickle_params2,
 730         .tokens = {        /* token list, NULL terminated */
 731                 (prog_void *)&cmd_spickle_params2_arg0,
 732                 (prog_void *)&cmd_spickle_params2_arg1,
 733                 (prog_void *)&cmd_spickle_params2_arg2,
 734                 (prog_void *)&cmd_spickle_params2_arg3,
 735                 NULL,
 736         },
 737 };
 738
 739 prog_char str_spickle_params2_arg1_show[] = "show";
 740 parse_pgm_token_string_t cmd_spickle_params2_arg1_show =
 741         TOKEN_STRING_INITIALIZER(struct cmd_spickle_params2_result, arg1, str_spickle_params2_arg1_show);
 742
 743 prog_char help_spickle_params2_show[] = "show spickle params";
 744 parse_pgm_inst_t cmd_spickle_params2_show = {
 745         .f = cmd_spickle_params2_parsed,  /* function to call */
 746         .data = NULL,      /* 2nd arg of func */
 747         .help_str = help_spickle_params2_show,
 748         .tokens = {        /* token list, NULL terminated */
 749                 (prog_void *)&cmd_spickle_params2_arg0,
 750                 (prog_void *)&cmd_spickle_params2_arg1_show,
 751                 NULL,
 752         },
 753 };
 754
 755 /**********************************************************/
 756 /* Test */
 757
 758 /* this structure is filled when cmd_test is parsed successfully */
 759 struct cmd_test_result {
 760         fixed_string_t arg0;
 761 };
 762
 763 /* function called when cmd_test is parsed successfully */
 764 static void cmd_test_parsed(__attribute__((unused)) void *parsed_result,
 765                             __attribute__((unused)) void *data)
 766 {
 767 }
 768
 769 prog_char str_test_arg0[] = "test";
 770 parse_pgm_token_string_t cmd_test_arg0 =
 771         TOKEN_STRING_INITIALIZER(struct cmd_test_result, arg0, str_test_arg0);
 772
 773 prog_char help_test[] = "Test function";
 774 parse_pgm_inst_t cmd_test = {
 775         .f = cmd_test_parsed,  /* function to call */
 776         .data = NULL,      /* 2nd arg of func */
 777         .help_str = help_test,
 778         .tokens = {        /* token list, NULL terminated */
 779                 (prog_void *)&cmd_test_arg0,
 780                 NULL,
 781         },
 782 };