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