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_utils.h>
45 #include <vect_base.h>
48 #include <obstacle_avoidance.h>
49 #include <blocking_detection_manager.h>
50 #include <robot_system.h>
51 #include <position_manager.h>
55 #include <parse_string.h>
56 #include <parse_num.h>
58 #include "../common/i2c_commands.h"
59 #include "../common/eeprom_mapping.h"
66 #include "strat_utils.h"
67 #include "strat_base.h"
68 #include "i2c_protocol.h"
71 struct cmd_event_result {
78 /* function called when cmd_event is parsed successfully */
79 static void cmd_event_parsed(void *parsed_result, void *data)
83 struct cmd_event_result * res = parsed_result;
85 if (!strcmp_P(res->arg1, PSTR("all"))) {
86 bit = DO_ENCODERS | DO_CS | DO_RS | DO_POS |
87 DO_BD | DO_TIMER | DO_POWER;
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");
111 if (!strcmp_P(res->arg1, PSTR("encoders")))
113 else if (!strcmp_P(res->arg1, PSTR("cs"))) {
117 else if (!strcmp_P(res->arg1, PSTR("rs")))
119 else if (!strcmp_P(res->arg1, PSTR("pos")))
121 else if (!strcmp_P(res->arg1, PSTR("bd")))
123 else if (!strcmp_P(res->arg1, PSTR("timer"))) {
127 else if (!strcmp_P(res->arg1, PSTR("power")))
130 if (!strcmp_P(res->arg2, PSTR("on")))
131 mainboard.flags |= bit;
132 else if (!strcmp_P(res->arg2, PSTR("off"))) {
133 if (!strcmp_P(res->arg1, PSTR("cs"))) {
135 robotsim_pwm(LEFT_PWM, 0);
136 robotsim_pwm(RIGHT_PWM, 0);
138 pwm_ng_set(LEFT_PWM, 0);
139 pwm_ng_set(RIGHT_PWM, 0);
142 mainboard.flags &= (~bit);
144 printf_P(PSTR("%s is %s\r\n"), res->arg1,
145 (bit & mainboard.flags) ? "on":"off");
148 prog_char str_event_arg0[] = "event";
149 parse_pgm_token_string_t cmd_event_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg0, str_event_arg0);
150 prog_char str_event_arg1[] = "all#encoders#cs#rs#pos#bd#timer#power";
151 parse_pgm_token_string_t cmd_event_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg1, str_event_arg1);
152 prog_char str_event_arg2[] = "on#off#show";
153 parse_pgm_token_string_t cmd_event_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg2, str_event_arg2);
155 prog_char help_event[] = "Enable/disable events";
156 parse_pgm_inst_t cmd_event = {
157 .f = cmd_event_parsed, /* function to call */
158 .data = NULL, /* 2nd arg of func */
159 .help_str = help_event,
160 .tokens = { /* token list, NULL terminated */
161 (prog_void *)&cmd_event_arg0,
162 (prog_void *)&cmd_event_arg1,
163 (prog_void *)&cmd_event_arg2,
169 /**********************************************************/
172 /* this structure is filled when cmd_spi_test is parsed successfully */
173 struct cmd_spi_test_result {
177 /* function called when cmd_spi_test is parsed successfully */
178 static void cmd_spi_test_parsed(void * parsed_result, void * data)
181 printf("not implemented\n");
183 uint16_t i = 0, ret = 0, ret2 = 0;
185 if (mainboard.flags & DO_ENCODERS) {
186 printf_P(PSTR("Disable encoder event first\r\n"));
192 ret = spi_send_and_receive_byte(i);
193 ret2 = spi_send_and_receive_byte(i);
194 spi_slave_deselect(0);
196 if ((i & 0x7ff) == 0)
197 printf_P(PSTR("Sent %.4x twice, received %x %x\r\n"),
201 } while(!cmdline_keypressed());
205 prog_char str_spi_test_arg0[] = "spi_test";
206 parse_pgm_token_string_t cmd_spi_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_spi_test_result, arg0, str_spi_test_arg0);
208 prog_char help_spi_test[] = "Test the SPI";
209 parse_pgm_inst_t cmd_spi_test = {
210 .f = cmd_spi_test_parsed, /* function to call */
211 .data = NULL, /* 2nd arg of func */
212 .help_str = help_spi_test,
213 .tokens = { /* token list, NULL terminated */
214 (prog_void *)&cmd_spi_test_arg0,
221 /**********************************************************/
224 /* this structure is filled when cmd_opponent is parsed successfully */
225 struct cmd_opponent_result {
232 /* function called when cmd_opponent is parsed successfully */
233 static void cmd_opponent_parsed(void *parsed_result, void *data)
237 if (get_opponent_xyda(&x, &y, &d, &a))
238 printf_P(PSTR("No opponent\r\n"));
240 printf_P(PSTR("x=%d y=%d, d=%d a=%d\r\n"), x, y, d, a);
243 prog_char str_opponent_arg0[] = "opponent";
244 parse_pgm_token_string_t cmd_opponent_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_opponent_result, arg0, str_opponent_arg0);
245 prog_char str_opponent_arg1[] = "show";
246 parse_pgm_token_string_t cmd_opponent_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_opponent_result, arg1, str_opponent_arg1);
248 prog_char help_opponent[] = "Show (x,y) opponent";
249 parse_pgm_inst_t cmd_opponent = {
250 .f = cmd_opponent_parsed, /* function to call */
251 .data = NULL, /* 2nd arg of func */
252 .help_str = help_opponent,
253 .tokens = { /* token list, NULL terminated */
254 (prog_void *)&cmd_opponent_arg0,
255 (prog_void *)&cmd_opponent_arg1,
261 prog_char str_opponent_arg1_set[] = "set";
262 parse_pgm_token_string_t cmd_opponent_arg1_set = TOKEN_STRING_INITIALIZER(struct cmd_opponent_result, arg1, str_opponent_arg1_set);
263 parse_pgm_token_num_t cmd_opponent_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_opponent_result, arg2, INT32);
264 parse_pgm_token_num_t cmd_opponent_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_opponent_result, arg3, INT32);
266 prog_char help_opponent_set[] = "Set (x,y) opponent";
267 parse_pgm_inst_t cmd_opponent_set = {
268 .f = cmd_opponent_parsed, /* function to call */
269 .data = NULL, /* 2nd arg of func */
270 .help_str = help_opponent_set,
271 .tokens = { /* token list, NULL terminated */
272 (prog_void *)&cmd_opponent_arg0,
273 (prog_void *)&cmd_opponent_arg1_set,
274 (prog_void *)&cmd_opponent_arg2,
275 (prog_void *)&cmd_opponent_arg3,
281 /**********************************************************/
284 /* this structure is filled when cmd_start is parsed successfully */
285 struct cmd_start_result {
287 fixed_string_t color;
288 fixed_string_t debug;
291 /* function called when cmd_start is parsed successfully */
292 static void cmd_start_parsed(void *parsed_result, void *data)
295 printf("not implemented\n");
297 struct cmd_start_result *res = parsed_result;
298 uint8_t old_level = gen.log_level;
300 gen.logs[NB_LOGS] = E_USER_STRAT;
301 if (!strcmp_P(res->debug, PSTR("debug"))) {
302 strat_infos.dump_enabled = 1;
306 strat_infos.dump_enabled = 0;
310 if (!strcmp_P(res->color, PSTR("yellow"))) {
311 mainboard.our_color = I2C_COLOR_YELLOW;
312 i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_YELLOW);
313 i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_YELLOW);
315 else if (!strcmp_P(res->color, PSTR("blue"))) {
316 mainboard.our_color = I2C_COLOR_BLUE;
317 i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_BLUE);
318 i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_BLUE);
323 gen.logs[NB_LOGS] = 0;
324 gen.log_level = old_level;
328 prog_char str_start_arg0[] = "start";
329 parse_pgm_token_string_t cmd_start_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_start_result, arg0, str_start_arg0);
330 prog_char str_start_color[] = "blue#yellow";
331 parse_pgm_token_string_t cmd_start_color = TOKEN_STRING_INITIALIZER(struct cmd_start_result, color, str_start_color);
332 prog_char str_start_debug[] = "debug#match";
333 parse_pgm_token_string_t cmd_start_debug = TOKEN_STRING_INITIALIZER(struct cmd_start_result, debug, str_start_debug);
335 prog_char help_start[] = "Start the robot";
336 parse_pgm_inst_t cmd_start = {
337 .f = cmd_start_parsed, /* function to call */
338 .data = NULL, /* 2nd arg of func */
339 .help_str = help_start,
340 .tokens = { /* token list, NULL terminated */
341 (prog_void *)&cmd_start_arg0,
342 (prog_void *)&cmd_start_color,
343 (prog_void *)&cmd_start_debug,
350 /**********************************************************/
353 /* this structure is filled when cmd_interact is parsed successfully */
354 struct cmd_interact_result {
358 static void print_cs(void)
360 printf_P(PSTR("cons_d=% .8"PRIi32" cons_a=% .8"PRIi32" fil_d=% .8"PRIi32" fil_a=% .8"PRIi32" "
361 "err_d=% .8"PRIi32" err_a=% .8"PRIi32" out_d=% .8"PRIi32" out_a=% .8"PRIi32"\r\n"),
362 cs_get_consign(&mainboard.distance.cs),
363 cs_get_consign(&mainboard.angle.cs),
364 cs_get_filtered_consign(&mainboard.distance.cs),
365 cs_get_filtered_consign(&mainboard.angle.cs),
366 cs_get_error(&mainboard.distance.cs),
367 cs_get_error(&mainboard.angle.cs),
368 cs_get_out(&mainboard.distance.cs),
369 cs_get_out(&mainboard.angle.cs));
372 static void print_pos(void)
374 printf_P(PSTR("x=% .8d y=% .8d a=% .8d\r\n"),
375 position_get_x_s16(&mainboard.pos),
376 position_get_y_s16(&mainboard.pos),
377 position_get_a_deg_s16(&mainboard.pos));
380 static void print_time(void)
382 printf_P(PSTR("time %d\r\n"),time_get_s());
386 static void print_sensors(void)
389 if (sensor_start_switch())
390 printf_P(PSTR("Start switch | "));
392 printf_P(PSTR(" | "));
394 if (IR_DISP_SENSOR())
395 printf_P(PSTR("IR disp | "));
397 printf_P(PSTR(" | "));
399 printf_P(PSTR("\r\n"));
403 static void print_pid(void)
405 printf_P(PSTR("P=% .8"PRIi32" I=% .8"PRIi32" D=% .8"PRIi32" out=% .8"PRIi32" | "
406 "P=% .8"PRIi32" I=% .8"PRIi32" D=% .8"PRIi32" out=% .8"PRIi32"\r\n"),
407 pid_get_value_in(&mainboard.distance.pid) * pid_get_gain_P(&mainboard.distance.pid),
408 pid_get_value_I(&mainboard.distance.pid) * pid_get_gain_I(&mainboard.distance.pid),
409 pid_get_value_D(&mainboard.distance.pid) * pid_get_gain_D(&mainboard.distance.pid),
410 pid_get_value_out(&mainboard.distance.pid),
411 pid_get_value_in(&mainboard.angle.pid) * pid_get_gain_P(&mainboard.angle.pid),
412 pid_get_value_I(&mainboard.angle.pid) * pid_get_gain_I(&mainboard.angle.pid),
413 pid_get_value_D(&mainboard.angle.pid) * pid_get_gain_D(&mainboard.angle.pid),
414 pid_get_value_out(&mainboard.angle.pid));
417 #define PRINT_POS (1<<0)
418 #define PRINT_PID (1<<1)
419 #define PRINT_CS (1<<2)
420 #define PRINT_SENSORS (1<<3)
421 #define PRINT_TIME (1<<4)
422 #define PRINT_BLOCKING (1<<5)
424 static void cmd_interact_parsed(void * parsed_result, void * data)
433 printf_P(PSTR("Display debugs:\r\n"
439 /* " 6:blocking\r\n" */
446 mainboard.flags &= (~DO_CS);
447 pwm_set_and_save(LEFT_PWM, 0);
448 pwm_set_and_save(RIGHT_PWM, 0);
451 if (print & PRINT_POS) {
455 if (print & PRINT_PID) {
459 if (print & PRINT_CS) {
463 if (print & PRINT_SENSORS) {
467 if (print & PRINT_TIME) {
470 /* if (print & PRINT_BLOCKING) { */
471 /* printf_P(PSTR("%s %s blocking=%d\r\n"), */
472 /* mainboard.blocking ? "BLOCK1":" ", */
473 /* rs_is_blocked(&mainboard.rs) ? "BLOCK2":" ", */
474 /* rs_get_blocking(&mainboard.rs)); */
477 c = cmdline_getchar();
482 cmd = vt100_parser(&vt100, c);
490 case '1': print ^= PRINT_POS; break;
491 case '2': print ^= PRINT_PID; break;
492 case '3': print ^= PRINT_CS; break;
493 case '4': print ^= PRINT_SENSORS; break;
494 case '5': print ^= PRINT_TIME; break;
495 case '6': print ^= PRINT_BLOCKING; break;
498 if (mainboard.flags & DO_CS)
500 pwm_set_and_save(LEFT_PWM, 0);
501 pwm_set_and_save(RIGHT_PWM, 0);
504 pwm_set_and_save(LEFT_PWM, 0);
505 pwm_set_and_save(RIGHT_PWM, 0);
514 pwm_set_and_save(LEFT_PWM, 1200);
515 pwm_set_and_save(RIGHT_PWM, 1200);
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);
535 prog_char str_interact_arg0[] = "interact";
536 parse_pgm_token_string_t cmd_interact_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_interact_result, arg0, str_interact_arg0);
538 prog_char help_interact[] = "Interactive mode";
539 parse_pgm_inst_t cmd_interact = {
540 .f = cmd_interact_parsed, /* function to call */
541 .data = NULL, /* 2nd arg of func */
542 .help_str = help_interact,
543 .tokens = { /* token list, NULL terminated */
544 (prog_void *)&cmd_interact_arg0,
550 /**********************************************************/
553 /* this structure is filled when cmd_color is parsed successfully */
554 struct cmd_color_result {
556 fixed_string_t color;
559 /* function called when cmd_color is parsed successfully */
560 static void cmd_color_parsed(void *parsed_result, void *data)
563 printf("not implemented\n");
565 struct cmd_color_result *res = (struct cmd_color_result *) parsed_result;
566 if (!strcmp_P(res->color, PSTR("yellow"))) {
567 mainboard.our_color = I2C_COLOR_YELLOW;
568 i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_YELLOW);
569 i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_YELLOW);
571 else if (!strcmp_P(res->color, PSTR("blue"))) {
572 mainboard.our_color = I2C_COLOR_BLUE;
573 i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_BLUE);
574 i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_BLUE);
576 printf_P(PSTR("Done\r\n"));
580 prog_char str_color_arg0[] = "color";
581 parse_pgm_token_string_t cmd_color_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_color_result, arg0, str_color_arg0);
582 prog_char str_color_color[] = "blue#yellow";
583 parse_pgm_token_string_t cmd_color_color = TOKEN_STRING_INITIALIZER(struct cmd_color_result, color, str_color_color);
585 prog_char help_color[] = "Set our color";
586 parse_pgm_inst_t cmd_color = {
587 .f = cmd_color_parsed, /* function to call */
588 .data = NULL, /* 2nd arg of func */
589 .help_str = help_color,
590 .tokens = { /* token list, NULL terminated */
591 (prog_void *)&cmd_color_arg0,
592 (prog_void *)&cmd_color_color,
598 /**********************************************************/
601 /* this structure is filled when cmd_rs is parsed successfully */
602 struct cmd_rs_result {
607 /* function called when cmd_rs is parsed successfully */
608 static void cmd_rs_parsed(void *parsed_result, void *data)
610 // struct cmd_rs_result *res = parsed_result;
612 printf_P(PSTR("angle cons=% .6"PRIi32" in=% .6"PRIi32" out=% .6"PRIi32" / "),
613 cs_get_consign(&mainboard.angle.cs),
614 cs_get_filtered_feedback(&mainboard.angle.cs),
615 cs_get_out(&mainboard.angle.cs));
616 printf_P(PSTR("distance cons=% .6"PRIi32" in=% .6"PRIi32" out=% .6"PRIi32" / "),
617 cs_get_consign(&mainboard.distance.cs),
618 cs_get_filtered_feedback(&mainboard.distance.cs),
619 cs_get_out(&mainboard.distance.cs));
620 printf_P(PSTR("l=% .4"PRIi32" r=% .4"PRIi32"\r\n"), mainboard.pwm_l,
623 } while(!cmdline_keypressed());
626 prog_char str_rs_arg0[] = "rs";
627 parse_pgm_token_string_t cmd_rs_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_rs_result, arg0, str_rs_arg0);
628 prog_char str_rs_arg1[] = "show";
629 parse_pgm_token_string_t cmd_rs_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_rs_result, arg1, str_rs_arg1);
631 prog_char help_rs[] = "Show rs (robot system) values";
632 parse_pgm_inst_t cmd_rs = {
633 .f = cmd_rs_parsed, /* function to call */
634 .data = NULL, /* 2nd arg of func */
636 .tokens = { /* token list, NULL terminated */
637 (prog_void *)&cmd_rs_arg0,
638 (prog_void *)&cmd_rs_arg1,
643 /**********************************************************/
646 /* this structure is filled when cmd_i2cdebug is parsed successfully */
647 struct cmd_i2cdebug_result {
651 /* function called when cmd_i2cdebug is parsed successfully */
652 static void cmd_i2cdebug_parsed(void * parsed_result, void * data)
655 printf("not implemented\n");
658 i2c_protocol_debug();
662 prog_char str_i2cdebug_arg0[] = "i2cdebug";
663 parse_pgm_token_string_t cmd_i2cdebug_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_i2cdebug_result, arg0, str_i2cdebug_arg0);
665 prog_char help_i2cdebug[] = "I2c debug infos";
666 parse_pgm_inst_t cmd_i2cdebug = {
667 .f = cmd_i2cdebug_parsed, /* function to call */
668 .data = NULL, /* 2nd arg of func */
669 .help_str = help_i2cdebug,
670 .tokens = { /* token list, NULL terminated */
671 (prog_void *)&cmd_i2cdebug_arg0,
676 /**********************************************************/
679 /* this structure is filled when cmd_cobboard_show is parsed successfully */
680 struct cmd_cobboard_show_result {
685 /* function called when cmd_cobboard_show is parsed successfully */
686 static void cmd_cobboard_show_parsed(void * parsed_result, void * data)
689 printf("not implemented\n");
691 printf_P(PSTR("mode = %x\r\n"), cobboard.mode);
692 printf_P(PSTR("status = %x\r\n"), cobboard.status);
693 printf_P(PSTR("cob_count = %x\r\n"), cobboard.cob_count);
694 printf_P(PSTR("left_cobroller_speed = %d\r\n"), cobboard.left_cobroller_speed);
695 printf_P(PSTR("right_cobroller_speed = %d\r\n"), cobboard.right_cobroller_speed);
699 prog_char str_cobboard_show_arg0[] = "cobboard";
700 parse_pgm_token_string_t cmd_cobboard_show_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_show_result, arg0, str_cobboard_show_arg0);
701 prog_char str_cobboard_show_arg1[] = "show";
702 parse_pgm_token_string_t cmd_cobboard_show_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_show_result, arg1, str_cobboard_show_arg1);
704 prog_char help_cobboard_show[] = "show cobboard status";
705 parse_pgm_inst_t cmd_cobboard_show = {
706 .f = cmd_cobboard_show_parsed, /* function to call */
707 .data = NULL, /* 2nd arg of func */
708 .help_str = help_cobboard_show,
709 .tokens = { /* token list, NULL terminated */
710 (prog_void *)&cmd_cobboard_show_arg0,
711 (prog_void *)&cmd_cobboard_show_arg1,
716 /**********************************************************/
717 /* Cobboard_Setmode1 */
719 /* this structure is filled when cmd_cobboard_setmode1 is parsed successfully */
720 struct cmd_cobboard_setmode1_result {
725 /* function called when cmd_cobboard_setmode1 is parsed successfully */
726 static void cmd_cobboard_setmode1_parsed(void *parsed_result, void *data)
729 printf("not implemented\n");
731 struct cmd_cobboard_setmode1_result *res = parsed_result;
733 if (!strcmp_P(res->arg1, PSTR("init")))
734 i2c_cobboard_mode_init();
735 else if (!strcmp_P(res->arg1, PSTR("eject")))
736 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)
771 printf("not implemented\n");
773 struct cmd_cobboard_setmode2_result *res = parsed_result;
774 uint8_t side = I2C_LEFT_SIDE;
776 if (!strcmp_P(res->arg2, PSTR("left")))
777 side = I2C_LEFT_SIDE;
778 else if (!strcmp_P(res->arg2, PSTR("right")))
779 side = I2C_RIGHT_SIDE;
781 if (!strcmp_P(res->arg1, PSTR("deploy")))
782 i2c_cobboard_mode_deploy(side);
783 else if (!strcmp_P(res->arg1, PSTR("harvest")))
784 i2c_cobboard_mode_harvest(side);
785 else if (!strcmp_P(res->arg1, PSTR("pack")))
786 i2c_cobboard_mode_pack(side);
790 prog_char str_cobboard_setmode2_arg0[] = "cobboard";
791 parse_pgm_token_string_t cmd_cobboard_setmode2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode2_result, arg0, str_cobboard_setmode2_arg0);
792 prog_char str_cobboard_setmode2_arg1[] = "harvest#deploy#pack";
793 parse_pgm_token_string_t cmd_cobboard_setmode2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode2_result, arg1, str_cobboard_setmode2_arg1);
794 prog_char str_cobboard_setmode2_arg2[] = "left#right";
795 parse_pgm_token_string_t cmd_cobboard_setmode2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode2_result, arg2, str_cobboard_setmode2_arg2);
797 prog_char help_cobboard_setmode2[] = "set cobboard mode (mode, side)";
798 parse_pgm_inst_t cmd_cobboard_setmode2 = {
799 .f = cmd_cobboard_setmode2_parsed, /* function to call */
800 .data = NULL, /* 2nd arg of func */
801 .help_str = help_cobboard_setmode2,
802 .tokens = { /* token list, NULL terminated */
803 (prog_void *)&cmd_cobboard_setmode2_arg0,
804 (prog_void *)&cmd_cobboard_setmode2_arg1,
805 (prog_void *)&cmd_cobboard_setmode2_arg2,
810 /**********************************************************/
811 /* Cobboard_Setmode3 */
813 /* this structure is filled when cmd_cobboard_setmode3 is parsed successfully */
814 struct cmd_cobboard_setmode3_result {
820 /* function called when cmd_cobboard_setmode3 is parsed successfully */
821 static void cmd_cobboard_setmode3_parsed(void *parsed_result, void *data)
824 printf("not implemented\n");
826 struct cmd_cobboard_setmode3_result *res = parsed_result;
827 if (!strcmp_P(res->arg1, PSTR("xxx")))
832 prog_char str_cobboard_setmode3_arg0[] = "cobboard";
833 parse_pgm_token_string_t cmd_cobboard_setmode3_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode3_result, arg0, str_cobboard_setmode3_arg0);
834 prog_char str_cobboard_setmode3_arg1[] = "xxx";
835 parse_pgm_token_string_t cmd_cobboard_setmode3_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode3_result, arg1, str_cobboard_setmode3_arg1);
836 parse_pgm_token_num_t cmd_cobboard_setmode3_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_cobboard_setmode3_result, level, UINT8);
838 prog_char help_cobboard_setmode3[] = "set cobboard mode (mode, level)";
839 parse_pgm_inst_t cmd_cobboard_setmode3 = {
840 .f = cmd_cobboard_setmode3_parsed, /* function to call */
841 .data = NULL, /* 2nd arg of func */
842 .help_str = help_cobboard_setmode3,
843 .tokens = { /* token list, NULL terminated */
844 (prog_void *)&cmd_cobboard_setmode3_arg0,
845 (prog_void *)&cmd_cobboard_setmode3_arg1,
846 (prog_void *)&cmd_cobboard_setmode3_arg2,
851 /**********************************************************/
854 /* this structure is filled when cmd_ballboard_show is parsed successfully */
855 struct cmd_ballboard_show_result {
860 /* function called when cmd_ballboard_show is parsed successfully */
861 static void cmd_ballboard_show_parsed(void * parsed_result, void * data)
864 printf("not implemented\n");
866 printf_P(PSTR("mode = %x\r\n"), ballboard.mode);
867 printf_P(PSTR("status = %x\r\n"), ballboard.status);
868 printf_P(PSTR("ball_count = %d\r\n"), ballboard.ball_count);
872 prog_char str_ballboard_show_arg0[] = "ballboard";
873 parse_pgm_token_string_t cmd_ballboard_show_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_show_result, arg0, str_ballboard_show_arg0);
874 prog_char str_ballboard_show_arg1[] = "show";
875 parse_pgm_token_string_t cmd_ballboard_show_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_show_result, arg1, str_ballboard_show_arg1);
877 prog_char help_ballboard_show[] = "show ballboard status";
878 parse_pgm_inst_t cmd_ballboard_show = {
879 .f = cmd_ballboard_show_parsed, /* function to call */
880 .data = NULL, /* 2nd arg of func */
881 .help_str = help_ballboard_show,
882 .tokens = { /* token list, NULL terminated */
883 (prog_void *)&cmd_ballboard_show_arg0,
884 (prog_void *)&cmd_ballboard_show_arg1,
889 /**********************************************************/
890 /* Ballboard_Setmode1 */
892 /* this structure is filled when cmd_ballboard_setmode1 is parsed successfully */
893 struct cmd_ballboard_setmode1_result {
898 /* function called when cmd_ballboard_setmode1 is parsed successfully */
899 static void cmd_ballboard_setmode1_parsed(void *parsed_result, void *data)
902 printf("not implemented\n");
904 struct cmd_ballboard_setmode1_result *res = parsed_result;
906 if (!strcmp_P(res->arg1, PSTR("init")))
907 i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_INIT);
908 else if (!strcmp_P(res->arg1, PSTR("off")))
909 i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_OFF);
910 else if (!strcmp_P(res->arg1, PSTR("eject")))
911 i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_EJECT);
912 else if (!strcmp_P(res->arg1, PSTR("harvest")))
913 i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_HARVEST);
919 prog_char str_ballboard_setmode1_arg0[] = "ballboard";
920 parse_pgm_token_string_t cmd_ballboard_setmode1_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode1_result, arg0, str_ballboard_setmode1_arg0);
921 prog_char str_ballboard_setmode1_arg1[] = "init#eject#harvest#off";
922 parse_pgm_token_string_t cmd_ballboard_setmode1_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode1_result, arg1, str_ballboard_setmode1_arg1);
924 prog_char help_ballboard_setmode1[] = "set ballboard mode (mode)";
925 parse_pgm_inst_t cmd_ballboard_setmode1 = {
926 .f = cmd_ballboard_setmode1_parsed, /* function to call */
927 .data = NULL, /* 2nd arg of func */
928 .help_str = help_ballboard_setmode1,
929 .tokens = { /* token list, NULL terminated */
930 (prog_void *)&cmd_ballboard_setmode1_arg0,
931 (prog_void *)&cmd_ballboard_setmode1_arg1,
936 /**********************************************************/
937 /* Ballboard_Setmode2 */
939 /* this structure is filled when cmd_ballboard_setmode2 is parsed successfully */
940 struct cmd_ballboard_setmode2_result {
946 /* function called when cmd_ballboard_setmode2 is parsed successfully */
947 static void cmd_ballboard_setmode2_parsed(void * parsed_result, void * data)
950 printf("not implemented\n");
952 struct cmd_ballboard_setmode2_result *res = parsed_result;
953 uint8_t mode = I2C_BALLBOARD_MODE_INIT;
955 if (!strcmp_P(res->arg2, PSTR("left"))) {
956 if (!strcmp_P(res->arg1, PSTR("prepare")))
957 mode = I2C_BALLBOARD_MODE_PREP_L_FORK;
958 else if (!strcmp_P(res->arg1, PSTR("take")))
959 mode = I2C_BALLBOARD_MODE_TAKE_L_FORK;
962 if (!strcmp_P(res->arg1, PSTR("prepare")))
963 mode = I2C_BALLBOARD_MODE_PREP_R_FORK;
964 else if (!strcmp_P(res->arg1, PSTR("take")))
965 mode = I2C_BALLBOARD_MODE_TAKE_R_FORK;
967 i2c_ballboard_set_mode(mode);
971 prog_char str_ballboard_setmode2_arg0[] = "ballboard";
972 parse_pgm_token_string_t cmd_ballboard_setmode2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode2_result, arg0, str_ballboard_setmode2_arg0);
973 prog_char str_ballboard_setmode2_arg1[] = "prepare#take";
974 parse_pgm_token_string_t cmd_ballboard_setmode2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode2_result, arg1, str_ballboard_setmode2_arg1);
975 prog_char str_ballboard_setmode2_arg2[] = "left#right";
976 parse_pgm_token_string_t cmd_ballboard_setmode2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode2_result, arg2, str_ballboard_setmode2_arg2);
978 prog_char help_ballboard_setmode2[] = "set ballboard mode (mode, side)";
979 parse_pgm_inst_t cmd_ballboard_setmode2 = {
980 .f = cmd_ballboard_setmode2_parsed, /* function to call */
981 .data = NULL, /* 2nd arg of func */
982 .help_str = help_ballboard_setmode2,
983 .tokens = { /* token list, NULL terminated */
984 (prog_void *)&cmd_ballboard_setmode2_arg0,
985 (prog_void *)&cmd_ballboard_setmode2_arg1,
986 (prog_void *)&cmd_ballboard_setmode2_arg2,
991 /**********************************************************/
992 /* Ballboard_Setmode3 */
994 /* this structure is filled when cmd_ballboard_setmode3 is parsed successfully */
995 struct cmd_ballboard_setmode3_result {
1001 /* function called when cmd_ballboard_setmode3 is parsed successfully */
1002 static void cmd_ballboard_setmode3_parsed(void *parsed_result, void *data)
1005 printf("not implemented\n");
1007 struct cmd_ballboard_setmode3_result *res = parsed_result;
1008 if (!strcmp_P(res->arg1, PSTR("xxx")))
1013 prog_char str_ballboard_setmode3_arg0[] = "ballboard";
1014 parse_pgm_token_string_t cmd_ballboard_setmode3_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode3_result, arg0, str_ballboard_setmode3_arg0);
1015 prog_char str_ballboard_setmode3_arg1[] = "xxx";
1016 parse_pgm_token_string_t cmd_ballboard_setmode3_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode3_result, arg1, str_ballboard_setmode3_arg1);
1017 parse_pgm_token_num_t cmd_ballboard_setmode3_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_ballboard_setmode3_result, level, UINT8);
1019 prog_char help_ballboard_setmode3[] = "set ballboard mode (mode, level)";
1020 parse_pgm_inst_t cmd_ballboard_setmode3 = {
1021 .f = cmd_ballboard_setmode3_parsed, /* function to call */
1022 .data = NULL, /* 2nd arg of func */
1023 .help_str = help_ballboard_setmode3,
1024 .tokens = { /* token list, NULL terminated */
1025 (prog_void *)&cmd_ballboard_setmode3_arg0,
1026 (prog_void *)&cmd_ballboard_setmode3_arg1,
1027 (prog_void *)&cmd_ballboard_setmode3_arg2,
1032 /**********************************************************/
1035 /* this structure is filled when cmd_servo_balls is parsed successfully */
1036 struct cmd_servo_balls_result {
1037 fixed_string_t arg0;
1038 fixed_string_t arg1;
1041 /* function called when cmd_servo_balls is parsed successfully */
1042 static void cmd_servo_balls_parsed(void *parsed_result,
1043 __attribute__((unused)) void *data)
1045 struct cmd_servo_balls_result *res = parsed_result;
1047 if (!strcmp_P(res->arg1, PSTR("deploy")))
1048 support_balls_deploy();
1049 else if (!strcmp_P(res->arg1, PSTR("pack")))
1050 support_balls_pack();
1053 prog_char str_servo_balls_arg0[] = "support_balls";
1054 parse_pgm_token_string_t cmd_servo_balls_arg0 =
1055 TOKEN_STRING_INITIALIZER(struct cmd_servo_balls_result, arg0, str_servo_balls_arg0);
1056 prog_char str_servo_balls_arg1[] = "deploy#pack";
1057 parse_pgm_token_string_t cmd_servo_balls_arg1 =
1058 TOKEN_STRING_INITIALIZER(struct cmd_servo_balls_result, arg1, str_servo_balls_arg1);
1060 prog_char help_servo_balls[] = "control support balls";
1061 parse_pgm_inst_t cmd_servo_balls = {
1062 .f = cmd_servo_balls_parsed, /* function to call */
1063 .data = NULL, /* 2nd arg of func */
1064 .help_str = help_servo_balls,
1065 .tokens = { /* token list, NULL terminated */
1066 (prog_void *)&cmd_servo_balls_arg0,
1067 (prog_void *)&cmd_servo_balls_arg1,
1072 /**********************************************************/
1075 /* this structure is filled when cmd_clitoid is parsed successfully */
1076 struct cmd_clitoid_result {
1077 fixed_string_t arg0;
1087 * do a superb curve joining line1 to line2 which is composed of:
1088 * - a clothoid starting from line1
1090 * - another clothoid up to line2
1092 * the function assumes that the initial linear speed is Vd and
1093 * angular speed is 0.
1095 * - alpha: total angle
1096 * - beta: circular part of angle (lower than alpha)
1097 * - R: the radius of the circle (must be != 0)
1098 * - Vd: linear speed to use (in imp per cs period)
1099 * - Amax: maximum angular acceleration
1100 * - d_inter: distance in mm until the intersection of the
1103 * return 0 on success: in this case these parameters are filled:
1104 * - Aa_out: the angular acceleration to configure in quadramp
1105 * - remain_d_mm_out: remaining distance before start to turn
1107 uint8_t clitoid(double alpha_deg, double beta_deg, double R_mm,
1108 double Vd, double Amax, double d_inter_mm)
1112 double t, d_mm, alpha_rad, beta_rad;
1114 double Aa, Aa_rd_s2;
1115 line_t line1, line2;
1117 point_t robot, intersect, pt2, center, proj;
1121 if (fabs(alpha_deg) <= fabs(beta_deg)) {
1122 DEBUG(E_USER_STRAT, "alpha is smaller than beta");
1126 /* get angular speed Va */
1127 Vd_mm_s = Vd * (CS_HZ/DIST_IMP_MM);
1128 DEBUG(E_USER_STRAT, "Vd_mm_s=%2.2f", Vd_mm_s);
1129 Va_rd_s = Vd_mm_s / R_mm;
1130 Va = Va_rd_s * (DIST_IMP_MM * EXT_TRACK_MM / (2 * CS_HZ));
1131 DEBUG(E_USER_STRAT, "Va_rd_s=%2.2f Va=%2.2f", Va_rd_s, Va);
1133 /* process 't', the time in seconds that we will take to do
1134 * the first clothoid */
1135 alpha_rad = RAD(alpha_deg);
1136 beta_rad = RAD(beta_deg);
1137 t = fabs(((alpha_rad - beta_rad) * R_mm) / Vd_mm_s);
1138 DEBUG(E_USER_STRAT, "R_mm=%2.2f alpha_rad=%2.2f beta_rad=%2.2f t=%2.2f",
1139 R_mm, alpha_rad, beta_rad, t);
1141 /* process the angular acceleration */
1142 Aa_rd_s2 = Va_rd_s / t;
1143 Aa = Aa_rd_s2 * (DIST_IMP_MM * EXT_TRACK_MM /
1144 (2 * CS_HZ * CS_HZ));
1145 DEBUG(E_USER_STRAT, "Aa_rd_s2=%2.2f Aa=%2.2f", Aa_rd_s2, Aa);
1147 /* exit if the robot cannot physically do it */
1149 DEBUG(E_USER_STRAT, "greater than max acceleration");
1153 /* the robot position */
1154 x = position_get_x_double(&mainboard.pos);
1155 y = position_get_y_double(&mainboard.pos);
1156 a_rad = position_get_a_rad_double(&mainboard.pos);
1158 /* define line1 and line2 */
1161 intersect.x = x + cos(a_rad) * d_inter_mm;
1162 intersect.y = y + sin(a_rad) * d_inter_mm;
1163 pts2line(&robot, &intersect, &line1);
1164 pt2.x = intersect.x + cos(a_rad + alpha_rad);
1165 pt2.y = intersect.y + sin(a_rad + alpha_rad);
1166 pts2line(&intersect, &pt2, &line2);
1167 DEBUG(E_USER_STRAT, "intersect=(%2.2f, %2.2f)",
1168 intersect.x, intersect.y);
1170 /* the center of the circle is at (d_mm, d_mm) when we have to
1171 * start the clothoid */
1172 d_mm = R_mm * sqrt(fabs(alpha_rad - beta_rad)) *
1174 DEBUG(E_USER_STRAT, "d_mm=%2.2f", d_mm);
1176 /* translate line1 */
1177 v.x = intersect.x - robot.x;
1178 v.y = intersect.y - robot.y;
1183 vect_resize(&v, d_mm);
1184 line_translate(&line1, &v);
1186 /* translate line2 */
1187 v.x = intersect.x - pt2.x;
1188 v.y = intersect.y - pt2.y;
1193 vect_resize(&v, d_mm);
1194 line_translate(&line2, &v);
1196 /* find the center of the circle, at the intersection of the
1197 * new translated lines */
1198 if (intersect_line(&line1, &line2, ¢er) != 1) {
1199 DEBUG(E_USER_STRAT, "cannot find circle center");
1202 DEBUG(E_USER_STRAT, "center=(%2.2f,%2.2f)", center.x, center.y);
1204 /* project center of circle on line1 */
1205 proj_pt_line(¢er, &line1, &proj);
1206 DEBUG(E_USER_STRAT, "proj=(%2.2f,%2.2f)", proj.x, proj.y);
1208 /* process remaining distance before start turning */
1209 remain_d_mm = d_inter_mm - (pt_norm(&proj, &intersect) + d_mm);
1210 DEBUG(E_USER_STRAT, "remain_d=%2.2f", remain_d_mm);
1211 if (remain_d_mm < 0) {
1212 DEBUG(E_USER_STRAT, "too late, cannot turn");
1219 /* function called when cmd_test is parsed successfully */
1220 static void cmd_clitoid_parsed(void *parsed_result, void *data)
1222 struct cmd_clitoid_result *res = parsed_result;
1223 clitoid(res->alpha_deg, res->beta_deg, res->R_mm,
1224 res->Vd, res->Amax, res->d_inter_mm);
1227 prog_char str_clitoid_arg0[] = "clitoid";
1228 parse_pgm_token_string_t cmd_clitoid_arg0 =
1229 TOKEN_STRING_INITIALIZER(struct cmd_clitoid_result,
1230 arg0, str_clitoid_arg0);
1231 parse_pgm_token_num_t cmd_clitoid_alpha_deg =
1232 TOKEN_NUM_INITIALIZER(struct cmd_clitoid_result,
1234 parse_pgm_token_num_t cmd_clitoid_beta_deg =
1235 TOKEN_NUM_INITIALIZER(struct cmd_clitoid_result,
1237 parse_pgm_token_num_t cmd_clitoid_R_mm =
1238 TOKEN_NUM_INITIALIZER(struct cmd_clitoid_result,
1240 parse_pgm_token_num_t cmd_clitoid_Vd =
1241 TOKEN_NUM_INITIALIZER(struct cmd_clitoid_result,
1243 parse_pgm_token_num_t cmd_clitoid_Amax =
1244 TOKEN_NUM_INITIALIZER(struct cmd_clitoid_result,
1246 parse_pgm_token_num_t cmd_clitoid_d_inter_mm =
1247 TOKEN_NUM_INITIALIZER(struct cmd_clitoid_result,
1250 prog_char help_clitoid[] = "do a clitoid (alpha, beta, R, Vd, Amax, d_inter)";
1251 parse_pgm_inst_t cmd_clitoid = {
1252 .f = cmd_clitoid_parsed, /* function to call */
1253 .data = NULL, /* 2nd arg of func */
1254 .help_str = help_clitoid,
1255 .tokens = { /* token list, NULL terminated */
1256 (prog_void *)&cmd_clitoid_arg0,
1257 (prog_void *)&cmd_clitoid_alpha_deg,
1258 (prog_void *)&cmd_clitoid_beta_deg,
1259 (prog_void *)&cmd_clitoid_R_mm,
1260 (prog_void *)&cmd_clitoid_Vd,
1261 (prog_void *)&cmd_clitoid_Amax,
1262 (prog_void *)&cmd_clitoid_d_inter_mm,
1267 //////////////////////
1271 #define TEST_SPEED 400
1274 static void line2line(double line1x1, double line1y1,
1275 double line1x2, double line1y2,
1276 double line2x1, double line2y1,
1277 double line2x2, double line2y2,
1278 double radius, double dist)
1281 double speed_d, speed_a;
1282 double distance, angle;
1283 double line1_angle = atan2(line1y2-line1y1, line1x2-line1x1);
1284 double line2_angle = atan2(line2y2-line2y1, line2x2-line2x1);
1286 printf_P(PSTR("%s()\r\n"), __FUNCTION__);
1288 strat_set_speed(TEST_SPEED, TEST_SPEED);
1289 quadramp_set_2nd_order_vars(&mainboard.angle.qr, TEST_ACC, TEST_ACC);
1291 circle_get_da_speed_from_radius(&mainboard.traj, radius,
1292 &speed_d, &speed_a);
1293 trajectory_line_abs(&mainboard.traj,
1295 line1x2, line1y2, 150.);
1296 err = WAIT_COND_OR_TRAJ_END(distance_from_robot(line1x2, line1y2) <
1297 dist, TRAJ_FLAGS_NO_NEAR);
1299 strat_set_speed(speed_d, speed_a);
1300 angle = line2_angle - line1_angle;
1301 distance = angle * radius;
1303 distance = -distance;
1304 angle = simple_modulo_2pi(angle);
1306 printf_P(PSTR("(%d,%d,%d) "),
1307 position_get_x_s16(&mainboard.pos),
1308 position_get_y_s16(&mainboard.pos),
1309 position_get_a_deg_s16(&mainboard.pos));
1310 printf_P(PSTR("circle distance=%2.2f angle=%2.2f\r\n"),
1313 /* take some margin on dist to avoid deceleration */
1314 trajectory_d_a_rel(&mainboard.traj, distance + 250, angle);
1316 /* circle exit condition */
1317 err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj),
1318 TRAJ_FLAGS_NO_NEAR);
1320 strat_set_speed(500, 500);
1321 printf_P(PSTR("(%d,%d,%d) "),
1322 position_get_x_s16(&mainboard.pos),
1323 position_get_y_s16(&mainboard.pos),
1324 position_get_a_deg_s16(&mainboard.pos));
1325 printf_P(PSTR("line\r\n"));
1326 trajectory_line_abs(&mainboard.traj,
1328 line2x2, line2y2, 150.);
1331 static void halfturn(double line1x1, double line1y1,
1332 double line1x2, double line1y2,
1333 double line2x1, double line2y1,
1334 double line2x2, double line2y2,
1335 double radius, double dist, double dir)
1338 double speed_d, speed_a;
1339 double distance, angle;
1341 printf_P(PSTR("%s()\r\n"), __FUNCTION__);
1343 strat_set_speed(TEST_SPEED, TEST_SPEED);
1344 quadramp_set_2nd_order_vars(&mainboard.angle.qr, TEST_ACC, TEST_ACC);
1346 circle_get_da_speed_from_radius(&mainboard.traj, radius,
1347 &speed_d, &speed_a);
1348 trajectory_line_abs(&mainboard.traj,
1350 line1x2, line1y2, 150.);
1351 err = WAIT_COND_OR_TRAJ_END(distance_from_robot(line1x2, line1y2) <
1352 dist, TRAJ_FLAGS_NO_NEAR);
1354 strat_set_speed(speed_d, speed_a);
1355 angle = dir * M_PI/2.;
1356 distance = angle * radius;
1358 distance = -distance;
1359 angle = simple_modulo_2pi(angle);
1362 /* take some margin on dist to avoid deceleration */
1363 DEBUG(E_USER_STRAT, "circle1 distance=%2.2f angle=%2.2f",
1365 trajectory_d_a_rel(&mainboard.traj, distance + 500, angle);
1367 /* circle exit condition */
1368 err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj),
1369 TRAJ_FLAGS_NO_NEAR);
1371 DEBUG(E_USER_STRAT, "miniline");
1372 err = WAIT_COND_OR_TRAJ_END(distance_from_robot(line2x1, line2y1) <
1373 dist, TRAJ_FLAGS_NO_NEAR);
1374 DEBUG(E_USER_STRAT, "circle2");
1375 /* take some margin on dist to avoid deceleration */
1376 trajectory_d_a_rel(&mainboard.traj, distance + 500, angle);
1378 err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj),
1379 TRAJ_FLAGS_NO_NEAR);
1381 strat_set_speed(500, 500);
1382 DEBUG(E_USER_STRAT, "line");
1383 trajectory_line_abs(&mainboard.traj,
1385 line2x2, line2y2, 150.);
1388 /**********************************************************/
1391 /* this structure is filled when cmd_test is parsed successfully */
1392 struct cmd_test_result {
1393 fixed_string_t arg0;
1398 /* function called when cmd_test is parsed successfully */
1399 static void cmd_test_parsed(void *parsed_result, void *data)
1401 // struct cmd_test_result *res = parsed_result;
1403 strat_reset_pos(400, 400, 90);
1404 mainboard.angle.on = 1;
1405 mainboard.distance.on = 1;
1407 printf_P(PSTR("%s()\r\n"), __FUNCTION__);
1408 while (!cmdline_keypressed()) {
1411 #define DIST_HARD_TURN 260
1412 #define RADIUS_HARD_TURN 100
1413 #define DIST_EASY_TURN 190
1414 #define RADIUS_EASY_TURN 190
1415 #define DIST_HALF_TURN 225
1416 #define RADIUS_HALF_TURN 130
1419 line2line(375, 597, 375, 1847,
1420 375, 1847, 1050, 1472,
1421 RADIUS_HARD_TURN, DIST_HARD_TURN);
1423 /* easy left and easy right !*/
1424 line2line(825, 1596, 1050, 1472,
1425 1050, 1472, 1500, 1722,
1426 RADIUS_EASY_TURN, DIST_EASY_TURN);
1427 line2line(1050, 1472, 1500, 1722,
1428 1500, 1722, 2175, 1347,
1429 RADIUS_EASY_TURN, DIST_EASY_TURN);
1430 line2line(1500, 1722, 2175, 1347,
1431 2175, 1347, 2175, 847,
1432 RADIUS_EASY_TURN, DIST_EASY_TURN);
1435 halfturn(2175, 1347, 2175, 722,
1436 2625, 722, 2625, 1597,
1437 RADIUS_HALF_TURN, DIST_HALF_TURN, 1.);
1438 halfturn(2625, 847, 2625, 1722,
1439 2175, 1722, 2175, 1097,
1440 RADIUS_HALF_TURN, DIST_HALF_TURN, 1.);
1443 line2line(2175, 1597, 2175, 1097,
1444 2175, 1097, 1500, 722,
1445 RADIUS_EASY_TURN, DIST_EASY_TURN);
1446 line2line(2175, 1097, 1500, 722,
1447 1500, 722, 1050, 972,
1448 RADIUS_EASY_TURN, DIST_EASY_TURN);
1449 line2line(1500, 722, 1050, 972,
1450 1050, 972, 375, 597,
1451 RADIUS_EASY_TURN, DIST_EASY_TURN);
1454 line2line(1050, 972, 375, 597,
1455 375, 597, 375, 1097,
1456 RADIUS_HARD_TURN, DIST_HARD_TURN);
1461 line2line(375, 597, 375, 1097,
1462 375, 1097, 1050, 1472,
1463 RADIUS_EASY_TURN, DIST_EASY_TURN);
1466 line2line(375, 1097, 1050, 1472,
1467 1050, 1472, 375, 1847,
1468 RADIUS_HARD_TURN, DIST_HARD_TURN);
1471 line2line(1050, 1472, 375, 1847,
1472 375, 1847, 375, 1347,
1473 RADIUS_HARD_TURN, DIST_HARD_TURN);
1476 line2line(375, 1847, 375, 1347,
1477 375, 1347, 1050, 972,
1478 RADIUS_EASY_TURN, DIST_EASY_TURN);
1481 line2line(375, 1347, 1050, 972,
1482 1050, 972, 375, 597,
1483 RADIUS_HARD_TURN, DIST_HARD_TURN);
1486 line2line(1050, 972, 375, 597,
1487 375, 597, 375, 1847,
1488 RADIUS_HARD_TURN, DIST_HARD_TURN);
1491 trajectory_hardstop(&mainboard.traj);
1494 prog_char str_test_arg0[] = "test";
1495 parse_pgm_token_string_t cmd_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_test_result, arg0, str_test_arg0);
1496 parse_pgm_token_num_t cmd_test_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_test_result, radius, INT32);
1497 parse_pgm_token_num_t cmd_test_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_test_result, dist, INT32);
1499 prog_char help_test[] = "Test function";
1500 parse_pgm_inst_t cmd_test = {
1501 .f = cmd_test_parsed, /* function to call */
1502 .data = NULL, /* 2nd arg of func */
1503 .help_str = help_test,
1504 .tokens = { /* token list, NULL terminated */
1505 (prog_void *)&cmd_test_arg0,