projects/microb2010/tests/test_board2008/commands_mainboard.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_mainboard.c,v 1.7 2009-05-02 10:08:09 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 #include <avr/eeprom.h>
  30
  31 #include <uart.h>
  32 #include <pwm_ng.h>
  33 #include <time.h>
  34
  35 #include <pid.h>
  36 #include <quadramp.h>
  37 #include <control_system_manager.h>
  38 #include <trajectory_manager.h>
  39 #include <blocking_detection_manager.h>
  40 #include <robot_system.h>
  41 #include <position_manager.h>
  42
  43 #include <rdline.h>
  44 #include <parse.h>
  45 #include <parse_string.h>
  46 #include <parse_num.h>
  47
  48 #include "main.h"
  49 #include "sensor.h"
  50 #include "cmdline.h"
  51 #include "actuator.h"
  52 #include "strat_base.h"
  53
  54 struct cmd_event_result {
  55         fixed_string_t arg0;
  56         fixed_string_t arg1;
  57         fixed_string_t arg2;
  58 };
  59
  60
  61 /* function called when cmd_event is parsed successfully */
  62 static void cmd_event_parsed(void *parsed_result, void *data)
  63 {
  64         u08 bit=0;
  65
  66         struct cmd_event_result * res = parsed_result;
  67
  68         if (!strcmp_P(res->arg1, PSTR("all"))) {
  69                 bit = DO_ENCODERS | DO_CS | DO_RS | DO_POS |
  70                         DO_BD | DO_TIMER | DO_POWER;
  71                 if (!strcmp_P(res->arg2, PSTR("on")))
  72                         mainboard.flags |= bit;
  73                 else if (!strcmp_P(res->arg2, PSTR("off")))
  74                         mainboard.flags &= bit;
  75                 else { /* show */
  76                         printf_P(PSTR("encoders is %s\r\n"),
  77                                  (DO_ENCODERS & mainboard.flags) ? "on":"off");
  78                         printf_P(PSTR("cs is %s\r\n"),
  79                                  (DO_CS & mainboard.flags) ? "on":"off");
  80                         printf_P(PSTR("rs is %s\r\n"),
  81                                  (DO_RS & mainboard.flags) ? "on":"off");
  82                         printf_P(PSTR("pos is %s\r\n"),
  83                                  (DO_POS & mainboard.flags) ? "on":"off");
  84                         printf_P(PSTR("bd is %s\r\n"),
  85                                  (DO_BD & mainboard.flags) ? "on":"off");
  86                         printf_P(PSTR("timer is %s\r\n"),
  87                                  (DO_TIMER & mainboard.flags) ? "on":"off");
  88                         printf_P(PSTR("power is %s\r\n"),
  89                                  (DO_POWER & mainboard.flags) ? "on":"off");
  90                 }
  91                 return;
  92         }
  93
  94         if (!strcmp_P(res->arg1, PSTR("encoders")))
  95                 bit = DO_ENCODERS;
  96         else if (!strcmp_P(res->arg1, PSTR("cs"))) {
  97                 strat_hardstop();
  98                 bit = DO_CS;
  99         }
 100         else if (!strcmp_P(res->arg1, PSTR("rs")))
 101                 bit = DO_RS;
 102         else if (!strcmp_P(res->arg1, PSTR("pos")))
 103                 bit = DO_POS;
 104         else if (!strcmp_P(res->arg1, PSTR("bd")))
 105                 bit = DO_BD;
 106         else if (!strcmp_P(res->arg1, PSTR("timer"))) {
 107                 time_reset();
 108                 bit = DO_TIMER;
 109         }
 110         else if (!strcmp_P(res->arg1, PSTR("power")))
 111                 bit = DO_POWER;
 112
 113         if (!strcmp_P(res->arg2, PSTR("on")))
 114                 mainboard.flags |= bit;
 115         else if (!strcmp_P(res->arg2, PSTR("off"))) {
 116                 if (!strcmp_P(res->arg1, PSTR("cs"))) {
 117                         pwm_ng_set(LEFT_PWM, 0);
 118                         pwm_ng_set(RIGHT_PWM, 0);
 119                 }
 120                 mainboard.flags &= (~bit);
 121         }
 122         printf_P(PSTR("%s is %s\r\n"), res->arg1,
 123                       (bit & mainboard.flags) ? "on":"off");
 124 }
 125
 126 prog_char str_event_arg0[] = "event";
 127 parse_pgm_token_string_t cmd_event_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg0, str_event_arg0);
 128 prog_char str_event_arg1[] = "all#encoders#cs#rs#pos#bd#timer#power";
 129 parse_pgm_token_string_t cmd_event_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg1, str_event_arg1);
 130 prog_char str_event_arg2[] = "on#off#show";
 131 parse_pgm_token_string_t cmd_event_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg2, str_event_arg2);
 132
 133 prog_char help_event[] = "Enable/disable events";
 134 parse_pgm_inst_t cmd_event = {
 135         .f = cmd_event_parsed,  /* function to call */
 136         .data = NULL,      /* 2nd arg of func */
 137         .help_str = help_event,
 138         .tokens = {        /* token list, NULL terminated */
 139                 (prog_void *)&cmd_event_arg0,
 140                 (prog_void *)&cmd_event_arg1,
 141                 (prog_void *)&cmd_event_arg2,
 142                 NULL,
 143         },
 144 };
 145
 146 /**********************************************************/
 147 /* Interact */
 148
 149 /* this structure is filled when cmd_interact is parsed successfully */
 150 struct cmd_interact_result {
 151         fixed_string_t arg0;
 152 };
 153
 154 static void print_cs(void)
 155 {
 156         printf_P(PSTR("cons_d=% .8ld cons_a=% .8ld fil_d=% .8ld fil_a=% .8ld "
 157                       "err_d=% .8ld err_a=% .8ld out_d=% .8ld out_a=% .8ld\r\n"),
 158                  cs_get_consign(&mainboard.distance.cs),
 159                  cs_get_consign(&mainboard.angle.cs),
 160                  cs_get_filtered_consign(&mainboard.distance.cs),
 161                  cs_get_filtered_consign(&mainboard.angle.cs),
 162                  cs_get_error(&mainboard.distance.cs),
 163                  cs_get_error(&mainboard.angle.cs),
 164                  cs_get_out(&mainboard.distance.cs),
 165                  cs_get_out(&mainboard.angle.cs));
 166 }
 167
 168 static void print_pos(void)
 169 {
 170         printf_P(PSTR("x=% .8d y=% .8d a=% .8d\r\n"),
 171                  position_get_x_s16(&mainboard.pos),
 172                  position_get_y_s16(&mainboard.pos),
 173                  position_get_a_deg_s16(&mainboard.pos));
 174 }
 175
 176 static void print_time(void)
 177 {
 178         printf_P(PSTR("time %d\r\n"),time_get_s());
 179 }
 180
 181
 182 static void print_sensors(void)
 183 {
 184 #ifdef notyet
 185         if (sensor_start_switch())
 186                 printf_P(PSTR("Start switch | "));
 187         else
 188                 printf_P(PSTR("             | "));
 189
 190         if (IR_DISP_SENSOR())
 191                 printf_P(PSTR("IR disp | "));
 192         else
 193                 printf_P(PSTR("        | "));
 194
 195         printf_P(PSTR("\r\n"));
 196 #endif
 197 }
 198
 199 static void print_pid(void)
 200 {
 201         printf_P(PSTR("P=% .8ld I=% .8ld D=% .8ld out=% .8ld | "
 202                       "P=% .8ld I=% .8ld D=% .8ld out=% .8ld\r\n"),
 203                  pid_get_value_in(&mainboard.distance.pid) * pid_get_gain_P(&mainboard.distance.pid),
 204                  pid_get_value_I(&mainboard.distance.pid) * pid_get_gain_I(&mainboard.distance.pid),
 205                  pid_get_value_D(&mainboard.distance.pid) * pid_get_gain_D(&mainboard.distance.pid),
 206                  pid_get_value_out(&mainboard.distance.pid),
 207                  pid_get_value_in(&mainboard.angle.pid) * pid_get_gain_P(&mainboard.angle.pid),
 208                  pid_get_value_I(&mainboard.angle.pid) * pid_get_gain_I(&mainboard.angle.pid),
 209                  pid_get_value_D(&mainboard.angle.pid) * pid_get_gain_D(&mainboard.angle.pid),
 210                  pid_get_value_out(&mainboard.angle.pid));
 211 }
 212
 213 #define PRINT_POS       (1<<0)
 214 #define PRINT_PID       (1<<1)
 215 #define PRINT_CS        (1<<2)
 216 #define PRINT_SENSORS   (1<<3)
 217 #define PRINT_TIME      (1<<4)
 218 #define PRINT_BLOCKING  (1<<5)
 219
 220 static void cmd_interact_parsed(void * parsed_result, void * data)
 221 {
 222         int c;
 223         int8_t cmd;
 224         uint8_t print = 0;
 225         struct vt100 vt100;
 226
 227         vt100_init(&vt100);
 228
 229         printf_P(PSTR("Display debugs:\r\n"
 230                       "  1:pos\r\n"
 231                       "  2:pid\r\n"
 232                       "  3:cs\r\n"
 233                       "  4:sensors\r\n"
 234                       "  5:time\r\n"
 235                       /* "  6:blocking\r\n" */
 236                       "Commands:\r\n"
 237                       "  arrows:move\r\n"
 238                       "  space:stop\r\n"
 239                       "  q:quit\r\n"));
 240
 241         /* stop motors */
 242         mainboard.flags &= (~DO_CS);
 243         pwm_set_and_save(LEFT_PWM, 0);
 244         pwm_set_and_save(RIGHT_PWM, 0);
 245
 246         while(1) {
 247                 if (print & PRINT_POS) {
 248                         print_pos();
 249                 }
 250
 251                 if (print & PRINT_PID) {
 252                         print_pid();
 253                 }
 254
 255                 if (print & PRINT_CS) {
 256                         print_cs();
 257                 }
 258
 259                 if (print & PRINT_SENSORS) {
 260                         print_sensors();
 261                 }
 262
 263                 if (print & PRINT_TIME) {
 264                         print_time();
 265                 }
 266 /*              if (print & PRINT_BLOCKING) { */
 267 /*                      printf_P(PSTR("%s %s blocking=%d\r\n"),  */
 268 /*                               mainboard.blocking ? "BLOCK1":"      ", */
 269 /*                               rs_is_blocked(&mainboard.rs) ? "BLOCK2":"      ", */
 270 /*                               rs_get_blocking(&mainboard.rs)); */
 271 /*              } */
 272
 273                 c = cmdline_getchar();
 274                 if (c == -1) {
 275                         wait_ms(10);
 276                         continue;
 277                 }
 278                 cmd = vt100_parser(&vt100, c);
 279                 if (cmd == -2) {
 280                         wait_ms(10);
 281                         continue;
 282                 }
 283
 284                 if (cmd == -1) {
 285                         switch(c) {
 286                         case '1': print ^= PRINT_POS; break;
 287                         case '2': print ^= PRINT_PID; break;
 288                         case '3': print ^= PRINT_CS; break;
 289                         case '4': print ^= PRINT_SENSORS; break;
 290                         case '5': print ^= PRINT_TIME; break;
 291                         case '6': print ^= PRINT_BLOCKING; break;
 292
 293                         case 'q':
 294 /*                              if (mainboard.flags & DO_CS) */
 295 /*                                      strat_hardstop(); */
 296                                 pwm_set_and_save(LEFT_PWM, 0);
 297                                 pwm_set_and_save(RIGHT_PWM, 0);
 298                                 return;
 299                         case ' ':
 300                                 pwm_set_and_save(LEFT_PWM, 0);
 301                                 pwm_set_and_save(RIGHT_PWM, 0);
 302                                 break;
 303                         default:
 304                                 break;
 305                         }
 306                 }
 307                 else {
 308                         switch(cmd) {
 309                         case KEY_UP_ARR:
 310                                 pwm_set_and_save(LEFT_PWM, 1200);
 311                                 pwm_set_and_save(RIGHT_PWM, 1200);
 312                                 break;
 313                         case KEY_LEFT_ARR:
 314                                 pwm_set_and_save(LEFT_PWM, -1200);
 315                                 pwm_set_and_save(RIGHT_PWM, 1200);
 316                                 break;
 317                         case KEY_DOWN_ARR:
 318                                 pwm_set_and_save(LEFT_PWM, -1200);
 319                                 pwm_set_and_save(RIGHT_PWM, -1200);
 320                                 break;
 321                         case KEY_RIGHT_ARR:
 322                                 pwm_set_and_save(LEFT_PWM, 1200);
 323                                 pwm_set_and_save(RIGHT_PWM, -1200);
 324                                 break;
 325                         }
 326                 }
 327                 wait_ms(10);
 328         }
 329 }
 330
 331 prog_char str_interact_arg0[] = "interact";
 332 parse_pgm_token_string_t cmd_interact_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_interact_result, arg0, str_interact_arg0);
 333
 334 prog_char help_interact[] = "Interactive mode";
 335 parse_pgm_inst_t cmd_interact = {
 336         .f = cmd_interact_parsed,  /* function to call */
 337         .data = NULL,      /* 2nd arg of func */
 338         .help_str = help_interact,
 339         .tokens = {        /* token list, NULL terminated */
 340                 (prog_void *)&cmd_interact_arg0,
 341                 NULL,
 342         },
 343 };
 344
 345
 346 /**********************************************************/
 347 /* Rs tests */
 348
 349 /* this structure is filled when cmd_rs is parsed successfully */
 350 struct cmd_rs_result {
 351         fixed_string_t arg0;
 352         fixed_string_t arg1;
 353 };
 354
 355 /* function called when cmd_rs is parsed successfully */
 356 static void cmd_rs_parsed(void *parsed_result, void *data)
 357 {
 358         //      struct cmd_rs_result *res = parsed_result;
 359         do {
 360                 printf_P(PSTR("angle cons=% .6ld in=% .6ld out=% .6ld / "),
 361                          cs_get_consign(&mainboard.angle.cs),
 362                          cs_get_filtered_feedback(&mainboard.angle.cs),
 363                          cs_get_out(&mainboard.angle.cs));
 364                 printf_P(PSTR("distance cons=% .6ld in=% .6ld out=% .6ld / "),
 365                          cs_get_consign(&mainboard.distance.cs),
 366                          cs_get_filtered_feedback(&mainboard.distance.cs),
 367                          cs_get_out(&mainboard.distance.cs));
 368                 printf_P(PSTR("l=% .4ld r=% .4ld\r\n"), mainboard.pwm_l,
 369                          mainboard.pwm_r);
 370                 wait_ms(100);
 371         } while(!cmdline_keypressed());
 372 }
 373
 374 prog_char str_rs_arg0[] = "rs";
 375 parse_pgm_token_string_t cmd_rs_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_rs_result, arg0, str_rs_arg0);
 376 prog_char str_rs_arg1[] = "show";
 377 parse_pgm_token_string_t cmd_rs_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_rs_result, arg1, str_rs_arg1);
 378
 379 prog_char help_rs[] = "Show rs (robot system) values";
 380 parse_pgm_inst_t cmd_rs = {
 381         .f = cmd_rs_parsed,  /* function to call */
 382         .data = NULL,      /* 2nd arg of func */
 383         .help_str = help_rs,
 384         .tokens = {        /* token list, NULL terminated */
 385                 (prog_void *)&cmd_rs_arg0,
 386                 (prog_void *)&cmd_rs_arg1,
 387                 NULL,
 388         },
 389 };
 390
 391
 392 /**********************************************************/
 393 /* Fessors tests */
 394
 395 /* this structure is filled when cmd_fessor is parsed successfully */
 396 struct cmd_fessor_result {
 397         fixed_string_t arg0;
 398         fixed_string_t arg1;
 399 };
 400
 401 /* function called when cmd_fessor is parsed successfully */
 402 static void cmd_fessor_parsed(void * parsed_result, void * data)
 403 {
 404         struct cmd_fessor_result * res = parsed_result;
 405
 406         if (!strcmp_P(res->arg1, PSTR("up"))) {
 407                 fessor_up();
 408         }
 409         else if (!strcmp_P(res->arg1, PSTR("down"))) {
 410                 fessor_down();
 411         }
 412         else if (!strcmp_P(res->arg1, PSTR("stop"))) {
 413                 fessor_stop();
 414         }
 415         else if (!strcmp_P(res->arg1, PSTR("auto"))) {
 416                 fessor_auto();
 417         }
 418
 419         printf_P(PSTR("done\r\n"));
 420 }
 421
 422 prog_char str_fessor_arg0[] = "fessor";
 423 parse_pgm_token_string_t cmd_fessor_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_fessor_result, arg0, str_fessor_arg0);
 424 prog_char str_fessor_arg1[] = "auto#up#down#stop";
 425 parse_pgm_token_string_t cmd_fessor_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_fessor_result, arg1, str_fessor_arg1);
 426
 427 prog_char help_fessor[] = "fessor auto mode: fessor auto delay_up delay_down";
 428 parse_pgm_inst_t cmd_fessor = {
 429         .f = cmd_fessor_parsed,  /* function to call */
 430         .data = NULL,      /* 2nd arg of func */
 431         .help_str = help_fessor,
 432         .tokens = {        /* token list, NULL terminated */
 433                 (prog_void *)&cmd_fessor_arg0,
 434                 (prog_void *)&cmd_fessor_arg1,
 435                 NULL,
 436         },
 437 };
 438
 439 /**********************************************************/
 440 /* Fessor_Paramss tests */
 441
 442 /* this structure is filled when cmd_fessor_params is parsed successfully */
 443 struct cmd_fessor_params_result {
 444         fixed_string_t arg0;
 445         fixed_string_t arg1;
 446         int32_t arg2;
 447         int32_t arg3;
 448 };
 449
 450 /* function called when cmd_fessor_params is parsed successfully */
 451 static void cmd_fessor_params_parsed(void * parsed_result, void * data)
 452 {
 453         struct cmd_fessor_params_result * res = parsed_result;
 454
 455
 456         if (!strcmp_P(res->arg1, PSTR("delay"))) {
 457                 fessor_set_delays(res->arg2, res->arg3);
 458         }
 459         else if (!strcmp_P(res->arg1, PSTR("coef"))) {
 460                 fessor_set_coefs(res->arg2, res->arg3);
 461         }
 462         else if (!strcmp_P(res->arg1, PSTR("pos"))) {
 463                 fessor_set_pos(res->arg2, res->arg3);
 464         }
 465
 466         /* else show */
 467         fessor_dump_params();
 468 }
 469
 470 prog_char str_fessor_params_arg0[] = "fessor_params";
 471 parse_pgm_token_string_t cmd_fessor_params_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_fessor_params_result, arg0, str_fessor_params_arg0);
 472 prog_char str_fessor_params_arg1[] = "delay#pos#coef";
 473 parse_pgm_token_string_t cmd_fessor_params_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_fessor_params_result, arg1, str_fessor_params_arg1);
 474 parse_pgm_token_num_t cmd_fessor_params_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_fessor_params_result, arg2, INT32);
 475 parse_pgm_token_num_t cmd_fessor_params_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_fessor_params_result, arg3, INT32);
 476
 477 prog_char help_fessor_params[] = "Set fessor_params values";
 478 parse_pgm_inst_t cmd_fessor_params = {
 479         .f = cmd_fessor_params_parsed,  /* function to call */
 480         .data = NULL,      /* 2nd arg of func */
 481         .help_str = help_fessor_params,
 482         .tokens = {        /* token list, NULL terminated */
 483                 (prog_void *)&cmd_fessor_params_arg0,
 484                 (prog_void *)&cmd_fessor_params_arg1,
 485                 (prog_void *)&cmd_fessor_params_arg2,
 486                 (prog_void *)&cmd_fessor_params_arg3,
 487                 NULL,
 488         },
 489 };
 490
 491 prog_char str_fessor_params_arg1_show[] = "show";
 492 parse_pgm_token_string_t cmd_fessor_params_arg1_show = TOKEN_STRING_INITIALIZER(struct cmd_fessor_params_result, arg1, str_fessor_params_arg1_show);
 493
 494 prog_char help_fessor_params_show[] = "show fessor params";
 495 parse_pgm_inst_t cmd_fessor_params_show = {
 496         .f = cmd_fessor_params_parsed,  /* function to call */
 497         .data = NULL,      /* 2nd arg of func */
 498         .help_str = help_fessor_params_show,
 499         .tokens = {        /* token list, NULL terminated */
 500                 (prog_void *)&cmd_fessor_params_arg0,
 501                 (prog_void *)&cmd_fessor_params_arg1_show,
 502                 NULL,
 503         },
 504 };
 505
 506
 507
 508
 509 /**********************************************************/
 510 /* Elevators tests */
 511
 512 /* this structure is filled when cmd_elevator is parsed successfully */
 513 struct cmd_elevator_result {
 514         fixed_string_t arg0;
 515         fixed_string_t arg1;
 516 };
 517
 518 /* function called when cmd_elevator is parsed successfully */
 519 static void cmd_elevator_parsed(void * parsed_result, void * data)
 520 {
 521         struct cmd_elevator_result * res = parsed_result;
 522
 523         if (!strcmp_P(res->arg1, PSTR("up"))) {
 524                 elevator_up();
 525         }
 526         else if (!strcmp_P(res->arg1, PSTR("down"))) {
 527                 elevator_down();
 528         }
 529         else if (!strcmp_P(res->arg1, PSTR("stop"))) {
 530                 elevator_stop();
 531         }
 532         else if (!strcmp_P(res->arg1, PSTR("auto"))) {
 533                 elevator_auto();
 534         }
 535
 536         printf_P(PSTR("done\r\n"));
 537 }
 538
 539 prog_char str_elevator_arg0[] = "elevator";
 540 parse_pgm_token_string_t cmd_elevator_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_elevator_result, arg0, str_elevator_arg0);
 541 prog_char str_elevator_arg1[] = "auto#up#down#stop";
 542 parse_pgm_token_string_t cmd_elevator_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_elevator_result, arg1, str_elevator_arg1);
 543
 544 prog_char help_elevator[] = "elevator auto mode: elevator auto delay_up delay_down";
 545 parse_pgm_inst_t cmd_elevator = {
 546         .f = cmd_elevator_parsed,  /* function to call */
 547         .data = NULL,      /* 2nd arg of func */
 548         .help_str = help_elevator,
 549         .tokens = {        /* token list, NULL terminated */
 550                 (prog_void *)&cmd_elevator_arg0,
 551                 (prog_void *)&cmd_elevator_arg1,
 552                 NULL,
 553         },
 554 };
 555
 556 /**********************************************************/
 557 /* Elevator_Paramss tests */
 558
 559 /* this structure is filled when cmd_elevator_params is parsed successfully */
 560 struct cmd_elevator_params_result {
 561         fixed_string_t arg0;
 562         fixed_string_t arg1;
 563         int32_t arg2;
 564         int32_t arg3;
 565 };
 566
 567 /* function called when cmd_elevator_params is parsed successfully */
 568 static void cmd_elevator_params_parsed(void * parsed_result, void * data)
 569 {
 570         struct cmd_elevator_params_result * res = parsed_result;
 571
 572
 573         if (!strcmp_P(res->arg1, PSTR("delay"))) {
 574                 elevator_set_delays(res->arg2, res->arg3);
 575         }
 576         else if (!strcmp_P(res->arg1, PSTR("coef"))) {
 577                 elevator_set_coefs(res->arg2, res->arg3);
 578         }
 579         else if (!strcmp_P(res->arg1, PSTR("pos"))) {
 580                 elevator_set_pos(res->arg2, res->arg3);
 581         }
 582
 583         /* else show */
 584         elevator_dump_params();
 585 }
 586
 587 prog_char str_elevator_params_arg0[] = "elevator_params";
 588 parse_pgm_token_string_t cmd_elevator_params_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_elevator_params_result, arg0, str_elevator_params_arg0);
 589 prog_char str_elevator_params_arg1[] = "delay#pos#coef";
 590 parse_pgm_token_string_t cmd_elevator_params_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_elevator_params_result, arg1, str_elevator_params_arg1);
 591 parse_pgm_token_num_t cmd_elevator_params_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_elevator_params_result, arg2, INT32);
 592 parse_pgm_token_num_t cmd_elevator_params_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_elevator_params_result, arg3, INT32);
 593
 594 prog_char help_elevator_params[] = "Set elevator_params values";
 595 parse_pgm_inst_t cmd_elevator_params = {
 596         .f = cmd_elevator_params_parsed,  /* function to call */
 597         .data = NULL,      /* 2nd arg of func */
 598         .help_str = help_elevator_params,
 599         .tokens = {        /* token list, NULL terminated */
 600                 (prog_void *)&cmd_elevator_params_arg0,
 601                 (prog_void *)&cmd_elevator_params_arg1,
 602                 (prog_void *)&cmd_elevator_params_arg2,
 603                 (prog_void *)&cmd_elevator_params_arg3,
 604                 NULL,
 605         },
 606 };
 607
 608 prog_char str_elevator_params_arg1_show[] = "show";
 609 parse_pgm_token_string_t cmd_elevator_params_arg1_show = TOKEN_STRING_INITIALIZER(struct cmd_elevator_params_result, arg1, str_elevator_params_arg1_show);
 610
 611 prog_char help_elevator_params_show[] = "show elevator params";
 612 parse_pgm_inst_t cmd_elevator_params_show = {
 613         .f = cmd_elevator_params_parsed,  /* function to call */
 614         .data = NULL,      /* 2nd arg of func */
 615         .help_str = help_elevator_params_show,
 616         .tokens = {        /* token list, NULL terminated */
 617                 (prog_void *)&cmd_elevator_params_arg0,
 618                 (prog_void *)&cmd_elevator_params_arg1_show,
 619                 NULL,
 620         },
 621 };
 622
 623
 624
 625
 626
 627
 628 /**********************************************************/
 629 /* Test */
 630
 631 /* this structure is filled when cmd_test is parsed successfully */
 632 struct cmd_test_result {
 633         fixed_string_t arg0;
 634 };
 635
 636 /* function called when cmd_test is parsed successfully */
 637 static void cmd_test_parsed(void *parsed_result, void *data)
 638 {
 639 }
 640
 641 prog_char str_test_arg0[] = "test";
 642 parse_pgm_token_string_t cmd_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_test_result, arg0, str_test_arg0);
 643
 644 prog_char help_test[] = "Test function";
 645 parse_pgm_inst_t cmd_test = {
 646         .f = cmd_test_parsed,  /* function to call */
 647         .data = NULL,      /* 2nd arg of func */
 648         .help_str = help_test,
 649         .tokens = {        /* token list, NULL terminated */
 650                 (prog_void *)&cmd_test_arg0,
 651                 NULL,
 652         },
 653 };