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