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