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"
70 #include "strat_corn.h"
71 #include "i2c_protocol.h"
74 struct cmd_event_result {
81 /* function called when cmd_event is parsed successfully */
82 static void cmd_event_parsed(void *parsed_result, void *data)
86 struct cmd_event_result * res = parsed_result;
88 if (!strcmp_P(res->arg1, PSTR("all"))) {
90 if (!strcmp_P(res->arg2, PSTR("on")))
91 mainboard.flags |= bit;
92 else if (!strcmp_P(res->arg2, PSTR("off")))
93 mainboard.flags &= bit;
95 printf_P(PSTR("encoders is %s\r\n"),
96 (DO_ENCODERS & mainboard.flags) ? "on":"off");
97 printf_P(PSTR("cs is %s\r\n"),
98 (DO_CS & mainboard.flags) ? "on":"off");
99 printf_P(PSTR("rs is %s\r\n"),
100 (DO_RS & mainboard.flags) ? "on":"off");
101 printf_P(PSTR("pos is %s\r\n"),
102 (DO_POS & mainboard.flags) ? "on":"off");
103 printf_P(PSTR("bd is %s\r\n"),
104 (DO_BD & mainboard.flags) ? "on":"off");
105 printf_P(PSTR("timer is %s\r\n"),
106 (DO_TIMER & mainboard.flags) ? "on":"off");
107 printf_P(PSTR("power is %s\r\n"),
108 (DO_POWER & mainboard.flags) ? "on":"off");
109 printf_P(PSTR("errblock is %s\r\n"),
110 (DO_ERRBLOCKING & mainboard.flags) ? "on":"off");
115 if (!strcmp_P(res->arg1, PSTR("encoders")))
117 else if (!strcmp_P(res->arg1, PSTR("cs"))) {
121 else if (!strcmp_P(res->arg1, PSTR("rs")))
123 else if (!strcmp_P(res->arg1, PSTR("pos")))
125 else if (!strcmp_P(res->arg1, PSTR("bd")))
127 else if (!strcmp_P(res->arg1, PSTR("timer"))) {
131 else if (!strcmp_P(res->arg1, PSTR("power")))
133 else if (!strcmp_P(res->arg1, PSTR("errblock")))
134 bit = DO_ERRBLOCKING;
136 if (!strcmp_P(res->arg2, PSTR("on")))
137 mainboard.flags |= bit;
138 else if (!strcmp_P(res->arg2, PSTR("off"))) {
139 if (!strcmp_P(res->arg1, PSTR("cs"))) {
141 robotsim_pwm(LEFT_PWM, 0);
142 robotsim_pwm(RIGHT_PWM, 0);
144 pwm_ng_set(LEFT_PWM, 0);
145 pwm_ng_set(RIGHT_PWM, 0);
148 mainboard.flags &= (~bit);
150 printf_P(PSTR("%s is %s\r\n"), res->arg1,
151 (bit & mainboard.flags) ? "on":"off");
154 prog_char str_event_arg0[] = "event";
155 parse_pgm_token_string_t cmd_event_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg0, str_event_arg0);
156 prog_char str_event_arg1[] = "all#encoders#cs#rs#pos#bd#timer#power#errblock";
157 parse_pgm_token_string_t cmd_event_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg1, str_event_arg1);
158 prog_char str_event_arg2[] = "on#off#show";
159 parse_pgm_token_string_t cmd_event_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg2, str_event_arg2);
161 prog_char help_event[] = "Enable/disable events";
162 parse_pgm_inst_t cmd_event = {
163 .f = cmd_event_parsed, /* function to call */
164 .data = NULL, /* 2nd arg of func */
165 .help_str = help_event,
166 .tokens = { /* token list, NULL terminated */
167 (prog_void *)&cmd_event_arg0,
168 (prog_void *)&cmd_event_arg1,
169 (prog_void *)&cmd_event_arg2,
175 /**********************************************************/
178 /* this structure is filled when cmd_spi_test is parsed successfully */
179 struct cmd_spi_test_result {
183 /* function called when cmd_spi_test is parsed successfully */
184 static void cmd_spi_test_parsed(void * parsed_result, void * data)
187 printf("not implemented\n");
189 uint16_t i = 0, ret = 0, ret2 = 0;
191 if (mainboard.flags & DO_ENCODERS) {
192 printf_P(PSTR("Disable encoder event first\r\n"));
198 ret = spi_send_and_receive_byte(i);
199 ret2 = spi_send_and_receive_byte(i);
200 spi_slave_deselect(0);
202 if ((i & 0x7ff) == 0)
203 printf_P(PSTR("Sent %.4x twice, received %x %x\r\n"),
207 } while(!cmdline_keypressed());
211 prog_char str_spi_test_arg0[] = "spi_test";
212 parse_pgm_token_string_t cmd_spi_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_spi_test_result, arg0, str_spi_test_arg0);
214 prog_char help_spi_test[] = "Test the SPI";
215 parse_pgm_inst_t cmd_spi_test = {
216 .f = cmd_spi_test_parsed, /* function to call */
217 .data = NULL, /* 2nd arg of func */
218 .help_str = help_spi_test,
219 .tokens = { /* token list, NULL terminated */
220 (prog_void *)&cmd_spi_test_arg0,
227 /**********************************************************/
230 /* this structure is filled when cmd_opponent is parsed successfully */
231 struct cmd_opponent_result {
238 /* function called when cmd_opponent is parsed successfully */
239 static void cmd_opponent_parsed(void *parsed_result, void *data)
243 if (get_opponent_xyda(&x, &y, &d, &a))
244 printf_P(PSTR("No opponent\r\n"));
246 printf_P(PSTR("x=%d y=%d, d=%d a=%d\r\n"), x, y, d, a);
249 prog_char str_opponent_arg0[] = "opponent";
250 parse_pgm_token_string_t cmd_opponent_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_opponent_result, arg0, str_opponent_arg0);
251 prog_char str_opponent_arg1[] = "show";
252 parse_pgm_token_string_t cmd_opponent_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_opponent_result, arg1, str_opponent_arg1);
254 prog_char help_opponent[] = "Show (x,y) opponent";
255 parse_pgm_inst_t cmd_opponent = {
256 .f = cmd_opponent_parsed, /* function to call */
257 .data = NULL, /* 2nd arg of func */
258 .help_str = help_opponent,
259 .tokens = { /* token list, NULL terminated */
260 (prog_void *)&cmd_opponent_arg0,
261 (prog_void *)&cmd_opponent_arg1,
267 prog_char str_opponent_arg1_set[] = "set";
268 parse_pgm_token_string_t cmd_opponent_arg1_set = TOKEN_STRING_INITIALIZER(struct cmd_opponent_result, arg1, str_opponent_arg1_set);
269 parse_pgm_token_num_t cmd_opponent_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_opponent_result, arg2, INT32);
270 parse_pgm_token_num_t cmd_opponent_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_opponent_result, arg3, INT32);
272 prog_char help_opponent_set[] = "Set (x,y) opponent";
273 parse_pgm_inst_t cmd_opponent_set = {
274 .f = cmd_opponent_parsed, /* function to call */
275 .data = NULL, /* 2nd arg of func */
276 .help_str = help_opponent_set,
277 .tokens = { /* token list, NULL terminated */
278 (prog_void *)&cmd_opponent_arg0,
279 (prog_void *)&cmd_opponent_arg1_set,
280 (prog_void *)&cmd_opponent_arg2,
281 (prog_void *)&cmd_opponent_arg3,
287 /**********************************************************/
290 /* this structure is filled when cmd_start is parsed successfully */
291 struct cmd_start_result {
293 fixed_string_t color;
294 fixed_string_t debug;
297 /* function called when cmd_start is parsed successfully */
298 static void cmd_start_parsed(void *parsed_result, void *data)
300 struct cmd_start_result *res = parsed_result;
301 uint8_t old_level = gen.log_level;
303 gen.logs[NB_LOGS] = E_USER_STRAT;
304 if (!strcmp_P(res->debug, PSTR("debug"))) {
305 strat_db.dump_enabled = 1;
309 strat_db.dump_enabled = 0;
313 if (!strcmp_P(res->color, PSTR("yellow"))) {
314 mainboard.our_color = I2C_COLOR_YELLOW;
315 i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_YELLOW);
316 i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_YELLOW);
318 else if (!strcmp_P(res->color, PSTR("blue"))) {
319 mainboard.our_color = I2C_COLOR_BLUE;
320 i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_BLUE);
321 i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_BLUE);
326 gen.logs[NB_LOGS] = 0;
327 gen.log_level = old_level;
330 prog_char str_start_arg0[] = "start";
331 parse_pgm_token_string_t cmd_start_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_start_result, arg0, str_start_arg0);
332 prog_char str_start_color[] = "blue#yellow";
333 parse_pgm_token_string_t cmd_start_color = TOKEN_STRING_INITIALIZER(struct cmd_start_result, color, str_start_color);
334 prog_char str_start_debug[] = "debug#match";
335 parse_pgm_token_string_t cmd_start_debug = TOKEN_STRING_INITIALIZER(struct cmd_start_result, debug, str_start_debug);
337 prog_char help_start[] = "Start the robot";
338 parse_pgm_inst_t cmd_start = {
339 .f = cmd_start_parsed, /* function to call */
340 .data = NULL, /* 2nd arg of func */
341 .help_str = help_start,
342 .tokens = { /* token list, NULL terminated */
343 (prog_void *)&cmd_start_arg0,
344 (prog_void *)&cmd_start_color,
345 (prog_void *)&cmd_start_debug,
352 /**********************************************************/
355 /* this structure is filled when cmd_interact is parsed successfully */
356 struct cmd_interact_result {
360 static void print_cs(void)
362 printf_P(PSTR("cons_d=% .8"PRIi32" cons_a=% .8"PRIi32" fil_d=% .8"PRIi32" fil_a=% .8"PRIi32" "
363 "err_d=% .8"PRIi32" err_a=% .8"PRIi32" out_d=% .8"PRIi32" out_a=% .8"PRIi32"\r\n"),
364 cs_get_consign(&mainboard.distance.cs),
365 cs_get_consign(&mainboard.angle.cs),
366 cs_get_filtered_consign(&mainboard.distance.cs),
367 cs_get_filtered_consign(&mainboard.angle.cs),
368 cs_get_error(&mainboard.distance.cs),
369 cs_get_error(&mainboard.angle.cs),
370 cs_get_out(&mainboard.distance.cs),
371 cs_get_out(&mainboard.angle.cs));
374 static void print_pos(void)
376 printf_P(PSTR("x=% .8d y=% .8d a=% .8d\r\n"),
377 position_get_x_s16(&mainboard.pos),
378 position_get_y_s16(&mainboard.pos),
379 position_get_a_deg_s16(&mainboard.pos));
382 static void print_time(void)
384 printf_P(PSTR("time %d\r\n"),time_get_s());
388 static void print_sensors(void)
391 if (sensor_start_switch())
392 printf_P(PSTR("Start switch | "));
394 printf_P(PSTR(" | "));
396 if (IR_DISP_SENSOR())
397 printf_P(PSTR("IR disp | "));
399 printf_P(PSTR(" | "));
401 printf_P(PSTR("\r\n"));
405 static void print_pid(void)
407 printf_P(PSTR("P=% .8"PRIi32" I=% .8"PRIi32" D=% .8"PRIi32" out=% .8"PRIi32" | "
408 "P=% .8"PRIi32" I=% .8"PRIi32" D=% .8"PRIi32" out=% .8"PRIi32"\r\n"),
409 pid_get_value_in(&mainboard.distance.pid) * pid_get_gain_P(&mainboard.distance.pid),
410 pid_get_value_I(&mainboard.distance.pid) * pid_get_gain_I(&mainboard.distance.pid),
411 pid_get_value_D(&mainboard.distance.pid) * pid_get_gain_D(&mainboard.distance.pid),
412 pid_get_value_out(&mainboard.distance.pid),
413 pid_get_value_in(&mainboard.angle.pid) * pid_get_gain_P(&mainboard.angle.pid),
414 pid_get_value_I(&mainboard.angle.pid) * pid_get_gain_I(&mainboard.angle.pid),
415 pid_get_value_D(&mainboard.angle.pid) * pid_get_gain_D(&mainboard.angle.pid),
416 pid_get_value_out(&mainboard.angle.pid));
419 #define PRINT_POS (1<<0)
420 #define PRINT_PID (1<<1)
421 #define PRINT_CS (1<<2)
422 #define PRINT_SENSORS (1<<3)
423 #define PRINT_TIME (1<<4)
424 #define PRINT_BLOCKING (1<<5)
426 static void cmd_interact_parsed(void * parsed_result, void * data)
435 printf_P(PSTR("Display debugs:\r\n"
441 /* " 6:blocking\r\n" */
448 mainboard.flags &= (~DO_CS);
449 pwm_set_and_save(LEFT_PWM, 0);
450 pwm_set_and_save(RIGHT_PWM, 0);
453 if (print & PRINT_POS) {
457 if (print & PRINT_PID) {
461 if (print & PRINT_CS) {
465 if (print & PRINT_SENSORS) {
469 if (print & PRINT_TIME) {
472 /* if (print & PRINT_BLOCKING) { */
473 /* printf_P(PSTR("%s %s blocking=%d\r\n"), */
474 /* mainboard.blocking ? "BLOCK1":" ", */
475 /* rs_is_blocked(&mainboard.rs) ? "BLOCK2":" ", */
476 /* rs_get_blocking(&mainboard.rs)); */
479 c = cmdline_getchar();
484 cmd = vt100_parser(&vt100, c);
492 case '1': print ^= PRINT_POS; break;
493 case '2': print ^= PRINT_PID; break;
494 case '3': print ^= PRINT_CS; break;
495 case '4': print ^= PRINT_SENSORS; break;
496 case '5': print ^= PRINT_TIME; break;
497 case '6': print ^= PRINT_BLOCKING; break;
500 if (mainboard.flags & DO_CS)
502 pwm_set_and_save(LEFT_PWM, 0);
503 pwm_set_and_save(RIGHT_PWM, 0);
506 pwm_set_and_save(LEFT_PWM, 0);
507 pwm_set_and_save(RIGHT_PWM, 0);
515 #define PWM_INTERACT 300
517 #define PWM_INTERACT 1200
521 pwm_set_and_save(LEFT_PWM, PWM_INTERACT);
522 pwm_set_and_save(RIGHT_PWM, PWM_INTERACT);
525 pwm_set_and_save(LEFT_PWM, -PWM_INTERACT);
526 pwm_set_and_save(RIGHT_PWM, PWM_INTERACT);
529 pwm_set_and_save(LEFT_PWM, -PWM_INTERACT);
530 pwm_set_and_save(RIGHT_PWM, -PWM_INTERACT);
533 pwm_set_and_save(LEFT_PWM, PWM_INTERACT);
534 pwm_set_and_save(RIGHT_PWM, -PWM_INTERACT);
542 prog_char str_interact_arg0[] = "interact";
543 parse_pgm_token_string_t cmd_interact_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_interact_result, arg0, str_interact_arg0);
545 prog_char help_interact[] = "Interactive mode";
546 parse_pgm_inst_t cmd_interact = {
547 .f = cmd_interact_parsed, /* function to call */
548 .data = NULL, /* 2nd arg of func */
549 .help_str = help_interact,
550 .tokens = { /* token list, NULL terminated */
551 (prog_void *)&cmd_interact_arg0,
557 /**********************************************************/
560 /* this structure is filled when cmd_color is parsed successfully */
561 struct cmd_color_result {
563 fixed_string_t color;
566 /* function called when cmd_color is parsed successfully */
567 static void cmd_color_parsed(void *parsed_result, void *data)
570 printf("not implemented\n");
572 struct cmd_color_result *res = (struct cmd_color_result *) parsed_result;
573 if (!strcmp_P(res->color, PSTR("yellow"))) {
574 mainboard.our_color = I2C_COLOR_YELLOW;
575 i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_YELLOW);
576 i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_YELLOW);
578 else if (!strcmp_P(res->color, PSTR("blue"))) {
579 mainboard.our_color = I2C_COLOR_BLUE;
580 i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_BLUE);
581 i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_BLUE);
583 printf_P(PSTR("Done\r\n"));
587 prog_char str_color_arg0[] = "color";
588 parse_pgm_token_string_t cmd_color_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_color_result, arg0, str_color_arg0);
589 prog_char str_color_color[] = "blue#yellow";
590 parse_pgm_token_string_t cmd_color_color = TOKEN_STRING_INITIALIZER(struct cmd_color_result, color, str_color_color);
592 prog_char help_color[] = "Set our color";
593 parse_pgm_inst_t cmd_color = {
594 .f = cmd_color_parsed, /* function to call */
595 .data = NULL, /* 2nd arg of func */
596 .help_str = help_color,
597 .tokens = { /* token list, NULL terminated */
598 (prog_void *)&cmd_color_arg0,
599 (prog_void *)&cmd_color_color,
605 /**********************************************************/
608 /* this structure is filled when cmd_rs is parsed successfully */
609 struct cmd_rs_result {
614 /* function called when cmd_rs is parsed successfully */
615 static void cmd_rs_parsed(void *parsed_result, void *data)
617 // struct cmd_rs_result *res = parsed_result;
619 printf_P(PSTR("angle cons=% .6"PRIi32" in=% .6"PRIi32" out=% .6"PRIi32" / "),
620 cs_get_consign(&mainboard.angle.cs),
621 cs_get_filtered_feedback(&mainboard.angle.cs),
622 cs_get_out(&mainboard.angle.cs));
623 printf_P(PSTR("distance cons=% .6"PRIi32" in=% .6"PRIi32" out=% .6"PRIi32" / "),
624 cs_get_consign(&mainboard.distance.cs),
625 cs_get_filtered_feedback(&mainboard.distance.cs),
626 cs_get_out(&mainboard.distance.cs));
627 printf_P(PSTR("l=% .4"PRIi32" r=% .4"PRIi32"\r\n"), mainboard.pwm_l,
630 } while(!cmdline_keypressed());
633 prog_char str_rs_arg0[] = "rs";
634 parse_pgm_token_string_t cmd_rs_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_rs_result, arg0, str_rs_arg0);
635 prog_char str_rs_arg1[] = "show";
636 parse_pgm_token_string_t cmd_rs_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_rs_result, arg1, str_rs_arg1);
638 prog_char help_rs[] = "Show rs (robot system) values";
639 parse_pgm_inst_t cmd_rs = {
640 .f = cmd_rs_parsed, /* function to call */
641 .data = NULL, /* 2nd arg of func */
643 .tokens = { /* token list, NULL terminated */
644 (prog_void *)&cmd_rs_arg0,
645 (prog_void *)&cmd_rs_arg1,
650 /**********************************************************/
653 /* this structure is filled when cmd_i2cdebug is parsed successfully */
654 struct cmd_i2cdebug_result {
658 /* function called when cmd_i2cdebug is parsed successfully */
659 static void cmd_i2cdebug_parsed(void * parsed_result, void * data)
662 printf("not implemented\n");
665 i2c_protocol_debug();
669 prog_char str_i2cdebug_arg0[] = "i2cdebug";
670 parse_pgm_token_string_t cmd_i2cdebug_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_i2cdebug_result, arg0, str_i2cdebug_arg0);
672 prog_char help_i2cdebug[] = "I2c debug infos";
673 parse_pgm_inst_t cmd_i2cdebug = {
674 .f = cmd_i2cdebug_parsed, /* function to call */
675 .data = NULL, /* 2nd arg of func */
676 .help_str = help_i2cdebug,
677 .tokens = { /* token list, NULL terminated */
678 (prog_void *)&cmd_i2cdebug_arg0,
683 /**********************************************************/
686 /* this structure is filled when cmd_cobboard_show is parsed successfully */
687 struct cmd_cobboard_show_result {
692 /* function called when cmd_cobboard_show is parsed successfully */
693 static void cmd_cobboard_show_parsed(void * parsed_result, void * data)
696 printf("not implemented\n");
698 printf_P(PSTR("mode = %x\r\n"), cobboard.mode);
699 printf_P(PSTR("status = %x\r\n"), cobboard.status);
700 printf_P(PSTR("cob_count = %x\r\n"), cobboard.cob_count);
701 printf_P(PSTR("left_cobroller_speed = %d\r\n"), cobboard.left_cobroller_speed);
702 printf_P(PSTR("right_cobroller_speed = %d\r\n"), cobboard.right_cobroller_speed);
706 prog_char str_cobboard_show_arg0[] = "cobboard";
707 parse_pgm_token_string_t cmd_cobboard_show_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_show_result, arg0, str_cobboard_show_arg0);
708 prog_char str_cobboard_show_arg1[] = "show";
709 parse_pgm_token_string_t cmd_cobboard_show_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_show_result, arg1, str_cobboard_show_arg1);
711 prog_char help_cobboard_show[] = "show cobboard status";
712 parse_pgm_inst_t cmd_cobboard_show = {
713 .f = cmd_cobboard_show_parsed, /* function to call */
714 .data = NULL, /* 2nd arg of func */
715 .help_str = help_cobboard_show,
716 .tokens = { /* token list, NULL terminated */
717 (prog_void *)&cmd_cobboard_show_arg0,
718 (prog_void *)&cmd_cobboard_show_arg1,
723 /**********************************************************/
724 /* Cobboard_Setmode1 */
726 /* this structure is filled when cmd_cobboard_setmode1 is parsed successfully */
727 struct cmd_cobboard_setmode1_result {
732 /* function called when cmd_cobboard_setmode1 is parsed successfully */
733 static void cmd_cobboard_setmode1_parsed(void *parsed_result, void *data)
735 struct cmd_cobboard_setmode1_result *res = parsed_result;
737 if (!strcmp_P(res->arg1, PSTR("init")))
738 i2c_cobboard_set_mode(I2C_COBBOARD_MODE_INIT);
739 else if (!strcmp_P(res->arg1, PSTR("eject")))
740 i2c_cobboard_set_mode(I2C_COBBOARD_MODE_EJECT);
741 else if (!strcmp_P(res->arg1, PSTR("kickstand")))
742 i2c_cobboard_set_mode(I2C_COBBOARD_MODE_KICKSTAND);
745 prog_char str_cobboard_setmode1_arg0[] = "cobboard";
746 parse_pgm_token_string_t cmd_cobboard_setmode1_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode1_result, arg0, str_cobboard_setmode1_arg0);
747 prog_char str_cobboard_setmode1_arg1[] = "init#eject#kickstand";
748 parse_pgm_token_string_t cmd_cobboard_setmode1_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode1_result, arg1, str_cobboard_setmode1_arg1);
750 prog_char help_cobboard_setmode1[] = "set cobboard mode (mode)";
751 parse_pgm_inst_t cmd_cobboard_setmode1 = {
752 .f = cmd_cobboard_setmode1_parsed, /* function to call */
753 .data = NULL, /* 2nd arg of func */
754 .help_str = help_cobboard_setmode1,
755 .tokens = { /* token list, NULL terminated */
756 (prog_void *)&cmd_cobboard_setmode1_arg0,
757 (prog_void *)&cmd_cobboard_setmode1_arg1,
762 /**********************************************************/
763 /* Cobboard_Setmode2 */
765 /* this structure is filled when cmd_cobboard_setmode2 is parsed successfully */
766 struct cmd_cobboard_setmode2_result {
772 /* function called when cmd_cobboard_setmode2 is parsed successfully */
773 static void cmd_cobboard_setmode2_parsed(void *parsed_result, void *data)
775 struct cmd_cobboard_setmode2_result *res = parsed_result;
776 uint8_t side = I2C_LEFT_SIDE;
778 if (!strcmp_P(res->arg2, PSTR("left")))
779 side = I2C_LEFT_SIDE;
780 else if (!strcmp_P(res->arg2, PSTR("right")))
781 side = I2C_RIGHT_SIDE;
783 if (!strcmp_P(res->arg1, PSTR("deploy"))) {
784 i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST);
785 i2c_cobboard_deploy(side);
787 else if (!strcmp_P(res->arg1, PSTR("harvest"))) {
788 i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST);
789 i2c_cobboard_autoharvest(side);
791 else if (!strcmp_P(res->arg1, PSTR("pack"))) {
792 i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST);
793 i2c_cobboard_pack(side);
795 else if (!strcmp_P(res->arg1, PSTR("weak_pack"))) {
796 i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST);
797 i2c_cobboard_pack_weak(side);
799 else if (!strcmp_P(res->arg1, PSTR("deploy_nomove"))) {
800 i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST);
801 i2c_cobboard_deploy_nomove(side);
803 else if (!strcmp_P(res->arg1, PSTR("harvest_nomove"))) {
804 i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST);
805 i2c_cobboard_autoharvest_nomove(side);
809 prog_char str_cobboard_setmode2_arg0[] = "cobboard";
810 parse_pgm_token_string_t cmd_cobboard_setmode2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode2_result, arg0, str_cobboard_setmode2_arg0);
811 prog_char str_cobboard_setmode2_arg1[] = "harvest#deploy#pack#weak_pack#harvest_nomove#deploy_nomove";
812 parse_pgm_token_string_t cmd_cobboard_setmode2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode2_result, arg1, str_cobboard_setmode2_arg1);
813 prog_char str_cobboard_setmode2_arg2[] = "left#right";
814 parse_pgm_token_string_t cmd_cobboard_setmode2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode2_result, arg2, str_cobboard_setmode2_arg2);
816 prog_char help_cobboard_setmode2[] = "set cobboard mode (mode, side)";
817 parse_pgm_inst_t cmd_cobboard_setmode2 = {
818 .f = cmd_cobboard_setmode2_parsed, /* function to call */
819 .data = NULL, /* 2nd arg of func */
820 .help_str = help_cobboard_setmode2,
821 .tokens = { /* token list, NULL terminated */
822 (prog_void *)&cmd_cobboard_setmode2_arg0,
823 (prog_void *)&cmd_cobboard_setmode2_arg1,
824 (prog_void *)&cmd_cobboard_setmode2_arg2,
829 /**********************************************************/
830 /* Cobboard_Setmode3 */
832 /* this structure is filled when cmd_cobboard_setmode3 is parsed successfully */
833 struct cmd_cobboard_setmode3_result {
839 /* function called when cmd_cobboard_setmode3 is parsed successfully */
840 static void cmd_cobboard_setmode3_parsed(void *parsed_result, void *data)
842 struct cmd_cobboard_setmode3_result *res = parsed_result;
843 if (!strcmp_P(res->arg1, PSTR("xxx")))
847 prog_char str_cobboard_setmode3_arg0[] = "cobboard";
848 parse_pgm_token_string_t cmd_cobboard_setmode3_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode3_result, arg0, str_cobboard_setmode3_arg0);
849 prog_char str_cobboard_setmode3_arg1[] = "xxx";
850 parse_pgm_token_string_t cmd_cobboard_setmode3_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode3_result, arg1, str_cobboard_setmode3_arg1);
851 parse_pgm_token_num_t cmd_cobboard_setmode3_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_cobboard_setmode3_result, level, UINT8);
853 prog_char help_cobboard_setmode3[] = "set cobboard mode (mode, level)";
854 parse_pgm_inst_t cmd_cobboard_setmode3 = {
855 .f = cmd_cobboard_setmode3_parsed, /* function to call */
856 .data = NULL, /* 2nd arg of func */
857 .help_str = help_cobboard_setmode3,
858 .tokens = { /* token list, NULL terminated */
859 (prog_void *)&cmd_cobboard_setmode3_arg0,
860 (prog_void *)&cmd_cobboard_setmode3_arg1,
861 (prog_void *)&cmd_cobboard_setmode3_arg2,
866 /**********************************************************/
869 /* this structure is filled when cmd_ballboard_show is parsed successfully */
870 struct cmd_ballboard_show_result {
875 /* function called when cmd_ballboard_show is parsed successfully */
876 static void cmd_ballboard_show_parsed(void * parsed_result, void * data)
879 printf("not implemented\n");
881 printf_P(PSTR("mode = %x\r\n"), ballboard.mode);
882 printf_P(PSTR("status = %x\r\n"), ballboard.status);
883 printf_P(PSTR("ball_count = %d\r\n"), ballboard.ball_count);
884 printf_P(PSTR("lcob = %d\r\n"), ballboard.lcob);
885 printf_P(PSTR("rcob = %d\r\n"), ballboard.rcob);
889 prog_char str_ballboard_show_arg0[] = "ballboard";
890 parse_pgm_token_string_t cmd_ballboard_show_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_show_result, arg0, str_ballboard_show_arg0);
891 prog_char str_ballboard_show_arg1[] = "show";
892 parse_pgm_token_string_t cmd_ballboard_show_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_show_result, arg1, str_ballboard_show_arg1);
894 prog_char help_ballboard_show[] = "show ballboard status";
895 parse_pgm_inst_t cmd_ballboard_show = {
896 .f = cmd_ballboard_show_parsed, /* function to call */
897 .data = NULL, /* 2nd arg of func */
898 .help_str = help_ballboard_show,
899 .tokens = { /* token list, NULL terminated */
900 (prog_void *)&cmd_ballboard_show_arg0,
901 (prog_void *)&cmd_ballboard_show_arg1,
906 /**********************************************************/
907 /* Ballboard_Setmode1 */
909 /* this structure is filled when cmd_ballboard_setmode1 is parsed successfully */
910 struct cmd_ballboard_setmode1_result {
915 /* function called when cmd_ballboard_setmode1 is parsed successfully */
916 static void cmd_ballboard_setmode1_parsed(void *parsed_result, void *data)
918 struct cmd_ballboard_setmode1_result *res = parsed_result;
920 if (!strcmp_P(res->arg1, PSTR("init")))
921 i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_INIT);
922 else if (!strcmp_P(res->arg1, PSTR("off")))
923 i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_OFF);
924 else if (!strcmp_P(res->arg1, PSTR("eject")))
925 i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_EJECT);
926 else if (!strcmp_P(res->arg1, PSTR("harvest")))
927 i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_HARVEST);
932 prog_char str_ballboard_setmode1_arg0[] = "ballboard";
933 parse_pgm_token_string_t cmd_ballboard_setmode1_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode1_result, arg0, str_ballboard_setmode1_arg0);
934 prog_char str_ballboard_setmode1_arg1[] = "init#eject#harvest#off";
935 parse_pgm_token_string_t cmd_ballboard_setmode1_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode1_result, arg1, str_ballboard_setmode1_arg1);
937 prog_char help_ballboard_setmode1[] = "set ballboard mode (mode)";
938 parse_pgm_inst_t cmd_ballboard_setmode1 = {
939 .f = cmd_ballboard_setmode1_parsed, /* function to call */
940 .data = NULL, /* 2nd arg of func */
941 .help_str = help_ballboard_setmode1,
942 .tokens = { /* token list, NULL terminated */
943 (prog_void *)&cmd_ballboard_setmode1_arg0,
944 (prog_void *)&cmd_ballboard_setmode1_arg1,
949 /**********************************************************/
950 /* Ballboard_Setmode2 */
952 /* this structure is filled when cmd_ballboard_setmode2 is parsed successfully */
953 struct cmd_ballboard_setmode2_result {
959 /* function called when cmd_ballboard_setmode2 is parsed successfully */
960 static void cmd_ballboard_setmode2_parsed(void * parsed_result, void * data)
962 struct cmd_ballboard_setmode2_result *res = parsed_result;
963 uint8_t mode = I2C_BALLBOARD_MODE_INIT;
965 if (!strcmp_P(res->arg2, PSTR("left"))) {
966 if (!strcmp_P(res->arg1, PSTR("prepare")))
967 mode = I2C_BALLBOARD_MODE_PREP_L_FORK;
968 else if (!strcmp_P(res->arg1, PSTR("take")))
969 mode = I2C_BALLBOARD_MODE_TAKE_L_FORK;
972 if (!strcmp_P(res->arg1, PSTR("prepare")))
973 mode = I2C_BALLBOARD_MODE_PREP_R_FORK;
974 else if (!strcmp_P(res->arg1, PSTR("take")))
975 mode = I2C_BALLBOARD_MODE_TAKE_R_FORK;
977 i2c_ballboard_set_mode(mode);
980 prog_char str_ballboard_setmode2_arg0[] = "ballboard";
981 parse_pgm_token_string_t cmd_ballboard_setmode2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode2_result, arg0, str_ballboard_setmode2_arg0);
982 prog_char str_ballboard_setmode2_arg1[] = "prepare#take";
983 parse_pgm_token_string_t cmd_ballboard_setmode2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode2_result, arg1, str_ballboard_setmode2_arg1);
984 prog_char str_ballboard_setmode2_arg2[] = "left#right";
985 parse_pgm_token_string_t cmd_ballboard_setmode2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode2_result, arg2, str_ballboard_setmode2_arg2);
987 prog_char help_ballboard_setmode2[] = "set ballboard mode (mode, side)";
988 parse_pgm_inst_t cmd_ballboard_setmode2 = {
989 .f = cmd_ballboard_setmode2_parsed, /* function to call */
990 .data = NULL, /* 2nd arg of func */
991 .help_str = help_ballboard_setmode2,
992 .tokens = { /* token list, NULL terminated */
993 (prog_void *)&cmd_ballboard_setmode2_arg0,
994 (prog_void *)&cmd_ballboard_setmode2_arg1,
995 (prog_void *)&cmd_ballboard_setmode2_arg2,
1000 /**********************************************************/
1001 /* Ballboard_Setmode3 */
1003 /* this structure is filled when cmd_ballboard_setmode3 is parsed successfully */
1004 struct cmd_ballboard_setmode3_result {
1005 fixed_string_t arg0;
1006 fixed_string_t arg1;
1010 /* function called when cmd_ballboard_setmode3 is parsed successfully */
1011 static void cmd_ballboard_setmode3_parsed(void *parsed_result, void *data)
1013 struct cmd_ballboard_setmode3_result *res = parsed_result;
1014 if (!strcmp_P(res->arg1, PSTR("xxx")))
1018 prog_char str_ballboard_setmode3_arg0[] = "ballboard";
1019 parse_pgm_token_string_t cmd_ballboard_setmode3_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode3_result, arg0, str_ballboard_setmode3_arg0);
1020 prog_char str_ballboard_setmode3_arg1[] = "xxx";
1021 parse_pgm_token_string_t cmd_ballboard_setmode3_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode3_result, arg1, str_ballboard_setmode3_arg1);
1022 parse_pgm_token_num_t cmd_ballboard_setmode3_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_ballboard_setmode3_result, level, UINT8);
1024 prog_char help_ballboard_setmode3[] = "set ballboard mode (mode, level)";
1025 parse_pgm_inst_t cmd_ballboard_setmode3 = {
1026 .f = cmd_ballboard_setmode3_parsed, /* function to call */
1027 .data = NULL, /* 2nd arg of func */
1028 .help_str = help_ballboard_setmode3,
1029 .tokens = { /* token list, NULL terminated */
1030 (prog_void *)&cmd_ballboard_setmode3_arg0,
1031 (prog_void *)&cmd_ballboard_setmode3_arg1,
1032 (prog_void *)&cmd_ballboard_setmode3_arg2,
1037 /**********************************************************/
1040 /* this structure is filled when cmd_servo_balls is parsed successfully */
1041 struct cmd_servo_balls_result {
1042 fixed_string_t arg0;
1043 fixed_string_t arg1;
1046 /* function called when cmd_servo_balls is parsed successfully */
1047 static void cmd_servo_balls_parsed(void *parsed_result,
1048 __attribute__((unused)) void *data)
1050 struct cmd_servo_balls_result *res = parsed_result;
1052 if (!strcmp_P(res->arg1, PSTR("deploy")))
1053 support_balls_deploy();
1054 else if (!strcmp_P(res->arg1, PSTR("pack")))
1055 support_balls_pack();
1058 prog_char str_servo_balls_arg0[] = "support_balls";
1059 parse_pgm_token_string_t cmd_servo_balls_arg0 =
1060 TOKEN_STRING_INITIALIZER(struct cmd_servo_balls_result, arg0, str_servo_balls_arg0);
1061 prog_char str_servo_balls_arg1[] = "deploy#pack";
1062 parse_pgm_token_string_t cmd_servo_balls_arg1 =
1063 TOKEN_STRING_INITIALIZER(struct cmd_servo_balls_result, arg1, str_servo_balls_arg1);
1065 prog_char help_servo_balls[] = "control support balls";
1066 parse_pgm_inst_t cmd_servo_balls = {
1067 .f = cmd_servo_balls_parsed, /* function to call */
1068 .data = NULL, /* 2nd arg of func */
1069 .help_str = help_servo_balls,
1070 .tokens = { /* token list, NULL terminated */
1071 (prog_void *)&cmd_servo_balls_arg0,
1072 (prog_void *)&cmd_servo_balls_arg1,
1077 /**********************************************************/
1080 /* this structure is filled when cmd_clitoid is parsed successfully */
1081 struct cmd_clitoid_result {
1082 fixed_string_t arg0;
1091 /* function called when cmd_test is parsed successfully */
1092 static void cmd_clitoid_parsed(void *parsed_result, void *data)
1094 struct cmd_clitoid_result *res = parsed_result;
1095 /* clitoid(res->alpha_deg, res->beta_deg, res->R_mm, */
1096 /* res->Vd, res->Amax, res->d_inter_mm); */
1097 double x = position_get_x_double(&mainboard.pos);
1098 double y = position_get_y_double(&mainboard.pos);
1099 double a = position_get_a_rad_double(&mainboard.pos);
1101 strat_set_speed(res->Vd, SPEED_ANGLE_FAST);
1102 trajectory_clitoid(&mainboard.traj, x, y, a, 150.,
1103 res->alpha_deg, res->beta_deg, res->R_mm,
1107 prog_char str_clitoid_arg0[] = "clitoid";
1108 parse_pgm_token_string_t cmd_clitoid_arg0 =
1109 TOKEN_STRING_INITIALIZER(struct cmd_clitoid_result,
1110 arg0, str_clitoid_arg0);
1111 parse_pgm_token_num_t cmd_clitoid_alpha_deg =
1112 TOKEN_NUM_INITIALIZER(struct cmd_clitoid_result,
1114 parse_pgm_token_num_t cmd_clitoid_beta_deg =
1115 TOKEN_NUM_INITIALIZER(struct cmd_clitoid_result,
1117 parse_pgm_token_num_t cmd_clitoid_R_mm =
1118 TOKEN_NUM_INITIALIZER(struct cmd_clitoid_result,
1120 parse_pgm_token_num_t cmd_clitoid_Vd =
1121 TOKEN_NUM_INITIALIZER(struct cmd_clitoid_result,
1123 parse_pgm_token_num_t cmd_clitoid_Amax =
1124 TOKEN_NUM_INITIALIZER(struct cmd_clitoid_result,
1126 parse_pgm_token_num_t cmd_clitoid_d_inter_mm =
1127 TOKEN_NUM_INITIALIZER(struct cmd_clitoid_result,
1130 prog_char help_clitoid[] = "do a clitoid (alpha, beta, R, Vd, Amax, d_inter)";
1131 parse_pgm_inst_t cmd_clitoid = {
1132 .f = cmd_clitoid_parsed, /* function to call */
1133 .data = NULL, /* 2nd arg of func */
1134 .help_str = help_clitoid,
1135 .tokens = { /* token list, NULL terminated */
1136 (prog_void *)&cmd_clitoid_arg0,
1137 (prog_void *)&cmd_clitoid_alpha_deg,
1138 (prog_void *)&cmd_clitoid_beta_deg,
1139 (prog_void *)&cmd_clitoid_R_mm,
1140 (prog_void *)&cmd_clitoid_Vd,
1141 (prog_void *)&cmd_clitoid_Amax,
1142 (prog_void *)&cmd_clitoid_d_inter_mm,
1147 /**********************************************************/
1150 /* this structure is filled when cmd_time_monitor is parsed successfully */
1151 struct cmd_time_monitor_result {
1152 fixed_string_t arg0;
1153 fixed_string_t arg1;
1156 /* function called when cmd_time_monitor is parsed successfully */
1157 static void cmd_time_monitor_parsed(void *parsed_result, void *data)
1159 #ifndef HOST_VERSION
1160 struct cmd_time_monitor_result *res = parsed_result;
1163 if (!strcmp_P(res->arg1, PSTR("reset"))) {
1164 eeprom_write_word(EEPROM_TIME_ADDRESS, 0);
1166 seconds = eeprom_read_word(EEPROM_TIME_ADDRESS);
1167 printf_P(PSTR("Running since %d mn %d\r\n"), seconds/60, seconds%60);
1171 prog_char str_time_monitor_arg0[] = "time_monitor";
1172 parse_pgm_token_string_t cmd_time_monitor_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_time_monitor_result, arg0, str_time_monitor_arg0);
1173 prog_char str_time_monitor_arg1[] = "show#reset";
1174 parse_pgm_token_string_t cmd_time_monitor_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_time_monitor_result, arg1, str_time_monitor_arg1);
1176 prog_char help_time_monitor[] = "Show since how long we are running";
1177 parse_pgm_inst_t cmd_time_monitor = {
1178 .f = cmd_time_monitor_parsed, /* function to call */
1179 .data = NULL, /* 2nd arg of func */
1180 .help_str = help_time_monitor,
1181 .tokens = { /* token list, NULL terminated */
1182 (prog_void *)&cmd_time_monitor_arg0,
1183 (prog_void *)&cmd_time_monitor_arg1,
1189 /**********************************************************/
1192 /* this structure is filled when cmd_strat_event is parsed successfully */
1193 struct cmd_strat_event_result {
1194 fixed_string_t arg0;
1195 fixed_string_t arg1;
1198 /* function called when cmd_strat_event is parsed successfully */
1199 static void cmd_strat_event_parsed(void *parsed_result, void *data)
1201 struct cmd_strat_event_result *res = parsed_result;
1203 if (!strcmp_P(res->arg1, PSTR("on")))
1204 strat_event_enable();
1206 strat_event_disable();
1209 prog_char str_strat_event_arg0[] = "strat_event";
1210 parse_pgm_token_string_t cmd_strat_event_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_event_result, arg0, str_strat_event_arg0);
1211 prog_char str_strat_event_arg1[] = "on#off";
1212 parse_pgm_token_string_t cmd_strat_event_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_event_result, arg1, str_strat_event_arg1);
1214 prog_char help_strat_event[] = "Enable/disable strat_event callback";
1215 parse_pgm_inst_t cmd_strat_event = {
1216 .f = cmd_strat_event_parsed, /* function to call */
1217 .data = NULL, /* 2nd arg of func */
1218 .help_str = help_strat_event,
1219 .tokens = { /* token list, NULL terminated */
1220 (prog_void *)&cmd_strat_event_arg0,
1221 (prog_void *)&cmd_strat_event_arg1,
1226 /**********************************************************/
1229 /* this structure is filled when cmd_sleep is parsed successfully */
1230 struct cmd_sleep_result {
1231 fixed_string_t arg0;
1235 /* function called when cmd_sleep is parsed successfully */
1236 static void cmd_sleep_parsed(void *parsed_result, void *data)
1238 struct cmd_sleep_result *res = parsed_result;
1239 time_wait_ms(res->ms);
1242 prog_char str_sleep_arg0[] = "sleep";
1243 parse_pgm_token_string_t cmd_sleep_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_sleep_result, arg0, str_sleep_arg0);
1244 parse_pgm_token_num_t cmd_sleep_ms = TOKEN_NUM_INITIALIZER(struct cmd_sleep_result, ms, UINT32);
1246 prog_char help_sleep[] = "Sleep during some miliseconds";
1247 parse_pgm_inst_t cmd_sleep = {
1248 .f = cmd_sleep_parsed, /* function to call */
1249 .data = NULL, /* 2nd arg of func */
1250 .help_str = help_sleep,
1251 .tokens = { /* token list, NULL terminated */
1252 (prog_void *)&cmd_sleep_arg0,
1253 (prog_void *)&cmd_sleep_ms,
1259 /**********************************************************/
1262 /* this structure is filled when cmd_test is parsed successfully */
1263 struct cmd_test_result {
1264 fixed_string_t arg0;
1269 /* function called when cmd_test is parsed successfully */
1270 static void cmd_test_parsed(void *parsed_result, void *data)
1272 strat_db.dump_enabled = 1;
1273 strat_db_dump(__FUNCTION__);
1275 corn_set_color(strat_db.corn_table[0], I2C_COB_BLACK);
1276 strat_db_dump(__FUNCTION__);
1278 corn_set_color(strat_db.corn_table[3], I2C_COB_WHITE);
1279 strat_db_dump(__FUNCTION__);
1280 corn_set_color(strat_db.corn_table[4], I2C_COB_WHITE);
1281 strat_db_dump(__FUNCTION__);
1282 corn_set_color(strat_db.corn_table[5], I2C_COB_WHITE);
1283 strat_db_dump(__FUNCTION__);
1286 prog_char str_test_arg0[] = "test";
1287 parse_pgm_token_string_t cmd_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_test_result, arg0, str_test_arg0);
1288 parse_pgm_token_num_t cmd_test_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_test_result, radius, INT32);
1289 parse_pgm_token_num_t cmd_test_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_test_result, dist, INT32);
1291 prog_char help_test[] = "Test function";
1292 parse_pgm_inst_t cmd_test = {
1293 .f = cmd_test_parsed, /* function to call */
1294 .data = NULL, /* 2nd arg of func */
1295 .help_str = help_test,
1296 .tokens = { /* token list, NULL terminated */
1297 (prog_void *)&cmd_test_arg0,
1305 /**********************************************************/
1308 /* this structure is filled when cmd_climb is parsed successfully */
1309 struct cmd_climb_result {
1310 fixed_string_t arg0;
1316 //#define DIM_BALLS_DOWN 620
1317 //#define DIM_BALLS_UP 700
1319 #define DIM_BALLS_DOWN 530
1320 #define DIM_BALLS_UP 700
1322 /* function called when cmd_climb is parsed successfully */
1323 static void cmd_climb_parsed(void *parsed_result, void *data)
1327 printf_P(PSTR("starting Clifenger\r\n"));
1328 strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW );
1330 trajectory_goto_xy_abs(&mainboard.traj, 250, 250);
1331 err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
1332 trajectory_a_abs(&mainboard.traj, 0);
1333 err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
1337 trajectory_goto_xy_abs(&mainboard.traj, 1250, 250);
1338 err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos)>DIM_BALLS_DOWN, 0xFF);
1339 printf_P(PSTR("ball_pack\r\n"));
1340 support_balls_pack();
1341 err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos)>DIM_BALLS_UP, 0xFF);
1342 printf_P(PSTR("ball_deploy\r\n"));
1343 support_balls_deploy();
1344 err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
1346 /* reach top, go down */
1347 trajectory_goto_xy_abs(&mainboard.traj, 250, 250);
1348 err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos)<DIM_BALLS_UP, 0xFF);
1349 printf_P(PSTR("ball_pack\r\n"));
1350 support_balls_pack();
1351 err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos)<DIM_BALLS_DOWN, 0xFF);
1352 printf_P(PSTR("ball_deploy\r\n"));
1353 support_balls_deploy();
1355 err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
1358 prog_char str_climb_arg0[] = "climb";
1359 parse_pgm_token_string_t cmd_climb_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_climb_result, arg0, str_climb_arg0);
1360 parse_pgm_token_num_t cmd_climb_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_climb_result, radius, INT32);
1361 parse_pgm_token_num_t cmd_climb_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_climb_result, dist, INT32);
1363 prog_char help_climb[] = "Climb function";
1364 parse_pgm_inst_t cmd_climb = {
1365 .f = cmd_climb_parsed, /* function to call */
1366 .data = NULL, /* 2nd arg of func */
1367 .help_str = help_climb,
1368 .tokens = { /* token list, NULL terminated */
1369 (prog_void *)&cmd_climb_arg0,