projects/microb2010/cobboard/commands_cobboard.c

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