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>
28 #include <aversive/pgmspace.h>
29 #include <aversive/wait.h>
30 #include <aversive/error.h>
31 #include <aversive/eeprom.h>
36 #include <clock_time.h>
42 #include <control_system_manager.h>
43 #include <trajectory_manager.h>
44 #include <trajectory_manager_core.h>
45 #include <trajectory_manager_utils.h>
46 #include <vect_base.h>
49 #include <obstacle_avoidance.h>
50 #include <blocking_detection_manager.h>
51 #include <robot_system.h>
52 #include <position_manager.h>
56 #include <parse_string.h>
57 #include <parse_num.h>
59 #include "../common/i2c_commands.h"
60 #include "../common/eeprom_mapping.h"
67 #include "strat_utils.h"
68 #include "strat_base.h"
69 #include "i2c_protocol.h"
72 struct cmd_event_result {
79 /* function called when cmd_event is parsed successfully */
80 static void cmd_event_parsed(void *parsed_result, void *data)
84 struct cmd_event_result * res = parsed_result;
86 if (!strcmp_P(res->arg1, PSTR("all"))) {
88 if (!strcmp_P(res->arg2, PSTR("on")))
89 mainboard.flags |= bit;
90 else if (!strcmp_P(res->arg2, PSTR("off")))
91 mainboard.flags &= bit;
93 printf_P(PSTR("encoders is %s\r\n"),
94 (DO_ENCODERS & mainboard.flags) ? "on":"off");
95 printf_P(PSTR("cs is %s\r\n"),
96 (DO_CS & mainboard.flags) ? "on":"off");
97 printf_P(PSTR("rs is %s\r\n"),
98 (DO_RS & mainboard.flags) ? "on":"off");
99 printf_P(PSTR("pos is %s\r\n"),
100 (DO_POS & mainboard.flags) ? "on":"off");
101 printf_P(PSTR("bd is %s\r\n"),
102 (DO_BD & mainboard.flags) ? "on":"off");
103 printf_P(PSTR("timer is %s\r\n"),
104 (DO_TIMER & mainboard.flags) ? "on":"off");
105 printf_P(PSTR("power is %s\r\n"),
106 (DO_POWER & mainboard.flags) ? "on":"off");
107 printf_P(PSTR("errblock is %s\r\n"),
108 (DO_ERRBLOCKING & mainboard.flags) ? "on":"off");
113 if (!strcmp_P(res->arg1, PSTR("encoders")))
115 else if (!strcmp_P(res->arg1, PSTR("cs"))) {
119 else if (!strcmp_P(res->arg1, PSTR("rs")))
121 else if (!strcmp_P(res->arg1, PSTR("pos")))
123 else if (!strcmp_P(res->arg1, PSTR("bd")))
125 else if (!strcmp_P(res->arg1, PSTR("timer"))) {
129 else if (!strcmp_P(res->arg1, PSTR("power")))
131 else if (!strcmp_P(res->arg1, PSTR("errblock")))
132 bit = DO_ERRBLOCKING;
134 if (!strcmp_P(res->arg2, PSTR("on")))
135 mainboard.flags |= bit;
136 else if (!strcmp_P(res->arg2, PSTR("off"))) {
137 if (!strcmp_P(res->arg1, PSTR("cs"))) {
139 robotsim_pwm(LEFT_PWM, 0);
140 robotsim_pwm(RIGHT_PWM, 0);
142 pwm_ng_set(LEFT_PWM, 0);
143 pwm_ng_set(RIGHT_PWM, 0);
146 mainboard.flags &= (~bit);
148 printf_P(PSTR("%s is %s\r\n"), res->arg1,
149 (bit & mainboard.flags) ? "on":"off");
152 prog_char str_event_arg0[] = "event";
153 parse_pgm_token_string_t cmd_event_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg0, str_event_arg0);
154 prog_char str_event_arg1[] = "all#encoders#cs#rs#pos#bd#timer#power#errblock";
155 parse_pgm_token_string_t cmd_event_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg1, str_event_arg1);
156 prog_char str_event_arg2[] = "on#off#show";
157 parse_pgm_token_string_t cmd_event_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg2, str_event_arg2);
159 prog_char help_event[] = "Enable/disable events";
160 parse_pgm_inst_t cmd_event = {
161 .f = cmd_event_parsed, /* function to call */
162 .data = NULL, /* 2nd arg of func */
163 .help_str = help_event,
164 .tokens = { /* token list, NULL terminated */
165 (prog_void *)&cmd_event_arg0,
166 (prog_void *)&cmd_event_arg1,
167 (prog_void *)&cmd_event_arg2,
173 /**********************************************************/
176 /* this structure is filled when cmd_spi_test is parsed successfully */
177 struct cmd_spi_test_result {
181 /* function called when cmd_spi_test is parsed successfully */
182 static void cmd_spi_test_parsed(void * parsed_result, void * data)
185 printf("not implemented\n");
187 uint16_t i = 0, ret = 0, ret2 = 0;
189 if (mainboard.flags & DO_ENCODERS) {
190 printf_P(PSTR("Disable encoder event first\r\n"));
196 ret = spi_send_and_receive_byte(i);
197 ret2 = spi_send_and_receive_byte(i);
198 spi_slave_deselect(0);
200 if ((i & 0x7ff) == 0)
201 printf_P(PSTR("Sent %.4x twice, received %x %x\r\n"),
205 } while(!cmdline_keypressed());
209 prog_char str_spi_test_arg0[] = "spi_test";
210 parse_pgm_token_string_t cmd_spi_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_spi_test_result, arg0, str_spi_test_arg0);
212 prog_char help_spi_test[] = "Test the SPI";
213 parse_pgm_inst_t cmd_spi_test = {
214 .f = cmd_spi_test_parsed, /* function to call */
215 .data = NULL, /* 2nd arg of func */
216 .help_str = help_spi_test,
217 .tokens = { /* token list, NULL terminated */
218 (prog_void *)&cmd_spi_test_arg0,
225 /**********************************************************/
228 /* this structure is filled when cmd_opponent is parsed successfully */
229 struct cmd_opponent_result {
236 /* function called when cmd_opponent is parsed successfully */
237 static void cmd_opponent_parsed(void *parsed_result, void *data)
241 if (get_opponent_xyda(&x, &y, &d, &a))
242 printf_P(PSTR("No opponent\r\n"));
244 printf_P(PSTR("x=%d y=%d, d=%d a=%d\r\n"), x, y, d, a);
247 prog_char str_opponent_arg0[] = "opponent";
248 parse_pgm_token_string_t cmd_opponent_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_opponent_result, arg0, str_opponent_arg0);
249 prog_char str_opponent_arg1[] = "show";
250 parse_pgm_token_string_t cmd_opponent_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_opponent_result, arg1, str_opponent_arg1);
252 prog_char help_opponent[] = "Show (x,y) opponent";
253 parse_pgm_inst_t cmd_opponent = {
254 .f = cmd_opponent_parsed, /* function to call */
255 .data = NULL, /* 2nd arg of func */
256 .help_str = help_opponent,
257 .tokens = { /* token list, NULL terminated */
258 (prog_void *)&cmd_opponent_arg0,
259 (prog_void *)&cmd_opponent_arg1,
265 prog_char str_opponent_arg1_set[] = "set";
266 parse_pgm_token_string_t cmd_opponent_arg1_set = TOKEN_STRING_INITIALIZER(struct cmd_opponent_result, arg1, str_opponent_arg1_set);
267 parse_pgm_token_num_t cmd_opponent_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_opponent_result, arg2, INT32);
268 parse_pgm_token_num_t cmd_opponent_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_opponent_result, arg3, INT32);
270 prog_char help_opponent_set[] = "Set (x,y) opponent";
271 parse_pgm_inst_t cmd_opponent_set = {
272 .f = cmd_opponent_parsed, /* function to call */
273 .data = NULL, /* 2nd arg of func */
274 .help_str = help_opponent_set,
275 .tokens = { /* token list, NULL terminated */
276 (prog_void *)&cmd_opponent_arg0,
277 (prog_void *)&cmd_opponent_arg1_set,
278 (prog_void *)&cmd_opponent_arg2,
279 (prog_void *)&cmd_opponent_arg3,
285 /**********************************************************/
288 /* this structure is filled when cmd_start is parsed successfully */
289 struct cmd_start_result {
291 fixed_string_t color;
292 fixed_string_t debug;
295 /* function called when cmd_start is parsed successfully */
296 static void cmd_start_parsed(void *parsed_result, void *data)
299 printf("not implemented\n");
301 struct cmd_start_result *res = parsed_result;
302 uint8_t old_level = gen.log_level;
304 gen.logs[NB_LOGS] = E_USER_STRAT;
305 if (!strcmp_P(res->debug, PSTR("debug"))) {
306 strat_infos.dump_enabled = 1;
310 strat_infos.dump_enabled = 0;
314 if (!strcmp_P(res->color, PSTR("yellow"))) {
315 mainboard.our_color = I2C_COLOR_YELLOW;
316 i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_YELLOW);
317 i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_YELLOW);
319 else if (!strcmp_P(res->color, PSTR("blue"))) {
320 mainboard.our_color = I2C_COLOR_BLUE;
321 i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_BLUE);
322 i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_BLUE);
327 gen.logs[NB_LOGS] = 0;
328 gen.log_level = old_level;
332 prog_char str_start_arg0[] = "start";
333 parse_pgm_token_string_t cmd_start_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_start_result, arg0, str_start_arg0);
334 prog_char str_start_color[] = "blue#yellow";
335 parse_pgm_token_string_t cmd_start_color = TOKEN_STRING_INITIALIZER(struct cmd_start_result, color, str_start_color);
336 prog_char str_start_debug[] = "debug#match";
337 parse_pgm_token_string_t cmd_start_debug = TOKEN_STRING_INITIALIZER(struct cmd_start_result, debug, str_start_debug);
339 prog_char help_start[] = "Start the robot";
340 parse_pgm_inst_t cmd_start = {
341 .f = cmd_start_parsed, /* function to call */
342 .data = NULL, /* 2nd arg of func */
343 .help_str = help_start,
344 .tokens = { /* token list, NULL terminated */
345 (prog_void *)&cmd_start_arg0,
346 (prog_void *)&cmd_start_color,
347 (prog_void *)&cmd_start_debug,
354 /**********************************************************/
357 /* this structure is filled when cmd_interact is parsed successfully */
358 struct cmd_interact_result {
362 static void print_cs(void)
364 printf_P(PSTR("cons_d=% .8"PRIi32" cons_a=% .8"PRIi32" fil_d=% .8"PRIi32" fil_a=% .8"PRIi32" "
365 "err_d=% .8"PRIi32" err_a=% .8"PRIi32" out_d=% .8"PRIi32" out_a=% .8"PRIi32"\r\n"),
366 cs_get_consign(&mainboard.distance.cs),
367 cs_get_consign(&mainboard.angle.cs),
368 cs_get_filtered_consign(&mainboard.distance.cs),
369 cs_get_filtered_consign(&mainboard.angle.cs),
370 cs_get_error(&mainboard.distance.cs),
371 cs_get_error(&mainboard.angle.cs),
372 cs_get_out(&mainboard.distance.cs),
373 cs_get_out(&mainboard.angle.cs));
376 static void print_pos(void)
378 printf_P(PSTR("x=% .8d y=% .8d a=% .8d\r\n"),
379 position_get_x_s16(&mainboard.pos),
380 position_get_y_s16(&mainboard.pos),
381 position_get_a_deg_s16(&mainboard.pos));
384 static void print_time(void)
386 printf_P(PSTR("time %d\r\n"),time_get_s());
390 static void print_sensors(void)
393 if (sensor_start_switch())
394 printf_P(PSTR("Start switch | "));
396 printf_P(PSTR(" | "));
398 if (IR_DISP_SENSOR())
399 printf_P(PSTR("IR disp | "));
401 printf_P(PSTR(" | "));
403 printf_P(PSTR("\r\n"));
407 static void print_pid(void)
409 printf_P(PSTR("P=% .8"PRIi32" I=% .8"PRIi32" D=% .8"PRIi32" out=% .8"PRIi32" | "
410 "P=% .8"PRIi32" I=% .8"PRIi32" D=% .8"PRIi32" out=% .8"PRIi32"\r\n"),
411 pid_get_value_in(&mainboard.distance.pid) * pid_get_gain_P(&mainboard.distance.pid),
412 pid_get_value_I(&mainboard.distance.pid) * pid_get_gain_I(&mainboard.distance.pid),
413 pid_get_value_D(&mainboard.distance.pid) * pid_get_gain_D(&mainboard.distance.pid),
414 pid_get_value_out(&mainboard.distance.pid),
415 pid_get_value_in(&mainboard.angle.pid) * pid_get_gain_P(&mainboard.angle.pid),
416 pid_get_value_I(&mainboard.angle.pid) * pid_get_gain_I(&mainboard.angle.pid),
417 pid_get_value_D(&mainboard.angle.pid) * pid_get_gain_D(&mainboard.angle.pid),
418 pid_get_value_out(&mainboard.angle.pid));
421 #define PRINT_POS (1<<0)
422 #define PRINT_PID (1<<1)
423 #define PRINT_CS (1<<2)
424 #define PRINT_SENSORS (1<<3)
425 #define PRINT_TIME (1<<4)
426 #define PRINT_BLOCKING (1<<5)
428 static void cmd_interact_parsed(void * parsed_result, void * data)
437 printf_P(PSTR("Display debugs:\r\n"
443 /* " 6:blocking\r\n" */
450 mainboard.flags &= (~DO_CS);
451 pwm_set_and_save(LEFT_PWM, 0);
452 pwm_set_and_save(RIGHT_PWM, 0);
455 if (print & PRINT_POS) {
459 if (print & PRINT_PID) {
463 if (print & PRINT_CS) {
467 if (print & PRINT_SENSORS) {
471 if (print & PRINT_TIME) {
474 /* if (print & PRINT_BLOCKING) { */
475 /* printf_P(PSTR("%s %s blocking=%d\r\n"), */
476 /* mainboard.blocking ? "BLOCK1":" ", */
477 /* rs_is_blocked(&mainboard.rs) ? "BLOCK2":" ", */
478 /* rs_get_blocking(&mainboard.rs)); */
481 c = cmdline_getchar();
486 cmd = vt100_parser(&vt100, c);
494 case '1': print ^= PRINT_POS; break;
495 case '2': print ^= PRINT_PID; break;
496 case '3': print ^= PRINT_CS; break;
497 case '4': print ^= PRINT_SENSORS; break;
498 case '5': print ^= PRINT_TIME; break;
499 case '6': print ^= PRINT_BLOCKING; break;
502 if (mainboard.flags & DO_CS)
504 pwm_set_and_save(LEFT_PWM, 0);
505 pwm_set_and_save(RIGHT_PWM, 0);
508 pwm_set_and_save(LEFT_PWM, 0);
509 pwm_set_and_save(RIGHT_PWM, 0);
518 pwm_set_and_save(LEFT_PWM, 1200);
519 pwm_set_and_save(RIGHT_PWM, 1200);
522 pwm_set_and_save(LEFT_PWM, -1200);
523 pwm_set_and_save(RIGHT_PWM, 1200);
526 pwm_set_and_save(LEFT_PWM, -1200);
527 pwm_set_and_save(RIGHT_PWM, -1200);
530 pwm_set_and_save(LEFT_PWM, 1200);
531 pwm_set_and_save(RIGHT_PWM, -1200);
539 prog_char str_interact_arg0[] = "interact";
540 parse_pgm_token_string_t cmd_interact_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_interact_result, arg0, str_interact_arg0);
542 prog_char help_interact[] = "Interactive mode";
543 parse_pgm_inst_t cmd_interact = {
544 .f = cmd_interact_parsed, /* function to call */
545 .data = NULL, /* 2nd arg of func */
546 .help_str = help_interact,
547 .tokens = { /* token list, NULL terminated */
548 (prog_void *)&cmd_interact_arg0,
554 /**********************************************************/
557 /* this structure is filled when cmd_color is parsed successfully */
558 struct cmd_color_result {
560 fixed_string_t color;
563 /* function called when cmd_color is parsed successfully */
564 static void cmd_color_parsed(void *parsed_result, void *data)
567 printf("not implemented\n");
569 struct cmd_color_result *res = (struct cmd_color_result *) parsed_result;
570 if (!strcmp_P(res->color, PSTR("yellow"))) {
571 mainboard.our_color = I2C_COLOR_YELLOW;
572 i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_YELLOW);
573 i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_YELLOW);
575 else if (!strcmp_P(res->color, PSTR("blue"))) {
576 mainboard.our_color = I2C_COLOR_BLUE;
577 i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_BLUE);
578 i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_BLUE);
580 printf_P(PSTR("Done\r\n"));
584 prog_char str_color_arg0[] = "color";
585 parse_pgm_token_string_t cmd_color_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_color_result, arg0, str_color_arg0);
586 prog_char str_color_color[] = "blue#yellow";
587 parse_pgm_token_string_t cmd_color_color = TOKEN_STRING_INITIALIZER(struct cmd_color_result, color, str_color_color);
589 prog_char help_color[] = "Set our color";
590 parse_pgm_inst_t cmd_color = {
591 .f = cmd_color_parsed, /* function to call */
592 .data = NULL, /* 2nd arg of func */
593 .help_str = help_color,
594 .tokens = { /* token list, NULL terminated */
595 (prog_void *)&cmd_color_arg0,
596 (prog_void *)&cmd_color_color,
602 /**********************************************************/
605 /* this structure is filled when cmd_rs is parsed successfully */
606 struct cmd_rs_result {
611 /* function called when cmd_rs is parsed successfully */
612 static void cmd_rs_parsed(void *parsed_result, void *data)
614 // struct cmd_rs_result *res = parsed_result;
616 printf_P(PSTR("angle cons=% .6"PRIi32" in=% .6"PRIi32" out=% .6"PRIi32" / "),
617 cs_get_consign(&mainboard.angle.cs),
618 cs_get_filtered_feedback(&mainboard.angle.cs),
619 cs_get_out(&mainboard.angle.cs));
620 printf_P(PSTR("distance cons=% .6"PRIi32" in=% .6"PRIi32" out=% .6"PRIi32" / "),
621 cs_get_consign(&mainboard.distance.cs),
622 cs_get_filtered_feedback(&mainboard.distance.cs),
623 cs_get_out(&mainboard.distance.cs));
624 printf_P(PSTR("l=% .4"PRIi32" r=% .4"PRIi32"\r\n"), mainboard.pwm_l,
627 } while(!cmdline_keypressed());
630 prog_char str_rs_arg0[] = "rs";
631 parse_pgm_token_string_t cmd_rs_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_rs_result, arg0, str_rs_arg0);
632 prog_char str_rs_arg1[] = "show";
633 parse_pgm_token_string_t cmd_rs_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_rs_result, arg1, str_rs_arg1);
635 prog_char help_rs[] = "Show rs (robot system) values";
636 parse_pgm_inst_t cmd_rs = {
637 .f = cmd_rs_parsed, /* function to call */
638 .data = NULL, /* 2nd arg of func */
640 .tokens = { /* token list, NULL terminated */
641 (prog_void *)&cmd_rs_arg0,
642 (prog_void *)&cmd_rs_arg1,
647 /**********************************************************/
650 /* this structure is filled when cmd_i2cdebug is parsed successfully */
651 struct cmd_i2cdebug_result {
655 /* function called when cmd_i2cdebug is parsed successfully */
656 static void cmd_i2cdebug_parsed(void * parsed_result, void * data)
659 printf("not implemented\n");
662 i2c_protocol_debug();
666 prog_char str_i2cdebug_arg0[] = "i2cdebug";
667 parse_pgm_token_string_t cmd_i2cdebug_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_i2cdebug_result, arg0, str_i2cdebug_arg0);
669 prog_char help_i2cdebug[] = "I2c debug infos";
670 parse_pgm_inst_t cmd_i2cdebug = {
671 .f = cmd_i2cdebug_parsed, /* function to call */
672 .data = NULL, /* 2nd arg of func */
673 .help_str = help_i2cdebug,
674 .tokens = { /* token list, NULL terminated */
675 (prog_void *)&cmd_i2cdebug_arg0,
680 /**********************************************************/
683 /* this structure is filled when cmd_cobboard_show is parsed successfully */
684 struct cmd_cobboard_show_result {
689 /* function called when cmd_cobboard_show is parsed successfully */
690 static void cmd_cobboard_show_parsed(void * parsed_result, void * data)
693 printf("not implemented\n");
695 printf_P(PSTR("mode = %x\r\n"), cobboard.mode);
696 printf_P(PSTR("status = %x\r\n"), cobboard.status);
697 printf_P(PSTR("cob_count = %x\r\n"), cobboard.cob_count);
698 printf_P(PSTR("left_cobroller_speed = %d\r\n"), cobboard.left_cobroller_speed);
699 printf_P(PSTR("right_cobroller_speed = %d\r\n"), cobboard.right_cobroller_speed);
703 prog_char str_cobboard_show_arg0[] = "cobboard";
704 parse_pgm_token_string_t cmd_cobboard_show_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_show_result, arg0, str_cobboard_show_arg0);
705 prog_char str_cobboard_show_arg1[] = "show";
706 parse_pgm_token_string_t cmd_cobboard_show_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_show_result, arg1, str_cobboard_show_arg1);
708 prog_char help_cobboard_show[] = "show cobboard status";
709 parse_pgm_inst_t cmd_cobboard_show = {
710 .f = cmd_cobboard_show_parsed, /* function to call */
711 .data = NULL, /* 2nd arg of func */
712 .help_str = help_cobboard_show,
713 .tokens = { /* token list, NULL terminated */
714 (prog_void *)&cmd_cobboard_show_arg0,
715 (prog_void *)&cmd_cobboard_show_arg1,
720 /**********************************************************/
721 /* Cobboard_Setmode1 */
723 /* this structure is filled when cmd_cobboard_setmode1 is parsed successfully */
724 struct cmd_cobboard_setmode1_result {
729 /* function called when cmd_cobboard_setmode1 is parsed successfully */
730 static void cmd_cobboard_setmode1_parsed(void *parsed_result, void *data)
732 struct cmd_cobboard_setmode1_result *res = parsed_result;
734 if (!strcmp_P(res->arg1, PSTR("init")))
735 i2c_cobboard_mode_init();
736 else if (!strcmp_P(res->arg1, PSTR("eject")))
737 i2c_cobboard_mode_eject();
740 prog_char str_cobboard_setmode1_arg0[] = "cobboard";
741 parse_pgm_token_string_t cmd_cobboard_setmode1_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode1_result, arg0, str_cobboard_setmode1_arg0);
742 prog_char str_cobboard_setmode1_arg1[] = "init#eject";
743 parse_pgm_token_string_t cmd_cobboard_setmode1_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode1_result, arg1, str_cobboard_setmode1_arg1);
745 prog_char help_cobboard_setmode1[] = "set cobboard mode (mode)";
746 parse_pgm_inst_t cmd_cobboard_setmode1 = {
747 .f = cmd_cobboard_setmode1_parsed, /* function to call */
748 .data = NULL, /* 2nd arg of func */
749 .help_str = help_cobboard_setmode1,
750 .tokens = { /* token list, NULL terminated */
751 (prog_void *)&cmd_cobboard_setmode1_arg0,
752 (prog_void *)&cmd_cobboard_setmode1_arg1,
757 /**********************************************************/
758 /* Cobboard_Setmode2 */
760 /* this structure is filled when cmd_cobboard_setmode2 is parsed successfully */
761 struct cmd_cobboard_setmode2_result {
767 /* function called when cmd_cobboard_setmode2 is parsed successfully */
768 static void cmd_cobboard_setmode2_parsed(void * parsed_result, void * data)
770 struct cmd_cobboard_setmode2_result *res = parsed_result;
771 uint8_t side = I2C_LEFT_SIDE;
773 if (!strcmp_P(res->arg2, PSTR("left")))
774 side = I2C_LEFT_SIDE;
775 else if (!strcmp_P(res->arg2, PSTR("right")))
776 side = I2C_RIGHT_SIDE;
778 if (!strcmp_P(res->arg1, PSTR("deploy")))
779 i2c_cobboard_mode_deploy(side);
780 else if (!strcmp_P(res->arg1, PSTR("harvest")))
781 i2c_cobboard_mode_harvest(side);
782 else if (!strcmp_P(res->arg1, PSTR("pack")))
783 i2c_cobboard_mode_pack(side);
786 prog_char str_cobboard_setmode2_arg0[] = "cobboard";
787 parse_pgm_token_string_t cmd_cobboard_setmode2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode2_result, arg0, str_cobboard_setmode2_arg0);
788 prog_char str_cobboard_setmode2_arg1[] = "harvest#deploy#pack";
789 parse_pgm_token_string_t cmd_cobboard_setmode2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode2_result, arg1, str_cobboard_setmode2_arg1);
790 prog_char str_cobboard_setmode2_arg2[] = "left#right";
791 parse_pgm_token_string_t cmd_cobboard_setmode2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode2_result, arg2, str_cobboard_setmode2_arg2);
793 prog_char help_cobboard_setmode2[] = "set cobboard mode (mode, side)";
794 parse_pgm_inst_t cmd_cobboard_setmode2 = {
795 .f = cmd_cobboard_setmode2_parsed, /* function to call */
796 .data = NULL, /* 2nd arg of func */
797 .help_str = help_cobboard_setmode2,
798 .tokens = { /* token list, NULL terminated */
799 (prog_void *)&cmd_cobboard_setmode2_arg0,
800 (prog_void *)&cmd_cobboard_setmode2_arg1,
801 (prog_void *)&cmd_cobboard_setmode2_arg2,
806 /**********************************************************/
807 /* Cobboard_Setmode3 */
809 /* this structure is filled when cmd_cobboard_setmode3 is parsed successfully */
810 struct cmd_cobboard_setmode3_result {
816 /* function called when cmd_cobboard_setmode3 is parsed successfully */
817 static void cmd_cobboard_setmode3_parsed(void *parsed_result, void *data)
819 struct cmd_cobboard_setmode3_result *res = parsed_result;
820 if (!strcmp_P(res->arg1, PSTR("xxx")))
824 prog_char str_cobboard_setmode3_arg0[] = "cobboard";
825 parse_pgm_token_string_t cmd_cobboard_setmode3_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode3_result, arg0, str_cobboard_setmode3_arg0);
826 prog_char str_cobboard_setmode3_arg1[] = "xxx";
827 parse_pgm_token_string_t cmd_cobboard_setmode3_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode3_result, arg1, str_cobboard_setmode3_arg1);
828 parse_pgm_token_num_t cmd_cobboard_setmode3_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_cobboard_setmode3_result, level, UINT8);
830 prog_char help_cobboard_setmode3[] = "set cobboard mode (mode, level)";
831 parse_pgm_inst_t cmd_cobboard_setmode3 = {
832 .f = cmd_cobboard_setmode3_parsed, /* function to call */
833 .data = NULL, /* 2nd arg of func */
834 .help_str = help_cobboard_setmode3,
835 .tokens = { /* token list, NULL terminated */
836 (prog_void *)&cmd_cobboard_setmode3_arg0,
837 (prog_void *)&cmd_cobboard_setmode3_arg1,
838 (prog_void *)&cmd_cobboard_setmode3_arg2,
843 /**********************************************************/
846 /* this structure is filled when cmd_ballboard_show is parsed successfully */
847 struct cmd_ballboard_show_result {
852 /* function called when cmd_ballboard_show is parsed successfully */
853 static void cmd_ballboard_show_parsed(void * parsed_result, void * data)
856 printf("not implemented\n");
858 printf_P(PSTR("mode = %x\r\n"), ballboard.mode);
859 printf_P(PSTR("status = %x\r\n"), ballboard.status);
860 printf_P(PSTR("ball_count = %d\r\n"), ballboard.ball_count);
864 prog_char str_ballboard_show_arg0[] = "ballboard";
865 parse_pgm_token_string_t cmd_ballboard_show_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_show_result, arg0, str_ballboard_show_arg0);
866 prog_char str_ballboard_show_arg1[] = "show";
867 parse_pgm_token_string_t cmd_ballboard_show_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_show_result, arg1, str_ballboard_show_arg1);
869 prog_char help_ballboard_show[] = "show ballboard status";
870 parse_pgm_inst_t cmd_ballboard_show = {
871 .f = cmd_ballboard_show_parsed, /* function to call */
872 .data = NULL, /* 2nd arg of func */
873 .help_str = help_ballboard_show,
874 .tokens = { /* token list, NULL terminated */
875 (prog_void *)&cmd_ballboard_show_arg0,
876 (prog_void *)&cmd_ballboard_show_arg1,
881 /**********************************************************/
882 /* Ballboard_Setmode1 */
884 /* this structure is filled when cmd_ballboard_setmode1 is parsed successfully */
885 struct cmd_ballboard_setmode1_result {
890 /* function called when cmd_ballboard_setmode1 is parsed successfully */
891 static void cmd_ballboard_setmode1_parsed(void *parsed_result, void *data)
893 struct cmd_ballboard_setmode1_result *res = parsed_result;
895 if (!strcmp_P(res->arg1, PSTR("init")))
896 i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_INIT);
897 else if (!strcmp_P(res->arg1, PSTR("off")))
898 i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_OFF);
899 else if (!strcmp_P(res->arg1, PSTR("eject")))
900 i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_EJECT);
901 else if (!strcmp_P(res->arg1, PSTR("harvest")))
902 i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_HARVEST);
907 prog_char str_ballboard_setmode1_arg0[] = "ballboard";
908 parse_pgm_token_string_t cmd_ballboard_setmode1_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode1_result, arg0, str_ballboard_setmode1_arg0);
909 prog_char str_ballboard_setmode1_arg1[] = "init#eject#harvest#off";
910 parse_pgm_token_string_t cmd_ballboard_setmode1_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode1_result, arg1, str_ballboard_setmode1_arg1);
912 prog_char help_ballboard_setmode1[] = "set ballboard mode (mode)";
913 parse_pgm_inst_t cmd_ballboard_setmode1 = {
914 .f = cmd_ballboard_setmode1_parsed, /* function to call */
915 .data = NULL, /* 2nd arg of func */
916 .help_str = help_ballboard_setmode1,
917 .tokens = { /* token list, NULL terminated */
918 (prog_void *)&cmd_ballboard_setmode1_arg0,
919 (prog_void *)&cmd_ballboard_setmode1_arg1,
924 /**********************************************************/
925 /* Ballboard_Setmode2 */
927 /* this structure is filled when cmd_ballboard_setmode2 is parsed successfully */
928 struct cmd_ballboard_setmode2_result {
934 /* function called when cmd_ballboard_setmode2 is parsed successfully */
935 static void cmd_ballboard_setmode2_parsed(void * parsed_result, void * data)
937 struct cmd_ballboard_setmode2_result *res = parsed_result;
938 uint8_t mode = I2C_BALLBOARD_MODE_INIT;
940 if (!strcmp_P(res->arg2, PSTR("left"))) {
941 if (!strcmp_P(res->arg1, PSTR("prepare")))
942 mode = I2C_BALLBOARD_MODE_PREP_L_FORK;
943 else if (!strcmp_P(res->arg1, PSTR("take")))
944 mode = I2C_BALLBOARD_MODE_TAKE_L_FORK;
947 if (!strcmp_P(res->arg1, PSTR("prepare")))
948 mode = I2C_BALLBOARD_MODE_PREP_R_FORK;
949 else if (!strcmp_P(res->arg1, PSTR("take")))
950 mode = I2C_BALLBOARD_MODE_TAKE_R_FORK;
952 i2c_ballboard_set_mode(mode);
955 prog_char str_ballboard_setmode2_arg0[] = "ballboard";
956 parse_pgm_token_string_t cmd_ballboard_setmode2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode2_result, arg0, str_ballboard_setmode2_arg0);
957 prog_char str_ballboard_setmode2_arg1[] = "prepare#take";
958 parse_pgm_token_string_t cmd_ballboard_setmode2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode2_result, arg1, str_ballboard_setmode2_arg1);
959 prog_char str_ballboard_setmode2_arg2[] = "left#right";
960 parse_pgm_token_string_t cmd_ballboard_setmode2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode2_result, arg2, str_ballboard_setmode2_arg2);
962 prog_char help_ballboard_setmode2[] = "set ballboard mode (mode, side)";
963 parse_pgm_inst_t cmd_ballboard_setmode2 = {
964 .f = cmd_ballboard_setmode2_parsed, /* function to call */
965 .data = NULL, /* 2nd arg of func */
966 .help_str = help_ballboard_setmode2,
967 .tokens = { /* token list, NULL terminated */
968 (prog_void *)&cmd_ballboard_setmode2_arg0,
969 (prog_void *)&cmd_ballboard_setmode2_arg1,
970 (prog_void *)&cmd_ballboard_setmode2_arg2,
975 /**********************************************************/
976 /* Ballboard_Setmode3 */
978 /* this structure is filled when cmd_ballboard_setmode3 is parsed successfully */
979 struct cmd_ballboard_setmode3_result {
985 /* function called when cmd_ballboard_setmode3 is parsed successfully */
986 static void cmd_ballboard_setmode3_parsed(void *parsed_result, void *data)
988 struct cmd_ballboard_setmode3_result *res = parsed_result;
989 if (!strcmp_P(res->arg1, PSTR("xxx")))
993 prog_char str_ballboard_setmode3_arg0[] = "ballboard";
994 parse_pgm_token_string_t cmd_ballboard_setmode3_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode3_result, arg0, str_ballboard_setmode3_arg0);
995 prog_char str_ballboard_setmode3_arg1[] = "xxx";
996 parse_pgm_token_string_t cmd_ballboard_setmode3_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode3_result, arg1, str_ballboard_setmode3_arg1);
997 parse_pgm_token_num_t cmd_ballboard_setmode3_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_ballboard_setmode3_result, level, UINT8);
999 prog_char help_ballboard_setmode3[] = "set ballboard mode (mode, level)";
1000 parse_pgm_inst_t cmd_ballboard_setmode3 = {
1001 .f = cmd_ballboard_setmode3_parsed, /* function to call */
1002 .data = NULL, /* 2nd arg of func */
1003 .help_str = help_ballboard_setmode3,
1004 .tokens = { /* token list, NULL terminated */
1005 (prog_void *)&cmd_ballboard_setmode3_arg0,
1006 (prog_void *)&cmd_ballboard_setmode3_arg1,
1007 (prog_void *)&cmd_ballboard_setmode3_arg2,
1012 /**********************************************************/
1015 /* this structure is filled when cmd_servo_balls is parsed successfully */
1016 struct cmd_servo_balls_result {
1017 fixed_string_t arg0;
1018 fixed_string_t arg1;
1021 /* function called when cmd_servo_balls is parsed successfully */
1022 static void cmd_servo_balls_parsed(void *parsed_result,
1023 __attribute__((unused)) void *data)
1025 struct cmd_servo_balls_result *res = parsed_result;
1027 if (!strcmp_P(res->arg1, PSTR("deploy")))
1028 support_balls_deploy();
1029 else if (!strcmp_P(res->arg1, PSTR("pack")))
1030 support_balls_pack();
1033 prog_char str_servo_balls_arg0[] = "support_balls";
1034 parse_pgm_token_string_t cmd_servo_balls_arg0 =
1035 TOKEN_STRING_INITIALIZER(struct cmd_servo_balls_result, arg0, str_servo_balls_arg0);
1036 prog_char str_servo_balls_arg1[] = "deploy#pack";
1037 parse_pgm_token_string_t cmd_servo_balls_arg1 =
1038 TOKEN_STRING_INITIALIZER(struct cmd_servo_balls_result, arg1, str_servo_balls_arg1);
1040 prog_char help_servo_balls[] = "control support balls";
1041 parse_pgm_inst_t cmd_servo_balls = {
1042 .f = cmd_servo_balls_parsed, /* function to call */
1043 .data = NULL, /* 2nd arg of func */
1044 .help_str = help_servo_balls,
1045 .tokens = { /* token list, NULL terminated */
1046 (prog_void *)&cmd_servo_balls_arg0,
1047 (prog_void *)&cmd_servo_balls_arg1,
1052 /**********************************************************/
1055 /* this structure is filled when cmd_clitoid is parsed successfully */
1056 struct cmd_clitoid_result {
1057 fixed_string_t arg0;
1066 /* function called when cmd_test is parsed successfully */
1067 static void cmd_clitoid_parsed(void *parsed_result, void *data)
1069 struct cmd_clitoid_result *res = parsed_result;
1070 /* clitoid(res->alpha_deg, res->beta_deg, res->R_mm, */
1071 /* res->Vd, res->Amax, res->d_inter_mm); */
1072 double x = position_get_x_double(&mainboard.pos);
1073 double y = position_get_y_double(&mainboard.pos);
1074 double a = position_get_a_rad_double(&mainboard.pos);
1076 strat_set_speed(res->Vd, SPEED_ANGLE_FAST);
1077 trajectory_clitoid(&mainboard.traj, x, y, a, 150.,
1078 res->alpha_deg, res->beta_deg, res->R_mm,
1082 prog_char str_clitoid_arg0[] = "clitoid";
1083 parse_pgm_token_string_t cmd_clitoid_arg0 =
1084 TOKEN_STRING_INITIALIZER(struct cmd_clitoid_result,
1085 arg0, str_clitoid_arg0);
1086 parse_pgm_token_num_t cmd_clitoid_alpha_deg =
1087 TOKEN_NUM_INITIALIZER(struct cmd_clitoid_result,
1089 parse_pgm_token_num_t cmd_clitoid_beta_deg =
1090 TOKEN_NUM_INITIALIZER(struct cmd_clitoid_result,
1092 parse_pgm_token_num_t cmd_clitoid_R_mm =
1093 TOKEN_NUM_INITIALIZER(struct cmd_clitoid_result,
1095 parse_pgm_token_num_t cmd_clitoid_Vd =
1096 TOKEN_NUM_INITIALIZER(struct cmd_clitoid_result,
1098 parse_pgm_token_num_t cmd_clitoid_Amax =
1099 TOKEN_NUM_INITIALIZER(struct cmd_clitoid_result,
1101 parse_pgm_token_num_t cmd_clitoid_d_inter_mm =
1102 TOKEN_NUM_INITIALIZER(struct cmd_clitoid_result,
1105 prog_char help_clitoid[] = "do a clitoid (alpha, beta, R, Vd, Amax, d_inter)";
1106 parse_pgm_inst_t cmd_clitoid = {
1107 .f = cmd_clitoid_parsed, /* function to call */
1108 .data = NULL, /* 2nd arg of func */
1109 .help_str = help_clitoid,
1110 .tokens = { /* token list, NULL terminated */
1111 (prog_void *)&cmd_clitoid_arg0,
1112 (prog_void *)&cmd_clitoid_alpha_deg,
1113 (prog_void *)&cmd_clitoid_beta_deg,
1114 (prog_void *)&cmd_clitoid_R_mm,
1115 (prog_void *)&cmd_clitoid_Vd,
1116 (prog_void *)&cmd_clitoid_Amax,
1117 (prog_void *)&cmd_clitoid_d_inter_mm,
1122 /**********************************************************/
1125 /* this structure is filled when cmd_test is parsed successfully */
1126 struct cmd_test_result {
1127 fixed_string_t arg0;
1135 #define LINE_L_DOWN 3
1137 #define LINE_R_DOWN 5
1144 static void num2line(struct line_2pts *l, uint8_t dir, uint8_t num)
1151 l->p1.x = n * 450 + 375;
1152 l->p1.y = COLOR_Y(0);
1153 l->p2.x = n * 450 + 375;
1154 l->p2.y = COLOR_Y(2100);
1157 l->p1.x = n * 450 + 375;
1158 l->p1.y = COLOR_Y(2100);
1159 l->p2.x = n * 450 + 375;
1160 l->p2.y = COLOR_Y(0);
1164 l->p1.y = COLOR_Y(-n * 500 + 1472);
1166 l->p2.y = COLOR_Y((-n + 4) * 500 + 972);
1170 l->p1.y = COLOR_Y((-n + 4) * 500 + 972);
1172 l->p2.y = COLOR_Y(-n * 500 + 1472);
1176 l->p1.y = COLOR_Y(-n * 500 + 1472);
1178 l->p2.y = COLOR_Y((-n + 4) * 500 + 972);
1182 l->p1.y = COLOR_Y((-n + 4) * 500 + 972);
1184 l->p2.y = COLOR_Y(-n * 500 + 1472);
1192 static void reverse_line(struct line_2pts *l)
1205 static void line2line(uint8_t dir1, uint8_t num1,
1206 uint8_t dir2, uint8_t num2)
1208 double line1_a_rad, line1_a_deg, line2_a_rad;
1209 double diff_a_deg, diff_a_deg_abs, beta_deg;
1211 struct line_2pts l1, l2;
1215 /* convert to 2 points */
1216 num2line(&l1, dir1, num1);
1217 num2line(&l2, dir2, num2);
1219 printf_P(PSTR("A2 (%2.2f, %2.2f) -> (%2.2f, %2.2f)\r\n"),
1220 l1.p1.x, l1.p1.y, l1.p2.x, l1.p2.y);
1221 printf_P(PSTR("B2 (%2.2f, %2.2f) -> (%2.2f, %2.2f)\r\n"),
1222 l2.p1.x, l2.p1.y, l2.p2.x, l2.p2.y);
1224 /* convert to line eq and find intersection */
1225 pts2line(&l1.p1, &l1.p2, &ll1);
1226 pts2line(&l2.p1, &l2.p2, &ll2);
1227 intersect_line(&ll1, &ll2, &p);
1229 line1_a_rad = atan2(l1.p2.y - l1.p1.y,
1231 line1_a_deg = DEG(line1_a_rad);
1232 line2_a_rad = atan2(l2.p2.y - l2.p1.y,
1234 diff_a_deg = DEG(line2_a_rad - line1_a_rad);
1235 diff_a_deg_abs = fabs(diff_a_deg);
1237 if (diff_a_deg_abs < 70.) {
1244 else if (diff_a_deg_abs < 100.) {
1258 trajectory_clitoid(&mainboard.traj, l1.p1.x, l1.p1.y,
1259 line1_a_deg, 150., diff_a_deg, beta_deg,
1260 radius, xy_norm(l1.p1.x, l1.p1.y,
1262 wait_traj_end(0xFF);
1265 /* function called when cmd_test is parsed successfully */
1266 static void cmd_test_parsed(void *parsed_result, void *data)
1269 strat_reset_pos(298.48, 309.21, 70.02);
1270 mainboard.angle.on = 1;
1271 mainboard.distance.on = 1;
1275 line2line(LINE_UP, 0, LINE_R_DOWN, 2);
1276 line2line(LINE_R_DOWN, 2, LINE_R_UP, 1);
1278 trajectory_hardstop(&mainboard.traj);
1281 prog_char str_test_arg0[] = "test";
1282 parse_pgm_token_string_t cmd_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_test_result, arg0, str_test_arg0);
1283 parse_pgm_token_num_t cmd_test_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_test_result, radius, INT32);
1284 parse_pgm_token_num_t cmd_test_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_test_result, dist, INT32);
1286 prog_char help_test[] = "Test function";
1287 parse_pgm_inst_t cmd_test = {
1288 .f = cmd_test_parsed, /* function to call */
1289 .data = NULL, /* 2nd arg of func */
1290 .help_str = help_test,
1291 .tokens = { /* token list, NULL terminated */
1292 (prog_void *)&cmd_test_arg0,