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>
27 #include <aversive/pgmspace.h>
28 #include <aversive/wait.h>
29 #include <aversive/error.h>
30 #include <aversive/eeprom.h>
35 #include <clock_time.h>
41 #include <control_system_manager.h>
42 #include <trajectory_manager.h>
43 #include <vect_base.h>
46 #include <obstacle_avoidance.h>
47 #include <blocking_detection_manager.h>
48 #include <robot_system.h>
49 #include <position_manager.h>
53 #include <parse_string.h>
54 #include <parse_num.h>
56 #include "../common/i2c_commands.h"
57 #include "../common/eeprom_mapping.h"
64 #include "strat_utils.h"
65 #include "strat_base.h"
66 #include "i2c_protocol.h"
69 struct cmd_event_result {
76 /* function called when cmd_event is parsed successfully */
77 static void cmd_event_parsed(void *parsed_result, void *data)
81 struct cmd_event_result * res = parsed_result;
83 if (!strcmp_P(res->arg1, PSTR("all"))) {
84 bit = DO_ENCODERS | DO_CS | DO_RS | DO_POS |
85 DO_BD | DO_TIMER | DO_POWER;
86 if (!strcmp_P(res->arg2, PSTR("on")))
87 mainboard.flags |= bit;
88 else if (!strcmp_P(res->arg2, PSTR("off")))
89 mainboard.flags &= bit;
91 printf_P(PSTR("encoders is %s\r\n"),
92 (DO_ENCODERS & mainboard.flags) ? "on":"off");
93 printf_P(PSTR("cs is %s\r\n"),
94 (DO_CS & mainboard.flags) ? "on":"off");
95 printf_P(PSTR("rs is %s\r\n"),
96 (DO_RS & mainboard.flags) ? "on":"off");
97 printf_P(PSTR("pos is %s\r\n"),
98 (DO_POS & mainboard.flags) ? "on":"off");
99 printf_P(PSTR("bd is %s\r\n"),
100 (DO_BD & mainboard.flags) ? "on":"off");
101 printf_P(PSTR("timer is %s\r\n"),
102 (DO_TIMER & mainboard.flags) ? "on":"off");
103 printf_P(PSTR("power is %s\r\n"),
104 (DO_POWER & mainboard.flags) ? "on":"off");
109 if (!strcmp_P(res->arg1, PSTR("encoders")))
111 else if (!strcmp_P(res->arg1, PSTR("cs"))) {
115 else if (!strcmp_P(res->arg1, PSTR("rs")))
117 else if (!strcmp_P(res->arg1, PSTR("pos")))
119 else if (!strcmp_P(res->arg1, PSTR("bd")))
121 else if (!strcmp_P(res->arg1, PSTR("timer"))) {
125 else if (!strcmp_P(res->arg1, PSTR("power")))
128 if (!strcmp_P(res->arg2, PSTR("on")))
129 mainboard.flags |= bit;
130 else if (!strcmp_P(res->arg2, PSTR("off"))) {
131 if (!strcmp_P(res->arg1, PSTR("cs"))) {
133 robotsim_pwm(LEFT_PWM, 0);
134 robotsim_pwm(RIGHT_PWM, 0);
136 pwm_ng_set(LEFT_PWM, 0);
137 pwm_ng_set(RIGHT_PWM, 0);
140 mainboard.flags &= (~bit);
142 printf_P(PSTR("%s is %s\r\n"), res->arg1,
143 (bit & mainboard.flags) ? "on":"off");
146 prog_char str_event_arg0[] = "event";
147 parse_pgm_token_string_t cmd_event_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg0, str_event_arg0);
148 prog_char str_event_arg1[] = "all#encoders#cs#rs#pos#bd#timer#power";
149 parse_pgm_token_string_t cmd_event_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg1, str_event_arg1);
150 prog_char str_event_arg2[] = "on#off#show";
151 parse_pgm_token_string_t cmd_event_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg2, str_event_arg2);
153 prog_char help_event[] = "Enable/disable events";
154 parse_pgm_inst_t cmd_event = {
155 .f = cmd_event_parsed, /* function to call */
156 .data = NULL, /* 2nd arg of func */
157 .help_str = help_event,
158 .tokens = { /* token list, NULL terminated */
159 (prog_void *)&cmd_event_arg0,
160 (prog_void *)&cmd_event_arg1,
161 (prog_void *)&cmd_event_arg2,
167 /**********************************************************/
170 /* this structure is filled when cmd_spi_test is parsed successfully */
171 struct cmd_spi_test_result {
175 /* function called when cmd_spi_test is parsed successfully */
176 static void cmd_spi_test_parsed(void * parsed_result, void * data)
179 printf("not implemented\n");
181 uint16_t i = 0, ret = 0, ret2 = 0;
183 if (mainboard.flags & DO_ENCODERS) {
184 printf_P(PSTR("Disable encoder event first\r\n"));
190 ret = spi_send_and_receive_byte(i);
191 ret2 = spi_send_and_receive_byte(i);
192 spi_slave_deselect(0);
194 if ((i & 0x7ff) == 0)
195 printf_P(PSTR("Sent %.4x twice, received %x %x\r\n"),
199 } while(!cmdline_keypressed());
203 prog_char str_spi_test_arg0[] = "spi_test";
204 parse_pgm_token_string_t cmd_spi_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_spi_test_result, arg0, str_spi_test_arg0);
206 prog_char help_spi_test[] = "Test the SPI";
207 parse_pgm_inst_t cmd_spi_test = {
208 .f = cmd_spi_test_parsed, /* function to call */
209 .data = NULL, /* 2nd arg of func */
210 .help_str = help_spi_test,
211 .tokens = { /* token list, NULL terminated */
212 (prog_void *)&cmd_spi_test_arg0,
219 /**********************************************************/
222 /* this structure is filled when cmd_opponent is parsed successfully */
223 struct cmd_opponent_result {
230 /* function called when cmd_opponent is parsed successfully */
231 static void cmd_opponent_parsed(void *parsed_result, void *data)
235 if (get_opponent_xyda(&x, &y, &d, &a))
236 printf_P(PSTR("No opponent\r\n"));
238 printf_P(PSTR("x=%d y=%d, d=%d a=%d\r\n"), x, y, d, a);
241 prog_char str_opponent_arg0[] = "opponent";
242 parse_pgm_token_string_t cmd_opponent_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_opponent_result, arg0, str_opponent_arg0);
243 prog_char str_opponent_arg1[] = "show";
244 parse_pgm_token_string_t cmd_opponent_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_opponent_result, arg1, str_opponent_arg1);
246 prog_char help_opponent[] = "Show (x,y) opponent";
247 parse_pgm_inst_t cmd_opponent = {
248 .f = cmd_opponent_parsed, /* function to call */
249 .data = NULL, /* 2nd arg of func */
250 .help_str = help_opponent,
251 .tokens = { /* token list, NULL terminated */
252 (prog_void *)&cmd_opponent_arg0,
253 (prog_void *)&cmd_opponent_arg1,
259 prog_char str_opponent_arg1_set[] = "set";
260 parse_pgm_token_string_t cmd_opponent_arg1_set = TOKEN_STRING_INITIALIZER(struct cmd_opponent_result, arg1, str_opponent_arg1_set);
261 parse_pgm_token_num_t cmd_opponent_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_opponent_result, arg2, INT32);
262 parse_pgm_token_num_t cmd_opponent_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_opponent_result, arg3, INT32);
264 prog_char help_opponent_set[] = "Set (x,y) opponent";
265 parse_pgm_inst_t cmd_opponent_set = {
266 .f = cmd_opponent_parsed, /* function to call */
267 .data = NULL, /* 2nd arg of func */
268 .help_str = help_opponent_set,
269 .tokens = { /* token list, NULL terminated */
270 (prog_void *)&cmd_opponent_arg0,
271 (prog_void *)&cmd_opponent_arg1_set,
272 (prog_void *)&cmd_opponent_arg2,
273 (prog_void *)&cmd_opponent_arg3,
279 /**********************************************************/
282 /* this structure is filled when cmd_start is parsed successfully */
283 struct cmd_start_result {
285 fixed_string_t color;
286 fixed_string_t debug;
289 /* function called when cmd_start is parsed successfully */
290 static void cmd_start_parsed(void *parsed_result, void *data)
293 printf("not implemented\n");
295 struct cmd_start_result *res = parsed_result;
296 uint8_t old_level = gen.log_level;
298 gen.logs[NB_LOGS] = E_USER_STRAT;
299 if (!strcmp_P(res->debug, PSTR("debug"))) {
300 strat_infos.dump_enabled = 1;
304 strat_infos.dump_enabled = 0;
308 if (!strcmp_P(res->color, PSTR("yellow"))) {
309 mainboard.our_color = I2C_COLOR_YELLOW;
310 i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_YELLOW);
311 i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_YELLOW);
313 else if (!strcmp_P(res->color, PSTR("blue"))) {
314 mainboard.our_color = I2C_COLOR_BLUE;
315 i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_BLUE);
316 i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_BLUE);
321 gen.logs[NB_LOGS] = 0;
322 gen.log_level = old_level;
326 prog_char str_start_arg0[] = "start";
327 parse_pgm_token_string_t cmd_start_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_start_result, arg0, str_start_arg0);
328 prog_char str_start_color[] = "blue#yellow";
329 parse_pgm_token_string_t cmd_start_color = TOKEN_STRING_INITIALIZER(struct cmd_start_result, color, str_start_color);
330 prog_char str_start_debug[] = "debug#match";
331 parse_pgm_token_string_t cmd_start_debug = TOKEN_STRING_INITIALIZER(struct cmd_start_result, debug, str_start_debug);
333 prog_char help_start[] = "Start the robot";
334 parse_pgm_inst_t cmd_start = {
335 .f = cmd_start_parsed, /* function to call */
336 .data = NULL, /* 2nd arg of func */
337 .help_str = help_start,
338 .tokens = { /* token list, NULL terminated */
339 (prog_void *)&cmd_start_arg0,
340 (prog_void *)&cmd_start_color,
341 (prog_void *)&cmd_start_debug,
348 /**********************************************************/
351 /* this structure is filled when cmd_interact is parsed successfully */
352 struct cmd_interact_result {
356 static void print_cs(void)
358 printf_P(PSTR("cons_d=% .8"PRIi32" cons_a=% .8"PRIi32" fil_d=% .8"PRIi32" fil_a=% .8"PRIi32" "
359 "err_d=% .8"PRIi32" err_a=% .8"PRIi32" out_d=% .8"PRIi32" out_a=% .8"PRIi32"\r\n"),
360 cs_get_consign(&mainboard.distance.cs),
361 cs_get_consign(&mainboard.angle.cs),
362 cs_get_filtered_consign(&mainboard.distance.cs),
363 cs_get_filtered_consign(&mainboard.angle.cs),
364 cs_get_error(&mainboard.distance.cs),
365 cs_get_error(&mainboard.angle.cs),
366 cs_get_out(&mainboard.distance.cs),
367 cs_get_out(&mainboard.angle.cs));
370 static void print_pos(void)
372 printf_P(PSTR("x=% .8d y=% .8d a=% .8d\r\n"),
373 position_get_x_s16(&mainboard.pos),
374 position_get_y_s16(&mainboard.pos),
375 position_get_a_deg_s16(&mainboard.pos));
378 static void print_time(void)
380 printf_P(PSTR("time %d\r\n"),time_get_s());
384 static void print_sensors(void)
387 if (sensor_start_switch())
388 printf_P(PSTR("Start switch | "));
390 printf_P(PSTR(" | "));
392 if (IR_DISP_SENSOR())
393 printf_P(PSTR("IR disp | "));
395 printf_P(PSTR(" | "));
397 printf_P(PSTR("\r\n"));
401 static void print_pid(void)
403 printf_P(PSTR("P=% .8"PRIi32" I=% .8"PRIi32" D=% .8"PRIi32" out=% .8"PRIi32" | "
404 "P=% .8"PRIi32" I=% .8"PRIi32" D=% .8"PRIi32" out=% .8"PRIi32"\r\n"),
405 pid_get_value_in(&mainboard.distance.pid) * pid_get_gain_P(&mainboard.distance.pid),
406 pid_get_value_I(&mainboard.distance.pid) * pid_get_gain_I(&mainboard.distance.pid),
407 pid_get_value_D(&mainboard.distance.pid) * pid_get_gain_D(&mainboard.distance.pid),
408 pid_get_value_out(&mainboard.distance.pid),
409 pid_get_value_in(&mainboard.angle.pid) * pid_get_gain_P(&mainboard.angle.pid),
410 pid_get_value_I(&mainboard.angle.pid) * pid_get_gain_I(&mainboard.angle.pid),
411 pid_get_value_D(&mainboard.angle.pid) * pid_get_gain_D(&mainboard.angle.pid),
412 pid_get_value_out(&mainboard.angle.pid));
415 #define PRINT_POS (1<<0)
416 #define PRINT_PID (1<<1)
417 #define PRINT_CS (1<<2)
418 #define PRINT_SENSORS (1<<3)
419 #define PRINT_TIME (1<<4)
420 #define PRINT_BLOCKING (1<<5)
422 static void cmd_interact_parsed(void * parsed_result, void * data)
431 printf_P(PSTR("Display debugs:\r\n"
437 /* " 6:blocking\r\n" */
444 mainboard.flags &= (~DO_CS);
445 pwm_set_and_save(LEFT_PWM, 0);
446 pwm_set_and_save(RIGHT_PWM, 0);
449 if (print & PRINT_POS) {
453 if (print & PRINT_PID) {
457 if (print & PRINT_CS) {
461 if (print & PRINT_SENSORS) {
465 if (print & PRINT_TIME) {
468 /* if (print & PRINT_BLOCKING) { */
469 /* printf_P(PSTR("%s %s blocking=%d\r\n"), */
470 /* mainboard.blocking ? "BLOCK1":" ", */
471 /* rs_is_blocked(&mainboard.rs) ? "BLOCK2":" ", */
472 /* rs_get_blocking(&mainboard.rs)); */
475 c = cmdline_getchar();
480 cmd = vt100_parser(&vt100, c);
488 case '1': print ^= PRINT_POS; break;
489 case '2': print ^= PRINT_PID; break;
490 case '3': print ^= PRINT_CS; break;
491 case '4': print ^= PRINT_SENSORS; break;
492 case '5': print ^= PRINT_TIME; break;
493 case '6': print ^= PRINT_BLOCKING; break;
496 if (mainboard.flags & DO_CS)
498 pwm_set_and_save(LEFT_PWM, 0);
499 pwm_set_and_save(RIGHT_PWM, 0);
502 pwm_set_and_save(LEFT_PWM, 0);
503 pwm_set_and_save(RIGHT_PWM, 0);
512 pwm_set_and_save(LEFT_PWM, 1200);
513 pwm_set_and_save(RIGHT_PWM, 1200);
516 pwm_set_and_save(LEFT_PWM, -1200);
517 pwm_set_and_save(RIGHT_PWM, 1200);
520 pwm_set_and_save(LEFT_PWM, -1200);
521 pwm_set_and_save(RIGHT_PWM, -1200);
524 pwm_set_and_save(LEFT_PWM, 1200);
525 pwm_set_and_save(RIGHT_PWM, -1200);
533 prog_char str_interact_arg0[] = "interact";
534 parse_pgm_token_string_t cmd_interact_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_interact_result, arg0, str_interact_arg0);
536 prog_char help_interact[] = "Interactive mode";
537 parse_pgm_inst_t cmd_interact = {
538 .f = cmd_interact_parsed, /* function to call */
539 .data = NULL, /* 2nd arg of func */
540 .help_str = help_interact,
541 .tokens = { /* token list, NULL terminated */
542 (prog_void *)&cmd_interact_arg0,
548 /**********************************************************/
551 /* this structure is filled when cmd_color is parsed successfully */
552 struct cmd_color_result {
554 fixed_string_t color;
557 /* function called when cmd_color is parsed successfully */
558 static void cmd_color_parsed(void *parsed_result, void *data)
561 printf("not implemented\n");
563 struct cmd_color_result *res = (struct cmd_color_result *) parsed_result;
564 if (!strcmp_P(res->color, PSTR("yellow"))) {
565 mainboard.our_color = I2C_COLOR_YELLOW;
566 i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_YELLOW);
567 i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_YELLOW);
569 else if (!strcmp_P(res->color, PSTR("blue"))) {
570 mainboard.our_color = I2C_COLOR_BLUE;
571 i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_BLUE);
572 i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_BLUE);
574 printf_P(PSTR("Done\r\n"));
578 prog_char str_color_arg0[] = "color";
579 parse_pgm_token_string_t cmd_color_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_color_result, arg0, str_color_arg0);
580 prog_char str_color_color[] = "blue#yellow";
581 parse_pgm_token_string_t cmd_color_color = TOKEN_STRING_INITIALIZER(struct cmd_color_result, color, str_color_color);
583 prog_char help_color[] = "Set our color";
584 parse_pgm_inst_t cmd_color = {
585 .f = cmd_color_parsed, /* function to call */
586 .data = NULL, /* 2nd arg of func */
587 .help_str = help_color,
588 .tokens = { /* token list, NULL terminated */
589 (prog_void *)&cmd_color_arg0,
590 (prog_void *)&cmd_color_color,
596 /**********************************************************/
599 /* this structure is filled when cmd_rs is parsed successfully */
600 struct cmd_rs_result {
605 /* function called when cmd_rs is parsed successfully */
606 static void cmd_rs_parsed(void *parsed_result, void *data)
608 // struct cmd_rs_result *res = parsed_result;
610 printf_P(PSTR("angle cons=% .6"PRIi32" in=% .6"PRIi32" out=% .6"PRIi32" / "),
611 cs_get_consign(&mainboard.angle.cs),
612 cs_get_filtered_feedback(&mainboard.angle.cs),
613 cs_get_out(&mainboard.angle.cs));
614 printf_P(PSTR("distance cons=% .6"PRIi32" in=% .6"PRIi32" out=% .6"PRIi32" / "),
615 cs_get_consign(&mainboard.distance.cs),
616 cs_get_filtered_feedback(&mainboard.distance.cs),
617 cs_get_out(&mainboard.distance.cs));
618 printf_P(PSTR("l=% .4"PRIi32" r=% .4"PRIi32"\r\n"), mainboard.pwm_l,
621 } while(!cmdline_keypressed());
624 prog_char str_rs_arg0[] = "rs";
625 parse_pgm_token_string_t cmd_rs_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_rs_result, arg0, str_rs_arg0);
626 prog_char str_rs_arg1[] = "show";
627 parse_pgm_token_string_t cmd_rs_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_rs_result, arg1, str_rs_arg1);
629 prog_char help_rs[] = "Show rs (robot system) values";
630 parse_pgm_inst_t cmd_rs = {
631 .f = cmd_rs_parsed, /* function to call */
632 .data = NULL, /* 2nd arg of func */
634 .tokens = { /* token list, NULL terminated */
635 (prog_void *)&cmd_rs_arg0,
636 (prog_void *)&cmd_rs_arg1,
641 /**********************************************************/
644 /* this structure is filled when cmd_i2cdebug is parsed successfully */
645 struct cmd_i2cdebug_result {
649 /* function called when cmd_i2cdebug is parsed successfully */
650 static void cmd_i2cdebug_parsed(void * parsed_result, void * data)
653 printf("not implemented\n");
656 i2c_protocol_debug();
660 prog_char str_i2cdebug_arg0[] = "i2cdebug";
661 parse_pgm_token_string_t cmd_i2cdebug_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_i2cdebug_result, arg0, str_i2cdebug_arg0);
663 prog_char help_i2cdebug[] = "I2c debug infos";
664 parse_pgm_inst_t cmd_i2cdebug = {
665 .f = cmd_i2cdebug_parsed, /* function to call */
666 .data = NULL, /* 2nd arg of func */
667 .help_str = help_i2cdebug,
668 .tokens = { /* token list, NULL terminated */
669 (prog_void *)&cmd_i2cdebug_arg0,
674 /**********************************************************/
677 /* this structure is filled when cmd_cobboard_show is parsed successfully */
678 struct cmd_cobboard_show_result {
683 /* function called when cmd_cobboard_show is parsed successfully */
684 static void cmd_cobboard_show_parsed(void * parsed_result, void * data)
687 printf("not implemented\n");
689 printf_P(PSTR("mode = %x\r\n"), cobboard.mode);
690 printf_P(PSTR("status = %x\r\n"), cobboard.status);
691 printf_P(PSTR("cob_count = %x\r\n"), cobboard.cob_count);
692 printf_P(PSTR("left_cobroller_speed = %d\r\n"), cobboard.left_cobroller_speed);
693 printf_P(PSTR("right_cobroller_speed = %d\r\n"), cobboard.right_cobroller_speed);
697 prog_char str_cobboard_show_arg0[] = "cobboard";
698 parse_pgm_token_string_t cmd_cobboard_show_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_show_result, arg0, str_cobboard_show_arg0);
699 prog_char str_cobboard_show_arg1[] = "show";
700 parse_pgm_token_string_t cmd_cobboard_show_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_show_result, arg1, str_cobboard_show_arg1);
702 prog_char help_cobboard_show[] = "show cobboard status";
703 parse_pgm_inst_t cmd_cobboard_show = {
704 .f = cmd_cobboard_show_parsed, /* function to call */
705 .data = NULL, /* 2nd arg of func */
706 .help_str = help_cobboard_show,
707 .tokens = { /* token list, NULL terminated */
708 (prog_void *)&cmd_cobboard_show_arg0,
709 (prog_void *)&cmd_cobboard_show_arg1,
714 /**********************************************************/
715 /* Cobboard_Setmode1 */
717 /* this structure is filled when cmd_cobboard_setmode1 is parsed successfully */
718 struct cmd_cobboard_setmode1_result {
723 /* function called when cmd_cobboard_setmode1 is parsed successfully */
724 static void cmd_cobboard_setmode1_parsed(void *parsed_result, void *data)
727 printf("not implemented\n");
729 struct cmd_cobboard_setmode1_result *res = parsed_result;
731 if (!strcmp_P(res->arg1, PSTR("init")))
732 i2c_cobboard_mode_init();
733 else if (!strcmp_P(res->arg1, PSTR("eject")))
734 i2c_cobboard_mode_eject();
738 prog_char str_cobboard_setmode1_arg0[] = "cobboard";
739 parse_pgm_token_string_t cmd_cobboard_setmode1_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode1_result, arg0, str_cobboard_setmode1_arg0);
740 prog_char str_cobboard_setmode1_arg1[] = "init#eject";
741 parse_pgm_token_string_t cmd_cobboard_setmode1_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode1_result, arg1, str_cobboard_setmode1_arg1);
743 prog_char help_cobboard_setmode1[] = "set cobboard mode (mode)";
744 parse_pgm_inst_t cmd_cobboard_setmode1 = {
745 .f = cmd_cobboard_setmode1_parsed, /* function to call */
746 .data = NULL, /* 2nd arg of func */
747 .help_str = help_cobboard_setmode1,
748 .tokens = { /* token list, NULL terminated */
749 (prog_void *)&cmd_cobboard_setmode1_arg0,
750 (prog_void *)&cmd_cobboard_setmode1_arg1,
755 /**********************************************************/
756 /* Cobboard_Setmode2 */
758 /* this structure is filled when cmd_cobboard_setmode2 is parsed successfully */
759 struct cmd_cobboard_setmode2_result {
765 /* function called when cmd_cobboard_setmode2 is parsed successfully */
766 static void cmd_cobboard_setmode2_parsed(void * parsed_result, void * data)
769 printf("not implemented\n");
771 struct cmd_cobboard_setmode2_result *res = parsed_result;
772 uint8_t side = I2C_LEFT_SIDE;
774 if (!strcmp_P(res->arg2, PSTR("left")))
775 side = I2C_LEFT_SIDE;
776 else if (!strcmp_P(res->arg2, PSTR("right")))
777 side = I2C_RIGHT_SIDE;
779 if (!strcmp_P(res->arg1, PSTR("deploy")))
780 i2c_cobboard_mode_deploy(side);
781 else if (!strcmp_P(res->arg1, PSTR("harvest")))
782 i2c_cobboard_mode_harvest(side);
783 else if (!strcmp_P(res->arg1, PSTR("pack")))
784 i2c_cobboard_mode_pack(side);
788 prog_char str_cobboard_setmode2_arg0[] = "cobboard";
789 parse_pgm_token_string_t cmd_cobboard_setmode2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode2_result, arg0, str_cobboard_setmode2_arg0);
790 prog_char str_cobboard_setmode2_arg1[] = "harvest#deploy#pack";
791 parse_pgm_token_string_t cmd_cobboard_setmode2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode2_result, arg1, str_cobboard_setmode2_arg1);
792 prog_char str_cobboard_setmode2_arg2[] = "left#right";
793 parse_pgm_token_string_t cmd_cobboard_setmode2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode2_result, arg2, str_cobboard_setmode2_arg2);
795 prog_char help_cobboard_setmode2[] = "set cobboard mode (mode, side)";
796 parse_pgm_inst_t cmd_cobboard_setmode2 = {
797 .f = cmd_cobboard_setmode2_parsed, /* function to call */
798 .data = NULL, /* 2nd arg of func */
799 .help_str = help_cobboard_setmode2,
800 .tokens = { /* token list, NULL terminated */
801 (prog_void *)&cmd_cobboard_setmode2_arg0,
802 (prog_void *)&cmd_cobboard_setmode2_arg1,
803 (prog_void *)&cmd_cobboard_setmode2_arg2,
808 /**********************************************************/
809 /* Cobboard_Setmode3 */
811 /* this structure is filled when cmd_cobboard_setmode3 is parsed successfully */
812 struct cmd_cobboard_setmode3_result {
818 /* function called when cmd_cobboard_setmode3 is parsed successfully */
819 static void cmd_cobboard_setmode3_parsed(void *parsed_result, void *data)
822 printf("not implemented\n");
824 struct cmd_cobboard_setmode3_result *res = parsed_result;
825 if (!strcmp_P(res->arg1, PSTR("xxx")))
830 prog_char str_cobboard_setmode3_arg0[] = "cobboard";
831 parse_pgm_token_string_t cmd_cobboard_setmode3_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode3_result, arg0, str_cobboard_setmode3_arg0);
832 prog_char str_cobboard_setmode3_arg1[] = "xxx";
833 parse_pgm_token_string_t cmd_cobboard_setmode3_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode3_result, arg1, str_cobboard_setmode3_arg1);
834 parse_pgm_token_num_t cmd_cobboard_setmode3_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_cobboard_setmode3_result, level, UINT8);
836 prog_char help_cobboard_setmode3[] = "set cobboard mode (mode, level)";
837 parse_pgm_inst_t cmd_cobboard_setmode3 = {
838 .f = cmd_cobboard_setmode3_parsed, /* function to call */
839 .data = NULL, /* 2nd arg of func */
840 .help_str = help_cobboard_setmode3,
841 .tokens = { /* token list, NULL terminated */
842 (prog_void *)&cmd_cobboard_setmode3_arg0,
843 (prog_void *)&cmd_cobboard_setmode3_arg1,
844 (prog_void *)&cmd_cobboard_setmode3_arg2,
849 /**********************************************************/
852 /* this structure is filled when cmd_ballboard_show is parsed successfully */
853 struct cmd_ballboard_show_result {
858 /* function called when cmd_ballboard_show is parsed successfully */
859 static void cmd_ballboard_show_parsed(void * parsed_result, void * data)
862 printf("not implemented\n");
864 printf_P(PSTR("mode = %x\r\n"), ballboard.mode);
865 printf_P(PSTR("status = %x\r\n"), ballboard.status);
866 printf_P(PSTR("ball_count = %d\r\n"), ballboard.ball_count);
870 prog_char str_ballboard_show_arg0[] = "ballboard";
871 parse_pgm_token_string_t cmd_ballboard_show_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_show_result, arg0, str_ballboard_show_arg0);
872 prog_char str_ballboard_show_arg1[] = "show";
873 parse_pgm_token_string_t cmd_ballboard_show_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_show_result, arg1, str_ballboard_show_arg1);
875 prog_char help_ballboard_show[] = "show ballboard status";
876 parse_pgm_inst_t cmd_ballboard_show = {
877 .f = cmd_ballboard_show_parsed, /* function to call */
878 .data = NULL, /* 2nd arg of func */
879 .help_str = help_ballboard_show,
880 .tokens = { /* token list, NULL terminated */
881 (prog_void *)&cmd_ballboard_show_arg0,
882 (prog_void *)&cmd_ballboard_show_arg1,
887 /**********************************************************/
888 /* Ballboard_Setmode1 */
890 /* this structure is filled when cmd_ballboard_setmode1 is parsed successfully */
891 struct cmd_ballboard_setmode1_result {
896 /* function called when cmd_ballboard_setmode1 is parsed successfully */
897 static void cmd_ballboard_setmode1_parsed(void *parsed_result, void *data)
900 printf("not implemented\n");
902 struct cmd_ballboard_setmode1_result *res = parsed_result;
904 if (!strcmp_P(res->arg1, PSTR("init")))
905 i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_INIT);
906 else if (!strcmp_P(res->arg1, PSTR("off")))
907 i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_OFF);
908 else if (!strcmp_P(res->arg1, PSTR("eject")))
909 i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_EJECT);
910 else if (!strcmp_P(res->arg1, PSTR("harvest")))
911 i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_HARVEST);
917 prog_char str_ballboard_setmode1_arg0[] = "ballboard";
918 parse_pgm_token_string_t cmd_ballboard_setmode1_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode1_result, arg0, str_ballboard_setmode1_arg0);
919 prog_char str_ballboard_setmode1_arg1[] = "init#eject#harvest#off";
920 parse_pgm_token_string_t cmd_ballboard_setmode1_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode1_result, arg1, str_ballboard_setmode1_arg1);
922 prog_char help_ballboard_setmode1[] = "set ballboard mode (mode)";
923 parse_pgm_inst_t cmd_ballboard_setmode1 = {
924 .f = cmd_ballboard_setmode1_parsed, /* function to call */
925 .data = NULL, /* 2nd arg of func */
926 .help_str = help_ballboard_setmode1,
927 .tokens = { /* token list, NULL terminated */
928 (prog_void *)&cmd_ballboard_setmode1_arg0,
929 (prog_void *)&cmd_ballboard_setmode1_arg1,
934 /**********************************************************/
935 /* Ballboard_Setmode2 */
937 /* this structure is filled when cmd_ballboard_setmode2 is parsed successfully */
938 struct cmd_ballboard_setmode2_result {
944 /* function called when cmd_ballboard_setmode2 is parsed successfully */
945 static void cmd_ballboard_setmode2_parsed(void * parsed_result, void * data)
948 printf("not implemented\n");
950 struct cmd_ballboard_setmode2_result *res = parsed_result;
951 uint8_t mode = I2C_BALLBOARD_MODE_INIT;
953 if (!strcmp_P(res->arg2, PSTR("left"))) {
954 if (!strcmp_P(res->arg1, PSTR("prepare")))
955 mode = I2C_BALLBOARD_MODE_PREP_L_FORK;
956 else if (!strcmp_P(res->arg1, PSTR("take")))
957 mode = I2C_BALLBOARD_MODE_TAKE_L_FORK;
960 if (!strcmp_P(res->arg1, PSTR("prepare")))
961 mode = I2C_BALLBOARD_MODE_PREP_R_FORK;
962 else if (!strcmp_P(res->arg1, PSTR("take")))
963 mode = I2C_BALLBOARD_MODE_TAKE_R_FORK;
965 i2c_ballboard_set_mode(mode);
969 prog_char str_ballboard_setmode2_arg0[] = "ballboard";
970 parse_pgm_token_string_t cmd_ballboard_setmode2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode2_result, arg0, str_ballboard_setmode2_arg0);
971 prog_char str_ballboard_setmode2_arg1[] = "prepare#take";
972 parse_pgm_token_string_t cmd_ballboard_setmode2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode2_result, arg1, str_ballboard_setmode2_arg1);
973 prog_char str_ballboard_setmode2_arg2[] = "left#right";
974 parse_pgm_token_string_t cmd_ballboard_setmode2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode2_result, arg2, str_ballboard_setmode2_arg2);
976 prog_char help_ballboard_setmode2[] = "set ballboard mode (mode, side)";
977 parse_pgm_inst_t cmd_ballboard_setmode2 = {
978 .f = cmd_ballboard_setmode2_parsed, /* function to call */
979 .data = NULL, /* 2nd arg of func */
980 .help_str = help_ballboard_setmode2,
981 .tokens = { /* token list, NULL terminated */
982 (prog_void *)&cmd_ballboard_setmode2_arg0,
983 (prog_void *)&cmd_ballboard_setmode2_arg1,
984 (prog_void *)&cmd_ballboard_setmode2_arg2,
989 /**********************************************************/
990 /* Ballboard_Setmode3 */
992 /* this structure is filled when cmd_ballboard_setmode3 is parsed successfully */
993 struct cmd_ballboard_setmode3_result {
999 /* function called when cmd_ballboard_setmode3 is parsed successfully */
1000 static void cmd_ballboard_setmode3_parsed(void *parsed_result, void *data)
1003 printf("not implemented\n");
1005 struct cmd_ballboard_setmode3_result *res = parsed_result;
1006 if (!strcmp_P(res->arg1, PSTR("xxx")))
1011 prog_char str_ballboard_setmode3_arg0[] = "ballboard";
1012 parse_pgm_token_string_t cmd_ballboard_setmode3_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode3_result, arg0, str_ballboard_setmode3_arg0);
1013 prog_char str_ballboard_setmode3_arg1[] = "xxx";
1014 parse_pgm_token_string_t cmd_ballboard_setmode3_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode3_result, arg1, str_ballboard_setmode3_arg1);
1015 parse_pgm_token_num_t cmd_ballboard_setmode3_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_ballboard_setmode3_result, level, UINT8);
1017 prog_char help_ballboard_setmode3[] = "set ballboard mode (mode, level)";
1018 parse_pgm_inst_t cmd_ballboard_setmode3 = {
1019 .f = cmd_ballboard_setmode3_parsed, /* function to call */
1020 .data = NULL, /* 2nd arg of func */
1021 .help_str = help_ballboard_setmode3,
1022 .tokens = { /* token list, NULL terminated */
1023 (prog_void *)&cmd_ballboard_setmode3_arg0,
1024 (prog_void *)&cmd_ballboard_setmode3_arg1,
1025 (prog_void *)&cmd_ballboard_setmode3_arg2,
1030 /**********************************************************/
1033 /* this structure is filled when cmd_servo_balls is parsed successfully */
1034 struct cmd_servo_balls_result {
1035 fixed_string_t arg0;
1036 fixed_string_t arg1;
1039 /* function called when cmd_servo_balls is parsed successfully */
1040 static void cmd_servo_balls_parsed(void *parsed_result,
1041 __attribute__((unused)) void *data)
1043 struct cmd_servo_balls_result *res = parsed_result;
1045 if (!strcmp_P(res->arg1, PSTR("deploy")))
1046 support_balls_deploy();
1047 else if (!strcmp_P(res->arg1, PSTR("pack")))
1048 support_balls_pack();
1051 prog_char str_servo_balls_arg0[] = "support_balls";
1052 parse_pgm_token_string_t cmd_servo_balls_arg0 =
1053 TOKEN_STRING_INITIALIZER(struct cmd_servo_balls_result, arg0, str_servo_balls_arg0);
1054 prog_char str_servo_balls_arg1[] = "deploy#pack";
1055 parse_pgm_token_string_t cmd_servo_balls_arg1 =
1056 TOKEN_STRING_INITIALIZER(struct cmd_servo_balls_result, arg1, str_servo_balls_arg1);
1058 prog_char help_servo_balls[] = "control support balls";
1059 parse_pgm_inst_t cmd_servo_balls = {
1060 .f = cmd_servo_balls_parsed, /* function to call */
1061 .data = NULL, /* 2nd arg of func */
1062 .help_str = help_servo_balls,
1063 .tokens = { /* token list, NULL terminated */
1064 (prog_void *)&cmd_servo_balls_arg0,
1065 (prog_void *)&cmd_servo_balls_arg1,
1070 /**********************************************************/
1073 /* this structure is filled when cmd_test is parsed successfully */
1074 struct cmd_test_result {
1075 fixed_string_t arg0;
1079 /* function called when cmd_test is parsed successfully */
1080 static void cmd_test_parsed(void *parsed_result, void *data)
1084 prog_char str_test_arg0[] = "test";
1085 parse_pgm_token_string_t cmd_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_test_result, arg0, str_test_arg0);
1086 parse_pgm_token_num_t cmd_test_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_test_result, radius, INT32);
1088 prog_char help_test[] = "Test function";
1089 parse_pgm_inst_t cmd_test = {
1090 .f = cmd_test_parsed, /* function to call */
1091 .data = NULL, /* 2nd arg of func */
1092 .help_str = help_test,
1093 .tokens = { /* token list, NULL terminated */
1094 (prog_void *)&cmd_test_arg0,
1095 (prog_void *)&cmd_test_arg1,