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"
62 #include "../common/i2c_commands.h"
63 #include "i2c_protocol.h"
65 /**********************************************************/
66 /* Traj_Speeds for trajectory_manager */
68 /* this structure is filled when cmd_traj_speed is parsed successfully */
69 struct cmd_traj_speed_result {
75 /* function called when cmd_traj_speed is parsed successfully */
76 static void cmd_traj_speed_parsed(void *parsed_result, void *data)
78 struct cmd_traj_speed_result * res = parsed_result;
80 if (!strcmp_P(res->arg1, PSTR("angle"))) {
81 trajectory_set_speed(&mainboard.traj, mainboard.traj.d_speed, res->s);
83 else if (!strcmp_P(res->arg1, PSTR("distance"))) {
84 trajectory_set_speed(&mainboard.traj, res->s, mainboard.traj.a_speed);
86 /* else it is a "show" */
88 printf_P(PSTR("angle %u, distance %u\r\n"),
89 mainboard.traj.a_speed,
90 mainboard.traj.d_speed);
93 prog_char str_traj_speed_arg0[] = "traj_speed";
94 parse_pgm_token_string_t cmd_traj_speed_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_traj_speed_result, arg0, str_traj_speed_arg0);
95 prog_char str_traj_speed_arg1[] = "angle#distance";
96 parse_pgm_token_string_t cmd_traj_speed_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_traj_speed_result, arg1, str_traj_speed_arg1);
97 parse_pgm_token_num_t cmd_traj_speed_s = TOKEN_NUM_INITIALIZER(struct cmd_traj_speed_result, s, UINT16);
99 prog_char help_traj_speed[] = "Set traj_speed values for trajectory manager";
100 parse_pgm_inst_t cmd_traj_speed = {
101 .f = cmd_traj_speed_parsed, /* function to call */
102 .data = NULL, /* 2nd arg of func */
103 .help_str = help_traj_speed,
104 .tokens = { /* token list, NULL terminated */
105 (prog_void *)&cmd_traj_speed_arg0,
106 (prog_void *)&cmd_traj_speed_arg1,
107 (prog_void *)&cmd_traj_speed_s,
114 prog_char str_traj_speed_show_arg[] = "show";
115 parse_pgm_token_string_t cmd_traj_speed_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_traj_speed_result, arg1, str_traj_speed_show_arg);
117 prog_char help_traj_speed_show[] = "Show traj_speed values for trajectory manager";
118 parse_pgm_inst_t cmd_traj_speed_show = {
119 .f = cmd_traj_speed_parsed, /* function to call */
120 .data = NULL, /* 2nd arg of func */
121 .help_str = help_traj_speed_show,
122 .tokens = { /* token list, NULL terminated */
123 (prog_void *)&cmd_traj_speed_arg0,
124 (prog_void *)&cmd_traj_speed_show_arg,
129 /**********************************************************/
130 /* circle coef configuration */
132 /* this structure is filled when cmd_circle_coef is parsed successfully */
133 struct cmd_circle_coef_result {
140 /* function called when cmd_circle_coef is parsed successfully */
141 static void cmd_circle_coef_parsed(void *parsed_result, void *data)
143 struct cmd_circle_coef_result *res = parsed_result;
145 if (!strcmp_P(res->arg1, PSTR("set"))) {
146 trajectory_set_circle_coef(&mainboard.traj, res->circle_coef);
149 printf_P(PSTR("circle_coef set %2.2f\r\n"), mainboard.traj.circle_coef);
152 prog_char str_circle_coef_arg0[] = "circle_coef";
153 parse_pgm_token_string_t cmd_circle_coef_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_circle_coef_result, arg0, str_circle_coef_arg0);
154 prog_char str_circle_coef_arg1[] = "set";
155 parse_pgm_token_string_t cmd_circle_coef_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_circle_coef_result, arg1, str_circle_coef_arg1);
156 parse_pgm_token_num_t cmd_circle_coef_val = TOKEN_NUM_INITIALIZER(struct cmd_circle_coef_result, circle_coef, FLOAT);
158 prog_char help_circle_coef[] = "Set circle coef";
159 parse_pgm_inst_t cmd_circle_coef = {
160 .f = cmd_circle_coef_parsed, /* function to call */
161 .data = NULL, /* 2nd arg of func */
162 .help_str = help_circle_coef,
163 .tokens = { /* token list, NULL terminated */
164 (prog_void *)&cmd_circle_coef_arg0,
165 (prog_void *)&cmd_circle_coef_arg1,
166 (prog_void *)&cmd_circle_coef_val,
173 prog_char str_circle_coef_show_arg[] = "show";
174 parse_pgm_token_string_t cmd_circle_coef_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_circle_coef_result, arg1, str_circle_coef_show_arg);
176 prog_char help_circle_coef_show[] = "Show circle coef";
177 parse_pgm_inst_t cmd_circle_coef_show = {
178 .f = cmd_circle_coef_parsed, /* function to call */
179 .data = NULL, /* 2nd arg of func */
180 .help_str = help_circle_coef_show,
181 .tokens = { /* token list, NULL terminated */
182 (prog_void *)&cmd_circle_coef_arg0,
183 (prog_void *)&cmd_circle_coef_show_arg,
188 /**********************************************************/
189 /* trajectory window configuration */
191 /* this structure is filled when cmd_trajectory is parsed successfully */
192 struct cmd_trajectory_result {
201 /* function called when cmd_trajectory is parsed successfully */
202 static void cmd_trajectory_parsed(void * parsed_result, void * data)
204 struct cmd_trajectory_result * res = parsed_result;
206 if (!strcmp_P(res->arg1, PSTR("set"))) {
207 trajectory_set_windows(&mainboard.traj, res->d_win,
208 res->a_win, res->a_start);
211 printf_P(PSTR("trajectory %2.2f %2.2f %2.2f\r\n"), mainboard.traj.d_win,
212 DEG(mainboard.traj.a_win_rad), DEG(mainboard.traj.a_start_rad));
215 prog_char str_trajectory_arg0[] = "trajectory";
216 parse_pgm_token_string_t cmd_trajectory_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_trajectory_result, arg0, str_trajectory_arg0);
217 prog_char str_trajectory_arg1[] = "set";
218 parse_pgm_token_string_t cmd_trajectory_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_trajectory_result, arg1, str_trajectory_arg1);
219 parse_pgm_token_num_t cmd_trajectory_d = TOKEN_NUM_INITIALIZER(struct cmd_trajectory_result, d_win, FLOAT);
220 parse_pgm_token_num_t cmd_trajectory_a = TOKEN_NUM_INITIALIZER(struct cmd_trajectory_result, a_win, FLOAT);
221 parse_pgm_token_num_t cmd_trajectory_as = TOKEN_NUM_INITIALIZER(struct cmd_trajectory_result, a_start, FLOAT);
223 prog_char help_trajectory[] = "Set trajectory windows (distance, angle, angle_start)";
224 parse_pgm_inst_t cmd_trajectory = {
225 .f = cmd_trajectory_parsed, /* function to call */
226 .data = NULL, /* 2nd arg of func */
227 .help_str = help_trajectory,
228 .tokens = { /* token list, NULL terminated */
229 (prog_void *)&cmd_trajectory_arg0,
230 (prog_void *)&cmd_trajectory_arg1,
231 (prog_void *)&cmd_trajectory_d,
232 (prog_void *)&cmd_trajectory_a,
233 (prog_void *)&cmd_trajectory_as,
240 prog_char str_trajectory_show_arg[] = "show";
241 parse_pgm_token_string_t cmd_trajectory_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_trajectory_result, arg1, str_trajectory_show_arg);
243 prog_char help_trajectory_show[] = "Show trajectory window configuration";
244 parse_pgm_inst_t cmd_trajectory_show = {
245 .f = cmd_trajectory_parsed, /* function to call */
246 .data = NULL, /* 2nd arg of func */
247 .help_str = help_trajectory_show,
248 .tokens = { /* token list, NULL terminated */
249 (prog_void *)&cmd_trajectory_arg0,
250 (prog_void *)&cmd_trajectory_show_arg,
255 /**********************************************************/
256 /* rs_gains configuration */
258 /* this structure is filled when cmd_rs_gains is parsed successfully */
259 struct cmd_rs_gains_result {
266 /* function called when cmd_rs_gains is parsed successfully */
267 static void cmd_rs_gains_parsed(void * parsed_result, void * data)
270 printf("not implemented\n");
272 struct cmd_rs_gains_result * res = parsed_result;
274 if (!strcmp_P(res->arg1, PSTR("set"))) {
275 rs_set_left_ext_encoder(&mainboard.rs, encoders_spi_get_value,
276 LEFT_ENCODER, res->left); // en augmentant on tourne à gauche
277 rs_set_right_ext_encoder(&mainboard.rs, encoders_spi_get_value,
278 RIGHT_ENCODER, res->right); //en augmentant on tourne à droite
280 printf_P(PSTR("rs_gains set %2.2f %2.2f\r\n"),
281 mainboard.rs.left_ext_gain, mainboard.rs.right_ext_gain);
285 prog_char str_rs_gains_arg0[] = "rs_gains";
286 parse_pgm_token_string_t cmd_rs_gains_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_rs_gains_result, arg0, str_rs_gains_arg0);
287 prog_char str_rs_gains_arg1[] = "set";
288 parse_pgm_token_string_t cmd_rs_gains_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_rs_gains_result, arg1, str_rs_gains_arg1);
289 parse_pgm_token_num_t cmd_rs_gains_l = TOKEN_NUM_INITIALIZER(struct cmd_rs_gains_result, left, FLOAT);
290 parse_pgm_token_num_t cmd_rs_gains_r = TOKEN_NUM_INITIALIZER(struct cmd_rs_gains_result, right, FLOAT);
292 prog_char help_rs_gains[] = "Set rs_gains (left, right)";
293 parse_pgm_inst_t cmd_rs_gains = {
294 .f = cmd_rs_gains_parsed, /* function to call */
295 .data = NULL, /* 2nd arg of func */
296 .help_str = help_rs_gains,
297 .tokens = { /* token list, NULL terminated */
298 (prog_void *)&cmd_rs_gains_arg0,
299 (prog_void *)&cmd_rs_gains_arg1,
300 (prog_void *)&cmd_rs_gains_l,
301 (prog_void *)&cmd_rs_gains_r,
308 prog_char str_rs_gains_show_arg[] = "show";
309 parse_pgm_token_string_t cmd_rs_gains_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_rs_gains_result, arg1, str_rs_gains_show_arg);
311 prog_char help_rs_gains_show[] = "Show rs_gains";
312 parse_pgm_inst_t cmd_rs_gains_show = {
313 .f = cmd_rs_gains_parsed, /* function to call */
314 .data = NULL, /* 2nd arg of func */
315 .help_str = help_rs_gains_show,
316 .tokens = { /* token list, NULL terminated */
317 (prog_void *)&cmd_rs_gains_arg0,
318 (prog_void *)&cmd_rs_gains_show_arg,
323 /**********************************************************/
324 /* track configuration */
326 /* this structure is filled when cmd_track is parsed successfully */
327 struct cmd_track_result {
333 /* function called when cmd_track is parsed successfully */
334 static void cmd_track_parsed(void * parsed_result, void * data)
336 struct cmd_track_result * res = parsed_result;
338 if (!strcmp_P(res->arg1, PSTR("set"))) {
339 position_set_physical_params(&mainboard.pos, res->val, DIST_IMP_MM);
341 printf_P(PSTR("track set %f\r\n"), mainboard.pos.phys.track_mm);
344 prog_char str_track_arg0[] = "track";
345 parse_pgm_token_string_t cmd_track_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_track_result, arg0, str_track_arg0);
346 prog_char str_track_arg1[] = "set";
347 parse_pgm_token_string_t cmd_track_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_track_result, arg1, str_track_arg1);
348 parse_pgm_token_num_t cmd_track_val = TOKEN_NUM_INITIALIZER(struct cmd_track_result, val, FLOAT);
350 prog_char help_track[] = "Set track in mm";
351 parse_pgm_inst_t cmd_track = {
352 .f = cmd_track_parsed, /* function to call */
353 .data = NULL, /* 2nd arg of func */
354 .help_str = help_track,
355 .tokens = { /* token list, NULL terminated */
356 (prog_void *)&cmd_track_arg0,
357 (prog_void *)&cmd_track_arg1,
358 (prog_void *)&cmd_track_val,
365 prog_char str_track_show_arg[] = "show";
366 parse_pgm_token_string_t cmd_track_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_track_result, arg1, str_track_show_arg);
368 prog_char help_track_show[] = "Show track";
369 parse_pgm_inst_t cmd_track_show = {
370 .f = cmd_track_parsed, /* function to call */
371 .data = NULL, /* 2nd arg of func */
372 .help_str = help_track_show,
373 .tokens = { /* token list, NULL terminated */
374 (prog_void *)&cmd_track_arg0,
375 (prog_void *)&cmd_track_show_arg,
382 /**********************************************************/
383 /* Pt_Lists for testing traj */
385 #define PT_LIST_SIZE 10
386 static struct xy_point pt_list[PT_LIST_SIZE];
387 static uint16_t pt_list_len = 0;
389 /* this structure is filled when cmd_pt_list is parsed successfully */
390 struct cmd_pt_list_result {
398 /* function called when cmd_pt_list is parsed successfully */
399 static void cmd_pt_list_parsed(void * parsed_result, void * data)
401 struct cmd_pt_list_result * res = parsed_result;
404 if (!strcmp_P(res->arg1, PSTR("append"))) {
405 res->arg2 = pt_list_len;
407 if (!strcmp_P(res->arg1, PSTR("avoid_start"))) {
408 printf_P(PSTR("removed\r\n"));
412 if (!strcmp_P(res->arg1, PSTR("insert")) ||
413 !strcmp_P(res->arg1, PSTR("append"))) {
414 if (res->arg2 > pt_list_len) {
415 printf_P(PSTR("Index too large\r\n"));
418 if (pt_list_len >= PT_LIST_SIZE) {
419 printf_P(PSTR("List is too large\r\n"));
422 memmove(&pt_list[res->arg2+1], &pt_list[res->arg2],
423 PT_LIST_SIZE-1-res->arg2);
424 pt_list[res->arg2].x = res->arg3;
425 pt_list[res->arg2].y = res->arg4;
428 else if (!strcmp_P(res->arg1, PSTR("del"))) {
429 if (pt_list_len <= 0) {
430 printf_P(PSTR("Error: list empty\r\n"));
433 if (res->arg2 > pt_list_len) {
434 printf_P(PSTR("Index too large\r\n"));
437 memmove(&pt_list[res->arg2], &pt_list[res->arg2+1],
438 (PT_LIST_SIZE-1-res->arg2)*sizeof(struct xy_point));
441 else if (!strcmp_P(res->arg1, PSTR("reset"))) {
445 /* else it is a "show" or a "start" */
446 if (pt_list_len == 0) {
447 printf_P(PSTR("List empty\r\n"));
451 for (i=0 ; i<pt_list_len ; i++) {
452 printf_P(PSTR("%d: x=%d y=%d\r\n"), i, pt_list[i].x, pt_list[i].y);
453 if (!strcmp_P(res->arg1, PSTR("start"))) {
454 trajectory_goto_xy_abs(&mainboard.traj, pt_list[i].x, pt_list[i].y);
455 why = wait_traj_end(0xFF); /* all */
457 else if (!strcmp_P(res->arg1, PSTR("loop_start"))) {
458 trajectory_goto_xy_abs(&mainboard.traj, pt_list[i].x, pt_list[i].y);
459 why = wait_traj_end(0xFF); /* all */
462 else if (!strcmp_P(res->arg1, PSTR("avoid_start"))) {
464 why = goto_and_avoid(pt_list[i].x, pt_list[i].y, 0xFF, 0xFF);
465 printf("next point\r\n");
466 if (why != END_OBSTACLE)
471 if (why & (~(END_TRAJ | END_NEAR)))
472 trajectory_stop(&mainboard.traj);
478 if (!strcmp_P(res->arg1, PSTR("loop_start")))
482 prog_char str_pt_list_arg0[] = "pt_list";
483 parse_pgm_token_string_t cmd_pt_list_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg0, str_pt_list_arg0);
484 prog_char str_pt_list_arg1[] = "insert";
485 parse_pgm_token_string_t cmd_pt_list_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_arg1);
486 parse_pgm_token_num_t cmd_pt_list_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_pt_list_result, arg2, UINT16);
487 parse_pgm_token_num_t cmd_pt_list_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_pt_list_result, arg3, INT16);
488 parse_pgm_token_num_t cmd_pt_list_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_pt_list_result, arg4, INT16);
490 prog_char help_pt_list[] = "Insert point in pt_list (idx,x,y)";
491 parse_pgm_inst_t cmd_pt_list = {
492 .f = cmd_pt_list_parsed, /* function to call */
493 .data = NULL, /* 2nd arg of func */
494 .help_str = help_pt_list,
495 .tokens = { /* token list, NULL terminated */
496 (prog_void *)&cmd_pt_list_arg0,
497 (prog_void *)&cmd_pt_list_arg1,
498 (prog_void *)&cmd_pt_list_arg2,
499 (prog_void *)&cmd_pt_list_arg3,
500 (prog_void *)&cmd_pt_list_arg4,
507 prog_char str_pt_list_arg1_append[] = "append";
508 parse_pgm_token_string_t cmd_pt_list_arg1_append = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_arg1_append);
510 prog_char help_pt_list_append[] = "Append point in pt_list (x,y)";
511 parse_pgm_inst_t cmd_pt_list_append = {
512 .f = cmd_pt_list_parsed, /* function to call */
513 .data = NULL, /* 2nd arg of func */
514 .help_str = help_pt_list_append,
515 .tokens = { /* token list, NULL terminated */
516 (prog_void *)&cmd_pt_list_arg0,
517 (prog_void *)&cmd_pt_list_arg1_append,
518 (prog_void *)&cmd_pt_list_arg3,
519 (prog_void *)&cmd_pt_list_arg4,
526 prog_char str_pt_list_del_arg[] = "del";
527 parse_pgm_token_string_t cmd_pt_list_del_arg = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_del_arg);
529 prog_char help_pt_list_del[] = "Del or insert point in pt_list (num)";
530 parse_pgm_inst_t cmd_pt_list_del = {
531 .f = cmd_pt_list_parsed, /* function to call */
532 .data = NULL, /* 2nd arg of func */
533 .help_str = help_pt_list_del,
534 .tokens = { /* token list, NULL terminated */
535 (prog_void *)&cmd_pt_list_arg0,
536 (prog_void *)&cmd_pt_list_del_arg,
537 (prog_void *)&cmd_pt_list_arg2,
543 prog_char str_pt_list_show_arg[] = "show#reset#start#avoid_start#loop_start";
544 parse_pgm_token_string_t cmd_pt_list_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_show_arg);
546 prog_char help_pt_list_show[] = "Show, start or reset pt_list";
547 parse_pgm_inst_t cmd_pt_list_show = {
548 .f = cmd_pt_list_parsed, /* function to call */
549 .data = NULL, /* 2nd arg of func */
550 .help_str = help_pt_list_show,
551 .tokens = { /* token list, NULL terminated */
552 (prog_void *)&cmd_pt_list_arg0,
553 (prog_void *)&cmd_pt_list_show_arg,
560 /**********************************************************/
563 /* this structure is filled when cmd_goto is parsed successfully */
564 struct cmd_goto_result {
572 /* function called when cmd_goto is parsed successfully */
573 static void cmd_goto_parsed(void * parsed_result, void * data)
575 struct cmd_goto_result * res = parsed_result;
579 interrupt_traj_reset();
580 if (!strcmp_P(res->arg1, PSTR("a_rel"))) {
581 trajectory_a_rel(&mainboard.traj, res->arg2);
583 else if (!strcmp_P(res->arg1, PSTR("d_rel"))) {
584 trajectory_d_rel(&mainboard.traj, res->arg2);
586 else if (!strcmp_P(res->arg1, PSTR("a_abs"))) {
587 trajectory_a_abs(&mainboard.traj, res->arg2);
589 else if (!strcmp_P(res->arg1, PSTR("a_to_xy"))) {
590 trajectory_turnto_xy(&mainboard.traj, res->arg2, res->arg3);
592 else if (!strcmp_P(res->arg1, PSTR("a_behind_xy"))) {
593 trajectory_turnto_xy_behind(&mainboard.traj, res->arg2, res->arg3);
595 else if (!strcmp_P(res->arg1, PSTR("xy_rel"))) {
596 trajectory_goto_xy_rel(&mainboard.traj, res->arg2, res->arg3);
598 else if (!strcmp_P(res->arg1, PSTR("xy_abs"))) {
599 trajectory_goto_xy_abs(&mainboard.traj, res->arg2, res->arg3);
601 else if (!strcmp_P(res->arg1, PSTR("avoid"))) {
603 err = goto_and_avoid_forward(res->arg2, res->arg3, 0xFF, 0xFF);
604 if (err != END_TRAJ && err != END_NEAR)
607 printf_P(PSTR("not implemented\r\n"));
611 else if (!strcmp_P(res->arg1, PSTR("avoid_bw"))) {
613 err = goto_and_avoid_backward(res->arg2, res->arg3, 0xFF, 0xFF);
614 if (err != END_TRAJ && err != END_NEAR)
617 printf_P(PSTR("not implemented\r\n"));
621 else if (!strcmp_P(res->arg1, PSTR("xy_abs_fow"))) {
622 trajectory_goto_forward_xy_abs(&mainboard.traj, res->arg2, res->arg3);
624 else if (!strcmp_P(res->arg1, PSTR("xy_abs_back"))) {
625 trajectory_goto_backward_xy_abs(&mainboard.traj, res->arg2, res->arg3);
627 else if (!strcmp_P(res->arg1, PSTR("da_rel"))) {
628 trajectory_d_a_rel(&mainboard.traj, res->arg2, res->arg3);
631 while ((err = test_traj_end(0xFF)) == 0) {
633 if (t2 - t1 > 200000) {
634 dump_cs_debug("angle", &mainboard.angle.cs);
635 dump_cs_debug("distance", &mainboard.distance.cs);
639 if (err != END_TRAJ && err != END_NEAR)
641 printf_P(PSTR("returned %s\r\n"), get_err(err));
644 prog_char str_goto_arg0[] = "goto";
645 parse_pgm_token_string_t cmd_goto_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_goto_result, arg0, str_goto_arg0);
646 prog_char str_goto_arg1_a[] = "d_rel#a_rel#a_abs";
647 parse_pgm_token_string_t cmd_goto_arg1_a = TOKEN_STRING_INITIALIZER(struct cmd_goto_result, arg1, str_goto_arg1_a);
648 parse_pgm_token_num_t cmd_goto_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_goto_result, arg2, INT32);
651 prog_char help_goto1[] = "Change orientation of the mainboard";
652 parse_pgm_inst_t cmd_goto1 = {
653 .f = cmd_goto_parsed, /* function to call */
654 .data = NULL, /* 2nd arg of func */
655 .help_str = help_goto1,
656 .tokens = { /* token list, NULL terminated */
657 (prog_void *)&cmd_goto_arg0,
658 (prog_void *)&cmd_goto_arg1_a,
659 (prog_void *)&cmd_goto_arg2,
664 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";
665 parse_pgm_token_string_t cmd_goto_arg1_b = TOKEN_STRING_INITIALIZER(struct cmd_goto_result, arg1, str_goto_arg1_b);
666 parse_pgm_token_num_t cmd_goto_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_goto_result, arg3, INT32);
669 prog_char help_goto2[] = "Go to a (x,y) or (d,a) position";
670 parse_pgm_inst_t cmd_goto2 = {
671 .f = cmd_goto_parsed, /* function to call */
672 .data = NULL, /* 2nd arg of func */
673 .help_str = help_goto2,
674 .tokens = { /* token list, NULL terminated */
675 (prog_void *)&cmd_goto_arg0,
676 (prog_void *)&cmd_goto_arg1_b,
677 (prog_void *)&cmd_goto_arg2,
678 (prog_void *)&cmd_goto_arg3,
683 /**********************************************************/
686 /* this structure is filled when cmd_position is parsed successfully */
687 struct cmd_position_result {
695 #define AUTOPOS_SPEED_FAST 200
696 static void auto_position(void)
699 uint16_t old_spdd, old_spda;
701 interrupt_traj_reset();
702 strat_get_speed(&old_spdd, &old_spda);
703 strat_set_speed(AUTOPOS_SPEED_FAST, AUTOPOS_SPEED_FAST);
705 trajectory_d_rel(&mainboard.traj, 300);
706 err = wait_traj_end(END_INTR|END_TRAJ|END_BLOCKING);
710 strat_reset_pos(ROBOT_WIDTH/2,
711 COLOR_Y(ROBOT_HALF_LENGTH_FRONT),
714 trajectory_d_rel(&mainboard.traj, -180);
715 err = wait_traj_end(END_INTR|END_TRAJ);
719 trajectory_a_rel(&mainboard.traj, COLOR_A(-90));
720 err = wait_traj_end(END_INTR|END_TRAJ);
724 trajectory_d_rel(&mainboard.traj, 300);
725 err = wait_traj_end(END_INTR|END_TRAJ|END_BLOCKING);
729 strat_reset_pos(ROBOT_HALF_LENGTH_FRONT,
733 trajectory_d_rel(&mainboard.traj, -170);
734 err = wait_traj_end(END_INTR|END_TRAJ);
739 trajectory_a_rel(&mainboard.traj, COLOR_A(-110));
740 err = wait_traj_end(END_INTR|END_TRAJ);
745 strat_set_speed(old_spdd, old_spda);
750 strat_set_speed(old_spdd, old_spda);
753 /* function called when cmd_position is parsed successfully */
754 static void cmd_position_parsed(void * parsed_result, void * data)
756 struct cmd_position_result * res = parsed_result;
758 /* display raw position values */
759 if (!strcmp_P(res->arg1, PSTR("reset"))) {
760 position_set(&mainboard.pos, 0, 0, 0);
762 else if (!strcmp_P(res->arg1, PSTR("set"))) {
763 position_set(&mainboard.pos, res->arg2, res->arg3, res->arg4);
765 else if (!strcmp_P(res->arg1, PSTR("autoset_blue"))) {
766 mainboard.our_color = I2C_COLOR_BLUE;
768 i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_BLUE);
769 i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_BLUE);
773 else if (!strcmp_P(res->arg1, PSTR("autoset_yellow"))) {
774 mainboard.our_color = I2C_COLOR_YELLOW;
776 i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_YELLOW);
777 i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_YELLOW);
782 /* else it's just a "show" */
783 printf_P(PSTR("x=%.2f y=%.2f a=%.2f\r\n"),
784 position_get_x_double(&mainboard.pos),
785 position_get_y_double(&mainboard.pos),
786 DEG(position_get_a_rad_double(&mainboard.pos)));
789 prog_char str_position_arg0[] = "position";
790 parse_pgm_token_string_t cmd_position_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg0, str_position_arg0);
791 prog_char str_position_arg1[] = "show#reset#autoset_blue#autoset_yellow";
792 parse_pgm_token_string_t cmd_position_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg1, str_position_arg1);
794 prog_char help_position[] = "Show/reset (x,y,a) position";
795 parse_pgm_inst_t cmd_position = {
796 .f = cmd_position_parsed, /* function to call */
797 .data = NULL, /* 2nd arg of func */
798 .help_str = help_position,
799 .tokens = { /* token list, NULL terminated */
800 (prog_void *)&cmd_position_arg0,
801 (prog_void *)&cmd_position_arg1,
807 prog_char str_position_arg1_set[] = "set";
808 parse_pgm_token_string_t cmd_position_arg1_set = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg1, str_position_arg1_set);
809 parse_pgm_token_num_t cmd_position_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg2, INT32);
810 parse_pgm_token_num_t cmd_position_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg3, INT32);
811 parse_pgm_token_num_t cmd_position_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg4, INT32);
813 prog_char help_position_set[] = "Set (x,y,a) position";
814 parse_pgm_inst_t cmd_position_set = {
815 .f = cmd_position_parsed, /* function to call */
816 .data = NULL, /* 2nd arg of func */
817 .help_str = help_position_set,
818 .tokens = { /* token list, NULL terminated */
819 (prog_void *)&cmd_position_arg0,
820 (prog_void *)&cmd_position_arg1_set,
821 (prog_void *)&cmd_position_arg2,
822 (prog_void *)&cmd_position_arg3,
823 (prog_void *)&cmd_position_arg4,
829 /**********************************************************/
830 /* strat configuration */
832 /* this structure is filled when cmd_strat_infos is parsed successfully */
833 struct cmd_strat_infos_result {
838 /* function called when cmd_strat_infos is parsed successfully */
839 static void cmd_strat_infos_parsed(void *parsed_result, void *data)
841 struct cmd_strat_infos_result *res = parsed_result;
843 if (!strcmp_P(res->arg1, PSTR("reset"))) {
846 strat_infos.dump_enabled = 1;
847 strat_dump_infos(__FUNCTION__);
850 prog_char str_strat_infos_arg0[] = "strat_infos";
851 parse_pgm_token_string_t cmd_strat_infos_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_infos_result, arg0, str_strat_infos_arg0);
852 prog_char str_strat_infos_arg1[] = "show#reset";
853 parse_pgm_token_string_t cmd_strat_infos_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_infos_result, arg1, str_strat_infos_arg1);
855 prog_char help_strat_infos[] = "reset/show strat_infos";
856 parse_pgm_inst_t cmd_strat_infos = {
857 .f = cmd_strat_infos_parsed, /* function to call */
858 .data = NULL, /* 2nd arg of func */
859 .help_str = help_strat_infos,
860 .tokens = { /* token list, NULL terminated */
861 (prog_void *)&cmd_strat_infos_arg0,
862 (prog_void *)&cmd_strat_infos_arg1,
867 /**********************************************************/
868 /* strat configuration */
870 /* this structure is filled when cmd_strat_conf is parsed successfully */
871 struct cmd_strat_conf_result {
876 /* function called when cmd_strat_conf is parsed successfully */
877 static void cmd_strat_conf_parsed(void *parsed_result, void *data)
879 // struct cmd_strat_conf_result *res = parsed_result;
881 strat_infos.dump_enabled = 1;
885 prog_char str_strat_conf_arg0[] = "strat_conf";
886 parse_pgm_token_string_t cmd_strat_conf_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf_result, arg0, str_strat_conf_arg0);
887 prog_char str_strat_conf_arg1[] = "show#base";
888 parse_pgm_token_string_t cmd_strat_conf_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf_result, arg1, str_strat_conf_arg1);
890 prog_char help_strat_conf[] = "configure strat options";
891 parse_pgm_inst_t cmd_strat_conf = {
892 .f = cmd_strat_conf_parsed, /* function to call */
893 .data = NULL, /* 2nd arg of func */
894 .help_str = help_strat_conf,
895 .tokens = { /* token list, NULL terminated */
896 (prog_void *)&cmd_strat_conf_arg0,
897 (prog_void *)&cmd_strat_conf_arg1,
902 /**********************************************************/
903 /* strat configuration */
905 /* this structure is filled when cmd_strat_conf2 is parsed successfully */
906 struct cmd_strat_conf2_result {
912 /* function called when cmd_strat_conf2 is parsed successfully */
913 static void cmd_strat_conf2_parsed(void *parsed_result, void *data)
915 struct cmd_strat_conf2_result *res = parsed_result;
918 if (!strcmp_P(res->arg2, PSTR("on")))
924 if (!strcmp_P(res->arg1, PSTR("one_temple_on_disc")))
925 bit = STRAT_CONF_ONLY_ONE_ON_DISC;
926 else if (!strcmp_P(res->arg1, PSTR("bypass_static2")))
927 bit = STRAT_CONF_BYPASS_STATIC2;
928 else if (!strcmp_P(res->arg1, PSTR("take_one_lintel")))
929 bit = STRAT_CONF_TAKE_ONE_LINTEL;
930 else if (!strcmp_P(res->arg1, PSTR("skip_when_check_fail")))
931 bit = STRAT_CONF_TAKE_ONE_LINTEL;
932 else if (!strcmp_P(res->arg1, PSTR("store_static2")))
933 bit = STRAT_CONF_STORE_STATIC2;
934 else if (!strcmp_P(res->arg1, PSTR("big3_temple")))
935 bit = STRAT_CONF_BIG_3_TEMPLE;
936 else if (!strcmp_P(res->arg1, PSTR("early_opp_scan")))
937 bit = STRAT_CONF_EARLY_SCAN;
938 else if (!strcmp_P(res->arg1, PSTR("push_opp_cols")))
939 bit = STRAT_CONF_PUSH_OPP_COLS;
943 strat_infos.conf.flags |= bit;
945 strat_infos.conf.flags &= (~bit);
947 strat_infos.dump_enabled = 1;
951 prog_char str_strat_conf2_arg0[] = "strat_conf";
952 parse_pgm_token_string_t cmd_strat_conf2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg0, str_strat_conf2_arg0);
953 prog_char str_strat_conf2_arg1[] = "faux";
954 parse_pgm_token_string_t cmd_strat_conf2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg1, str_strat_conf2_arg1);
955 prog_char str_strat_conf2_arg2[] = "on#off";
956 parse_pgm_token_string_t cmd_strat_conf2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg2, str_strat_conf2_arg2);
959 prog_char help_strat_conf2[] = "configure strat options";
960 parse_pgm_inst_t cmd_strat_conf2 = {
961 .f = cmd_strat_conf2_parsed, /* function to call */
962 .data = NULL, /* 2nd arg of func */
963 .help_str = help_strat_conf2,
964 .tokens = { /* token list, NULL terminated */
965 (prog_void *)&cmd_strat_conf2_arg0,
966 (prog_void *)&cmd_strat_conf2_arg1,
967 (prog_void *)&cmd_strat_conf2_arg2,
972 /**********************************************************/
973 /* strat configuration */
975 /* this structure is filled when cmd_strat_conf3 is parsed successfully */
976 struct cmd_strat_conf3_result {
982 /* function called when cmd_strat_conf3 is parsed successfully */
983 static void cmd_strat_conf3_parsed(void *parsed_result, void *data)
986 struct cmd_strat_conf3_result *res = parsed_result;
988 if (!strcmp_P(res->arg1, PSTR("scan_opponent_min_time"))) {
991 strat_infos.conf.scan_opp_min_time = res->arg2;
993 else if (!strcmp_P(res->arg1, PSTR("delay_between_opponent_scan"))) {
996 strat_infos.conf.delay_between_opp_scan = res->arg2;
998 else if (!strcmp_P(res->arg1, PSTR("scan_our_min_time"))) {
1001 strat_infos.conf.scan_our_min_time = res->arg2;
1003 else if (!strcmp_P(res->arg1, PSTR("delay_between_our_scan"))) {
1006 strat_infos.conf.delay_between_our_scan = res->arg2;
1008 else if (!strcmp_P(res->arg1, PSTR("wait_opponent"))) {
1009 strat_infos.conf.wait_opponent = res->arg2;
1011 else if (!strcmp_P(res->arg1, PSTR("lintel_min_time"))) {
1012 strat_infos.conf.lintel_min_time = res->arg2;
1015 strat_infos.dump_enabled = 1;
1019 prog_char str_strat_conf3_arg0[] = "strat_conf";
1020 parse_pgm_token_string_t cmd_strat_conf3_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf3_result, arg0, str_strat_conf3_arg0);
1021 prog_char str_strat_conf3_arg1[] = "faux2";
1022 parse_pgm_token_string_t cmd_strat_conf3_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf3_result, arg1, str_strat_conf3_arg1);
1023 parse_pgm_token_num_t cmd_strat_conf3_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_strat_conf3_result, arg2, UINT16);
1025 prog_char help_strat_conf3[] = "configure strat options";
1026 parse_pgm_inst_t cmd_strat_conf3 = {
1027 .f = cmd_strat_conf3_parsed, /* function to call */
1028 .data = NULL, /* 2nd arg of func */
1029 .help_str = help_strat_conf3,
1030 .tokens = { /* token list, NULL terminated */
1031 (prog_void *)&cmd_strat_conf3_arg0,
1032 (prog_void *)&cmd_strat_conf3_arg1,
1033 (prog_void *)&cmd_strat_conf3_arg2,
1039 /**********************************************************/
1042 /* this structure is filled when cmd_subtraj is parsed successfully */
1043 struct cmd_subtraj_result {
1044 fixed_string_t arg0;
1045 fixed_string_t arg1;
1052 /* function called when cmd_subtraj is parsed successfully */
1053 static void cmd_subtraj_parsed(void *parsed_result, void *data)
1055 /* struct cmd_subtraj_result *res = parsed_result; */
1057 printf_P(PSTR("TODO\r\n"));
1058 trajectory_hardstop(&mainboard.traj);
1061 prog_char str_subtraj_arg0[] = "subtraj";
1062 parse_pgm_token_string_t cmd_subtraj_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_subtraj_result, arg0, str_subtraj_arg0);
1063 prog_char str_subtraj_arg1[] = "faux";
1064 parse_pgm_token_string_t cmd_subtraj_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_subtraj_result, arg1, str_subtraj_arg1);
1065 parse_pgm_token_num_t cmd_subtraj_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg2, INT32);
1066 parse_pgm_token_num_t cmd_subtraj_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg3, INT32);
1067 parse_pgm_token_num_t cmd_subtraj_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg4, INT32);
1068 parse_pgm_token_num_t cmd_subtraj_arg5 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg5, INT32);
1070 prog_char help_subtraj[] = "Test sub-trajectories (a,b,c,d: specific params)";
1071 parse_pgm_inst_t cmd_subtraj = {
1072 .f = cmd_subtraj_parsed, /* function to call */
1073 .data = NULL, /* 2nd arg of func */
1074 .help_str = help_subtraj,
1075 .tokens = { /* token list, NULL terminated */
1076 (prog_void *)&cmd_subtraj_arg0,
1077 (prog_void *)&cmd_subtraj_arg1,
1078 (prog_void *)&cmd_subtraj_arg2,
1079 (prog_void *)&cmd_subtraj_arg3,
1080 (prog_void *)&cmd_subtraj_arg4,
1081 (prog_void *)&cmd_subtraj_arg5,