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>
26 #include <aversive/pgmspace.h>
27 #include <aversive/wait.h>
28 #include <aversive/error.h>
33 #include <clock_time.h>
35 #include <encoders_spi.h>
39 #include <control_system_manager.h>
40 #include <trajectory_manager.h>
41 #include <vect_base.h>
44 #include <obstacle_avoidance.h>
45 #include <blocking_detection_manager.h>
46 #include <robot_system.h>
47 #include <position_manager.h>
51 #include <parse_string.h>
52 #include <parse_num.h>
57 #include "strat_utils.h"
58 #include "strat_base.h"
60 #include "../common/i2c_commands.h"
61 #include "i2c_protocol.h"
63 /**********************************************************/
64 /* Traj_Speeds for trajectory_manager */
66 /* this structure is filled when cmd_traj_speed is parsed successfully */
67 struct cmd_traj_speed_result {
73 /* function called when cmd_traj_speed is parsed successfully */
74 static void cmd_traj_speed_parsed(void *parsed_result, void *data)
76 struct cmd_traj_speed_result * res = parsed_result;
78 if (!strcmp_P(res->arg1, PSTR("angle"))) {
79 trajectory_set_speed(&mainboard.traj, mainboard.traj.d_speed, res->s);
81 else if (!strcmp_P(res->arg1, PSTR("distance"))) {
82 trajectory_set_speed(&mainboard.traj, res->s, mainboard.traj.a_speed);
84 /* else it is a "show" */
86 printf_P(PSTR("angle %u, distance %u\r\n"),
87 mainboard.traj.a_speed,
88 mainboard.traj.d_speed);
91 prog_char str_traj_speed_arg0[] = "traj_speed";
92 parse_pgm_token_string_t cmd_traj_speed_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_traj_speed_result, arg0, str_traj_speed_arg0);
93 prog_char str_traj_speed_arg1[] = "angle#distance";
94 parse_pgm_token_string_t cmd_traj_speed_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_traj_speed_result, arg1, str_traj_speed_arg1);
95 parse_pgm_token_num_t cmd_traj_speed_s = TOKEN_NUM_INITIALIZER(struct cmd_traj_speed_result, s, UINT16);
97 prog_char help_traj_speed[] = "Set traj_speed values for trajectory manager";
98 parse_pgm_inst_t cmd_traj_speed = {
99 .f = cmd_traj_speed_parsed, /* function to call */
100 .data = NULL, /* 2nd arg of func */
101 .help_str = help_traj_speed,
102 .tokens = { /* token list, NULL terminated */
103 (prog_void *)&cmd_traj_speed_arg0,
104 (prog_void *)&cmd_traj_speed_arg1,
105 (prog_void *)&cmd_traj_speed_s,
112 prog_char str_traj_speed_show_arg[] = "show";
113 parse_pgm_token_string_t cmd_traj_speed_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_traj_speed_result, arg1, str_traj_speed_show_arg);
115 prog_char help_traj_speed_show[] = "Show traj_speed values for trajectory manager";
116 parse_pgm_inst_t cmd_traj_speed_show = {
117 .f = cmd_traj_speed_parsed, /* function to call */
118 .data = NULL, /* 2nd arg of func */
119 .help_str = help_traj_speed_show,
120 .tokens = { /* token list, NULL terminated */
121 (prog_void *)&cmd_traj_speed_arg0,
122 (prog_void *)&cmd_traj_speed_show_arg,
127 /**********************************************************/
128 /* circle coef configuration */
130 /* this structure is filled when cmd_circle_coef is parsed successfully */
131 struct cmd_circle_coef_result {
138 /* function called when cmd_circle_coef is parsed successfully */
139 static void cmd_circle_coef_parsed(void *parsed_result, void *data)
141 struct cmd_circle_coef_result *res = parsed_result;
143 if (!strcmp_P(res->arg1, PSTR("set"))) {
144 trajectory_set_circle_coef(&mainboard.traj, res->circle_coef);
147 printf_P(PSTR("circle_coef %2.2f\r\n"), mainboard.traj.circle_coef);
150 prog_char str_circle_coef_arg0[] = "circle_coef";
151 parse_pgm_token_string_t cmd_circle_coef_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_circle_coef_result, arg0, str_circle_coef_arg0);
152 prog_char str_circle_coef_arg1[] = "set";
153 parse_pgm_token_string_t cmd_circle_coef_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_circle_coef_result, arg1, str_circle_coef_arg1);
154 parse_pgm_token_num_t cmd_circle_coef_val = TOKEN_NUM_INITIALIZER(struct cmd_circle_coef_result, circle_coef, FLOAT);
156 prog_char help_circle_coef[] = "Set circle coef";
157 parse_pgm_inst_t cmd_circle_coef = {
158 .f = cmd_circle_coef_parsed, /* function to call */
159 .data = NULL, /* 2nd arg of func */
160 .help_str = help_circle_coef,
161 .tokens = { /* token list, NULL terminated */
162 (prog_void *)&cmd_circle_coef_arg0,
163 (prog_void *)&cmd_circle_coef_arg1,
164 (prog_void *)&cmd_circle_coef_val,
171 prog_char str_circle_coef_show_arg[] = "show";
172 parse_pgm_token_string_t cmd_circle_coef_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_circle_coef_result, arg1, str_circle_coef_show_arg);
174 prog_char help_circle_coef_show[] = "Show circle coef";
175 parse_pgm_inst_t cmd_circle_coef_show = {
176 .f = cmd_circle_coef_parsed, /* function to call */
177 .data = NULL, /* 2nd arg of func */
178 .help_str = help_circle_coef_show,
179 .tokens = { /* token list, NULL terminated */
180 (prog_void *)&cmd_circle_coef_arg0,
181 (prog_void *)&cmd_circle_coef_show_arg,
186 /**********************************************************/
187 /* trajectory window configuration */
189 /* this structure is filled when cmd_trajectory is parsed successfully */
190 struct cmd_trajectory_result {
199 /* function called when cmd_trajectory is parsed successfully */
200 static void cmd_trajectory_parsed(void * parsed_result, void * data)
202 struct cmd_trajectory_result * res = parsed_result;
204 if (!strcmp_P(res->arg1, PSTR("set"))) {
205 trajectory_set_windows(&mainboard.traj, res->d_win,
206 res->a_win, res->a_start);
209 printf_P(PSTR("trajectory %2.2f %2.2f %2.2f\r\n"), mainboard.traj.d_win,
210 DEG(mainboard.traj.a_win_rad), DEG(mainboard.traj.a_start_rad));
213 prog_char str_trajectory_arg0[] = "trajectory";
214 parse_pgm_token_string_t cmd_trajectory_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_trajectory_result, arg0, str_trajectory_arg0);
215 prog_char str_trajectory_arg1[] = "set";
216 parse_pgm_token_string_t cmd_trajectory_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_trajectory_result, arg1, str_trajectory_arg1);
217 parse_pgm_token_num_t cmd_trajectory_d = TOKEN_NUM_INITIALIZER(struct cmd_trajectory_result, d_win, FLOAT);
218 parse_pgm_token_num_t cmd_trajectory_a = TOKEN_NUM_INITIALIZER(struct cmd_trajectory_result, a_win, FLOAT);
219 parse_pgm_token_num_t cmd_trajectory_as = TOKEN_NUM_INITIALIZER(struct cmd_trajectory_result, a_start, FLOAT);
221 prog_char help_trajectory[] = "Set trajectory windows (distance, angle, angle_start)";
222 parse_pgm_inst_t cmd_trajectory = {
223 .f = cmd_trajectory_parsed, /* function to call */
224 .data = NULL, /* 2nd arg of func */
225 .help_str = help_trajectory,
226 .tokens = { /* token list, NULL terminated */
227 (prog_void *)&cmd_trajectory_arg0,
228 (prog_void *)&cmd_trajectory_arg1,
229 (prog_void *)&cmd_trajectory_d,
230 (prog_void *)&cmd_trajectory_a,
231 (prog_void *)&cmd_trajectory_as,
238 prog_char str_trajectory_show_arg[] = "show";
239 parse_pgm_token_string_t cmd_trajectory_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_trajectory_result, arg1, str_trajectory_show_arg);
241 prog_char help_trajectory_show[] = "Show trajectory window configuration";
242 parse_pgm_inst_t cmd_trajectory_show = {
243 .f = cmd_trajectory_parsed, /* function to call */
244 .data = NULL, /* 2nd arg of func */
245 .help_str = help_trajectory_show,
246 .tokens = { /* token list, NULL terminated */
247 (prog_void *)&cmd_trajectory_arg0,
248 (prog_void *)&cmd_trajectory_show_arg,
253 /**********************************************************/
254 /* rs_gains configuration */
256 /* this structure is filled when cmd_rs_gains is parsed successfully */
257 struct cmd_rs_gains_result {
264 /* function called when cmd_rs_gains is parsed successfully */
265 static void cmd_rs_gains_parsed(void * parsed_result, void * data)
267 struct cmd_rs_gains_result * res = parsed_result;
269 if (!strcmp_P(res->arg1, PSTR("set"))) {
270 rs_set_left_ext_encoder(&mainboard.rs, encoders_spi_get_value,
271 LEFT_ENCODER, res->left); // en augmentant on tourne à gauche
272 rs_set_right_ext_encoder(&mainboard.rs, encoders_spi_get_value,
273 RIGHT_ENCODER, res->right); //en augmentant on tourne à droite
275 printf_P(PSTR("rs_gains set %2.2f %2.2f\r\n"),
276 mainboard.rs.left_ext_gain, mainboard.rs.right_ext_gain);
279 prog_char str_rs_gains_arg0[] = "rs_gains";
280 parse_pgm_token_string_t cmd_rs_gains_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_rs_gains_result, arg0, str_rs_gains_arg0);
281 prog_char str_rs_gains_arg1[] = "set";
282 parse_pgm_token_string_t cmd_rs_gains_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_rs_gains_result, arg1, str_rs_gains_arg1);
283 parse_pgm_token_num_t cmd_rs_gains_l = TOKEN_NUM_INITIALIZER(struct cmd_rs_gains_result, left, FLOAT);
284 parse_pgm_token_num_t cmd_rs_gains_r = TOKEN_NUM_INITIALIZER(struct cmd_rs_gains_result, right, FLOAT);
286 prog_char help_rs_gains[] = "Set rs_gains (left, right)";
287 parse_pgm_inst_t cmd_rs_gains = {
288 .f = cmd_rs_gains_parsed, /* function to call */
289 .data = NULL, /* 2nd arg of func */
290 .help_str = help_rs_gains,
291 .tokens = { /* token list, NULL terminated */
292 (prog_void *)&cmd_rs_gains_arg0,
293 (prog_void *)&cmd_rs_gains_arg1,
294 (prog_void *)&cmd_rs_gains_l,
295 (prog_void *)&cmd_rs_gains_r,
302 prog_char str_rs_gains_show_arg[] = "show";
303 parse_pgm_token_string_t cmd_rs_gains_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_rs_gains_result, arg1, str_rs_gains_show_arg);
305 prog_char help_rs_gains_show[] = "Show rs_gains";
306 parse_pgm_inst_t cmd_rs_gains_show = {
307 .f = cmd_rs_gains_parsed, /* function to call */
308 .data = NULL, /* 2nd arg of func */
309 .help_str = help_rs_gains_show,
310 .tokens = { /* token list, NULL terminated */
311 (prog_void *)&cmd_rs_gains_arg0,
312 (prog_void *)&cmd_rs_gains_show_arg,
317 /**********************************************************/
318 /* track configuration */
320 /* this structure is filled when cmd_track is parsed successfully */
321 struct cmd_track_result {
327 /* function called when cmd_track is parsed successfully */
328 static void cmd_track_parsed(void * parsed_result, void * data)
330 struct cmd_track_result * res = parsed_result;
332 if (!strcmp_P(res->arg1, PSTR("set"))) {
333 position_set_physical_params(&mainboard.pos, res->val, DIST_IMP_MM);
335 printf_P(PSTR("track set %f\r\n"), mainboard.pos.phys.track_mm);
338 prog_char str_track_arg0[] = "track";
339 parse_pgm_token_string_t cmd_track_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_track_result, arg0, str_track_arg0);
340 prog_char str_track_arg1[] = "set";
341 parse_pgm_token_string_t cmd_track_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_track_result, arg1, str_track_arg1);
342 parse_pgm_token_num_t cmd_track_val = TOKEN_NUM_INITIALIZER(struct cmd_track_result, val, FLOAT);
344 prog_char help_track[] = "Set track in mm";
345 parse_pgm_inst_t cmd_track = {
346 .f = cmd_track_parsed, /* function to call */
347 .data = NULL, /* 2nd arg of func */
348 .help_str = help_track,
349 .tokens = { /* token list, NULL terminated */
350 (prog_void *)&cmd_track_arg0,
351 (prog_void *)&cmd_track_arg1,
352 (prog_void *)&cmd_track_val,
359 prog_char str_track_show_arg[] = "show";
360 parse_pgm_token_string_t cmd_track_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_track_result, arg1, str_track_show_arg);
362 prog_char help_track_show[] = "Show track";
363 parse_pgm_inst_t cmd_track_show = {
364 .f = cmd_track_parsed, /* function to call */
365 .data = NULL, /* 2nd arg of func */
366 .help_str = help_track_show,
367 .tokens = { /* token list, NULL terminated */
368 (prog_void *)&cmd_track_arg0,
369 (prog_void *)&cmd_track_show_arg,
376 /**********************************************************/
377 /* Pt_Lists for testing traj */
379 #define PT_LIST_SIZE 10
380 static struct xy_point pt_list[PT_LIST_SIZE];
381 static uint16_t pt_list_len = 0;
383 /* this structure is filled when cmd_pt_list is parsed successfully */
384 struct cmd_pt_list_result {
392 /* function called when cmd_pt_list is parsed successfully */
393 static void cmd_pt_list_parsed(void * parsed_result, void * data)
395 struct cmd_pt_list_result * res = parsed_result;
398 if (!strcmp_P(res->arg1, PSTR("avoid_start"))) {
399 printf_P(PSTR("not implemented\r\n"));
403 if (!strcmp_P(res->arg1, PSTR("append"))) {
404 res->arg2 = pt_list_len;
407 if (!strcmp_P(res->arg1, PSTR("insert")) ||
408 !strcmp_P(res->arg1, PSTR("append"))) {
409 if (res->arg2 > pt_list_len) {
410 printf_P(PSTR("Index too large\r\n"));
413 if (pt_list_len >= PT_LIST_SIZE) {
414 printf_P(PSTR("List is too large\r\n"));
417 memmove(&pt_list[res->arg2+1], &pt_list[res->arg2],
418 PT_LIST_SIZE-1-res->arg2);
419 pt_list[res->arg2].x = res->arg3;
420 pt_list[res->arg2].y = res->arg4;
423 else if (!strcmp_P(res->arg1, PSTR("del"))) {
424 if (pt_list_len <= 0) {
425 printf_P(PSTR("Error: list empty\r\n"));
428 if (res->arg2 > pt_list_len) {
429 printf_P(PSTR("Index too large\r\n"));
432 memmove(&pt_list[res->arg2], &pt_list[res->arg2+1],
433 (PT_LIST_SIZE-1-res->arg2)*sizeof(struct xy_point));
436 else if (!strcmp_P(res->arg1, PSTR("reset"))) {
440 /* else it is a "show" or a "start" */
441 if (pt_list_len == 0) {
442 printf_P(PSTR("List empty\r\n"));
445 for (i=0 ; i<pt_list_len ; i++) {
446 printf_P(PSTR("%d: x=%d y=%d\r\n"), i, pt_list[i].x, pt_list[i].y);
447 if (!strcmp_P(res->arg1, PSTR("start"))) {
448 trajectory_goto_xy_abs(&mainboard.traj, pt_list[i].x, pt_list[i].y);
449 why = wait_traj_end(0xFF); /* all */
452 else if (!strcmp_P(res->arg1, PSTR("avoid_start"))) {
454 why = goto_and_avoid(pt_list[i].x, pt_list[i].y, 0xFF, 0xFF);
455 printf("next point\r\n");
456 if (why != END_OBSTACLE)
461 if (why & (~(END_TRAJ | END_NEAR)))
462 trajectory_stop(&mainboard.traj);
466 prog_char str_pt_list_arg0[] = "pt_list";
467 parse_pgm_token_string_t cmd_pt_list_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg0, str_pt_list_arg0);
468 prog_char str_pt_list_arg1[] = "insert";
469 parse_pgm_token_string_t cmd_pt_list_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_arg1);
470 parse_pgm_token_num_t cmd_pt_list_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_pt_list_result, arg2, UINT16);
471 parse_pgm_token_num_t cmd_pt_list_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_pt_list_result, arg3, INT16);
472 parse_pgm_token_num_t cmd_pt_list_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_pt_list_result, arg4, INT16);
474 prog_char help_pt_list[] = "Insert point in pt_list (idx,x,y)";
475 parse_pgm_inst_t cmd_pt_list = {
476 .f = cmd_pt_list_parsed, /* function to call */
477 .data = NULL, /* 2nd arg of func */
478 .help_str = help_pt_list,
479 .tokens = { /* token list, NULL terminated */
480 (prog_void *)&cmd_pt_list_arg0,
481 (prog_void *)&cmd_pt_list_arg1,
482 (prog_void *)&cmd_pt_list_arg2,
483 (prog_void *)&cmd_pt_list_arg3,
484 (prog_void *)&cmd_pt_list_arg4,
491 prog_char str_pt_list_arg1_append[] = "append";
492 parse_pgm_token_string_t cmd_pt_list_arg1_append = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_arg1_append);
494 prog_char help_pt_list_append[] = "Append point in pt_list (x,y)";
495 parse_pgm_inst_t cmd_pt_list_append = {
496 .f = cmd_pt_list_parsed, /* function to call */
497 .data = NULL, /* 2nd arg of func */
498 .help_str = help_pt_list_append,
499 .tokens = { /* token list, NULL terminated */
500 (prog_void *)&cmd_pt_list_arg0,
501 (prog_void *)&cmd_pt_list_arg1_append,
502 (prog_void *)&cmd_pt_list_arg3,
503 (prog_void *)&cmd_pt_list_arg4,
510 prog_char str_pt_list_del_arg[] = "del";
511 parse_pgm_token_string_t cmd_pt_list_del_arg = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_del_arg);
513 prog_char help_pt_list_del[] = "Del or insert point in pt_list (num)";
514 parse_pgm_inst_t cmd_pt_list_del = {
515 .f = cmd_pt_list_parsed, /* function to call */
516 .data = NULL, /* 2nd arg of func */
517 .help_str = help_pt_list_del,
518 .tokens = { /* token list, NULL terminated */
519 (prog_void *)&cmd_pt_list_arg0,
520 (prog_void *)&cmd_pt_list_del_arg,
521 (prog_void *)&cmd_pt_list_arg2,
527 prog_char str_pt_list_show_arg[] = "show#reset#start#avoid_start";
528 parse_pgm_token_string_t cmd_pt_list_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_show_arg);
530 prog_char help_pt_list_show[] = "Show, start or reset pt_list";
531 parse_pgm_inst_t cmd_pt_list_show = {
532 .f = cmd_pt_list_parsed, /* function to call */
533 .data = NULL, /* 2nd arg of func */
534 .help_str = help_pt_list_show,
535 .tokens = { /* token list, NULL terminated */
536 (prog_void *)&cmd_pt_list_arg0,
537 (prog_void *)&cmd_pt_list_show_arg,
544 /**********************************************************/
547 /* this structure is filled when cmd_goto is parsed successfully */
548 struct cmd_goto_result {
556 /* function called when cmd_goto is parsed successfully */
557 static void cmd_goto_parsed(void * parsed_result, void * data)
559 struct cmd_goto_result * res = parsed_result;
563 interrupt_traj_reset();
564 if (!strcmp_P(res->arg1, PSTR("a_rel"))) {
565 trajectory_a_rel(&mainboard.traj, res->arg2);
567 else if (!strcmp_P(res->arg1, PSTR("d_rel"))) {
568 trajectory_d_rel(&mainboard.traj, res->arg2);
570 else if (!strcmp_P(res->arg1, PSTR("a_abs"))) {
571 trajectory_a_abs(&mainboard.traj, res->arg2);
573 else if (!strcmp_P(res->arg1, PSTR("a_to_xy"))) {
574 trajectory_turnto_xy(&mainboard.traj, res->arg2, res->arg3);
576 else if (!strcmp_P(res->arg1, PSTR("a_behind_xy"))) {
577 trajectory_turnto_xy_behind(&mainboard.traj, res->arg2, res->arg3);
579 else if (!strcmp_P(res->arg1, PSTR("xy_rel"))) {
580 trajectory_goto_xy_rel(&mainboard.traj, res->arg2, res->arg3);
582 else if (!strcmp_P(res->arg1, PSTR("xy_abs"))) {
583 trajectory_goto_xy_abs(&mainboard.traj, res->arg2, res->arg3);
585 else if (!strcmp_P(res->arg1, PSTR("avoid"))) {
587 err = goto_and_avoid_forward(res->arg2, res->arg3, 0xFF, 0xFF);
588 if (err != END_TRAJ && err != END_NEAR)
591 printf_P(PSTR("not implemented\r\n"));
595 else if (!strcmp_P(res->arg1, PSTR("avoid_bw"))) {
597 err = goto_and_avoid_backward(res->arg2, res->arg3, 0xFF, 0xFF);
598 if (err != END_TRAJ && err != END_NEAR)
601 printf_P(PSTR("not implemented\r\n"));
605 else if (!strcmp_P(res->arg1, PSTR("xy_abs_fow"))) {
606 trajectory_goto_forward_xy_abs(&mainboard.traj, res->arg2, res->arg3);
608 else if (!strcmp_P(res->arg1, PSTR("xy_abs_back"))) {
609 trajectory_goto_backward_xy_abs(&mainboard.traj, res->arg2, res->arg3);
611 else if (!strcmp_P(res->arg1, PSTR("da_rel"))) {
612 trajectory_d_a_rel(&mainboard.traj, res->arg2, res->arg3);
615 while ((err = test_traj_end(0xFF)) == 0) {
617 if (t2 - t1 > 200000) {
618 dump_cs_debug("angle", &mainboard.angle.cs);
619 dump_cs_debug("distance", &mainboard.distance.cs);
623 if (err != END_TRAJ && err != END_NEAR)
625 printf_P(PSTR("returned %s\r\n"), get_err(err));
628 prog_char str_goto_arg0[] = "goto";
629 parse_pgm_token_string_t cmd_goto_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_goto_result, arg0, str_goto_arg0);
630 prog_char str_goto_arg1_a[] = "d_rel#a_rel#a_abs";
631 parse_pgm_token_string_t cmd_goto_arg1_a = TOKEN_STRING_INITIALIZER(struct cmd_goto_result, arg1, str_goto_arg1_a);
632 parse_pgm_token_num_t cmd_goto_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_goto_result, arg2, INT32);
635 prog_char help_goto1[] = "Change orientation of the mainboard";
636 parse_pgm_inst_t cmd_goto1 = {
637 .f = cmd_goto_parsed, /* function to call */
638 .data = NULL, /* 2nd arg of func */
639 .help_str = help_goto1,
640 .tokens = { /* token list, NULL terminated */
641 (prog_void *)&cmd_goto_arg0,
642 (prog_void *)&cmd_goto_arg1_a,
643 (prog_void *)&cmd_goto_arg2,
648 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";
649 parse_pgm_token_string_t cmd_goto_arg1_b = TOKEN_STRING_INITIALIZER(struct cmd_goto_result, arg1, str_goto_arg1_b);
650 parse_pgm_token_num_t cmd_goto_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_goto_result, arg3, INT32);
653 prog_char help_goto2[] = "Go to a (x,y) or (d,a) position";
654 parse_pgm_inst_t cmd_goto2 = {
655 .f = cmd_goto_parsed, /* function to call */
656 .data = NULL, /* 2nd arg of func */
657 .help_str = help_goto2,
658 .tokens = { /* token list, NULL terminated */
659 (prog_void *)&cmd_goto_arg0,
660 (prog_void *)&cmd_goto_arg1_b,
661 (prog_void *)&cmd_goto_arg2,
662 (prog_void *)&cmd_goto_arg3,
667 /**********************************************************/
670 /* this structure is filled when cmd_position is parsed successfully */
671 struct cmd_position_result {
679 #define AUTOPOS_SPEED_FAST 200
680 static void auto_position(void)
683 uint16_t old_spdd, old_spda;
685 interrupt_traj_reset();
686 strat_get_speed(&old_spdd, &old_spda);
687 strat_set_speed(AUTOPOS_SPEED_FAST, AUTOPOS_SPEED_FAST);
689 trajectory_d_rel(&mainboard.traj, -300);
690 err = wait_traj_end(END_INTR|END_TRAJ|END_BLOCKING);
694 strat_reset_pos(ROBOT_LENGTH/2, 0, 0);
696 trajectory_d_rel(&mainboard.traj, 120);
697 err = wait_traj_end(END_INTR|END_TRAJ);
701 trajectory_a_rel(&mainboard.traj, COLOR_A(90));
702 err = wait_traj_end(END_INTR|END_TRAJ);
706 trajectory_d_rel(&mainboard.traj, -300);
707 err = wait_traj_end(END_INTR|END_TRAJ|END_BLOCKING);
711 strat_reset_pos(DO_NOT_SET_POS, COLOR_Y(ROBOT_LENGTH/2),
714 trajectory_d_rel(&mainboard.traj, 120);
715 err = wait_traj_end(END_INTR|END_TRAJ);
720 trajectory_a_rel(&mainboard.traj, COLOR_A(-40));
721 err = wait_traj_end(END_INTR|END_TRAJ);
726 strat_set_speed(old_spdd, old_spda);
731 strat_set_speed(old_spdd, old_spda);
734 /* function called when cmd_position is parsed successfully */
735 static void cmd_position_parsed(void * parsed_result, void * data)
737 struct cmd_position_result * res = parsed_result;
739 /* display raw position values */
740 if (!strcmp_P(res->arg1, PSTR("reset"))) {
741 position_set(&mainboard.pos, 0, 0, 0);
743 else if (!strcmp_P(res->arg1, PSTR("set"))) {
744 position_set(&mainboard.pos, res->arg2, res->arg3, res->arg4);
746 else if (!strcmp_P(res->arg1, PSTR("autoset_green"))) {
747 mainboard.our_color = I2C_COLOR_GREEN;
748 i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_GREEN);
749 i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_GREEN);
752 else if (!strcmp_P(res->arg1, PSTR("autoset_red"))) {
753 mainboard.our_color = I2C_COLOR_RED;
754 i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_RED);
755 i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_RED);
759 /* else it's just a "show" */
760 printf_P(PSTR("x=%.2f y=%.2f a=%.2f\r\n"),
761 position_get_x_double(&mainboard.pos),
762 position_get_y_double(&mainboard.pos),
763 DEG(position_get_a_rad_double(&mainboard.pos)));
766 prog_char str_position_arg0[] = "position";
767 parse_pgm_token_string_t cmd_position_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg0, str_position_arg0);
768 prog_char str_position_arg1[] = "show#reset#autoset_green#autoset_red";
769 parse_pgm_token_string_t cmd_position_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg1, str_position_arg1);
771 prog_char help_position[] = "Show/reset (x,y,a) position";
772 parse_pgm_inst_t cmd_position = {
773 .f = cmd_position_parsed, /* function to call */
774 .data = NULL, /* 2nd arg of func */
775 .help_str = help_position,
776 .tokens = { /* token list, NULL terminated */
777 (prog_void *)&cmd_position_arg0,
778 (prog_void *)&cmd_position_arg1,
784 prog_char str_position_arg1_set[] = "set";
785 parse_pgm_token_string_t cmd_position_arg1_set = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg1, str_position_arg1_set);
786 parse_pgm_token_num_t cmd_position_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg2, INT32);
787 parse_pgm_token_num_t cmd_position_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg3, INT32);
788 parse_pgm_token_num_t cmd_position_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg4, INT32);
790 prog_char help_position_set[] = "Set (x,y,a) position";
791 parse_pgm_inst_t cmd_position_set = {
792 .f = cmd_position_parsed, /* function to call */
793 .data = NULL, /* 2nd arg of func */
794 .help_str = help_position_set,
795 .tokens = { /* token list, NULL terminated */
796 (prog_void *)&cmd_position_arg0,
797 (prog_void *)&cmd_position_arg1_set,
798 (prog_void *)&cmd_position_arg2,
799 (prog_void *)&cmd_position_arg3,
800 (prog_void *)&cmd_position_arg4,
806 /**********************************************************/
807 /* strat configuration */
809 /* this structure is filled when cmd_strat_infos is parsed successfully */
810 struct cmd_strat_infos_result {
815 /* function called when cmd_strat_infos is parsed successfully */
816 static void cmd_strat_infos_parsed(void *parsed_result, void *data)
818 struct cmd_strat_infos_result *res = parsed_result;
820 if (!strcmp_P(res->arg1, PSTR("reset"))) {
823 strat_infos.dump_enabled = 1;
824 strat_dump_infos(__FUNCTION__);
827 prog_char str_strat_infos_arg0[] = "strat_infos";
828 parse_pgm_token_string_t cmd_strat_infos_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_infos_result, arg0, str_strat_infos_arg0);
829 prog_char str_strat_infos_arg1[] = "show#reset";
830 parse_pgm_token_string_t cmd_strat_infos_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_infos_result, arg1, str_strat_infos_arg1);
832 prog_char help_strat_infos[] = "reset/show strat_infos";
833 parse_pgm_inst_t cmd_strat_infos = {
834 .f = cmd_strat_infos_parsed, /* function to call */
835 .data = NULL, /* 2nd arg of func */
836 .help_str = help_strat_infos,
837 .tokens = { /* token list, NULL terminated */
838 (prog_void *)&cmd_strat_infos_arg0,
839 (prog_void *)&cmd_strat_infos_arg1,
844 /**********************************************************/
845 /* strat configuration */
847 /* this structure is filled when cmd_strat_conf is parsed successfully */
848 struct cmd_strat_conf_result {
853 /* function called when cmd_strat_conf is parsed successfully */
854 static void cmd_strat_conf_parsed(void *parsed_result, void *data)
856 // struct cmd_strat_conf_result *res = parsed_result;
858 strat_infos.dump_enabled = 1;
862 prog_char str_strat_conf_arg0[] = "strat_conf";
863 parse_pgm_token_string_t cmd_strat_conf_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf_result, arg0, str_strat_conf_arg0);
864 prog_char str_strat_conf_arg1[] = "show#base";
865 parse_pgm_token_string_t cmd_strat_conf_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf_result, arg1, str_strat_conf_arg1);
867 prog_char help_strat_conf[] = "configure strat options";
868 parse_pgm_inst_t cmd_strat_conf = {
869 .f = cmd_strat_conf_parsed, /* function to call */
870 .data = NULL, /* 2nd arg of func */
871 .help_str = help_strat_conf,
872 .tokens = { /* token list, NULL terminated */
873 (prog_void *)&cmd_strat_conf_arg0,
874 (prog_void *)&cmd_strat_conf_arg1,
879 /**********************************************************/
880 /* strat configuration */
882 /* this structure is filled when cmd_strat_conf2 is parsed successfully */
883 struct cmd_strat_conf2_result {
889 /* function called when cmd_strat_conf2 is parsed successfully */
890 static void cmd_strat_conf2_parsed(void *parsed_result, void *data)
892 struct cmd_strat_conf2_result *res = parsed_result;
895 if (!strcmp_P(res->arg2, PSTR("on")))
901 if (!strcmp_P(res->arg1, PSTR("one_temple_on_disc")))
902 bit = STRAT_CONF_ONLY_ONE_ON_DISC;
903 else if (!strcmp_P(res->arg1, PSTR("bypass_static2")))
904 bit = STRAT_CONF_BYPASS_STATIC2;
905 else if (!strcmp_P(res->arg1, PSTR("take_one_lintel")))
906 bit = STRAT_CONF_TAKE_ONE_LINTEL;
907 else if (!strcmp_P(res->arg1, PSTR("skip_when_check_fail")))
908 bit = STRAT_CONF_TAKE_ONE_LINTEL;
909 else if (!strcmp_P(res->arg1, PSTR("store_static2")))
910 bit = STRAT_CONF_STORE_STATIC2;
911 else if (!strcmp_P(res->arg1, PSTR("big3_temple")))
912 bit = STRAT_CONF_BIG_3_TEMPLE;
913 else if (!strcmp_P(res->arg1, PSTR("early_opp_scan")))
914 bit = STRAT_CONF_EARLY_SCAN;
915 else if (!strcmp_P(res->arg1, PSTR("push_opp_cols")))
916 bit = STRAT_CONF_PUSH_OPP_COLS;
920 strat_infos.conf.flags |= bit;
922 strat_infos.conf.flags &= (~bit);
924 strat_infos.dump_enabled = 1;
928 prog_char str_strat_conf2_arg0[] = "strat_conf";
929 parse_pgm_token_string_t cmd_strat_conf2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg0, str_strat_conf2_arg0);
930 prog_char str_strat_conf2_arg1[] = "faux";
931 parse_pgm_token_string_t cmd_strat_conf2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg1, str_strat_conf2_arg1);
932 prog_char str_strat_conf2_arg2[] = "on#off";
933 parse_pgm_token_string_t cmd_strat_conf2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg2, str_strat_conf2_arg2);
936 prog_char help_strat_conf2[] = "configure strat options";
937 parse_pgm_inst_t cmd_strat_conf2 = {
938 .f = cmd_strat_conf2_parsed, /* function to call */
939 .data = NULL, /* 2nd arg of func */
940 .help_str = help_strat_conf2,
941 .tokens = { /* token list, NULL terminated */
942 (prog_void *)&cmd_strat_conf2_arg0,
943 (prog_void *)&cmd_strat_conf2_arg1,
944 (prog_void *)&cmd_strat_conf2_arg2,
949 /**********************************************************/
950 /* strat configuration */
952 /* this structure is filled when cmd_strat_conf3 is parsed successfully */
953 struct cmd_strat_conf3_result {
959 /* function called when cmd_strat_conf3 is parsed successfully */
960 static void cmd_strat_conf3_parsed(void *parsed_result, void *data)
963 struct cmd_strat_conf3_result *res = parsed_result;
965 if (!strcmp_P(res->arg1, PSTR("scan_opponent_min_time"))) {
968 strat_infos.conf.scan_opp_min_time = res->arg2;
970 else if (!strcmp_P(res->arg1, PSTR("delay_between_opponent_scan"))) {
973 strat_infos.conf.delay_between_opp_scan = res->arg2;
975 else if (!strcmp_P(res->arg1, PSTR("scan_our_min_time"))) {
978 strat_infos.conf.scan_our_min_time = res->arg2;
980 else if (!strcmp_P(res->arg1, PSTR("delay_between_our_scan"))) {
983 strat_infos.conf.delay_between_our_scan = res->arg2;
985 else if (!strcmp_P(res->arg1, PSTR("wait_opponent"))) {
986 strat_infos.conf.wait_opponent = res->arg2;
988 else if (!strcmp_P(res->arg1, PSTR("lintel_min_time"))) {
989 strat_infos.conf.lintel_min_time = res->arg2;
992 strat_infos.dump_enabled = 1;
996 prog_char str_strat_conf3_arg0[] = "strat_conf";
997 parse_pgm_token_string_t cmd_strat_conf3_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf3_result, arg0, str_strat_conf3_arg0);
998 prog_char str_strat_conf3_arg1[] = "faux2";
999 parse_pgm_token_string_t cmd_strat_conf3_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf3_result, arg1, str_strat_conf3_arg1);
1000 parse_pgm_token_num_t cmd_strat_conf3_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_strat_conf3_result, arg2, UINT16);
1002 prog_char help_strat_conf3[] = "configure strat options";
1003 parse_pgm_inst_t cmd_strat_conf3 = {
1004 .f = cmd_strat_conf3_parsed, /* function to call */
1005 .data = NULL, /* 2nd arg of func */
1006 .help_str = help_strat_conf3,
1007 .tokens = { /* token list, NULL terminated */
1008 (prog_void *)&cmd_strat_conf3_arg0,
1009 (prog_void *)&cmd_strat_conf3_arg1,
1010 (prog_void *)&cmd_strat_conf3_arg2,
1016 /**********************************************************/
1019 /* this structure is filled when cmd_subtraj is parsed successfully */
1020 struct cmd_subtraj_result {
1021 fixed_string_t arg0;
1022 fixed_string_t arg1;
1029 /* function called when cmd_subtraj is parsed successfully */
1030 static void cmd_subtraj_parsed(void *parsed_result, void *data)
1032 /* struct cmd_subtraj_result *res = parsed_result; */
1034 printf_P(PSTR("TODO\r\n"));
1035 trajectory_hardstop(&mainboard.traj);
1038 prog_char str_subtraj_arg0[] = "subtraj";
1039 parse_pgm_token_string_t cmd_subtraj_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_subtraj_result, arg0, str_subtraj_arg0);
1040 prog_char str_subtraj_arg1[] = "faux";
1041 parse_pgm_token_string_t cmd_subtraj_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_subtraj_result, arg1, str_subtraj_arg1);
1042 parse_pgm_token_num_t cmd_subtraj_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg2, INT32);
1043 parse_pgm_token_num_t cmd_subtraj_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg3, INT32);
1044 parse_pgm_token_num_t cmd_subtraj_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg4, INT32);
1045 parse_pgm_token_num_t cmd_subtraj_arg5 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg5, INT32);
1047 prog_char help_subtraj[] = "Test sub-trajectories (a,b,c,d: specific params)";
1048 parse_pgm_inst_t cmd_subtraj = {
1049 .f = cmd_subtraj_parsed, /* function to call */
1050 .data = NULL, /* 2nd arg of func */
1051 .help_str = help_subtraj,
1052 .tokens = { /* token list, NULL terminated */
1053 (prog_void *)&cmd_subtraj_arg0,
1054 (prog_void *)&cmd_subtraj_arg1,
1055 (prog_void *)&cmd_subtraj_arg2,
1056 (prog_void *)&cmd_subtraj_arg3,
1057 (prog_void *)&cmd_subtraj_arg4,
1058 (prog_void *)&cmd_subtraj_arg5,