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 "strat_corn.h"
70 #include "i2c_protocol.h"
73 struct cmd_event_result {
80 /* function called when cmd_event is parsed successfully */
81 static void cmd_event_parsed(void *parsed_result, void *data)
85 struct cmd_event_result * res = parsed_result;
87 if (!strcmp_P(res->arg1, PSTR("all"))) {
89 if (!strcmp_P(res->arg2, PSTR("on")))
90 mainboard.flags |= bit;
91 else if (!strcmp_P(res->arg2, PSTR("off")))
92 mainboard.flags &= bit;
94 printf_P(PSTR("encoders is %s\r\n"),
95 (DO_ENCODERS & mainboard.flags) ? "on":"off");
96 printf_P(PSTR("cs is %s\r\n"),
97 (DO_CS & mainboard.flags) ? "on":"off");
98 printf_P(PSTR("rs is %s\r\n"),
99 (DO_RS & mainboard.flags) ? "on":"off");
100 printf_P(PSTR("pos is %s\r\n"),
101 (DO_POS & mainboard.flags) ? "on":"off");
102 printf_P(PSTR("bd is %s\r\n"),
103 (DO_BD & mainboard.flags) ? "on":"off");
104 printf_P(PSTR("timer is %s\r\n"),
105 (DO_TIMER & mainboard.flags) ? "on":"off");
106 printf_P(PSTR("power is %s\r\n"),
107 (DO_POWER & mainboard.flags) ? "on":"off");
108 printf_P(PSTR("errblock is %s\r\n"),
109 (DO_ERRBLOCKING & mainboard.flags) ? "on":"off");
114 if (!strcmp_P(res->arg1, PSTR("encoders")))
116 else if (!strcmp_P(res->arg1, PSTR("cs"))) {
120 else if (!strcmp_P(res->arg1, PSTR("rs")))
122 else if (!strcmp_P(res->arg1, PSTR("pos")))
124 else if (!strcmp_P(res->arg1, PSTR("bd")))
126 else if (!strcmp_P(res->arg1, PSTR("timer"))) {
130 else if (!strcmp_P(res->arg1, PSTR("power")))
132 else if (!strcmp_P(res->arg1, PSTR("errblock")))
133 bit = DO_ERRBLOCKING;
135 if (!strcmp_P(res->arg2, PSTR("on")))
136 mainboard.flags |= bit;
137 else if (!strcmp_P(res->arg2, PSTR("off"))) {
138 if (!strcmp_P(res->arg1, PSTR("cs"))) {
140 robotsim_pwm(LEFT_PWM, 0);
141 robotsim_pwm(RIGHT_PWM, 0);
143 pwm_ng_set(LEFT_PWM, 0);
144 pwm_ng_set(RIGHT_PWM, 0);
147 mainboard.flags &= (~bit);
149 printf_P(PSTR("%s is %s\r\n"), res->arg1,
150 (bit & mainboard.flags) ? "on":"off");
153 prog_char str_event_arg0[] = "event";
154 parse_pgm_token_string_t cmd_event_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg0, str_event_arg0);
155 prog_char str_event_arg1[] = "all#encoders#cs#rs#pos#bd#timer#power#errblock";
156 parse_pgm_token_string_t cmd_event_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg1, str_event_arg1);
157 prog_char str_event_arg2[] = "on#off#show";
158 parse_pgm_token_string_t cmd_event_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg2, str_event_arg2);
160 prog_char help_event[] = "Enable/disable events";
161 parse_pgm_inst_t cmd_event = {
162 .f = cmd_event_parsed, /* function to call */
163 .data = NULL, /* 2nd arg of func */
164 .help_str = help_event,
165 .tokens = { /* token list, NULL terminated */
166 (prog_void *)&cmd_event_arg0,
167 (prog_void *)&cmd_event_arg1,
168 (prog_void *)&cmd_event_arg2,
174 /**********************************************************/
177 /* this structure is filled when cmd_spi_test is parsed successfully */
178 struct cmd_spi_test_result {
182 /* function called when cmd_spi_test is parsed successfully */
183 static void cmd_spi_test_parsed(void * parsed_result, void * data)
186 printf("not implemented\n");
188 uint16_t i = 0, ret = 0, ret2 = 0;
190 if (mainboard.flags & DO_ENCODERS) {
191 printf_P(PSTR("Disable encoder event first\r\n"));
197 ret = spi_send_and_receive_byte(i);
198 ret2 = spi_send_and_receive_byte(i);
199 spi_slave_deselect(0);
201 if ((i & 0x7ff) == 0)
202 printf_P(PSTR("Sent %.4x twice, received %x %x\r\n"),
206 } while(!cmdline_keypressed());
210 prog_char str_spi_test_arg0[] = "spi_test";
211 parse_pgm_token_string_t cmd_spi_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_spi_test_result, arg0, str_spi_test_arg0);
213 prog_char help_spi_test[] = "Test the SPI";
214 parse_pgm_inst_t cmd_spi_test = {
215 .f = cmd_spi_test_parsed, /* function to call */
216 .data = NULL, /* 2nd arg of func */
217 .help_str = help_spi_test,
218 .tokens = { /* token list, NULL terminated */
219 (prog_void *)&cmd_spi_test_arg0,
226 /**********************************************************/
229 /* this structure is filled when cmd_opponent is parsed successfully */
230 struct cmd_opponent_result {
237 /* function called when cmd_opponent is parsed successfully */
238 static void cmd_opponent_parsed(void *parsed_result, void *data)
242 if (get_opponent_xyda(&x, &y, &d, &a))
243 printf_P(PSTR("No opponent\r\n"));
245 printf_P(PSTR("x=%d y=%d, d=%d a=%d\r\n"), x, y, d, a);
248 prog_char str_opponent_arg0[] = "opponent";
249 parse_pgm_token_string_t cmd_opponent_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_opponent_result, arg0, str_opponent_arg0);
250 prog_char str_opponent_arg1[] = "show";
251 parse_pgm_token_string_t cmd_opponent_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_opponent_result, arg1, str_opponent_arg1);
253 prog_char help_opponent[] = "Show (x,y) opponent";
254 parse_pgm_inst_t cmd_opponent = {
255 .f = cmd_opponent_parsed, /* function to call */
256 .data = NULL, /* 2nd arg of func */
257 .help_str = help_opponent,
258 .tokens = { /* token list, NULL terminated */
259 (prog_void *)&cmd_opponent_arg0,
260 (prog_void *)&cmd_opponent_arg1,
266 prog_char str_opponent_arg1_set[] = "set";
267 parse_pgm_token_string_t cmd_opponent_arg1_set = TOKEN_STRING_INITIALIZER(struct cmd_opponent_result, arg1, str_opponent_arg1_set);
268 parse_pgm_token_num_t cmd_opponent_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_opponent_result, arg2, INT32);
269 parse_pgm_token_num_t cmd_opponent_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_opponent_result, arg3, INT32);
271 prog_char help_opponent_set[] = "Set (x,y) opponent";
272 parse_pgm_inst_t cmd_opponent_set = {
273 .f = cmd_opponent_parsed, /* function to call */
274 .data = NULL, /* 2nd arg of func */
275 .help_str = help_opponent_set,
276 .tokens = { /* token list, NULL terminated */
277 (prog_void *)&cmd_opponent_arg0,
278 (prog_void *)&cmd_opponent_arg1_set,
279 (prog_void *)&cmd_opponent_arg2,
280 (prog_void *)&cmd_opponent_arg3,
286 /**********************************************************/
289 /* this structure is filled when cmd_start is parsed successfully */
290 struct cmd_start_result {
292 fixed_string_t color;
293 fixed_string_t debug;
296 /* function called when cmd_start is parsed successfully */
297 static void cmd_start_parsed(void *parsed_result, void *data)
300 printf("not implemented\n");
302 struct cmd_start_result *res = parsed_result;
303 uint8_t old_level = gen.log_level;
305 gen.logs[NB_LOGS] = E_USER_STRAT;
306 if (!strcmp_P(res->debug, PSTR("debug"))) {
307 strat_infos.dump_enabled = 1;
311 strat_infos.dump_enabled = 0;
315 if (!strcmp_P(res->color, PSTR("yellow"))) {
316 mainboard.our_color = I2C_COLOR_YELLOW;
317 i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_YELLOW);
318 i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_YELLOW);
320 else if (!strcmp_P(res->color, PSTR("blue"))) {
321 mainboard.our_color = I2C_COLOR_BLUE;
322 i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_BLUE);
323 i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_BLUE);
328 gen.logs[NB_LOGS] = 0;
329 gen.log_level = old_level;
333 prog_char str_start_arg0[] = "start";
334 parse_pgm_token_string_t cmd_start_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_start_result, arg0, str_start_arg0);
335 prog_char str_start_color[] = "blue#yellow";
336 parse_pgm_token_string_t cmd_start_color = TOKEN_STRING_INITIALIZER(struct cmd_start_result, color, str_start_color);
337 prog_char str_start_debug[] = "debug#match";
338 parse_pgm_token_string_t cmd_start_debug = TOKEN_STRING_INITIALIZER(struct cmd_start_result, debug, str_start_debug);
340 prog_char help_start[] = "Start the robot";
341 parse_pgm_inst_t cmd_start = {
342 .f = cmd_start_parsed, /* function to call */
343 .data = NULL, /* 2nd arg of func */
344 .help_str = help_start,
345 .tokens = { /* token list, NULL terminated */
346 (prog_void *)&cmd_start_arg0,
347 (prog_void *)&cmd_start_color,
348 (prog_void *)&cmd_start_debug,
355 /**********************************************************/
358 /* this structure is filled when cmd_interact is parsed successfully */
359 struct cmd_interact_result {
363 static void print_cs(void)
365 printf_P(PSTR("cons_d=% .8"PRIi32" cons_a=% .8"PRIi32" fil_d=% .8"PRIi32" fil_a=% .8"PRIi32" "
366 "err_d=% .8"PRIi32" err_a=% .8"PRIi32" out_d=% .8"PRIi32" out_a=% .8"PRIi32"\r\n"),
367 cs_get_consign(&mainboard.distance.cs),
368 cs_get_consign(&mainboard.angle.cs),
369 cs_get_filtered_consign(&mainboard.distance.cs),
370 cs_get_filtered_consign(&mainboard.angle.cs),
371 cs_get_error(&mainboard.distance.cs),
372 cs_get_error(&mainboard.angle.cs),
373 cs_get_out(&mainboard.distance.cs),
374 cs_get_out(&mainboard.angle.cs));
377 static void print_pos(void)
379 printf_P(PSTR("x=% .8d y=% .8d a=% .8d\r\n"),
380 position_get_x_s16(&mainboard.pos),
381 position_get_y_s16(&mainboard.pos),
382 position_get_a_deg_s16(&mainboard.pos));
385 static void print_time(void)
387 printf_P(PSTR("time %d\r\n"),time_get_s());
391 static void print_sensors(void)
394 if (sensor_start_switch())
395 printf_P(PSTR("Start switch | "));
397 printf_P(PSTR(" | "));
399 if (IR_DISP_SENSOR())
400 printf_P(PSTR("IR disp | "));
402 printf_P(PSTR(" | "));
404 printf_P(PSTR("\r\n"));
408 static void print_pid(void)
410 printf_P(PSTR("P=% .8"PRIi32" I=% .8"PRIi32" D=% .8"PRIi32" out=% .8"PRIi32" | "
411 "P=% .8"PRIi32" I=% .8"PRIi32" D=% .8"PRIi32" out=% .8"PRIi32"\r\n"),
412 pid_get_value_in(&mainboard.distance.pid) * pid_get_gain_P(&mainboard.distance.pid),
413 pid_get_value_I(&mainboard.distance.pid) * pid_get_gain_I(&mainboard.distance.pid),
414 pid_get_value_D(&mainboard.distance.pid) * pid_get_gain_D(&mainboard.distance.pid),
415 pid_get_value_out(&mainboard.distance.pid),
416 pid_get_value_in(&mainboard.angle.pid) * pid_get_gain_P(&mainboard.angle.pid),
417 pid_get_value_I(&mainboard.angle.pid) * pid_get_gain_I(&mainboard.angle.pid),
418 pid_get_value_D(&mainboard.angle.pid) * pid_get_gain_D(&mainboard.angle.pid),
419 pid_get_value_out(&mainboard.angle.pid));
422 #define PRINT_POS (1<<0)
423 #define PRINT_PID (1<<1)
424 #define PRINT_CS (1<<2)
425 #define PRINT_SENSORS (1<<3)
426 #define PRINT_TIME (1<<4)
427 #define PRINT_BLOCKING (1<<5)
429 static void cmd_interact_parsed(void * parsed_result, void * data)
438 printf_P(PSTR("Display debugs:\r\n"
444 /* " 6:blocking\r\n" */
451 mainboard.flags &= (~DO_CS);
452 pwm_set_and_save(LEFT_PWM, 0);
453 pwm_set_and_save(RIGHT_PWM, 0);
456 if (print & PRINT_POS) {
460 if (print & PRINT_PID) {
464 if (print & PRINT_CS) {
468 if (print & PRINT_SENSORS) {
472 if (print & PRINT_TIME) {
475 /* if (print & PRINT_BLOCKING) { */
476 /* printf_P(PSTR("%s %s blocking=%d\r\n"), */
477 /* mainboard.blocking ? "BLOCK1":" ", */
478 /* rs_is_blocked(&mainboard.rs) ? "BLOCK2":" ", */
479 /* rs_get_blocking(&mainboard.rs)); */
482 c = cmdline_getchar();
487 cmd = vt100_parser(&vt100, c);
495 case '1': print ^= PRINT_POS; break;
496 case '2': print ^= PRINT_PID; break;
497 case '3': print ^= PRINT_CS; break;
498 case '4': print ^= PRINT_SENSORS; break;
499 case '5': print ^= PRINT_TIME; break;
500 case '6': print ^= PRINT_BLOCKING; break;
503 if (mainboard.flags & DO_CS)
505 pwm_set_and_save(LEFT_PWM, 0);
506 pwm_set_and_save(RIGHT_PWM, 0);
509 pwm_set_and_save(LEFT_PWM, 0);
510 pwm_set_and_save(RIGHT_PWM, 0);
519 pwm_set_and_save(LEFT_PWM, 1200);
520 pwm_set_and_save(RIGHT_PWM, 1200);
523 pwm_set_and_save(LEFT_PWM, -1200);
524 pwm_set_and_save(RIGHT_PWM, 1200);
527 pwm_set_and_save(LEFT_PWM, -1200);
528 pwm_set_and_save(RIGHT_PWM, -1200);
531 pwm_set_and_save(LEFT_PWM, 1200);
532 pwm_set_and_save(RIGHT_PWM, -1200);
540 prog_char str_interact_arg0[] = "interact";
541 parse_pgm_token_string_t cmd_interact_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_interact_result, arg0, str_interact_arg0);
543 prog_char help_interact[] = "Interactive mode";
544 parse_pgm_inst_t cmd_interact = {
545 .f = cmd_interact_parsed, /* function to call */
546 .data = NULL, /* 2nd arg of func */
547 .help_str = help_interact,
548 .tokens = { /* token list, NULL terminated */
549 (prog_void *)&cmd_interact_arg0,
555 /**********************************************************/
558 /* this structure is filled when cmd_color is parsed successfully */
559 struct cmd_color_result {
561 fixed_string_t color;
564 /* function called when cmd_color is parsed successfully */
565 static void cmd_color_parsed(void *parsed_result, void *data)
568 printf("not implemented\n");
570 struct cmd_color_result *res = (struct cmd_color_result *) parsed_result;
571 if (!strcmp_P(res->color, PSTR("yellow"))) {
572 mainboard.our_color = I2C_COLOR_YELLOW;
573 i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_YELLOW);
574 i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_YELLOW);
576 else if (!strcmp_P(res->color, PSTR("blue"))) {
577 mainboard.our_color = I2C_COLOR_BLUE;
578 i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_BLUE);
579 i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_BLUE);
581 printf_P(PSTR("Done\r\n"));
585 prog_char str_color_arg0[] = "color";
586 parse_pgm_token_string_t cmd_color_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_color_result, arg0, str_color_arg0);
587 prog_char str_color_color[] = "blue#yellow";
588 parse_pgm_token_string_t cmd_color_color = TOKEN_STRING_INITIALIZER(struct cmd_color_result, color, str_color_color);
590 prog_char help_color[] = "Set our color";
591 parse_pgm_inst_t cmd_color = {
592 .f = cmd_color_parsed, /* function to call */
593 .data = NULL, /* 2nd arg of func */
594 .help_str = help_color,
595 .tokens = { /* token list, NULL terminated */
596 (prog_void *)&cmd_color_arg0,
597 (prog_void *)&cmd_color_color,
603 /**********************************************************/
606 /* this structure is filled when cmd_rs is parsed successfully */
607 struct cmd_rs_result {
612 /* function called when cmd_rs is parsed successfully */
613 static void cmd_rs_parsed(void *parsed_result, void *data)
615 // struct cmd_rs_result *res = parsed_result;
617 printf_P(PSTR("angle cons=% .6"PRIi32" in=% .6"PRIi32" out=% .6"PRIi32" / "),
618 cs_get_consign(&mainboard.angle.cs),
619 cs_get_filtered_feedback(&mainboard.angle.cs),
620 cs_get_out(&mainboard.angle.cs));
621 printf_P(PSTR("distance cons=% .6"PRIi32" in=% .6"PRIi32" out=% .6"PRIi32" / "),
622 cs_get_consign(&mainboard.distance.cs),
623 cs_get_filtered_feedback(&mainboard.distance.cs),
624 cs_get_out(&mainboard.distance.cs));
625 printf_P(PSTR("l=% .4"PRIi32" r=% .4"PRIi32"\r\n"), mainboard.pwm_l,
628 } while(!cmdline_keypressed());
631 prog_char str_rs_arg0[] = "rs";
632 parse_pgm_token_string_t cmd_rs_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_rs_result, arg0, str_rs_arg0);
633 prog_char str_rs_arg1[] = "show";
634 parse_pgm_token_string_t cmd_rs_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_rs_result, arg1, str_rs_arg1);
636 prog_char help_rs[] = "Show rs (robot system) values";
637 parse_pgm_inst_t cmd_rs = {
638 .f = cmd_rs_parsed, /* function to call */
639 .data = NULL, /* 2nd arg of func */
641 .tokens = { /* token list, NULL terminated */
642 (prog_void *)&cmd_rs_arg0,
643 (prog_void *)&cmd_rs_arg1,
648 /**********************************************************/
651 /* this structure is filled when cmd_i2cdebug is parsed successfully */
652 struct cmd_i2cdebug_result {
656 /* function called when cmd_i2cdebug is parsed successfully */
657 static void cmd_i2cdebug_parsed(void * parsed_result, void * data)
660 printf("not implemented\n");
663 i2c_protocol_debug();
667 prog_char str_i2cdebug_arg0[] = "i2cdebug";
668 parse_pgm_token_string_t cmd_i2cdebug_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_i2cdebug_result, arg0, str_i2cdebug_arg0);
670 prog_char help_i2cdebug[] = "I2c debug infos";
671 parse_pgm_inst_t cmd_i2cdebug = {
672 .f = cmd_i2cdebug_parsed, /* function to call */
673 .data = NULL, /* 2nd arg of func */
674 .help_str = help_i2cdebug,
675 .tokens = { /* token list, NULL terminated */
676 (prog_void *)&cmd_i2cdebug_arg0,
681 /**********************************************************/
684 /* this structure is filled when cmd_cobboard_show is parsed successfully */
685 struct cmd_cobboard_show_result {
690 /* function called when cmd_cobboard_show is parsed successfully */
691 static void cmd_cobboard_show_parsed(void * parsed_result, void * data)
694 printf("not implemented\n");
696 printf_P(PSTR("mode = %x\r\n"), cobboard.mode);
697 printf_P(PSTR("status = %x\r\n"), cobboard.status);
698 printf_P(PSTR("cob_count = %x\r\n"), cobboard.cob_count);
699 printf_P(PSTR("left_cobroller_speed = %d\r\n"), cobboard.left_cobroller_speed);
700 printf_P(PSTR("right_cobroller_speed = %d\r\n"), cobboard.right_cobroller_speed);
704 prog_char str_cobboard_show_arg0[] = "cobboard";
705 parse_pgm_token_string_t cmd_cobboard_show_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_show_result, arg0, str_cobboard_show_arg0);
706 prog_char str_cobboard_show_arg1[] = "show";
707 parse_pgm_token_string_t cmd_cobboard_show_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_show_result, arg1, str_cobboard_show_arg1);
709 prog_char help_cobboard_show[] = "show cobboard status";
710 parse_pgm_inst_t cmd_cobboard_show = {
711 .f = cmd_cobboard_show_parsed, /* function to call */
712 .data = NULL, /* 2nd arg of func */
713 .help_str = help_cobboard_show,
714 .tokens = { /* token list, NULL terminated */
715 (prog_void *)&cmd_cobboard_show_arg0,
716 (prog_void *)&cmd_cobboard_show_arg1,
721 /**********************************************************/
722 /* Cobboard_Setmode1 */
724 /* this structure is filled when cmd_cobboard_setmode1 is parsed successfully */
725 struct cmd_cobboard_setmode1_result {
730 /* function called when cmd_cobboard_setmode1 is parsed successfully */
731 static void cmd_cobboard_setmode1_parsed(void *parsed_result, void *data)
733 struct cmd_cobboard_setmode1_result *res = parsed_result;
735 if (!strcmp_P(res->arg1, PSTR("init")))
736 i2c_cobboard_mode_init();
737 else if (!strcmp_P(res->arg1, PSTR("eject")))
738 i2c_cobboard_mode_eject();
741 prog_char str_cobboard_setmode1_arg0[] = "cobboard";
742 parse_pgm_token_string_t cmd_cobboard_setmode1_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode1_result, arg0, str_cobboard_setmode1_arg0);
743 prog_char str_cobboard_setmode1_arg1[] = "init#eject";
744 parse_pgm_token_string_t cmd_cobboard_setmode1_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode1_result, arg1, str_cobboard_setmode1_arg1);
746 prog_char help_cobboard_setmode1[] = "set cobboard mode (mode)";
747 parse_pgm_inst_t cmd_cobboard_setmode1 = {
748 .f = cmd_cobboard_setmode1_parsed, /* function to call */
749 .data = NULL, /* 2nd arg of func */
750 .help_str = help_cobboard_setmode1,
751 .tokens = { /* token list, NULL terminated */
752 (prog_void *)&cmd_cobboard_setmode1_arg0,
753 (prog_void *)&cmd_cobboard_setmode1_arg1,
758 /**********************************************************/
759 /* Cobboard_Setmode2 */
761 /* this structure is filled when cmd_cobboard_setmode2 is parsed successfully */
762 struct cmd_cobboard_setmode2_result {
768 /* function called when cmd_cobboard_setmode2 is parsed successfully */
769 static void cmd_cobboard_setmode2_parsed(void * parsed_result, void * data)
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);
787 prog_char str_cobboard_setmode2_arg0[] = "cobboard";
788 parse_pgm_token_string_t cmd_cobboard_setmode2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode2_result, arg0, str_cobboard_setmode2_arg0);
789 prog_char str_cobboard_setmode2_arg1[] = "harvest#deploy#pack";
790 parse_pgm_token_string_t cmd_cobboard_setmode2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode2_result, arg1, str_cobboard_setmode2_arg1);
791 prog_char str_cobboard_setmode2_arg2[] = "left#right";
792 parse_pgm_token_string_t cmd_cobboard_setmode2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode2_result, arg2, str_cobboard_setmode2_arg2);
794 prog_char help_cobboard_setmode2[] = "set cobboard mode (mode, side)";
795 parse_pgm_inst_t cmd_cobboard_setmode2 = {
796 .f = cmd_cobboard_setmode2_parsed, /* function to call */
797 .data = NULL, /* 2nd arg of func */
798 .help_str = help_cobboard_setmode2,
799 .tokens = { /* token list, NULL terminated */
800 (prog_void *)&cmd_cobboard_setmode2_arg0,
801 (prog_void *)&cmd_cobboard_setmode2_arg1,
802 (prog_void *)&cmd_cobboard_setmode2_arg2,
807 /**********************************************************/
808 /* Cobboard_Setmode3 */
810 /* this structure is filled when cmd_cobboard_setmode3 is parsed successfully */
811 struct cmd_cobboard_setmode3_result {
817 /* function called when cmd_cobboard_setmode3 is parsed successfully */
818 static void cmd_cobboard_setmode3_parsed(void *parsed_result, void *data)
820 struct cmd_cobboard_setmode3_result *res = parsed_result;
821 if (!strcmp_P(res->arg1, PSTR("xxx")))
825 prog_char str_cobboard_setmode3_arg0[] = "cobboard";
826 parse_pgm_token_string_t cmd_cobboard_setmode3_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode3_result, arg0, str_cobboard_setmode3_arg0);
827 prog_char str_cobboard_setmode3_arg1[] = "xxx";
828 parse_pgm_token_string_t cmd_cobboard_setmode3_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode3_result, arg1, str_cobboard_setmode3_arg1);
829 parse_pgm_token_num_t cmd_cobboard_setmode3_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_cobboard_setmode3_result, level, UINT8);
831 prog_char help_cobboard_setmode3[] = "set cobboard mode (mode, level)";
832 parse_pgm_inst_t cmd_cobboard_setmode3 = {
833 .f = cmd_cobboard_setmode3_parsed, /* function to call */
834 .data = NULL, /* 2nd arg of func */
835 .help_str = help_cobboard_setmode3,
836 .tokens = { /* token list, NULL terminated */
837 (prog_void *)&cmd_cobboard_setmode3_arg0,
838 (prog_void *)&cmd_cobboard_setmode3_arg1,
839 (prog_void *)&cmd_cobboard_setmode3_arg2,
844 /**********************************************************/
847 /* this structure is filled when cmd_ballboard_show is parsed successfully */
848 struct cmd_ballboard_show_result {
853 /* function called when cmd_ballboard_show is parsed successfully */
854 static void cmd_ballboard_show_parsed(void * parsed_result, void * data)
857 printf("not implemented\n");
859 printf_P(PSTR("mode = %x\r\n"), ballboard.mode);
860 printf_P(PSTR("status = %x\r\n"), ballboard.status);
861 printf_P(PSTR("ball_count = %d\r\n"), ballboard.ball_count);
865 prog_char str_ballboard_show_arg0[] = "ballboard";
866 parse_pgm_token_string_t cmd_ballboard_show_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_show_result, arg0, str_ballboard_show_arg0);
867 prog_char str_ballboard_show_arg1[] = "show";
868 parse_pgm_token_string_t cmd_ballboard_show_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_show_result, arg1, str_ballboard_show_arg1);
870 prog_char help_ballboard_show[] = "show ballboard status";
871 parse_pgm_inst_t cmd_ballboard_show = {
872 .f = cmd_ballboard_show_parsed, /* function to call */
873 .data = NULL, /* 2nd arg of func */
874 .help_str = help_ballboard_show,
875 .tokens = { /* token list, NULL terminated */
876 (prog_void *)&cmd_ballboard_show_arg0,
877 (prog_void *)&cmd_ballboard_show_arg1,
882 /**********************************************************/
883 /* Ballboard_Setmode1 */
885 /* this structure is filled when cmd_ballboard_setmode1 is parsed successfully */
886 struct cmd_ballboard_setmode1_result {
891 /* function called when cmd_ballboard_setmode1 is parsed successfully */
892 static void cmd_ballboard_setmode1_parsed(void *parsed_result, void *data)
894 struct cmd_ballboard_setmode1_result *res = parsed_result;
896 if (!strcmp_P(res->arg1, PSTR("init")))
897 i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_INIT);
898 else if (!strcmp_P(res->arg1, PSTR("off")))
899 i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_OFF);
900 else if (!strcmp_P(res->arg1, PSTR("eject")))
901 i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_EJECT);
902 else if (!strcmp_P(res->arg1, PSTR("harvest")))
903 i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_HARVEST);
908 prog_char str_ballboard_setmode1_arg0[] = "ballboard";
909 parse_pgm_token_string_t cmd_ballboard_setmode1_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode1_result, arg0, str_ballboard_setmode1_arg0);
910 prog_char str_ballboard_setmode1_arg1[] = "init#eject#harvest#off";
911 parse_pgm_token_string_t cmd_ballboard_setmode1_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode1_result, arg1, str_ballboard_setmode1_arg1);
913 prog_char help_ballboard_setmode1[] = "set ballboard mode (mode)";
914 parse_pgm_inst_t cmd_ballboard_setmode1 = {
915 .f = cmd_ballboard_setmode1_parsed, /* function to call */
916 .data = NULL, /* 2nd arg of func */
917 .help_str = help_ballboard_setmode1,
918 .tokens = { /* token list, NULL terminated */
919 (prog_void *)&cmd_ballboard_setmode1_arg0,
920 (prog_void *)&cmd_ballboard_setmode1_arg1,
925 /**********************************************************/
926 /* Ballboard_Setmode2 */
928 /* this structure is filled when cmd_ballboard_setmode2 is parsed successfully */
929 struct cmd_ballboard_setmode2_result {
935 /* function called when cmd_ballboard_setmode2 is parsed successfully */
936 static void cmd_ballboard_setmode2_parsed(void * parsed_result, void * data)
938 struct cmd_ballboard_setmode2_result *res = parsed_result;
939 uint8_t mode = I2C_BALLBOARD_MODE_INIT;
941 if (!strcmp_P(res->arg2, PSTR("left"))) {
942 if (!strcmp_P(res->arg1, PSTR("prepare")))
943 mode = I2C_BALLBOARD_MODE_PREP_L_FORK;
944 else if (!strcmp_P(res->arg1, PSTR("take")))
945 mode = I2C_BALLBOARD_MODE_TAKE_L_FORK;
948 if (!strcmp_P(res->arg1, PSTR("prepare")))
949 mode = I2C_BALLBOARD_MODE_PREP_R_FORK;
950 else if (!strcmp_P(res->arg1, PSTR("take")))
951 mode = I2C_BALLBOARD_MODE_TAKE_R_FORK;
953 i2c_ballboard_set_mode(mode);
956 prog_char str_ballboard_setmode2_arg0[] = "ballboard";
957 parse_pgm_token_string_t cmd_ballboard_setmode2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode2_result, arg0, str_ballboard_setmode2_arg0);
958 prog_char str_ballboard_setmode2_arg1[] = "prepare#take";
959 parse_pgm_token_string_t cmd_ballboard_setmode2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode2_result, arg1, str_ballboard_setmode2_arg1);
960 prog_char str_ballboard_setmode2_arg2[] = "left#right";
961 parse_pgm_token_string_t cmd_ballboard_setmode2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode2_result, arg2, str_ballboard_setmode2_arg2);
963 prog_char help_ballboard_setmode2[] = "set ballboard mode (mode, side)";
964 parse_pgm_inst_t cmd_ballboard_setmode2 = {
965 .f = cmd_ballboard_setmode2_parsed, /* function to call */
966 .data = NULL, /* 2nd arg of func */
967 .help_str = help_ballboard_setmode2,
968 .tokens = { /* token list, NULL terminated */
969 (prog_void *)&cmd_ballboard_setmode2_arg0,
970 (prog_void *)&cmd_ballboard_setmode2_arg1,
971 (prog_void *)&cmd_ballboard_setmode2_arg2,
976 /**********************************************************/
977 /* Ballboard_Setmode3 */
979 /* this structure is filled when cmd_ballboard_setmode3 is parsed successfully */
980 struct cmd_ballboard_setmode3_result {
986 /* function called when cmd_ballboard_setmode3 is parsed successfully */
987 static void cmd_ballboard_setmode3_parsed(void *parsed_result, void *data)
989 struct cmd_ballboard_setmode3_result *res = parsed_result;
990 if (!strcmp_P(res->arg1, PSTR("xxx")))
994 prog_char str_ballboard_setmode3_arg0[] = "ballboard";
995 parse_pgm_token_string_t cmd_ballboard_setmode3_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode3_result, arg0, str_ballboard_setmode3_arg0);
996 prog_char str_ballboard_setmode3_arg1[] = "xxx";
997 parse_pgm_token_string_t cmd_ballboard_setmode3_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode3_result, arg1, str_ballboard_setmode3_arg1);
998 parse_pgm_token_num_t cmd_ballboard_setmode3_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_ballboard_setmode3_result, level, UINT8);
1000 prog_char help_ballboard_setmode3[] = "set ballboard mode (mode, level)";
1001 parse_pgm_inst_t cmd_ballboard_setmode3 = {
1002 .f = cmd_ballboard_setmode3_parsed, /* function to call */
1003 .data = NULL, /* 2nd arg of func */
1004 .help_str = help_ballboard_setmode3,
1005 .tokens = { /* token list, NULL terminated */
1006 (prog_void *)&cmd_ballboard_setmode3_arg0,
1007 (prog_void *)&cmd_ballboard_setmode3_arg1,
1008 (prog_void *)&cmd_ballboard_setmode3_arg2,
1013 /**********************************************************/
1016 /* this structure is filled when cmd_servo_balls is parsed successfully */
1017 struct cmd_servo_balls_result {
1018 fixed_string_t arg0;
1019 fixed_string_t arg1;
1022 /* function called when cmd_servo_balls is parsed successfully */
1023 static void cmd_servo_balls_parsed(void *parsed_result,
1024 __attribute__((unused)) void *data)
1026 struct cmd_servo_balls_result *res = parsed_result;
1028 if (!strcmp_P(res->arg1, PSTR("deploy")))
1029 support_balls_deploy();
1030 else if (!strcmp_P(res->arg1, PSTR("pack")))
1031 support_balls_pack();
1034 prog_char str_servo_balls_arg0[] = "support_balls";
1035 parse_pgm_token_string_t cmd_servo_balls_arg0 =
1036 TOKEN_STRING_INITIALIZER(struct cmd_servo_balls_result, arg0, str_servo_balls_arg0);
1037 prog_char str_servo_balls_arg1[] = "deploy#pack";
1038 parse_pgm_token_string_t cmd_servo_balls_arg1 =
1039 TOKEN_STRING_INITIALIZER(struct cmd_servo_balls_result, arg1, str_servo_balls_arg1);
1041 prog_char help_servo_balls[] = "control support balls";
1042 parse_pgm_inst_t cmd_servo_balls = {
1043 .f = cmd_servo_balls_parsed, /* function to call */
1044 .data = NULL, /* 2nd arg of func */
1045 .help_str = help_servo_balls,
1046 .tokens = { /* token list, NULL terminated */
1047 (prog_void *)&cmd_servo_balls_arg0,
1048 (prog_void *)&cmd_servo_balls_arg1,
1053 /**********************************************************/
1056 /* this structure is filled when cmd_clitoid is parsed successfully */
1057 struct cmd_clitoid_result {
1058 fixed_string_t arg0;
1067 /* function called when cmd_test is parsed successfully */
1068 static void cmd_clitoid_parsed(void *parsed_result, void *data)
1070 struct cmd_clitoid_result *res = parsed_result;
1071 /* clitoid(res->alpha_deg, res->beta_deg, res->R_mm, */
1072 /* res->Vd, res->Amax, res->d_inter_mm); */
1073 double x = position_get_x_double(&mainboard.pos);
1074 double y = position_get_y_double(&mainboard.pos);
1075 double a = position_get_a_rad_double(&mainboard.pos);
1077 strat_set_speed(res->Vd, SPEED_ANGLE_FAST);
1078 trajectory_clitoid(&mainboard.traj, x, y, a, 150.,
1079 res->alpha_deg, res->beta_deg, res->R_mm,
1083 prog_char str_clitoid_arg0[] = "clitoid";
1084 parse_pgm_token_string_t cmd_clitoid_arg0 =
1085 TOKEN_STRING_INITIALIZER(struct cmd_clitoid_result,
1086 arg0, str_clitoid_arg0);
1087 parse_pgm_token_num_t cmd_clitoid_alpha_deg =
1088 TOKEN_NUM_INITIALIZER(struct cmd_clitoid_result,
1090 parse_pgm_token_num_t cmd_clitoid_beta_deg =
1091 TOKEN_NUM_INITIALIZER(struct cmd_clitoid_result,
1093 parse_pgm_token_num_t cmd_clitoid_R_mm =
1094 TOKEN_NUM_INITIALIZER(struct cmd_clitoid_result,
1096 parse_pgm_token_num_t cmd_clitoid_Vd =
1097 TOKEN_NUM_INITIALIZER(struct cmd_clitoid_result,
1099 parse_pgm_token_num_t cmd_clitoid_Amax =
1100 TOKEN_NUM_INITIALIZER(struct cmd_clitoid_result,
1102 parse_pgm_token_num_t cmd_clitoid_d_inter_mm =
1103 TOKEN_NUM_INITIALIZER(struct cmd_clitoid_result,
1106 prog_char help_clitoid[] = "do a clitoid (alpha, beta, R, Vd, Amax, d_inter)";
1107 parse_pgm_inst_t cmd_clitoid = {
1108 .f = cmd_clitoid_parsed, /* function to call */
1109 .data = NULL, /* 2nd arg of func */
1110 .help_str = help_clitoid,
1111 .tokens = { /* token list, NULL terminated */
1112 (prog_void *)&cmd_clitoid_arg0,
1113 (prog_void *)&cmd_clitoid_alpha_deg,
1114 (prog_void *)&cmd_clitoid_beta_deg,
1115 (prog_void *)&cmd_clitoid_R_mm,
1116 (prog_void *)&cmd_clitoid_Vd,
1117 (prog_void *)&cmd_clitoid_Amax,
1118 (prog_void *)&cmd_clitoid_d_inter_mm,
1123 /**********************************************************/
1126 /* this structure is filled when cmd_test is parsed successfully */
1127 struct cmd_test_result {
1128 fixed_string_t arg0;
1136 #define LINE_L_DOWN 3
1138 #define LINE_R_DOWN 5
1145 static void num2line(struct line_2pts *l, uint8_t dir, uint8_t num)
1152 l->p1.x = n * 450 + 375;
1153 l->p1.y = COLOR_Y(0);
1154 l->p2.x = n * 450 + 375;
1155 l->p2.y = COLOR_Y(2100);
1158 l->p1.x = n * 450 + 375;
1159 l->p1.y = COLOR_Y(2100);
1160 l->p2.x = n * 450 + 375;
1161 l->p2.y = COLOR_Y(0);
1165 l->p1.y = COLOR_Y(-n * 500 + 1472);
1167 l->p2.y = COLOR_Y((-n + 4) * 500 + 972);
1171 l->p1.y = COLOR_Y((-n + 4) * 500 + 972);
1173 l->p2.y = COLOR_Y(-n * 500 + 1472);
1177 l->p1.y = COLOR_Y(-n * 500 + 1472);
1179 l->p2.y = COLOR_Y((-n + 4) * 500 + 972);
1183 l->p1.y = COLOR_Y((-n + 4) * 500 + 972);
1185 l->p2.y = COLOR_Y(-n * 500 + 1472);
1193 static void reverse_line(struct line_2pts *l)
1206 /* return 1 if there is a corn near, and fill the index ptr */
1207 static uint8_t corn_is_near(int8_t *corn_idx, uint8_t side)
1209 #define SENSOR_CORN_DIST 225
1210 #define SENSOR_CORN_ANGLE 90
1211 double x = position_get_x_double(&mainboard.pos);
1212 double y = position_get_y_double(&mainboard.pos);
1213 double a_rad = position_get_a_rad_double(&mainboard.pos);
1214 double x_corn, y_corn;
1215 int16_t x_corn_int, y_corn_int;
1217 if (side == I2C_LEFT_SIDE) {
1218 x_corn = x + cos(a_rad + RAD(SENSOR_CORN_ANGLE)) * SENSOR_CORN_DIST;
1219 y_corn = y + sin(a_rad + RAD(SENSOR_CORN_ANGLE)) * SENSOR_CORN_DIST;
1222 x_corn = x + cos(a_rad + RAD(-SENSOR_CORN_ANGLE)) * SENSOR_CORN_DIST;
1223 y_corn = y + sin(a_rad + RAD(-SENSOR_CORN_ANGLE)) * SENSOR_CORN_DIST;
1225 x_corn_int = x_corn;
1226 y_corn_int = y_corn;
1228 *corn_idx = xycoord_to_corn_idx(&x_corn_int, &y_corn_int);
1235 * - send the correct commands to the spickles
1236 * - return 1 if we need to stop (cobboard is stucked)
1238 static uint8_t handle_spickles(void)
1242 if (!corn_is_near(&corn_idx, I2C_LEFT_SIDE))
1243 i2c_cobboard_mode_deploy(I2C_LEFT_SIDE);
1245 if (corn_table[corn_idx] == TYPE_WHITE_CORN)
1246 i2c_cobboard_mode_harvest(I2C_LEFT_SIDE);
1248 i2c_cobboard_mode_pack(I2C_LEFT_SIDE);
1250 /* printf("%d %d\n", corn_idx, corn_table[corn_idx]); */
1251 /* time_wait_ms(100); */
1253 if (!corn_is_near(&corn_idx, I2C_RIGHT_SIDE))
1254 i2c_cobboard_mode_deploy(I2C_RIGHT_SIDE);
1256 if (corn_table[corn_idx] == TYPE_WHITE_CORN)
1257 i2c_cobboard_mode_harvest(I2C_RIGHT_SIDE);
1259 i2c_cobboard_mode_pack(I2C_RIGHT_SIDE);
1265 static void line2line(uint8_t dir1, uint8_t num1,
1266 uint8_t dir2, uint8_t num2)
1268 double line1_a_rad, line1_a_deg, line2_a_rad;
1269 double diff_a_deg, diff_a_deg_abs, beta_deg;
1271 struct line_2pts l1, l2;
1276 /* convert to 2 points */
1277 num2line(&l1, dir1, num1);
1278 num2line(&l2, dir2, num2);
1280 printf_P(PSTR("A2 (%2.2f, %2.2f) -> (%2.2f, %2.2f)\r\n"),
1281 l1.p1.x, l1.p1.y, l1.p2.x, l1.p2.y);
1282 printf_P(PSTR("B2 (%2.2f, %2.2f) -> (%2.2f, %2.2f)\r\n"),
1283 l2.p1.x, l2.p1.y, l2.p2.x, l2.p2.y);
1285 /* convert to line eq and find intersection */
1286 pts2line(&l1.p1, &l1.p2, &ll1);
1287 pts2line(&l2.p1, &l2.p2, &ll2);
1288 intersect_line(&ll1, &ll2, &p);
1290 line1_a_rad = atan2(l1.p2.y - l1.p1.y,
1292 line1_a_deg = DEG(line1_a_rad);
1293 line2_a_rad = atan2(l2.p2.y - l2.p1.y,
1295 diff_a_deg = DEG(line2_a_rad - line1_a_rad);
1296 diff_a_deg_abs = fabs(diff_a_deg);
1298 if (diff_a_deg_abs < 70.) {
1305 else if (diff_a_deg_abs < 100.) {
1320 trajectory_clitoid(&mainboard.traj, l1.p1.x, l1.p1.y,
1321 line1_a_deg, 150., diff_a_deg, beta_deg,
1322 radius, xy_norm(l1.p1.x, l1.p1.y,
1326 err = WAIT_COND_OR_TRAJ_END(handle_spickles(), 0xFF);
1328 /* cobboard is stucked */
1329 trajectory_hardstop(&mainboard.traj);
1330 return; /* XXX do something */
1332 err = test_traj_end(0xFF);
1337 /* function called when cmd_test is parsed successfully */
1338 static void cmd_test_parsed(void *parsed_result, void *data)
1341 strat_reset_pos(298.48, 309.21, 70.02);
1342 mainboard.angle.on = 1;
1343 mainboard.distance.on = 1;
1344 strat_set_speed(250, SPEED_ANGLE_FAST);
1346 init_corn_table(0, 0);
1349 line2line(LINE_UP, 0, LINE_R_DOWN, 2);
1350 line2line(LINE_R_DOWN, 2, LINE_R_UP, 2);
1351 line2line(LINE_R_UP, 2, LINE_UP, 5);
1353 trajectory_hardstop(&mainboard.traj);
1356 prog_char str_test_arg0[] = "test";
1357 parse_pgm_token_string_t cmd_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_test_result, arg0, str_test_arg0);
1358 parse_pgm_token_num_t cmd_test_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_test_result, radius, INT32);
1359 parse_pgm_token_num_t cmd_test_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_test_result, dist, INT32);
1361 prog_char help_test[] = "Test function";
1362 parse_pgm_inst_t cmd_test = {
1363 .f = cmd_test_parsed, /* function to call */
1364 .data = NULL, /* 2nd arg of func */
1365 .help_str = help_test,
1366 .tokens = { /* token list, NULL terminated */
1367 (prog_void *)&cmd_test_arg0,