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