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