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