projects/microb2010/ballboard/commands_ballboard.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_ballboard.c,v 1.2 2009-04-24 19:30:42 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 "main.h"
  47 #include "state.h"
  48 #include "cmdline.h"
  49 #include "../common/i2c_commands.h"
  50 #include "i2c_protocol.h"
  51 #include "actuator.h"
  52
  53 struct cmd_event_result {
  54         fixed_string_t arg0;
  55         fixed_string_t arg1;
  56         fixed_string_t arg2;
  57 };
  58
  59
  60 /* function called when cmd_event is parsed successfully */
  61 static void cmd_event_parsed(void *parsed_result, void *data)
  62 {
  63         u08 bit=0;
  64
  65         struct cmd_event_result * res = parsed_result;
  66
  67         if (!strcmp_P(res->arg1, PSTR("all"))) {
  68                 bit = DO_ENCODERS | DO_CS | DO_BD | DO_POWER;
  69                 if (!strcmp_P(res->arg2, PSTR("on")))
  70                         ballboard.flags |= bit;
  71                 else if (!strcmp_P(res->arg2, PSTR("off")))
  72                         ballboard.flags &= bit;
  73                 else { /* show */
  74                         printf_P(PSTR("encoders is %s\r\n"),
  75                                  (DO_ENCODERS & ballboard.flags) ? "on":"off");
  76                         printf_P(PSTR("cs is %s\r\n"),
  77                                  (DO_CS & ballboard.flags) ? "on":"off");
  78                         printf_P(PSTR("bd is %s\r\n"),
  79                                  (DO_BD & ballboard.flags) ? "on":"off");
  80                         printf_P(PSTR("power is %s\r\n"),
  81                                  (DO_POWER & ballboard.flags) ? "on":"off");
  82                 }
  83                 return;
  84         }
  85
  86         if (!strcmp_P(res->arg1, PSTR("encoders")))
  87                 bit = DO_ENCODERS;
  88         else if (!strcmp_P(res->arg1, PSTR("cs"))) {
  89                 bit = DO_CS;
  90         }
  91         else if (!strcmp_P(res->arg1, PSTR("bd")))
  92                 bit = DO_BD;
  93         else if (!strcmp_P(res->arg1, PSTR("power")))
  94                 bit = DO_POWER;
  95
  96
  97         if (!strcmp_P(res->arg2, PSTR("on")))
  98                 ballboard.flags |= bit;
  99         else if (!strcmp_P(res->arg2, PSTR("off"))) {
 100                 if (!strcmp_P(res->arg1, PSTR("cs"))) {
 101                         pwm_ng_set(ROLLER_PWM, 0);
 102                         pwm_ng_set(FORKTRANS_PWM, 0);
 103                         pwm_ng_set(FORKROT_PWM, 0);
 104                 }
 105                 ballboard.flags &= (~bit);
 106         }
 107         printf_P(PSTR("%s is %s\r\n"), res->arg1,
 108                       (bit & ballboard.flags) ? "on":"off");
 109 }
 110
 111 prog_char str_event_arg0[] = "event";
 112 parse_pgm_token_string_t cmd_event_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg0, str_event_arg0);
 113 prog_char str_event_arg1[] = "all#encoders#cs#bd#power";
 114 parse_pgm_token_string_t cmd_event_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg1, str_event_arg1);
 115 prog_char str_event_arg2[] = "on#off#show";
 116 parse_pgm_token_string_t cmd_event_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg2, str_event_arg2);
 117
 118 prog_char help_event[] = "Enable/disable events";
 119 parse_pgm_inst_t cmd_event = {
 120         .f = cmd_event_parsed,  /* function to call */
 121         .data = NULL,      /* 2nd arg of func */
 122         .help_str = help_event,
 123         .tokens = {        /* token list, NULL terminated */
 124                 (prog_void *)&cmd_event_arg0,
 125                 (prog_void *)&cmd_event_arg1,
 126                 (prog_void *)&cmd_event_arg2,
 127                 NULL,
 128         },
 129 };
 130
 131 /**********************************************************/
 132 /* Color */
 133
 134 /* this structure is filled when cmd_color is parsed successfully */
 135 struct cmd_color_result {
 136         fixed_string_t arg0;
 137         fixed_string_t color;
 138 };
 139
 140 /* function called when cmd_color is parsed successfully */
 141 static void cmd_color_parsed(void *parsed_result, void *data)
 142 {
 143         struct cmd_color_result *res = (struct cmd_color_result *) parsed_result;
 144         if (!strcmp_P(res->color, PSTR("yellow"))) {
 145                 ballboard.our_color = I2C_COLOR_YELLOW;
 146         }
 147         else if (!strcmp_P(res->color, PSTR("blue"))) {
 148                 ballboard.our_color = I2C_COLOR_BLUE;
 149         }
 150         printf_P(PSTR("Done\r\n"));
 151 }
 152
 153 prog_char str_color_arg0[] = "color";
 154 parse_pgm_token_string_t cmd_color_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_color_result, arg0, str_color_arg0);
 155 prog_char str_color_color[] = "blue#yellow";
 156 parse_pgm_token_string_t cmd_color_color = TOKEN_STRING_INITIALIZER(struct cmd_color_result, color, str_color_color);
 157
 158 prog_char help_color[] = "Set our color";
 159 parse_pgm_inst_t cmd_color = {
 160         .f = cmd_color_parsed,  /* function to call */
 161         .data = NULL,      /* 2nd arg of func */
 162         .help_str = help_color,
 163         .tokens = {        /* token list, NULL terminated */
 164                 (prog_void *)&cmd_color_arg0,
 165                 (prog_void *)&cmd_color_color,
 166                 NULL,
 167         },
 168 };
 169
 170
 171 /**********************************************************/
 172 /* State1 */
 173
 174 /* this structure is filled when cmd_state1 is parsed successfully */
 175 struct cmd_state1_result {
 176         fixed_string_t arg0;
 177         fixed_string_t arg1;
 178 };
 179
 180 /* function called when cmd_state1 is parsed successfully */
 181 static void cmd_state1_parsed(void *parsed_result,
 182                               __attribute__((unused)) void *data)
 183 {
 184         struct cmd_state1_result *res = parsed_result;
 185
 186         if (!strcmp_P(res->arg1, PSTR("init")))
 187                 state_init();
 188         else if (!strcmp_P(res->arg1, PSTR("off")))
 189                 state_set_mode(I2C_BALLBOARD_MODE_OFF);
 190         else if (!strcmp_P(res->arg1, PSTR("eject")))
 191                 state_set_mode(I2C_BALLBOARD_MODE_EJECT);
 192         else if (!strcmp_P(res->arg1, PSTR("harvest")))
 193                 state_set_mode(I2C_BALLBOARD_MODE_HARVEST);
 194
 195         /* other commands */
 196 }
 197
 198 prog_char str_state1_arg0[] = "ballboard";
 199 parse_pgm_token_string_t cmd_state1_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_state1_result, arg0, str_state1_arg0);
 200 prog_char str_state1_arg1[] = "init#eject#harvest#off";
 201 parse_pgm_token_string_t cmd_state1_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_state1_result, arg1, str_state1_arg1);
 202
 203 prog_char help_state1[] = "set ballboard mode";
 204 parse_pgm_inst_t cmd_state1 = {
 205         .f = cmd_state1_parsed,  /* function to call */
 206         .data = NULL,      /* 2nd arg of func */
 207         .help_str = help_state1,
 208         .tokens = {        /* token list, NULL terminated */
 209                 (prog_void *)&cmd_state1_arg0,
 210                 (prog_void *)&cmd_state1_arg1,
 211                 NULL,
 212         },
 213 };
 214
 215 /**********************************************************/
 216 /* State2 */
 217
 218 /* this structure is filled when cmd_state2 is parsed successfully */
 219 struct cmd_state2_result {
 220         fixed_string_t arg0;
 221         fixed_string_t arg1;
 222         fixed_string_t arg2;
 223 };
 224
 225 /* function called when cmd_state2 is parsed successfully */
 226 static void cmd_state2_parsed(void *parsed_result,
 227                               __attribute__((unused)) void *data)
 228 {
 229         struct cmd_state2_result *res = parsed_result;
 230         uint8_t mode;
 231
 232         if (!strcmp_P(res->arg2, PSTR("left"))) {
 233                 if (!strcmp_P(res->arg1, PSTR("prepare")))
 234                         mode = I2C_BALLBOARD_MODE_PREP_L_FORK;
 235                 else if (!strcmp_P(res->arg1, PSTR("take")))
 236                         mode = I2C_BALLBOARD_MODE_TAKE_L_FORK;
 237         }
 238         else {
 239                 if (!strcmp_P(res->arg1, PSTR("prepare")))
 240                         mode = I2C_BALLBOARD_MODE_PREP_R_FORK;
 241                 else if (!strcmp_P(res->arg1, PSTR("take")))
 242                         mode = I2C_BALLBOARD_MODE_TAKE_R_FORK;
 243         }
 244         //state_set_mode(mode);
 245 }
 246
 247 prog_char str_state2_arg0[] = "ballboard";
 248 parse_pgm_token_string_t cmd_state2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_state2_result, arg0, str_state2_arg0);
 249 prog_char str_state2_arg1[] = "prepare#take";
 250 parse_pgm_token_string_t cmd_state2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_state2_result, arg1, str_state2_arg1);
 251 prog_char str_state2_arg2[] = "left#right";
 252 parse_pgm_token_string_t cmd_state2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_state2_result, arg2, str_state2_arg2);
 253
 254 prog_char help_state2[] = "set ballboard mode";
 255 parse_pgm_inst_t cmd_state2 = {
 256         .f = cmd_state2_parsed,  /* function to call */
 257         .data = NULL,      /* 2nd arg of func */
 258         .help_str = help_state2,
 259         .tokens = {        /* token list, NULL terminated */
 260                 (prog_void *)&cmd_state2_arg0,
 261                 (prog_void *)&cmd_state2_arg1,
 262                 (prog_void *)&cmd_state2_arg2,
 263                 NULL,
 264         },
 265 };
 266
 267 /**********************************************************/
 268 /* State3 */
 269
 270 /* this structure is filled when cmd_state3 is parsed successfully */
 271 struct cmd_state3_result {
 272         fixed_string_t arg0;
 273         fixed_string_t arg1;
 274         uint8_t arg2;
 275 };
 276
 277 /* function called when cmd_state3 is parsed successfully */
 278 static void cmd_state3_parsed(void *parsed_result,
 279                               __attribute__((unused)) void *data)
 280 {
 281         struct cmd_state3_result *res = parsed_result;
 282
 283         if (!strcmp_P(res->arg1, PSTR("xxx"))) {
 284                 /* xxx = res->arg2 */
 285         }
 286         else if (!strcmp_P(res->arg1, PSTR("yyy"))) {
 287         }
 288         state_set_mode(0);
 289 }
 290
 291 prog_char str_state3_arg0[] = "ballboard";
 292 parse_pgm_token_string_t cmd_state3_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_state3_result, arg0, str_state3_arg0);
 293 prog_char str_state3_arg1[] = "xxx";
 294 parse_pgm_token_string_t cmd_state3_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_state3_result, arg1, str_state3_arg1);
 295 parse_pgm_token_num_t cmd_state3_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_state3_result, arg2, UINT8);
 296
 297 prog_char help_state3[] = "set ballboard mode";
 298 parse_pgm_inst_t cmd_state3 = {
 299         .f = cmd_state3_parsed,  /* function to call */
 300         .data = NULL,      /* 2nd arg of func */
 301         .help_str = help_state3,
 302         .tokens = {        /* token list, NULL terminated */
 303                 (prog_void *)&cmd_state3_arg0,
 304                 (prog_void *)&cmd_state3_arg1,
 305                 (prog_void *)&cmd_state3_arg2,
 306                 NULL,
 307         },
 308 };
 309
 310 /**********************************************************/
 311 /* State_Machine */
 312
 313 /* this structure is filled when cmd_state_machine is parsed successfully */
 314 struct cmd_state_machine_result {
 315         fixed_string_t arg0;
 316 };
 317
 318 /* function called when cmd_state_machine is parsed successfully */
 319 static void cmd_state_machine_parsed(__attribute__((unused)) void *parsed_result,
 320                                      __attribute__((unused)) void *data)
 321 {
 322         state_machine();
 323 }
 324
 325 prog_char str_state_machine_arg0[] = "state_machine";
 326 parse_pgm_token_string_t cmd_state_machine_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_state_machine_result, arg0, str_state_machine_arg0);
 327
 328 prog_char help_state_machine[] = "launch state machine";
 329 parse_pgm_inst_t cmd_state_machine = {
 330         .f = cmd_state_machine_parsed,  /* function to call */
 331         .data = NULL,      /* 2nd arg of func */
 332         .help_str = help_state_machine,
 333         .tokens = {        /* token list, NULL terminated */
 334                 (prog_void *)&cmd_state_machine_arg0,
 335                 NULL,
 336         },
 337 };
 338
 339 /**********************************************************/
 340 /* State_Debug */
 341
 342 /* this structure is filled when cmd_state_debug is parsed successfully */
 343 struct cmd_state_debug_result {
 344         fixed_string_t arg0;
 345         uint8_t on;
 346 };
 347
 348 /* function called when cmd_state_debug is parsed successfully */
 349 static void cmd_state_debug_parsed(void *parsed_result,
 350                                    __attribute__((unused)) void *data)
 351 {
 352         struct cmd_state_debug_result *res = parsed_result;
 353         state_debug = res->on;
 354 }
 355
 356 prog_char str_state_debug_arg0[] = "state_debug";
 357 parse_pgm_token_string_t cmd_state_debug_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_state_debug_result, arg0, str_state_debug_arg0);
 358 parse_pgm_token_num_t cmd_state_debug_on = TOKEN_NUM_INITIALIZER(struct cmd_state_debug_result, on, UINT8);
 359
 360 prog_char help_state_debug[] = "Set debug for state machine";
 361 parse_pgm_inst_t cmd_state_debug = {
 362         .f = cmd_state_debug_parsed,  /* function to call */
 363         .data = NULL,      /* 2nd arg of func */
 364         .help_str = help_state_debug,
 365         .tokens = {        /* token list, NULL terminated */
 366                 (prog_void *)&cmd_state_debug_arg0,
 367                 (prog_void *)&cmd_state_debug_on,
 368                 NULL,
 369         },
 370 };
 371
 372 /**********************************************************/
 373 /* Fork */
 374
 375 /* this structure is filled when cmd_fork is parsed successfully */
 376 struct cmd_fork_result {
 377         fixed_string_t arg0;
 378         fixed_string_t arg1;
 379 };
 380
 381 /* function called when cmd_fork is parsed successfully */
 382 static void cmd_fork_parsed(void *parsed_result,
 383                                     __attribute__((unused)) void *data)
 384 {
 385         struct cmd_fork_result *res = parsed_result;
 386         if (!strcmp_P(res->arg1, PSTR("deploy")))
 387                 fork_deploy();
 388         else if (!strcmp_P(res->arg1, PSTR("pack")))
 389                 fork_pack();
 390         else if (!strcmp_P(res->arg1, PSTR("left")))
 391                 fork_left();
 392         else if (!strcmp_P(res->arg1, PSTR("right")))
 393                 fork_right();
 394         else if (!strcmp_P(res->arg1, PSTR("middle")))
 395                 fork_middle();
 396 }
 397
 398 prog_char str_fork_arg0[] = "fork";
 399 parse_pgm_token_string_t cmd_fork_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_fork_result, arg0, str_fork_arg0);
 400 prog_char str_fork_arg1[] = "deploy#pack#left#right#middle";
 401 parse_pgm_token_string_t cmd_fork_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_fork_result, arg1, str_fork_arg1);
 402
 403 prog_char help_fork[] = "move fork";
 404 parse_pgm_inst_t cmd_fork = {
 405         .f = cmd_fork_parsed,  /* function to call */
 406         .data = NULL,      /* 2nd arg of func */
 407         .help_str = help_fork,
 408         .tokens = {        /* token list, NULL terminated */
 409                 (prog_void *)&cmd_fork_arg0,
 410                 (prog_void *)&cmd_fork_arg1,
 411                 NULL,
 412         },
 413 };
 414
 415 /**********************************************************/
 416 /* Roller */
 417
 418 /* this structure is filled when cmd_roller is parsed successfully */
 419 struct cmd_roller_result {
 420         fixed_string_t arg0;
 421         fixed_string_t arg1;
 422 };
 423
 424 /* function called when cmd_roller is parsed successfully */
 425 static void cmd_roller_parsed(void *parsed_result,
 426                                     __attribute__((unused)) void *data)
 427 {
 428         struct cmd_roller_result *res = parsed_result;
 429         if (!strcmp_P(res->arg1, PSTR("on")))
 430                 roller_on();
 431         else if (!strcmp_P(res->arg1, PSTR("off")))
 432                 roller_off();
 433         else if (!strcmp_P(res->arg1, PSTR("reverse")))
 434                 roller_reverse();
 435 }
 436
 437 prog_char str_roller_arg0[] = "roller";
 438 parse_pgm_token_string_t cmd_roller_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_roller_result, arg0, str_roller_arg0);
 439 prog_char str_roller_arg1[] = "on#off#reverse";
 440 parse_pgm_token_string_t cmd_roller_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_roller_result, arg1, str_roller_arg1);
 441
 442 prog_char help_roller[] = "Servo door function";
 443 parse_pgm_inst_t cmd_roller = {
 444         .f = cmd_roller_parsed,  /* function to call */
 445         .data = NULL,      /* 2nd arg of func */
 446         .help_str = help_roller,
 447         .tokens = {        /* token list, NULL terminated */
 448                 (prog_void *)&cmd_roller_arg0,
 449                 (prog_void *)&cmd_roller_arg1,
 450                 NULL,
 451         },
 452 };
 453
 454 /**********************************************************/
 455 /* Test */
 456
 457 /* this structure is filled when cmd_test is parsed successfully */
 458 struct cmd_test_result {
 459         fixed_string_t arg0;
 460 };
 461
 462 /* function called when cmd_test is parsed successfully */
 463 static void cmd_test_parsed(void *parsed_result, void *data)
 464 {
 465         //struct cmd_test_result *res = parsed_result;
 466 }
 467
 468 prog_char str_test_arg0[] = "test";
 469 parse_pgm_token_string_t cmd_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_test_result, arg0, str_test_arg0);
 470
 471 prog_char help_test[] = "Test function";
 472 parse_pgm_inst_t cmd_test = {
 473         .f = cmd_test_parsed,  /* function to call */
 474         .data = NULL,      /* 2nd arg of func */
 475         .help_str = help_test,
 476         .tokens = {        /* token list, NULL terminated */
 477                 (prog_void *)&cmd_test_arg0,
 478                 NULL,
 479         },
 480 };