2 * Copyright Droids Corporation (2009)
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.
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.
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
18 * Revision : $Id: commands_mainboard.c,v 1.9 2009-11-08 17:24:33 zer0 Exp $
20 * Olivier MATZ <zer0@droids-corp.org>
26 #include <aversive/pgmspace.h>
27 #include <aversive/wait.h>
28 #include <aversive/error.h>
29 #include <aversive/eeprom.h>
34 #include <clock_time.h>
40 #include <control_system_manager.h>
41 #include <trajectory_manager.h>
42 #include <vect_base.h>
45 #include <obstacle_avoidance.h>
46 #include <blocking_detection_manager.h>
47 #include <robot_system.h>
48 #include <position_manager.h>
52 #include <parse_string.h>
53 #include <parse_num.h>
55 #include "../common/i2c_commands.h"
56 #include "../common/eeprom_mapping.h"
62 #include "strat_utils.h"
63 #include "strat_base.h"
64 #include "i2c_protocol.h"
67 struct cmd_event_result {
74 /* function called when cmd_event is parsed successfully */
75 static void cmd_event_parsed(void *parsed_result, void *data)
79 struct cmd_event_result * res = parsed_result;
81 if (!strcmp_P(res->arg1, PSTR("all"))) {
82 bit = DO_ENCODERS | DO_CS | DO_RS | DO_POS |
83 DO_BD | DO_TIMER | DO_POWER;
84 if (!strcmp_P(res->arg2, PSTR("on")))
85 mainboard.flags |= bit;
86 else if (!strcmp_P(res->arg2, PSTR("off")))
87 mainboard.flags &= bit;
89 printf_P(PSTR("encoders is %s\r\n"),
90 (DO_ENCODERS & mainboard.flags) ? "on":"off");
91 printf_P(PSTR("cs is %s\r\n"),
92 (DO_CS & mainboard.flags) ? "on":"off");
93 printf_P(PSTR("rs is %s\r\n"),
94 (DO_RS & mainboard.flags) ? "on":"off");
95 printf_P(PSTR("pos is %s\r\n"),
96 (DO_POS & mainboard.flags) ? "on":"off");
97 printf_P(PSTR("bd is %s\r\n"),
98 (DO_BD & mainboard.flags) ? "on":"off");
99 printf_P(PSTR("timer is %s\r\n"),
100 (DO_TIMER & mainboard.flags) ? "on":"off");
101 printf_P(PSTR("power is %s\r\n"),
102 (DO_POWER & mainboard.flags) ? "on":"off");
107 if (!strcmp_P(res->arg1, PSTR("encoders")))
109 else if (!strcmp_P(res->arg1, PSTR("cs"))) {
113 else if (!strcmp_P(res->arg1, PSTR("rs")))
115 else if (!strcmp_P(res->arg1, PSTR("pos")))
117 else if (!strcmp_P(res->arg1, PSTR("bd")))
119 else if (!strcmp_P(res->arg1, PSTR("timer"))) {
123 else if (!strcmp_P(res->arg1, PSTR("power")))
126 if (!strcmp_P(res->arg2, PSTR("on")))
127 mainboard.flags |= bit;
128 else if (!strcmp_P(res->arg2, PSTR("off"))) {
129 if (!strcmp_P(res->arg1, PSTR("cs"))) {
130 pwm_ng_set(LEFT_PWM, 0);
131 pwm_ng_set(RIGHT_PWM, 0);
133 mainboard.flags &= (~bit);
135 printf_P(PSTR("%s is %s\r\n"), res->arg1,
136 (bit & mainboard.flags) ? "on":"off");
139 prog_char str_event_arg0[] = "event";
140 parse_pgm_token_string_t cmd_event_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg0, str_event_arg0);
141 prog_char str_event_arg1[] = "all#encoders#cs#rs#pos#bd#timer#power";
142 parse_pgm_token_string_t cmd_event_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg1, str_event_arg1);
143 prog_char str_event_arg2[] = "on#off#show";
144 parse_pgm_token_string_t cmd_event_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg2, str_event_arg2);
146 prog_char help_event[] = "Enable/disable events";
147 parse_pgm_inst_t cmd_event = {
148 .f = cmd_event_parsed, /* function to call */
149 .data = NULL, /* 2nd arg of func */
150 .help_str = help_event,
151 .tokens = { /* token list, NULL terminated */
152 (prog_void *)&cmd_event_arg0,
153 (prog_void *)&cmd_event_arg1,
154 (prog_void *)&cmd_event_arg2,
160 /**********************************************************/
163 /* this structure is filled when cmd_spi_test is parsed successfully */
164 struct cmd_spi_test_result {
168 /* function called when cmd_spi_test is parsed successfully */
169 static void cmd_spi_test_parsed(void * parsed_result, void * data)
171 uint16_t i = 0, ret = 0, ret2 = 0;
173 if (mainboard.flags & DO_ENCODERS) {
174 printf_P(PSTR("Disable encoder event first\r\n"));
180 ret = spi_send_and_receive_byte(i);
181 ret2 = spi_send_and_receive_byte(i);
182 spi_slave_deselect(0);
184 if ((i & 0x7ff) == 0)
185 printf_P(PSTR("Sent %.4x twice, received %x %x\r\n"),
189 } while(!cmdline_keypressed());
192 prog_char str_spi_test_arg0[] = "spi_test";
193 parse_pgm_token_string_t cmd_spi_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_spi_test_result, arg0, str_spi_test_arg0);
195 prog_char help_spi_test[] = "Test the SPI";
196 parse_pgm_inst_t cmd_spi_test = {
197 .f = cmd_spi_test_parsed, /* function to call */
198 .data = NULL, /* 2nd arg of func */
199 .help_str = help_spi_test,
200 .tokens = { /* token list, NULL terminated */
201 (prog_void *)&cmd_spi_test_arg0,
208 /**********************************************************/
211 /* this structure is filled when cmd_opponent is parsed successfully */
212 struct cmd_opponent_result {
219 /* function called when cmd_opponent is parsed successfully */
220 static void cmd_opponent_parsed(void *parsed_result, void *data)
224 if (get_opponent_xyda(&x, &y, &d, &a))
225 printf_P(PSTR("No opponent\r\n"));
227 printf_P(PSTR("x=%d y=%d, d=%d a=%d\r\n"), x, y, d, a);
230 prog_char str_opponent_arg0[] = "opponent";
231 parse_pgm_token_string_t cmd_opponent_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_opponent_result, arg0, str_opponent_arg0);
232 prog_char str_opponent_arg1[] = "show";
233 parse_pgm_token_string_t cmd_opponent_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_opponent_result, arg1, str_opponent_arg1);
235 prog_char help_opponent[] = "Show (x,y) opponent";
236 parse_pgm_inst_t cmd_opponent = {
237 .f = cmd_opponent_parsed, /* function to call */
238 .data = NULL, /* 2nd arg of func */
239 .help_str = help_opponent,
240 .tokens = { /* token list, NULL terminated */
241 (prog_void *)&cmd_opponent_arg0,
242 (prog_void *)&cmd_opponent_arg1,
248 prog_char str_opponent_arg1_set[] = "set";
249 parse_pgm_token_string_t cmd_opponent_arg1_set = TOKEN_STRING_INITIALIZER(struct cmd_opponent_result, arg1, str_opponent_arg1_set);
250 parse_pgm_token_num_t cmd_opponent_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_opponent_result, arg2, INT32);
251 parse_pgm_token_num_t cmd_opponent_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_opponent_result, arg3, INT32);
253 prog_char help_opponent_set[] = "Set (x,y) opponent";
254 parse_pgm_inst_t cmd_opponent_set = {
255 .f = cmd_opponent_parsed, /* function to call */
256 .data = NULL, /* 2nd arg of func */
257 .help_str = help_opponent_set,
258 .tokens = { /* token list, NULL terminated */
259 (prog_void *)&cmd_opponent_arg0,
260 (prog_void *)&cmd_opponent_arg1_set,
261 (prog_void *)&cmd_opponent_arg2,
262 (prog_void *)&cmd_opponent_arg3,
268 /**********************************************************/
271 /* this structure is filled when cmd_start is parsed successfully */
272 struct cmd_start_result {
274 fixed_string_t color;
275 fixed_string_t debug;
278 /* function called when cmd_start is parsed successfully */
279 static void cmd_start_parsed(void *parsed_result, void *data)
281 struct cmd_start_result *res = parsed_result;
282 uint8_t old_level = gen.log_level;
284 gen.logs[NB_LOGS] = E_USER_STRAT;
285 if (!strcmp_P(res->debug, PSTR("debug"))) {
286 strat_infos.dump_enabled = 1;
290 strat_infos.dump_enabled = 0;
294 if (!strcmp_P(res->color, PSTR("yellow"))) {
295 mainboard.our_color = I2C_COLOR_YELLOW;
296 i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_YELLOW);
297 i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_YELLOW);
299 else if (!strcmp_P(res->color, PSTR("blue"))) {
300 mainboard.our_color = I2C_COLOR_BLUE;
301 i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_BLUE);
302 i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_BLUE);
307 gen.logs[NB_LOGS] = 0;
308 gen.log_level = old_level;
311 prog_char str_start_arg0[] = "start";
312 parse_pgm_token_string_t cmd_start_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_start_result, arg0, str_start_arg0);
313 prog_char str_start_color[] = "blue#yellow";
314 parse_pgm_token_string_t cmd_start_color = TOKEN_STRING_INITIALIZER(struct cmd_start_result, color, str_start_color);
315 prog_char str_start_debug[] = "debug#match";
316 parse_pgm_token_string_t cmd_start_debug = TOKEN_STRING_INITIALIZER(struct cmd_start_result, debug, str_start_debug);
318 prog_char help_start[] = "Start the robot";
319 parse_pgm_inst_t cmd_start = {
320 .f = cmd_start_parsed, /* function to call */
321 .data = NULL, /* 2nd arg of func */
322 .help_str = help_start,
323 .tokens = { /* token list, NULL terminated */
324 (prog_void *)&cmd_start_arg0,
325 (prog_void *)&cmd_start_color,
326 (prog_void *)&cmd_start_debug,
333 /**********************************************************/
336 /* this structure is filled when cmd_interact is parsed successfully */
337 struct cmd_interact_result {
341 static void print_cs(void)
343 printf_P(PSTR("cons_d=% .8ld cons_a=% .8ld fil_d=% .8ld fil_a=% .8ld "
344 "err_d=% .8ld err_a=% .8ld out_d=% .8ld out_a=% .8ld\r\n"),
345 cs_get_consign(&mainboard.distance.cs),
346 cs_get_consign(&mainboard.angle.cs),
347 cs_get_filtered_consign(&mainboard.distance.cs),
348 cs_get_filtered_consign(&mainboard.angle.cs),
349 cs_get_error(&mainboard.distance.cs),
350 cs_get_error(&mainboard.angle.cs),
351 cs_get_out(&mainboard.distance.cs),
352 cs_get_out(&mainboard.angle.cs));
355 static void print_pos(void)
357 printf_P(PSTR("x=% .8d y=% .8d a=% .8d\r\n"),
358 position_get_x_s16(&mainboard.pos),
359 position_get_y_s16(&mainboard.pos),
360 position_get_a_deg_s16(&mainboard.pos));
363 static void print_time(void)
365 printf_P(PSTR("time %d\r\n"),time_get_s());
369 static void print_sensors(void)
372 if (sensor_start_switch())
373 printf_P(PSTR("Start switch | "));
375 printf_P(PSTR(" | "));
377 if (IR_DISP_SENSOR())
378 printf_P(PSTR("IR disp | "));
380 printf_P(PSTR(" | "));
382 printf_P(PSTR("\r\n"));
386 static void print_pid(void)
388 printf_P(PSTR("P=% .8ld I=% .8ld D=% .8ld out=% .8ld | "
389 "P=% .8ld I=% .8ld D=% .8ld out=% .8ld\r\n"),
390 pid_get_value_in(&mainboard.distance.pid) * pid_get_gain_P(&mainboard.distance.pid),
391 pid_get_value_I(&mainboard.distance.pid) * pid_get_gain_I(&mainboard.distance.pid),
392 pid_get_value_D(&mainboard.distance.pid) * pid_get_gain_D(&mainboard.distance.pid),
393 pid_get_value_out(&mainboard.distance.pid),
394 pid_get_value_in(&mainboard.angle.pid) * pid_get_gain_P(&mainboard.angle.pid),
395 pid_get_value_I(&mainboard.angle.pid) * pid_get_gain_I(&mainboard.angle.pid),
396 pid_get_value_D(&mainboard.angle.pid) * pid_get_gain_D(&mainboard.angle.pid),
397 pid_get_value_out(&mainboard.angle.pid));
400 #define PRINT_POS (1<<0)
401 #define PRINT_PID (1<<1)
402 #define PRINT_CS (1<<2)
403 #define PRINT_SENSORS (1<<3)
404 #define PRINT_TIME (1<<4)
405 #define PRINT_BLOCKING (1<<5)
407 static void cmd_interact_parsed(void * parsed_result, void * data)
416 printf_P(PSTR("Display debugs:\r\n"
422 /* " 6:blocking\r\n" */
429 mainboard.flags &= (~DO_CS);
430 pwm_set_and_save(LEFT_PWM, 0);
431 pwm_set_and_save(RIGHT_PWM, 0);
434 if (print & PRINT_POS) {
438 if (print & PRINT_PID) {
442 if (print & PRINT_CS) {
446 if (print & PRINT_SENSORS) {
450 if (print & PRINT_TIME) {
453 /* if (print & PRINT_BLOCKING) { */
454 /* printf_P(PSTR("%s %s blocking=%d\r\n"), */
455 /* mainboard.blocking ? "BLOCK1":" ", */
456 /* rs_is_blocked(&mainboard.rs) ? "BLOCK2":" ", */
457 /* rs_get_blocking(&mainboard.rs)); */
460 c = cmdline_getchar();
465 cmd = vt100_parser(&vt100, c);
473 case '1': print ^= PRINT_POS; break;
474 case '2': print ^= PRINT_PID; break;
475 case '3': print ^= PRINT_CS; break;
476 case '4': print ^= PRINT_SENSORS; break;
477 case '5': print ^= PRINT_TIME; break;
478 case '6': print ^= PRINT_BLOCKING; break;
481 if (mainboard.flags & DO_CS)
483 pwm_set_and_save(LEFT_PWM, 0);
484 pwm_set_and_save(RIGHT_PWM, 0);
487 pwm_set_and_save(LEFT_PWM, 0);
488 pwm_set_and_save(RIGHT_PWM, 0);
497 pwm_set_and_save(LEFT_PWM, 1200);
498 pwm_set_and_save(RIGHT_PWM, 1200);
501 pwm_set_and_save(LEFT_PWM, -1200);
502 pwm_set_and_save(RIGHT_PWM, 1200);
505 pwm_set_and_save(LEFT_PWM, -1200);
506 pwm_set_and_save(RIGHT_PWM, -1200);
509 pwm_set_and_save(LEFT_PWM, 1200);
510 pwm_set_and_save(RIGHT_PWM, -1200);
518 prog_char str_interact_arg0[] = "interact";
519 parse_pgm_token_string_t cmd_interact_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_interact_result, arg0, str_interact_arg0);
521 prog_char help_interact[] = "Interactive mode";
522 parse_pgm_inst_t cmd_interact = {
523 .f = cmd_interact_parsed, /* function to call */
524 .data = NULL, /* 2nd arg of func */
525 .help_str = help_interact,
526 .tokens = { /* token list, NULL terminated */
527 (prog_void *)&cmd_interact_arg0,
533 /**********************************************************/
536 /* this structure is filled when cmd_color is parsed successfully */
537 struct cmd_color_result {
539 fixed_string_t color;
542 /* function called when cmd_color is parsed successfully */
543 static void cmd_color_parsed(void *parsed_result, void *data)
545 struct cmd_color_result *res = (struct cmd_color_result *) parsed_result;
546 if (!strcmp_P(res->color, PSTR("yellow"))) {
547 mainboard.our_color = I2C_COLOR_YELLOW;
548 i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_YELLOW);
549 i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_YELLOW);
551 else if (!strcmp_P(res->color, PSTR("blue"))) {
552 mainboard.our_color = I2C_COLOR_BLUE;
553 i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_BLUE);
554 i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_BLUE);
556 printf_P(PSTR("Done\r\n"));
559 prog_char str_color_arg0[] = "color";
560 parse_pgm_token_string_t cmd_color_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_color_result, arg0, str_color_arg0);
561 prog_char str_color_color[] = "blue#yellow";
562 parse_pgm_token_string_t cmd_color_color = TOKEN_STRING_INITIALIZER(struct cmd_color_result, color, str_color_color);
564 prog_char help_color[] = "Set our color";
565 parse_pgm_inst_t cmd_color = {
566 .f = cmd_color_parsed, /* function to call */
567 .data = NULL, /* 2nd arg of func */
568 .help_str = help_color,
569 .tokens = { /* token list, NULL terminated */
570 (prog_void *)&cmd_color_arg0,
571 (prog_void *)&cmd_color_color,
577 /**********************************************************/
580 /* this structure is filled when cmd_rs is parsed successfully */
581 struct cmd_rs_result {
586 /* function called when cmd_rs is parsed successfully */
587 static void cmd_rs_parsed(void *parsed_result, void *data)
589 // struct cmd_rs_result *res = parsed_result;
591 printf_P(PSTR("angle cons=% .6ld in=% .6ld out=% .6ld / "),
592 cs_get_consign(&mainboard.angle.cs),
593 cs_get_filtered_feedback(&mainboard.angle.cs),
594 cs_get_out(&mainboard.angle.cs));
595 printf_P(PSTR("distance cons=% .6ld in=% .6ld out=% .6ld / "),
596 cs_get_consign(&mainboard.distance.cs),
597 cs_get_filtered_feedback(&mainboard.distance.cs),
598 cs_get_out(&mainboard.distance.cs));
599 printf_P(PSTR("l=% .4ld r=% .4ld\r\n"), mainboard.pwm_l,
602 } while(!cmdline_keypressed());
605 prog_char str_rs_arg0[] = "rs";
606 parse_pgm_token_string_t cmd_rs_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_rs_result, arg0, str_rs_arg0);
607 prog_char str_rs_arg1[] = "show";
608 parse_pgm_token_string_t cmd_rs_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_rs_result, arg1, str_rs_arg1);
610 prog_char help_rs[] = "Show rs (robot system) values";
611 parse_pgm_inst_t cmd_rs = {
612 .f = cmd_rs_parsed, /* function to call */
613 .data = NULL, /* 2nd arg of func */
615 .tokens = { /* token list, NULL terminated */
616 (prog_void *)&cmd_rs_arg0,
617 (prog_void *)&cmd_rs_arg1,
622 /**********************************************************/
625 /* this structure is filled when cmd_i2cdebug is parsed successfully */
626 struct cmd_i2cdebug_result {
630 /* function called when cmd_i2cdebug is parsed successfully */
631 static void cmd_i2cdebug_parsed(void * parsed_result, void * data)
634 i2c_protocol_debug();
637 prog_char str_i2cdebug_arg0[] = "i2cdebug";
638 parse_pgm_token_string_t cmd_i2cdebug_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_i2cdebug_result, arg0, str_i2cdebug_arg0);
640 prog_char help_i2cdebug[] = "I2c debug infos";
641 parse_pgm_inst_t cmd_i2cdebug = {
642 .f = cmd_i2cdebug_parsed, /* function to call */
643 .data = NULL, /* 2nd arg of func */
644 .help_str = help_i2cdebug,
645 .tokens = { /* token list, NULL terminated */
646 (prog_void *)&cmd_i2cdebug_arg0,
651 /**********************************************************/
654 /* this structure is filled when cmd_cobboard_show is parsed successfully */
655 struct cmd_cobboard_show_result {
660 /* function called when cmd_cobboard_show is parsed successfully */
661 static void cmd_cobboard_show_parsed(void * parsed_result, void * data)
663 printf_P(PSTR("mode = %x\r\n"), cobboard.mode);
664 printf_P(PSTR("status = %x\r\n"), cobboard.status);
665 printf_P(PSTR("cob_count = %x\r\n"), cobboard.cob_count);
666 printf_P(PSTR("left_cobroller_speed = %d\r\n"), cobboard.left_cobroller_speed);
667 printf_P(PSTR("right_cobroller_speed = %d\r\n"), cobboard.right_cobroller_speed);
670 prog_char str_cobboard_show_arg0[] = "cobboard";
671 parse_pgm_token_string_t cmd_cobboard_show_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_show_result, arg0, str_cobboard_show_arg0);
672 prog_char str_cobboard_show_arg1[] = "show";
673 parse_pgm_token_string_t cmd_cobboard_show_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_show_result, arg1, str_cobboard_show_arg1);
675 prog_char help_cobboard_show[] = "show cobboard status";
676 parse_pgm_inst_t cmd_cobboard_show = {
677 .f = cmd_cobboard_show_parsed, /* function to call */
678 .data = NULL, /* 2nd arg of func */
679 .help_str = help_cobboard_show,
680 .tokens = { /* token list, NULL terminated */
681 (prog_void *)&cmd_cobboard_show_arg0,
682 (prog_void *)&cmd_cobboard_show_arg1,
687 /**********************************************************/
688 /* Cobboard_Setmode1 */
690 /* this structure is filled when cmd_cobboard_setmode1 is parsed successfully */
691 struct cmd_cobboard_setmode1_result {
696 /* function called when cmd_cobboard_setmode1 is parsed successfully */
697 static void cmd_cobboard_setmode1_parsed(void *parsed_result, void *data)
699 struct cmd_cobboard_setmode1_result *res = parsed_result;
701 if (!strcmp_P(res->arg1, PSTR("init")))
702 i2c_cobboard_mode_init();
703 else if (!strcmp_P(res->arg1, PSTR("eject")))
704 i2c_cobboard_mode_eject();
707 prog_char str_cobboard_setmode1_arg0[] = "cobboard";
708 parse_pgm_token_string_t cmd_cobboard_setmode1_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode1_result, arg0, str_cobboard_setmode1_arg0);
709 prog_char str_cobboard_setmode1_arg1[] = "init#eject";
710 parse_pgm_token_string_t cmd_cobboard_setmode1_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode1_result, arg1, str_cobboard_setmode1_arg1);
712 prog_char help_cobboard_setmode1[] = "set cobboard mode (mode)";
713 parse_pgm_inst_t cmd_cobboard_setmode1 = {
714 .f = cmd_cobboard_setmode1_parsed, /* function to call */
715 .data = NULL, /* 2nd arg of func */
716 .help_str = help_cobboard_setmode1,
717 .tokens = { /* token list, NULL terminated */
718 (prog_void *)&cmd_cobboard_setmode1_arg0,
719 (prog_void *)&cmd_cobboard_setmode1_arg1,
724 /**********************************************************/
725 /* Cobboard_Setmode2 */
727 /* this structure is filled when cmd_cobboard_setmode2 is parsed successfully */
728 struct cmd_cobboard_setmode2_result {
734 /* function called when cmd_cobboard_setmode2 is parsed successfully */
735 static void cmd_cobboard_setmode2_parsed(void * parsed_result, void * data)
737 struct cmd_cobboard_setmode2_result *res = parsed_result;
738 uint8_t side = I2C_LEFT_SIDE;
740 if (!strcmp_P(res->arg2, PSTR("left")))
741 side = I2C_LEFT_SIDE;
742 else if (!strcmp_P(res->arg2, PSTR("right")))
743 side = I2C_RIGHT_SIDE;
745 if (!strcmp_P(res->arg1, PSTR("deploy")))
746 i2c_cobboard_mode_deploy(side);
747 else if (!strcmp_P(res->arg1, PSTR("harvest")))
748 i2c_cobboard_mode_harvest(side);
749 else if (!strcmp_P(res->arg1, PSTR("pack")))
750 i2c_cobboard_mode_pack(side);
753 prog_char str_cobboard_setmode2_arg0[] = "cobboard";
754 parse_pgm_token_string_t cmd_cobboard_setmode2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode2_result, arg0, str_cobboard_setmode2_arg0);
755 prog_char str_cobboard_setmode2_arg1[] = "harvest#deploy#pack";
756 parse_pgm_token_string_t cmd_cobboard_setmode2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode2_result, arg1, str_cobboard_setmode2_arg1);
757 prog_char str_cobboard_setmode2_arg2[] = "left#right";
758 parse_pgm_token_string_t cmd_cobboard_setmode2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode2_result, arg2, str_cobboard_setmode2_arg2);
760 prog_char help_cobboard_setmode2[] = "set cobboard mode (mode, side)";
761 parse_pgm_inst_t cmd_cobboard_setmode2 = {
762 .f = cmd_cobboard_setmode2_parsed, /* function to call */
763 .data = NULL, /* 2nd arg of func */
764 .help_str = help_cobboard_setmode2,
765 .tokens = { /* token list, NULL terminated */
766 (prog_void *)&cmd_cobboard_setmode2_arg0,
767 (prog_void *)&cmd_cobboard_setmode2_arg1,
768 (prog_void *)&cmd_cobboard_setmode2_arg2,
773 /**********************************************************/
774 /* Cobboard_Setmode3 */
776 /* this structure is filled when cmd_cobboard_setmode3 is parsed successfully */
777 struct cmd_cobboard_setmode3_result {
783 /* function called when cmd_cobboard_setmode3 is parsed successfully */
784 static void cmd_cobboard_setmode3_parsed(void *parsed_result, void *data)
786 struct cmd_cobboard_setmode3_result *res = parsed_result;
787 if (!strcmp_P(res->arg1, PSTR("xxx")))
791 prog_char str_cobboard_setmode3_arg0[] = "cobboard";
792 parse_pgm_token_string_t cmd_cobboard_setmode3_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode3_result, arg0, str_cobboard_setmode3_arg0);
793 prog_char str_cobboard_setmode3_arg1[] = "xxx";
794 parse_pgm_token_string_t cmd_cobboard_setmode3_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode3_result, arg1, str_cobboard_setmode3_arg1);
795 parse_pgm_token_num_t cmd_cobboard_setmode3_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_cobboard_setmode3_result, level, UINT8);
797 prog_char help_cobboard_setmode3[] = "set cobboard mode (mode, level)";
798 parse_pgm_inst_t cmd_cobboard_setmode3 = {
799 .f = cmd_cobboard_setmode3_parsed, /* function to call */
800 .data = NULL, /* 2nd arg of func */
801 .help_str = help_cobboard_setmode3,
802 .tokens = { /* token list, NULL terminated */
803 (prog_void *)&cmd_cobboard_setmode3_arg0,
804 (prog_void *)&cmd_cobboard_setmode3_arg1,
805 (prog_void *)&cmd_cobboard_setmode3_arg2,
810 /**********************************************************/
813 /* this structure is filled when cmd_ballboard_show is parsed successfully */
814 struct cmd_ballboard_show_result {
819 /* function called when cmd_ballboard_show is parsed successfully */
820 static void cmd_ballboard_show_parsed(void * parsed_result, void * data)
822 printf_P(PSTR("mode = %x\r\n"), ballboard.mode);
823 printf_P(PSTR("status = %x\r\n"), ballboard.status);
824 printf_P(PSTR("ball_count = %d\r\n"), ballboard.ball_count);
827 prog_char str_ballboard_show_arg0[] = "ballboard";
828 parse_pgm_token_string_t cmd_ballboard_show_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_show_result, arg0, str_ballboard_show_arg0);
829 prog_char str_ballboard_show_arg1[] = "show";
830 parse_pgm_token_string_t cmd_ballboard_show_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_show_result, arg1, str_ballboard_show_arg1);
832 prog_char help_ballboard_show[] = "show ballboard status";
833 parse_pgm_inst_t cmd_ballboard_show = {
834 .f = cmd_ballboard_show_parsed, /* function to call */
835 .data = NULL, /* 2nd arg of func */
836 .help_str = help_ballboard_show,
837 .tokens = { /* token list, NULL terminated */
838 (prog_void *)&cmd_ballboard_show_arg0,
839 (prog_void *)&cmd_ballboard_show_arg1,
844 /**********************************************************/
845 /* Ballboard_Setmode1 */
847 /* this structure is filled when cmd_ballboard_setmode1 is parsed successfully */
848 struct cmd_ballboard_setmode1_result {
853 /* function called when cmd_ballboard_setmode1 is parsed successfully */
854 static void cmd_ballboard_setmode1_parsed(void *parsed_result, void *data)
856 struct cmd_ballboard_setmode1_result *res = parsed_result;
858 if (!strcmp_P(res->arg1, PSTR("init")))
859 i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_INIT);
860 else if (!strcmp_P(res->arg1, PSTR("off")))
861 i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_OFF);
862 else if (!strcmp_P(res->arg1, PSTR("eject")))
863 i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_EJECT);
864 else if (!strcmp_P(res->arg1, PSTR("harvest")))
865 i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_HARVEST);
870 prog_char str_ballboard_setmode1_arg0[] = "ballboard";
871 parse_pgm_token_string_t cmd_ballboard_setmode1_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode1_result, arg0, str_ballboard_setmode1_arg0);
872 prog_char str_ballboard_setmode1_arg1[] = "init#eject#harvest#off";
873 parse_pgm_token_string_t cmd_ballboard_setmode1_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode1_result, arg1, str_ballboard_setmode1_arg1);
875 prog_char help_ballboard_setmode1[] = "set ballboard mode (mode)";
876 parse_pgm_inst_t cmd_ballboard_setmode1 = {
877 .f = cmd_ballboard_setmode1_parsed, /* function to call */
878 .data = NULL, /* 2nd arg of func */
879 .help_str = help_ballboard_setmode1,
880 .tokens = { /* token list, NULL terminated */
881 (prog_void *)&cmd_ballboard_setmode1_arg0,
882 (prog_void *)&cmd_ballboard_setmode1_arg1,
887 /**********************************************************/
888 /* Ballboard_Setmode2 */
890 /* this structure is filled when cmd_ballboard_setmode2 is parsed successfully */
891 struct cmd_ballboard_setmode2_result {
897 /* function called when cmd_ballboard_setmode2 is parsed successfully */
898 static void cmd_ballboard_setmode2_parsed(void * parsed_result, void * data)
900 struct cmd_ballboard_setmode2_result *res = parsed_result;
901 uint8_t mode = I2C_BALLBOARD_MODE_INIT;
903 if (!strcmp_P(res->arg2, PSTR("left"))) {
904 if (!strcmp_P(res->arg1, PSTR("prepare")))
905 mode = I2C_BALLBOARD_MODE_PREP_L_FORK;
906 else if (!strcmp_P(res->arg1, PSTR("take")))
907 mode = I2C_BALLBOARD_MODE_TAKE_L_FORK;
910 if (!strcmp_P(res->arg1, PSTR("prepare")))
911 mode = I2C_BALLBOARD_MODE_PREP_R_FORK;
912 else if (!strcmp_P(res->arg1, PSTR("take")))
913 mode = I2C_BALLBOARD_MODE_TAKE_R_FORK;
915 i2c_ballboard_set_mode(mode);
918 prog_char str_ballboard_setmode2_arg0[] = "ballboard";
919 parse_pgm_token_string_t cmd_ballboard_setmode2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode2_result, arg0, str_ballboard_setmode2_arg0);
920 prog_char str_ballboard_setmode2_arg1[] = "prepare#take";
921 parse_pgm_token_string_t cmd_ballboard_setmode2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode2_result, arg1, str_ballboard_setmode2_arg1);
922 prog_char str_ballboard_setmode2_arg2[] = "left#right";
923 parse_pgm_token_string_t cmd_ballboard_setmode2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode2_result, arg2, str_ballboard_setmode2_arg2);
925 prog_char help_ballboard_setmode2[] = "set ballboard mode (mode, side)";
926 parse_pgm_inst_t cmd_ballboard_setmode2 = {
927 .f = cmd_ballboard_setmode2_parsed, /* function to call */
928 .data = NULL, /* 2nd arg of func */
929 .help_str = help_ballboard_setmode2,
930 .tokens = { /* token list, NULL terminated */
931 (prog_void *)&cmd_ballboard_setmode2_arg0,
932 (prog_void *)&cmd_ballboard_setmode2_arg1,
933 (prog_void *)&cmd_ballboard_setmode2_arg2,
938 /**********************************************************/
939 /* Ballboard_Setmode3 */
941 /* this structure is filled when cmd_ballboard_setmode3 is parsed successfully */
942 struct cmd_ballboard_setmode3_result {
948 /* function called when cmd_ballboard_setmode3 is parsed successfully */
949 static void cmd_ballboard_setmode3_parsed(void *parsed_result, void *data)
951 struct cmd_ballboard_setmode3_result *res = parsed_result;
952 if (!strcmp_P(res->arg1, PSTR("xxx")))
956 prog_char str_ballboard_setmode3_arg0[] = "ballboard";
957 parse_pgm_token_string_t cmd_ballboard_setmode3_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode3_result, arg0, str_ballboard_setmode3_arg0);
958 prog_char str_ballboard_setmode3_arg1[] = "xxx";
959 parse_pgm_token_string_t cmd_ballboard_setmode3_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode3_result, arg1, str_ballboard_setmode3_arg1);
960 parse_pgm_token_num_t cmd_ballboard_setmode3_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_ballboard_setmode3_result, level, UINT8);
962 prog_char help_ballboard_setmode3[] = "set ballboard mode (mode, level)";
963 parse_pgm_inst_t cmd_ballboard_setmode3 = {
964 .f = cmd_ballboard_setmode3_parsed, /* function to call */
965 .data = NULL, /* 2nd arg of func */
966 .help_str = help_ballboard_setmode3,
967 .tokens = { /* token list, NULL terminated */
968 (prog_void *)&cmd_ballboard_setmode3_arg0,
969 (prog_void *)&cmd_ballboard_setmode3_arg1,
970 (prog_void *)&cmd_ballboard_setmode3_arg2,
975 /**********************************************************/
978 /* this structure is filled when cmd_servo_balls is parsed successfully */
979 struct cmd_servo_balls_result {
984 /* function called when cmd_servo_balls is parsed successfully */
985 static void cmd_servo_balls_parsed(void *parsed_result,
986 __attribute__((unused)) void *data)
988 struct cmd_servo_balls_result *res = parsed_result;
990 if (!strcmp_P(res->arg1, PSTR("deploy")))
991 support_balls_deploy();
992 else if (!strcmp_P(res->arg1, PSTR("pack")))
993 support_balls_pack();
996 prog_char str_servo_balls_arg0[] = "support_balls";
997 parse_pgm_token_string_t cmd_servo_balls_arg0 =
998 TOKEN_STRING_INITIALIZER(struct cmd_servo_balls_result, arg0, str_servo_balls_arg0);
999 prog_char str_servo_balls_arg1[] = "deploy#pack";
1000 parse_pgm_token_string_t cmd_servo_balls_arg1 =
1001 TOKEN_STRING_INITIALIZER(struct cmd_servo_balls_result, arg1, str_servo_balls_arg1);
1003 prog_char help_servo_balls[] = "control support balls";
1004 parse_pgm_inst_t cmd_servo_balls = {
1005 .f = cmd_servo_balls_parsed, /* function to call */
1006 .data = NULL, /* 2nd arg of func */
1007 .help_str = help_servo_balls,
1008 .tokens = { /* token list, NULL terminated */
1009 (prog_void *)&cmd_servo_balls_arg0,
1010 (prog_void *)&cmd_servo_balls_arg1,
1015 /**********************************************************/
1018 /* this structure is filled when cmd_test is parsed successfully */
1019 struct cmd_test_result {
1020 fixed_string_t arg0;
1024 /* function called when cmd_test is parsed successfully */
1025 static void cmd_test_parsed(void *parsed_result, void *data)
1029 prog_char str_test_arg0[] = "test";
1030 parse_pgm_token_string_t cmd_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_test_result, arg0, str_test_arg0);
1031 parse_pgm_token_num_t cmd_test_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_test_result, radius, INT32);
1033 prog_char help_test[] = "Test function";
1034 parse_pgm_inst_t cmd_test = {
1035 .f = cmd_test_parsed, /* function to call */
1036 .data = NULL, /* 2nd arg of func */
1037 .help_str = help_test,
1038 .tokens = { /* token list, NULL terminated */
1039 (prog_void *)&cmd_test_arg0,
1040 (prog_void *)&cmd_test_arg1,