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_traj.c,v 1.8 2009-11-08 17:24:33 zer0 Exp $
20 * Olivier MATZ <zer0@droids-corp.org>
27 #include <aversive/pgmspace.h>
28 #include <aversive/wait.h>
29 #include <aversive/error.h>
34 #include <clock_time.h>
36 #include <encoders_spi.h>
40 #include <control_system_manager.h>
41 #include <trajectory_manager.h>
42 #include <trajectory_manager_utils.h>
43 #include <vect_base.h>
46 #include <obstacle_avoidance.h>
47 #include <blocking_detection_manager.h>
48 #include <robot_system.h>
49 #include <position_manager.h>
53 #include <parse_string.h>
54 #include <parse_num.h>
59 #include "strat_utils.h"
60 #include "strat_base.h"
63 #include "../common/i2c_commands.h"
64 #include "i2c_protocol.h"
67 /**********************************************************/
68 /* Traj_Speeds for trajectory_manager */
70 /* this structure is filled when cmd_traj_speed is parsed successfully */
71 struct cmd_traj_speed_result {
77 /* function called when cmd_traj_speed is parsed successfully */
78 static void cmd_traj_speed_parsed(void *parsed_result, void *data)
80 struct cmd_traj_speed_result * res = parsed_result;
82 if (!strcmp_P(res->arg1, PSTR("angle"))) {
83 trajectory_set_speed(&mainboard.traj, mainboard.traj.d_speed, res->s);
85 else if (!strcmp_P(res->arg1, PSTR("distance"))) {
86 trajectory_set_speed(&mainboard.traj, res->s, mainboard.traj.a_speed);
88 /* else it is a "show" */
90 printf_P(PSTR("angle %2.2f, distance %2.2f\r\n"),
91 mainboard.traj.a_speed,
92 mainboard.traj.d_speed);
95 prog_char str_traj_speed_arg0[] = "traj_speed";
96 parse_pgm_token_string_t cmd_traj_speed_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_traj_speed_result, arg0, str_traj_speed_arg0);
97 prog_char str_traj_speed_arg1[] = "angle#distance";
98 parse_pgm_token_string_t cmd_traj_speed_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_traj_speed_result, arg1, str_traj_speed_arg1);
99 parse_pgm_token_num_t cmd_traj_speed_s = TOKEN_NUM_INITIALIZER(struct cmd_traj_speed_result, s, FLOAT);
101 prog_char help_traj_speed[] = "Set traj_speed values for trajectory manager";
102 parse_pgm_inst_t cmd_traj_speed = {
103 .f = cmd_traj_speed_parsed, /* function to call */
104 .data = NULL, /* 2nd arg of func */
105 .help_str = help_traj_speed,
106 .tokens = { /* token list, NULL terminated */
107 (prog_void *)&cmd_traj_speed_arg0,
108 (prog_void *)&cmd_traj_speed_arg1,
109 (prog_void *)&cmd_traj_speed_s,
116 prog_char str_traj_speed_show_arg[] = "show";
117 parse_pgm_token_string_t cmd_traj_speed_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_traj_speed_result, arg1, str_traj_speed_show_arg);
119 prog_char help_traj_speed_show[] = "Show traj_speed values for trajectory manager";
120 parse_pgm_inst_t cmd_traj_speed_show = {
121 .f = cmd_traj_speed_parsed, /* function to call */
122 .data = NULL, /* 2nd arg of func */
123 .help_str = help_traj_speed_show,
124 .tokens = { /* token list, NULL terminated */
125 (prog_void *)&cmd_traj_speed_arg0,
126 (prog_void *)&cmd_traj_speed_show_arg,
131 /**********************************************************/
132 /* Traj_Accs for trajectory_manager */
134 /* this structure is filled when cmd_traj_acc is parsed successfully */
135 struct cmd_traj_acc_result {
141 /* function called when cmd_traj_acc is parsed successfully */
142 static void cmd_traj_acc_parsed(void *parsed_result, void *data)
144 struct cmd_traj_acc_result * res = parsed_result;
146 if (!strcmp_P(res->arg1, PSTR("angle"))) {
147 trajectory_set_acc(&mainboard.traj, mainboard.traj.d_acc, res->s);
149 else if (!strcmp_P(res->arg1, PSTR("distance"))) {
150 trajectory_set_acc(&mainboard.traj, res->s, mainboard.traj.a_acc);
152 /* else it is a "show" */
154 printf_P(PSTR("angle %2.2f, distance %2.2f\r\n"),
155 mainboard.traj.a_acc,
156 mainboard.traj.d_acc);
159 prog_char str_traj_acc_arg0[] = "traj_acc";
160 parse_pgm_token_string_t cmd_traj_acc_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_traj_acc_result, arg0, str_traj_acc_arg0);
161 prog_char str_traj_acc_arg1[] = "angle#distance";
162 parse_pgm_token_string_t cmd_traj_acc_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_traj_acc_result, arg1, str_traj_acc_arg1);
163 parse_pgm_token_num_t cmd_traj_acc_s = TOKEN_NUM_INITIALIZER(struct cmd_traj_acc_result, s, FLOAT);
165 prog_char help_traj_acc[] = "Set traj_acc values for trajectory manager";
166 parse_pgm_inst_t cmd_traj_acc = {
167 .f = cmd_traj_acc_parsed, /* function to call */
168 .data = NULL, /* 2nd arg of func */
169 .help_str = help_traj_acc,
170 .tokens = { /* token list, NULL terminated */
171 (prog_void *)&cmd_traj_acc_arg0,
172 (prog_void *)&cmd_traj_acc_arg1,
173 (prog_void *)&cmd_traj_acc_s,
180 prog_char str_traj_acc_show_arg[] = "show";
181 parse_pgm_token_string_t cmd_traj_acc_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_traj_acc_result, arg1, str_traj_acc_show_arg);
183 prog_char help_traj_acc_show[] = "Show traj_acc values for trajectory manager";
184 parse_pgm_inst_t cmd_traj_acc_show = {
185 .f = cmd_traj_acc_parsed, /* function to call */
186 .data = NULL, /* 2nd arg of func */
187 .help_str = help_traj_acc_show,
188 .tokens = { /* token list, NULL terminated */
189 (prog_void *)&cmd_traj_acc_arg0,
190 (prog_void *)&cmd_traj_acc_show_arg,
195 /**********************************************************/
196 /* circle coef configuration */
198 /* this structure is filled when cmd_circle_coef is parsed successfully */
199 struct cmd_circle_coef_result {
206 /* function called when cmd_circle_coef is parsed successfully */
207 static void cmd_circle_coef_parsed(void *parsed_result, void *data)
209 struct cmd_circle_coef_result *res = parsed_result;
211 if (!strcmp_P(res->arg1, PSTR("set"))) {
212 trajectory_set_circle_coef(&mainboard.traj, res->circle_coef);
215 printf_P(PSTR("circle_coef set %2.2f\r\n"), mainboard.traj.circle_coef);
218 prog_char str_circle_coef_arg0[] = "circle_coef";
219 parse_pgm_token_string_t cmd_circle_coef_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_circle_coef_result, arg0, str_circle_coef_arg0);
220 prog_char str_circle_coef_arg1[] = "set";
221 parse_pgm_token_string_t cmd_circle_coef_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_circle_coef_result, arg1, str_circle_coef_arg1);
222 parse_pgm_token_num_t cmd_circle_coef_val = TOKEN_NUM_INITIALIZER(struct cmd_circle_coef_result, circle_coef, FLOAT);
224 prog_char help_circle_coef[] = "Set circle coef";
225 parse_pgm_inst_t cmd_circle_coef = {
226 .f = cmd_circle_coef_parsed, /* function to call */
227 .data = NULL, /* 2nd arg of func */
228 .help_str = help_circle_coef,
229 .tokens = { /* token list, NULL terminated */
230 (prog_void *)&cmd_circle_coef_arg0,
231 (prog_void *)&cmd_circle_coef_arg1,
232 (prog_void *)&cmd_circle_coef_val,
239 prog_char str_circle_coef_show_arg[] = "show";
240 parse_pgm_token_string_t cmd_circle_coef_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_circle_coef_result, arg1, str_circle_coef_show_arg);
242 prog_char help_circle_coef_show[] = "Show circle coef";
243 parse_pgm_inst_t cmd_circle_coef_show = {
244 .f = cmd_circle_coef_parsed, /* function to call */
245 .data = NULL, /* 2nd arg of func */
246 .help_str = help_circle_coef_show,
247 .tokens = { /* token list, NULL terminated */
248 (prog_void *)&cmd_circle_coef_arg0,
249 (prog_void *)&cmd_circle_coef_show_arg,
254 /**********************************************************/
255 /* trajectory window configuration */
257 /* this structure is filled when cmd_trajectory is parsed successfully */
258 struct cmd_trajectory_result {
267 /* function called when cmd_trajectory is parsed successfully */
268 static void cmd_trajectory_parsed(void * parsed_result, void * data)
270 struct cmd_trajectory_result * res = parsed_result;
272 if (!strcmp_P(res->arg1, PSTR("set"))) {
273 trajectory_set_windows(&mainboard.traj, res->d_win,
274 res->a_win, res->a_start);
277 printf_P(PSTR("trajectory %2.2f %2.2f %2.2f\r\n"), mainboard.traj.d_win,
278 DEG(mainboard.traj.a_win_rad), DEG(mainboard.traj.a_start_rad));
281 prog_char str_trajectory_arg0[] = "trajectory";
282 parse_pgm_token_string_t cmd_trajectory_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_trajectory_result, arg0, str_trajectory_arg0);
283 prog_char str_trajectory_arg1[] = "set";
284 parse_pgm_token_string_t cmd_trajectory_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_trajectory_result, arg1, str_trajectory_arg1);
285 parse_pgm_token_num_t cmd_trajectory_d = TOKEN_NUM_INITIALIZER(struct cmd_trajectory_result, d_win, FLOAT);
286 parse_pgm_token_num_t cmd_trajectory_a = TOKEN_NUM_INITIALIZER(struct cmd_trajectory_result, a_win, FLOAT);
287 parse_pgm_token_num_t cmd_trajectory_as = TOKEN_NUM_INITIALIZER(struct cmd_trajectory_result, a_start, FLOAT);
289 prog_char help_trajectory[] = "Set trajectory windows (distance, angle, angle_start)";
290 parse_pgm_inst_t cmd_trajectory = {
291 .f = cmd_trajectory_parsed, /* function to call */
292 .data = NULL, /* 2nd arg of func */
293 .help_str = help_trajectory,
294 .tokens = { /* token list, NULL terminated */
295 (prog_void *)&cmd_trajectory_arg0,
296 (prog_void *)&cmd_trajectory_arg1,
297 (prog_void *)&cmd_trajectory_d,
298 (prog_void *)&cmd_trajectory_a,
299 (prog_void *)&cmd_trajectory_as,
306 prog_char str_trajectory_show_arg[] = "show";
307 parse_pgm_token_string_t cmd_trajectory_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_trajectory_result, arg1, str_trajectory_show_arg);
309 prog_char help_trajectory_show[] = "Show trajectory window configuration";
310 parse_pgm_inst_t cmd_trajectory_show = {
311 .f = cmd_trajectory_parsed, /* function to call */
312 .data = NULL, /* 2nd arg of func */
313 .help_str = help_trajectory_show,
314 .tokens = { /* token list, NULL terminated */
315 (prog_void *)&cmd_trajectory_arg0,
316 (prog_void *)&cmd_trajectory_show_arg,
321 /**********************************************************/
322 /* rs_gains configuration */
324 /* this structure is filled when cmd_rs_gains is parsed successfully */
325 struct cmd_rs_gains_result {
332 /* function called when cmd_rs_gains is parsed successfully */
333 static void cmd_rs_gains_parsed(void * parsed_result, void * data)
336 printf("not implemented\n");
338 struct cmd_rs_gains_result * res = parsed_result;
340 if (!strcmp_P(res->arg1, PSTR("set"))) {
341 rs_set_left_ext_encoder(&mainboard.rs, encoders_spi_get_value,
342 LEFT_ENCODER, res->left); // en augmentant on tourne à gauche
343 rs_set_right_ext_encoder(&mainboard.rs, encoders_spi_get_value,
344 RIGHT_ENCODER, res->right); //en augmentant on tourne à droite
346 printf_P(PSTR("rs_gains set %2.2f %2.2f\r\n"),
347 mainboard.rs.left_ext_gain, mainboard.rs.right_ext_gain);
351 prog_char str_rs_gains_arg0[] = "rs_gains";
352 parse_pgm_token_string_t cmd_rs_gains_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_rs_gains_result, arg0, str_rs_gains_arg0);
353 prog_char str_rs_gains_arg1[] = "set";
354 parse_pgm_token_string_t cmd_rs_gains_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_rs_gains_result, arg1, str_rs_gains_arg1);
355 parse_pgm_token_num_t cmd_rs_gains_l = TOKEN_NUM_INITIALIZER(struct cmd_rs_gains_result, left, FLOAT);
356 parse_pgm_token_num_t cmd_rs_gains_r = TOKEN_NUM_INITIALIZER(struct cmd_rs_gains_result, right, FLOAT);
358 prog_char help_rs_gains[] = "Set rs_gains (left, right)";
359 parse_pgm_inst_t cmd_rs_gains = {
360 .f = cmd_rs_gains_parsed, /* function to call */
361 .data = NULL, /* 2nd arg of func */
362 .help_str = help_rs_gains,
363 .tokens = { /* token list, NULL terminated */
364 (prog_void *)&cmd_rs_gains_arg0,
365 (prog_void *)&cmd_rs_gains_arg1,
366 (prog_void *)&cmd_rs_gains_l,
367 (prog_void *)&cmd_rs_gains_r,
374 prog_char str_rs_gains_show_arg[] = "show";
375 parse_pgm_token_string_t cmd_rs_gains_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_rs_gains_result, arg1, str_rs_gains_show_arg);
377 prog_char help_rs_gains_show[] = "Show rs_gains";
378 parse_pgm_inst_t cmd_rs_gains_show = {
379 .f = cmd_rs_gains_parsed, /* function to call */
380 .data = NULL, /* 2nd arg of func */
381 .help_str = help_rs_gains_show,
382 .tokens = { /* token list, NULL terminated */
383 (prog_void *)&cmd_rs_gains_arg0,
384 (prog_void *)&cmd_rs_gains_show_arg,
389 /**********************************************************/
390 /* track configuration */
392 /* this structure is filled when cmd_track is parsed successfully */
393 struct cmd_track_result {
399 /* function called when cmd_track is parsed successfully */
400 static void cmd_track_parsed(void * parsed_result, void * data)
402 struct cmd_track_result * res = parsed_result;
404 if (!strcmp_P(res->arg1, PSTR("set"))) {
405 position_set_physical_params(&mainboard.pos, res->val, DIST_IMP_MM);
407 printf_P(PSTR("track set %f\r\n"), mainboard.pos.phys.track_mm);
410 prog_char str_track_arg0[] = "track";
411 parse_pgm_token_string_t cmd_track_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_track_result, arg0, str_track_arg0);
412 prog_char str_track_arg1[] = "set";
413 parse_pgm_token_string_t cmd_track_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_track_result, arg1, str_track_arg1);
414 parse_pgm_token_num_t cmd_track_val = TOKEN_NUM_INITIALIZER(struct cmd_track_result, val, FLOAT);
416 prog_char help_track[] = "Set track in mm";
417 parse_pgm_inst_t cmd_track = {
418 .f = cmd_track_parsed, /* function to call */
419 .data = NULL, /* 2nd arg of func */
420 .help_str = help_track,
421 .tokens = { /* token list, NULL terminated */
422 (prog_void *)&cmd_track_arg0,
423 (prog_void *)&cmd_track_arg1,
424 (prog_void *)&cmd_track_val,
431 prog_char str_track_show_arg[] = "show";
432 parse_pgm_token_string_t cmd_track_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_track_result, arg1, str_track_show_arg);
434 prog_char help_track_show[] = "Show track";
435 parse_pgm_inst_t cmd_track_show = {
436 .f = cmd_track_parsed, /* function to call */
437 .data = NULL, /* 2nd arg of func */
438 .help_str = help_track_show,
439 .tokens = { /* token list, NULL terminated */
440 (prog_void *)&cmd_track_arg0,
441 (prog_void *)&cmd_track_show_arg,
446 /**********************************************************/
447 /* centrifugal configuration */
449 /* this structure is filled when cmd_centrifugal is parsed successfully */
450 struct cmd_centrifugal_result {
456 /* function called when cmd_centrifugal is parsed successfully */
457 static void cmd_centrifugal_parsed(void * parsed_result, void * data)
459 struct cmd_centrifugal_result * res = parsed_result;
461 if (!strcmp_P(res->arg1, PSTR("set"))) {
462 position_set_centrifugal_coef(&mainboard.pos, res->val);
464 printf_P(PSTR("centrifugal set %f\r\n"), mainboard.pos.centrifugal_coef);
467 prog_char str_centrifugal_arg0[] = "centrifugal";
468 parse_pgm_token_string_t cmd_centrifugal_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_centrifugal_result, arg0, str_centrifugal_arg0);
469 prog_char str_centrifugal_arg1[] = "set";
470 parse_pgm_token_string_t cmd_centrifugal_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_centrifugal_result, arg1, str_centrifugal_arg1);
471 parse_pgm_token_num_t cmd_centrifugal_val = TOKEN_NUM_INITIALIZER(struct cmd_centrifugal_result, val, FLOAT);
473 prog_char help_centrifugal[] = "Set centrifugal coef";
474 parse_pgm_inst_t cmd_centrifugal = {
475 .f = cmd_centrifugal_parsed, /* function to call */
476 .data = NULL, /* 2nd arg of func */
477 .help_str = help_centrifugal,
478 .tokens = { /* token list, NULL terminated */
479 (prog_void *)&cmd_centrifugal_arg0,
480 (prog_void *)&cmd_centrifugal_arg1,
481 (prog_void *)&cmd_centrifugal_val,
488 prog_char str_centrifugal_show_arg[] = "show";
489 parse_pgm_token_string_t cmd_centrifugal_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_centrifugal_result, arg1, str_centrifugal_show_arg);
491 prog_char help_centrifugal_show[] = "Show centrifugal";
492 parse_pgm_inst_t cmd_centrifugal_show = {
493 .f = cmd_centrifugal_parsed, /* function to call */
494 .data = NULL, /* 2nd arg of func */
495 .help_str = help_centrifugal_show,
496 .tokens = { /* token list, NULL terminated */
497 (prog_void *)&cmd_centrifugal_arg0,
498 (prog_void *)&cmd_centrifugal_show_arg,
505 /**********************************************************/
506 /* Pt_Lists for testing traj */
508 #define PT_LIST_SIZE 10
509 static struct xy_point pt_list[PT_LIST_SIZE];
510 static uint16_t pt_list_len = 0;
512 /* this structure is filled when cmd_pt_list is parsed successfully */
513 struct cmd_pt_list_result {
521 /* function called when cmd_pt_list is parsed successfully */
522 static void cmd_pt_list_parsed(void * parsed_result, void * data)
524 struct cmd_pt_list_result * res = parsed_result;
527 if (!strcmp_P(res->arg1, PSTR("append"))) {
528 res->arg2 = pt_list_len;
530 if (!strcmp_P(res->arg1, PSTR("avoid_start"))) {
531 printf_P(PSTR("removed\r\n"));
535 if (!strcmp_P(res->arg1, PSTR("insert")) ||
536 !strcmp_P(res->arg1, PSTR("append"))) {
537 if (res->arg2 > pt_list_len) {
538 printf_P(PSTR("Index too large\r\n"));
541 if (pt_list_len >= PT_LIST_SIZE) {
542 printf_P(PSTR("List is too large\r\n"));
545 memmove(&pt_list[res->arg2+1], &pt_list[res->arg2],
546 PT_LIST_SIZE-1-res->arg2);
547 pt_list[res->arg2].x = res->arg3;
548 pt_list[res->arg2].y = res->arg4;
551 else if (!strcmp_P(res->arg1, PSTR("del"))) {
552 if (pt_list_len <= 0) {
553 printf_P(PSTR("Error: list empty\r\n"));
556 if (res->arg2 > pt_list_len) {
557 printf_P(PSTR("Index too large\r\n"));
560 memmove(&pt_list[res->arg2], &pt_list[res->arg2+1],
561 (PT_LIST_SIZE-1-res->arg2)*sizeof(struct xy_point));
564 else if (!strcmp_P(res->arg1, PSTR("reset"))) {
568 /* else it is a "show" or a "start" */
569 if (pt_list_len == 0) {
570 printf_P(PSTR("List empty\r\n"));
574 for (i=0 ; i<pt_list_len ; i++) {
575 printf_P(PSTR("%d: x=%d y=%d\r\n"), i, pt_list[i].x, pt_list[i].y);
576 if (!strcmp_P(res->arg1, PSTR("start"))) {
577 trajectory_goto_xy_abs(&mainboard.traj, pt_list[i].x, pt_list[i].y);
578 why = wait_traj_end(0xFF); /* all */
580 else if (!strcmp_P(res->arg1, PSTR("loop_start"))) {
581 trajectory_goto_xy_abs(&mainboard.traj, pt_list[i].x, pt_list[i].y);
582 why = wait_traj_end(0xFF); /* all */
585 else if (!strcmp_P(res->arg1, PSTR("avoid_start"))) {
587 why = goto_and_avoid(pt_list[i].x, pt_list[i].y, 0xFF, 0xFF);
588 printf("next point\r\n");
589 if (why != END_OBSTACLE)
594 if (why & (~(END_TRAJ | END_NEAR)))
595 trajectory_stop(&mainboard.traj);
601 if (!strcmp_P(res->arg1, PSTR("loop_start")))
605 prog_char str_pt_list_arg0[] = "pt_list";
606 parse_pgm_token_string_t cmd_pt_list_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg0, str_pt_list_arg0);
607 prog_char str_pt_list_arg1[] = "insert";
608 parse_pgm_token_string_t cmd_pt_list_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_arg1);
609 parse_pgm_token_num_t cmd_pt_list_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_pt_list_result, arg2, UINT16);
610 parse_pgm_token_num_t cmd_pt_list_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_pt_list_result, arg3, INT16);
611 parse_pgm_token_num_t cmd_pt_list_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_pt_list_result, arg4, INT16);
613 prog_char help_pt_list[] = "Insert point in pt_list (idx,x,y)";
614 parse_pgm_inst_t cmd_pt_list = {
615 .f = cmd_pt_list_parsed, /* function to call */
616 .data = NULL, /* 2nd arg of func */
617 .help_str = help_pt_list,
618 .tokens = { /* token list, NULL terminated */
619 (prog_void *)&cmd_pt_list_arg0,
620 (prog_void *)&cmd_pt_list_arg1,
621 (prog_void *)&cmd_pt_list_arg2,
622 (prog_void *)&cmd_pt_list_arg3,
623 (prog_void *)&cmd_pt_list_arg4,
630 prog_char str_pt_list_arg1_append[] = "append";
631 parse_pgm_token_string_t cmd_pt_list_arg1_append = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_arg1_append);
633 prog_char help_pt_list_append[] = "Append point in pt_list (x,y)";
634 parse_pgm_inst_t cmd_pt_list_append = {
635 .f = cmd_pt_list_parsed, /* function to call */
636 .data = NULL, /* 2nd arg of func */
637 .help_str = help_pt_list_append,
638 .tokens = { /* token list, NULL terminated */
639 (prog_void *)&cmd_pt_list_arg0,
640 (prog_void *)&cmd_pt_list_arg1_append,
641 (prog_void *)&cmd_pt_list_arg3,
642 (prog_void *)&cmd_pt_list_arg4,
649 prog_char str_pt_list_del_arg[] = "del";
650 parse_pgm_token_string_t cmd_pt_list_del_arg = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_del_arg);
652 prog_char help_pt_list_del[] = "Del or insert point in pt_list (num)";
653 parse_pgm_inst_t cmd_pt_list_del = {
654 .f = cmd_pt_list_parsed, /* function to call */
655 .data = NULL, /* 2nd arg of func */
656 .help_str = help_pt_list_del,
657 .tokens = { /* token list, NULL terminated */
658 (prog_void *)&cmd_pt_list_arg0,
659 (prog_void *)&cmd_pt_list_del_arg,
660 (prog_void *)&cmd_pt_list_arg2,
666 prog_char str_pt_list_show_arg[] = "show#reset#start#avoid_start#loop_start";
667 parse_pgm_token_string_t cmd_pt_list_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_show_arg);
669 prog_char help_pt_list_show[] = "Show, start or reset pt_list";
670 parse_pgm_inst_t cmd_pt_list_show = {
671 .f = cmd_pt_list_parsed, /* function to call */
672 .data = NULL, /* 2nd arg of func */
673 .help_str = help_pt_list_show,
674 .tokens = { /* token list, NULL terminated */
675 (prog_void *)&cmd_pt_list_arg0,
676 (prog_void *)&cmd_pt_list_show_arg,
683 /**********************************************************/
686 /* this structure is filled when cmd_goto is parsed successfully */
687 struct cmd_goto_result {
695 /* function called when cmd_goto is parsed successfully */
696 static void cmd_goto_parsed(void * parsed_result, void * data)
698 struct cmd_goto_result * res = parsed_result;
702 interrupt_traj_reset();
703 if (!strcmp_P(res->arg1, PSTR("a_rel"))) {
704 trajectory_a_rel(&mainboard.traj, res->arg2);
706 else if (!strcmp_P(res->arg1, PSTR("d_rel"))) {
707 trajectory_d_rel(&mainboard.traj, res->arg2);
709 else if (!strcmp_P(res->arg1, PSTR("a_abs"))) {
710 trajectory_a_abs(&mainboard.traj, res->arg2);
712 else if (!strcmp_P(res->arg1, PSTR("a_to_xy"))) {
713 trajectory_turnto_xy(&mainboard.traj, res->arg2, res->arg3);
715 else if (!strcmp_P(res->arg1, PSTR("a_behind_xy"))) {
716 trajectory_turnto_xy_behind(&mainboard.traj, res->arg2, res->arg3);
718 else if (!strcmp_P(res->arg1, PSTR("xy_rel"))) {
719 trajectory_goto_xy_rel(&mainboard.traj, res->arg2, res->arg3);
721 else if (!strcmp_P(res->arg1, PSTR("xy_abs"))) {
722 trajectory_goto_xy_abs(&mainboard.traj, res->arg2, res->arg3);
724 else if (!strcmp_P(res->arg1, PSTR("avoid"))) {
726 err = goto_and_avoid_forward(res->arg2, res->arg3, 0xFF, 0xFF);
727 if (err != END_TRAJ && err != END_NEAR)
730 printf_P(PSTR("not implemented\r\n"));
734 else if (!strcmp_P(res->arg1, PSTR("avoid_bw"))) {
736 err = goto_and_avoid_backward(res->arg2, res->arg3, 0xFF, 0xFF);
737 if (err != END_TRAJ && err != END_NEAR)
740 printf_P(PSTR("not implemented\r\n"));
744 else if (!strcmp_P(res->arg1, PSTR("xy_abs_fow"))) {
745 trajectory_goto_forward_xy_abs(&mainboard.traj, res->arg2, res->arg3);
747 else if (!strcmp_P(res->arg1, PSTR("xy_abs_back"))) {
748 trajectory_goto_backward_xy_abs(&mainboard.traj, res->arg2, res->arg3);
750 else if (!strcmp_P(res->arg1, PSTR("da_rel"))) {
751 trajectory_d_a_rel(&mainboard.traj, res->arg2, res->arg3);
754 while ((err = test_traj_end(0xFF)) == 0) {
756 if (t2 - t1 > 200000) {
757 dump_cs_debug("angle", &mainboard.angle.cs);
758 dump_cs_debug("distance", &mainboard.distance.cs);
762 if (err != END_TRAJ && err != END_NEAR)
764 printf_P(PSTR("returned %s\r\n"), get_err(err));
767 prog_char str_goto_arg0[] = "goto";
768 parse_pgm_token_string_t cmd_goto_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_goto_result, arg0, str_goto_arg0);
769 prog_char str_goto_arg1_a[] = "d_rel#a_rel#a_abs";
770 parse_pgm_token_string_t cmd_goto_arg1_a = TOKEN_STRING_INITIALIZER(struct cmd_goto_result, arg1, str_goto_arg1_a);
771 parse_pgm_token_num_t cmd_goto_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_goto_result, arg2, INT32);
774 prog_char help_goto1[] = "Change orientation of the mainboard";
775 parse_pgm_inst_t cmd_goto1 = {
776 .f = cmd_goto_parsed, /* function to call */
777 .data = NULL, /* 2nd arg of func */
778 .help_str = help_goto1,
779 .tokens = { /* token list, NULL terminated */
780 (prog_void *)&cmd_goto_arg0,
781 (prog_void *)&cmd_goto_arg1_a,
782 (prog_void *)&cmd_goto_arg2,
787 prog_char str_goto_arg1_b[] = "xy_rel#xy_abs#xy_abs_fow#xy_abs_back#da_rel#a_to_xy#avoid#avoid_bw#a_behind_xy";
788 parse_pgm_token_string_t cmd_goto_arg1_b = TOKEN_STRING_INITIALIZER(struct cmd_goto_result, arg1, str_goto_arg1_b);
789 parse_pgm_token_num_t cmd_goto_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_goto_result, arg3, INT32);
792 prog_char help_goto2[] = "Go to a (x,y) or (d,a) position";
793 parse_pgm_inst_t cmd_goto2 = {
794 .f = cmd_goto_parsed, /* function to call */
795 .data = NULL, /* 2nd arg of func */
796 .help_str = help_goto2,
797 .tokens = { /* token list, NULL terminated */
798 (prog_void *)&cmd_goto_arg0,
799 (prog_void *)&cmd_goto_arg1_b,
800 (prog_void *)&cmd_goto_arg2,
801 (prog_void *)&cmd_goto_arg3,
806 /**********************************************************/
809 /* this structure is filled when cmd_position is parsed successfully */
810 struct cmd_position_result {
818 #define AUTOPOS_SPEED_FAST 200
819 static void auto_position(void)
822 uint16_t old_spdd, old_spda;
824 interrupt_traj_reset();
825 strat_get_speed(&old_spdd, &old_spda);
826 strat_set_speed(AUTOPOS_SPEED_FAST, AUTOPOS_SPEED_FAST);
829 err = strat_calib(300, END_INTR|END_TRAJ|END_BLOCKING);
832 strat_reset_pos(ROBOT_WIDTH/2 + 100,
833 COLOR_Y(ROBOT_HALF_LENGTH_FRONT),
837 trajectory_d_rel(&mainboard.traj, -180);
838 err = wait_traj_end(END_INTR|END_TRAJ);
843 trajectory_a_rel(&mainboard.traj, COLOR_A(-90));
844 err = wait_traj_end(END_INTR|END_TRAJ);
849 err = strat_calib(300, END_INTR|END_TRAJ|END_BLOCKING);
852 strat_reset_pos(ROBOT_HALF_LENGTH_FRONT,
857 trajectory_d_rel(&mainboard.traj, -170);
858 err = wait_traj_end(END_INTR|END_TRAJ);
863 trajectory_a_rel(&mainboard.traj, COLOR_A(-110));
864 err = wait_traj_end(END_INTR|END_TRAJ);
869 strat_set_speed(old_spdd, old_spda);
874 strat_set_speed(old_spdd, old_spda);
877 /* function called when cmd_position is parsed successfully */
878 static void cmd_position_parsed(void * parsed_result, void * data)
880 struct cmd_position_result * res = parsed_result;
882 /* display raw position values */
883 if (!strcmp_P(res->arg1, PSTR("reset"))) {
884 position_set(&mainboard.pos, 0, 0, 0);
886 else if (!strcmp_P(res->arg1, PSTR("set"))) {
887 position_set(&mainboard.pos, res->arg2, res->arg3, res->arg4);
889 else if (!strcmp_P(res->arg1, PSTR("autoset_blue")) ||
890 !strcmp_P(res->arg1, PSTR("autoset_or_blue"))) {
891 mainboard.our_color = I2C_COLOR_BLUE;
893 i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_BLUE);
894 i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_BLUE);
895 beacon_set_color(I2C_COLOR_YELLOW);
899 else if (!strcmp_P(res->arg1, PSTR("autoset_yellow")) ||
900 !strcmp_P(res->arg1, PSTR("autoset_or_yellow"))) {
901 mainboard.our_color = I2C_COLOR_YELLOW;
903 i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_YELLOW);
904 i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_YELLOW);
905 beacon_set_color(I2C_COLOR_BLUE);
910 if (!strcmp_P(res->arg1, PSTR("autoset_or_blue"))) {
911 strat_conf.flags |= STRAT_CONF_OUR_ORANGE;
912 prepare_hill(I2C_COLOR_BLUE, 340);
914 else if (!strcmp_P(res->arg1, PSTR("autoset_or_yellow"))) {
915 strat_conf.flags |= STRAT_CONF_OUR_ORANGE;
916 prepare_hill(I2C_COLOR_YELLOW, 340);
919 /* else it's just a "show" */
920 printf_P(PSTR("x=%.2f y=%.2f a=%.2f\r\n"),
921 position_get_x_double(&mainboard.pos),
922 position_get_y_double(&mainboard.pos),
923 DEG(position_get_a_rad_double(&mainboard.pos)));
926 prog_char str_position_arg0[] = "position";
927 parse_pgm_token_string_t cmd_position_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg0, str_position_arg0);
928 prog_char str_position_arg1[] = "show#reset#autoset_blue#autoset_yellow#autoset_or_blue#autoset_or_yellow";
929 parse_pgm_token_string_t cmd_position_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg1, str_position_arg1);
931 prog_char help_position[] = "Show/reset (x,y,a) position";
932 parse_pgm_inst_t cmd_position = {
933 .f = cmd_position_parsed, /* function to call */
934 .data = NULL, /* 2nd arg of func */
935 .help_str = help_position,
936 .tokens = { /* token list, NULL terminated */
937 (prog_void *)&cmd_position_arg0,
938 (prog_void *)&cmd_position_arg1,
944 prog_char str_position_arg1_set[] = "set";
945 parse_pgm_token_string_t cmd_position_arg1_set = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg1, str_position_arg1_set);
946 parse_pgm_token_num_t cmd_position_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg2, INT32);
947 parse_pgm_token_num_t cmd_position_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg3, INT32);
948 parse_pgm_token_num_t cmd_position_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg4, INT32);
950 prog_char help_position_set[] = "Set (x,y,a) position";
951 parse_pgm_inst_t cmd_position_set = {
952 .f = cmd_position_parsed, /* function to call */
953 .data = NULL, /* 2nd arg of func */
954 .help_str = help_position_set,
955 .tokens = { /* token list, NULL terminated */
956 (prog_void *)&cmd_position_arg0,
957 (prog_void *)&cmd_position_arg1_set,
958 (prog_void *)&cmd_position_arg2,
959 (prog_void *)&cmd_position_arg3,
960 (prog_void *)&cmd_position_arg4,
966 /**********************************************************/
967 /* strat configuration */
969 /* this structure is filled when cmd_strat_db is parsed successfully */
970 struct cmd_strat_db_result {
975 /* function called when cmd_strat_db is parsed successfully */
976 static void cmd_strat_db_parsed(void *parsed_result, void *data)
978 struct cmd_strat_db_result *res = parsed_result;
980 if (!strcmp_P(res->arg1, PSTR("reset"))) {
983 strat_db.dump_enabled = 1;
984 strat_db_dump(__FUNCTION__);
987 prog_char str_strat_db_arg0[] = "strat_db";
988 parse_pgm_token_string_t cmd_strat_db_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_db_result, arg0, str_strat_db_arg0);
989 prog_char str_strat_db_arg1[] = "show#reset";
990 parse_pgm_token_string_t cmd_strat_db_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_db_result, arg1, str_strat_db_arg1);
992 prog_char help_strat_db[] = "reset/show strat_db";
993 parse_pgm_inst_t cmd_strat_db = {
994 .f = cmd_strat_db_parsed, /* function to call */
995 .data = NULL, /* 2nd arg of func */
996 .help_str = help_strat_db,
997 .tokens = { /* token list, NULL terminated */
998 (prog_void *)&cmd_strat_db_arg0,
999 (prog_void *)&cmd_strat_db_arg1,
1004 /**********************************************************/
1005 /* strat configuration */
1007 /* this structure is filled when cmd_strat_conf is parsed successfully */
1008 struct cmd_strat_conf_result {
1009 fixed_string_t arg0;
1010 fixed_string_t arg1;
1013 /* function called when cmd_strat_conf is parsed successfully */
1014 static void cmd_strat_conf_parsed(void *parsed_result, void *data)
1016 // struct cmd_strat_conf_result *res = parsed_result;
1017 strat_conf.dump_enabled = 1;
1018 strat_conf_dump(__FUNCTION__);
1021 prog_char str_strat_conf_arg0[] = "strat_conf";
1022 parse_pgm_token_string_t cmd_strat_conf_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf_result, arg0, str_strat_conf_arg0);
1023 prog_char str_strat_conf_arg1[] = "show#base";
1024 parse_pgm_token_string_t cmd_strat_conf_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf_result, arg1, str_strat_conf_arg1);
1026 prog_char help_strat_conf[] = "configure strat options";
1027 parse_pgm_inst_t cmd_strat_conf = {
1028 .f = cmd_strat_conf_parsed, /* function to call */
1029 .data = NULL, /* 2nd arg of func */
1030 .help_str = help_strat_conf,
1031 .tokens = { /* token list, NULL terminated */
1032 (prog_void *)&cmd_strat_conf_arg0,
1033 (prog_void *)&cmd_strat_conf_arg1,
1038 /**********************************************************/
1039 /* strat configuration */
1041 /* this structure is filled when cmd_strat_conf2 is parsed successfully */
1042 struct cmd_strat_conf2_result {
1043 fixed_string_t arg0;
1044 fixed_string_t arg1;
1045 fixed_string_t arg2;
1048 /* function called when cmd_strat_conf2 is parsed successfully */
1049 static void cmd_strat_conf2_parsed(void *parsed_result, void *data)
1051 struct cmd_strat_conf2_result *res = parsed_result;
1052 uint8_t on, bit = 0;
1054 if (!strcmp_P(res->arg2, PSTR("on")))
1059 if (!strcmp_P(res->arg1, PSTR("our_orange")))
1060 bit = STRAT_CONF_OUR_ORANGE;
1061 else if (!strcmp_P(res->arg1, PSTR("wait_obstacle")))
1062 bit = STRAT_CONF_WAIT_OBSTACLE;
1065 strat_conf.flags |= bit;
1067 strat_conf.flags &= (~bit);
1069 strat_conf.dump_enabled = 1;
1070 strat_conf_dump(__FUNCTION__);
1073 prog_char str_strat_conf2_arg0[] = "strat_conf";
1074 parse_pgm_token_string_t cmd_strat_conf2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg0, str_strat_conf2_arg0);
1075 prog_char str_strat_conf2_arg1[] = "our_orange#wait_obstacle";
1076 parse_pgm_token_string_t cmd_strat_conf2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg1, str_strat_conf2_arg1);
1077 prog_char str_strat_conf2_arg2[] = "on#off";
1078 parse_pgm_token_string_t cmd_strat_conf2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg2, str_strat_conf2_arg2);
1081 prog_char help_strat_conf2[] = "configure strat options";
1082 parse_pgm_inst_t cmd_strat_conf2 = {
1083 .f = cmd_strat_conf2_parsed, /* function to call */
1084 .data = NULL, /* 2nd arg of func */
1085 .help_str = help_strat_conf2,
1086 .tokens = { /* token list, NULL terminated */
1087 (prog_void *)&cmd_strat_conf2_arg0,
1088 (prog_void *)&cmd_strat_conf2_arg1,
1089 (prog_void *)&cmd_strat_conf2_arg2,
1094 /**********************************************************/
1095 /* strat configuration */
1097 /* this structure is filled when cmd_strat_conf3 is parsed successfully */
1098 struct cmd_strat_conf3_result {
1099 fixed_string_t arg0;
1100 fixed_string_t arg1;
1104 /* function called when cmd_strat_conf3 is parsed successfully */
1105 static void cmd_strat_conf3_parsed(void *parsed_result, void *data)
1107 struct cmd_strat_conf3_result *res = parsed_result;
1109 if (!strcmp_P(res->arg1, PSTR("orphan_tomato"))) {
1112 strat_conf.orphan_tomato = res->arg2;
1114 else if (!strcmp_P(res->arg1, PSTR("opp_orange"))) {
1117 strat_conf.opp_orange = res->arg2;
1119 strat_conf.dump_enabled = 1;
1120 strat_conf_dump(__FUNCTION__);
1123 prog_char str_strat_conf3_arg0[] = "strat_conf";
1124 parse_pgm_token_string_t cmd_strat_conf3_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf3_result, arg0, str_strat_conf3_arg0);
1125 prog_char str_strat_conf3_arg1[] = "orphan_tomato#opp_orange";
1126 parse_pgm_token_string_t cmd_strat_conf3_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf3_result, arg1, str_strat_conf3_arg1);
1127 parse_pgm_token_num_t cmd_strat_conf3_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_strat_conf3_result, arg2, UINT8);
1129 prog_char help_strat_conf3[] = "configure strat options";
1130 parse_pgm_inst_t cmd_strat_conf3 = {
1131 .f = cmd_strat_conf3_parsed, /* function to call */
1132 .data = NULL, /* 2nd arg of func */
1133 .help_str = help_strat_conf3,
1134 .tokens = { /* token list, NULL terminated */
1135 (prog_void *)&cmd_strat_conf3_arg0,
1136 (prog_void *)&cmd_strat_conf3_arg1,
1137 (prog_void *)&cmd_strat_conf3_arg2,
1143 /**********************************************************/
1147 /* function called when cmd_test is parsed successfully */
1148 static void subtraj_test(void)
1153 /* this structure is filled when cmd_subtraj is parsed successfully */
1154 struct cmd_subtraj_result {
1155 fixed_string_t arg0;
1156 fixed_string_t arg1;
1163 /* function called when cmd_subtraj is parsed successfully */
1164 static void cmd_subtraj_parsed(void *parsed_result, void *data)
1166 struct cmd_subtraj_result *res = parsed_result;
1168 if (!strcmp_P(res->arg1, PSTR("test")))
1170 else if (!strcmp_P(res->arg1, PSTR("orange_yellow")))
1171 run_to_the_hills(I2C_COLOR_YELLOW);
1172 else if (!strcmp_P(res->arg1, PSTR("orange_blue")))
1173 run_to_the_hills(I2C_COLOR_BLUE);
1174 else if (!strcmp_P(res->arg1, PSTR("tomato"))) {
1175 position_set(&mainboard.pos, 2625,
1176 COLOR_Y(1847), COLOR_A(0.00));
1177 get_orphan_tomatoes();
1180 trajectory_hardstop(&mainboard.traj);
1183 prog_char str_subtraj_arg0[] = "subtraj";
1184 parse_pgm_token_string_t cmd_subtraj_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_subtraj_result, arg0, str_subtraj_arg0);
1185 prog_char str_subtraj_arg1[] = "test#orange_yellow#orange_blue#tomato";
1186 parse_pgm_token_string_t cmd_subtraj_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_subtraj_result, arg1, str_subtraj_arg1);
1187 parse_pgm_token_num_t cmd_subtraj_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg2, INT32);
1188 parse_pgm_token_num_t cmd_subtraj_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg3, INT32);
1189 parse_pgm_token_num_t cmd_subtraj_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg4, INT32);
1190 parse_pgm_token_num_t cmd_subtraj_arg5 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg5, INT32);
1192 prog_char help_subtraj[] = "Test sub-trajectories (a,b,c,d: specific params)";
1193 parse_pgm_inst_t cmd_subtraj = {
1194 .f = cmd_subtraj_parsed, /* function to call */
1195 .data = NULL, /* 2nd arg of func */
1196 .help_str = help_subtraj,
1197 .tokens = { /* token list, NULL terminated */
1198 (prog_void *)&cmd_subtraj_arg0,
1199 (prog_void *)&cmd_subtraj_arg1,
1200 (prog_void *)&cmd_subtraj_arg2,
1201 (prog_void *)&cmd_subtraj_arg3,
1202 (prog_void *)&cmd_subtraj_arg4,
1203 (prog_void *)&cmd_subtraj_arg5,