projects/microb2010/cobboard/commands_cobboard.c

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