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