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;
1080 /* function called when cmd_test is parsed successfully */
1081 static void line2line(double line1x1, double line1y1,
1082 double line1x2, double line1y2,
1083 double line2x1, double line2y1,
1084 double line2x2, double line2y2,
1085 double radius, double dist)
1088 int32_t dist_imp_target;
1089 double speed_d, speed_a;
1090 double distance, angle;
1091 double line1_angle = atan2(line1y2-line1y1, line1x2-line1x1);
1092 double line2_angle = atan2(line2y2-line2y1, line2x2-line2x1);
1094 printf("%s()\n", __FUNCTION__);
1095 strat_set_speed(500, 500);
1096 circle_get_da_speed_from_radius(&mainboard.traj, radius,
1097 &speed_d, &speed_a);
1098 trajectory_line_abs(&mainboard.traj,
1100 line1x2, line1y2, 150.);
1101 err = WAIT_COND_OR_TRAJ_END(distance_from_robot(line1x2, line1y2) <
1102 dist, TRAJ_FLAGS_NO_NEAR);
1104 strat_set_speed(speed_d, speed_a);
1105 angle = line2_angle - line1_angle;
1106 distance = angle * radius;
1108 distance = -distance;
1109 dist_imp_target = rs_get_distance(&mainboard.rs) +
1110 distance * mainboard.pos.phys.distance_imp_per_mm;
1112 distance += 100; /* take some margin to avoid deceleration */
1113 trajectory_d_a_rel(&mainboard.traj, distance, angle);
1115 err = WAIT_COND_OR_TRAJ_END(rs_get_distance(&mainboard.rs) > dist_imp_target,
1116 TRAJ_FLAGS_NO_NEAR);
1118 strat_set_speed(500, 500);
1119 trajectory_line_abs(&mainboard.traj,
1121 line2x2, line2y2, 150.);
1124 /* function called when cmd_test is parsed successfully */
1125 static void cmd_test_parsed(void *parsed_result, void *data)
1127 // struct cmd_test_result *res = parsed_result;
1129 strat_reset_pos(400, 400, 90);
1130 mainboard.angle.on = 1;
1131 mainboard.distance.on = 1;
1133 line2line(375, 347, 375, 1847,
1134 375, 1847, 1050, 1472,
1136 line2line(825, 1596, 1050, 1472,
1137 1050, 1472, 1500, 1722,
1139 line2line(1050, 1472, 1500, 1722,
1140 1500, 1722, 2175, 1347,
1142 line2line(1500, 1722, 2175, 1347,
1143 2175, 1347, 2175, 847,
1145 line2line(2175, 1347, 2175, 847,
1146 2175, 847, 2400, 722,
1148 line2line(2175, 847, 2400, 722,
1149 2400, 722, 2625, 847,
1151 line2line(2400, 722, 2625, 847,
1152 2625, 847, 2625, 1847,
1154 line2line(2625, 847, 2625, 1847,
1155 2625, 1847, 375, 597,
1159 prog_char str_test_arg0[] = "test";
1160 parse_pgm_token_string_t cmd_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_test_result, arg0, str_test_arg0);
1161 parse_pgm_token_num_t cmd_test_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_test_result, radius, INT32);
1162 parse_pgm_token_num_t cmd_test_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_test_result, dist, INT32);
1164 prog_char help_test[] = "Test function";
1165 parse_pgm_inst_t cmd_test = {
1166 .f = cmd_test_parsed, /* function to call */
1167 .data = NULL, /* 2nd arg of func */
1168 .help_str = help_test,
1169 .tokens = { /* token list, NULL terminated */
1170 (prog_void *)&cmd_test_arg0,