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