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>
32 #include <clock_time.h>
36 #include <control_system_manager.h>
37 #include <trajectory_manager.h>
38 #include <vect_base.h>
41 #include <obstacle_avoidance.h>
42 #include <blocking_detection_manager.h>
43 #include <robot_system.h>
44 #include <position_manager.h>
48 #include <parse_string.h>
49 #include <parse_num.h>
51 #include "i2c_commands.h"
56 #include "strat_utils.h"
57 #include "strat_base.h"
58 #include "strat_avoid.h"
60 /**********************************************************/
61 /* Traj_Speeds for trajectory_manager */
63 /* this structure is filled when cmd_traj_speed is parsed successfully */
64 struct cmd_traj_speed_result {
70 /* function called when cmd_traj_speed is parsed successfully */
71 static void cmd_traj_speed_parsed(void *parsed_result, void *data)
73 struct cmd_traj_speed_result * res = parsed_result;
75 if (!strcmp_P(res->arg1, PSTR("angle"))) {
76 trajectory_set_speed(&mainboard.traj, mainboard.traj.d_speed, res->s);
78 else if (!strcmp_P(res->arg1, PSTR("distance"))) {
79 trajectory_set_speed(&mainboard.traj, res->s, mainboard.traj.a_speed);
81 /* else it is a "show" */
83 printf_P(PSTR("angle %u, distance %u\r\n"),
84 mainboard.traj.a_speed,
85 mainboard.traj.d_speed);
88 prog_char str_traj_speed_arg0[] = "traj_speed";
89 parse_pgm_token_string_t cmd_traj_speed_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_traj_speed_result, arg0, str_traj_speed_arg0);
90 prog_char str_traj_speed_arg1[] = "angle#distance";
91 parse_pgm_token_string_t cmd_traj_speed_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_traj_speed_result, arg1, str_traj_speed_arg1);
92 parse_pgm_token_num_t cmd_traj_speed_s = TOKEN_NUM_INITIALIZER(struct cmd_traj_speed_result, s, UINT16);
94 prog_char help_traj_speed[] = "Set traj_speed values for trajectory manager";
95 parse_pgm_inst_t cmd_traj_speed = {
96 .f = cmd_traj_speed_parsed, /* function to call */
97 .data = NULL, /* 2nd arg of func */
98 .help_str = help_traj_speed,
99 .tokens = { /* token list, NULL terminated */
100 (prog_void *)&cmd_traj_speed_arg0,
101 (prog_void *)&cmd_traj_speed_arg1,
102 (prog_void *)&cmd_traj_speed_s,
109 prog_char str_traj_speed_show_arg[] = "show";
110 parse_pgm_token_string_t cmd_traj_speed_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_traj_speed_result, arg1, str_traj_speed_show_arg);
112 prog_char help_traj_speed_show[] = "Show traj_speed values for trajectory manager";
113 parse_pgm_inst_t cmd_traj_speed_show = {
114 .f = cmd_traj_speed_parsed, /* function to call */
115 .data = NULL, /* 2nd arg of func */
116 .help_str = help_traj_speed_show,
117 .tokens = { /* token list, NULL terminated */
118 (prog_void *)&cmd_traj_speed_arg0,
119 (prog_void *)&cmd_traj_speed_show_arg,
124 /**********************************************************/
125 /* circle coef configuration */
127 /* this structure is filled when cmd_circle_coef is parsed successfully */
128 struct cmd_circle_coef_result {
135 /* function called when cmd_circle_coef is parsed successfully */
136 static void cmd_circle_coef_parsed(void *parsed_result, void *data)
138 struct cmd_circle_coef_result *res = parsed_result;
140 if (!strcmp_P(res->arg1, PSTR("set"))) {
141 trajectory_set_circle_coef(&mainboard.traj, res->circle_coef);
144 printf_P(PSTR("circle_coef %2.2f\r\n"), mainboard.traj.circle_coef);
147 prog_char str_circle_coef_arg0[] = "circle_coef";
148 parse_pgm_token_string_t cmd_circle_coef_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_circle_coef_result, arg0, str_circle_coef_arg0);
149 prog_char str_circle_coef_arg1[] = "set";
150 parse_pgm_token_string_t cmd_circle_coef_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_circle_coef_result, arg1, str_circle_coef_arg1);
151 parse_pgm_token_num_t cmd_circle_coef_val = TOKEN_NUM_INITIALIZER(struct cmd_circle_coef_result, circle_coef, FLOAT);
153 prog_char help_circle_coef[] = "Set circle coef";
154 parse_pgm_inst_t cmd_circle_coef = {
155 .f = cmd_circle_coef_parsed, /* function to call */
156 .data = NULL, /* 2nd arg of func */
157 .help_str = help_circle_coef,
158 .tokens = { /* token list, NULL terminated */
159 (prog_void *)&cmd_circle_coef_arg0,
160 (prog_void *)&cmd_circle_coef_arg1,
161 (prog_void *)&cmd_circle_coef_val,
168 prog_char str_circle_coef_show_arg[] = "show";
169 parse_pgm_token_string_t cmd_circle_coef_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_circle_coef_result, arg1, str_circle_coef_show_arg);
171 prog_char help_circle_coef_show[] = "Show circle coef";
172 parse_pgm_inst_t cmd_circle_coef_show = {
173 .f = cmd_circle_coef_parsed, /* function to call */
174 .data = NULL, /* 2nd arg of func */
175 .help_str = help_circle_coef_show,
176 .tokens = { /* token list, NULL terminated */
177 (prog_void *)&cmd_circle_coef_arg0,
178 (prog_void *)&cmd_circle_coef_show_arg,
183 /**********************************************************/
184 /* trajectory window configuration */
186 /* this structure is filled when cmd_trajectory is parsed successfully */
187 struct cmd_trajectory_result {
196 /* function called when cmd_trajectory is parsed successfully */
197 static void cmd_trajectory_parsed(void * parsed_result, void * data)
199 struct cmd_trajectory_result * res = parsed_result;
201 if (!strcmp_P(res->arg1, PSTR("set"))) {
202 trajectory_set_windows(&mainboard.traj, res->d_win,
203 res->a_win, res->a_start);
206 printf_P(PSTR("trajectory %2.2f %2.2f %2.2f\r\n"), mainboard.traj.d_win,
207 DEG(mainboard.traj.a_win_rad), DEG(mainboard.traj.a_start_rad));
210 prog_char str_trajectory_arg0[] = "trajectory";
211 parse_pgm_token_string_t cmd_trajectory_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_trajectory_result, arg0, str_trajectory_arg0);
212 prog_char str_trajectory_arg1[] = "set";
213 parse_pgm_token_string_t cmd_trajectory_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_trajectory_result, arg1, str_trajectory_arg1);
214 parse_pgm_token_num_t cmd_trajectory_d = TOKEN_NUM_INITIALIZER(struct cmd_trajectory_result, d_win, FLOAT);
215 parse_pgm_token_num_t cmd_trajectory_a = TOKEN_NUM_INITIALIZER(struct cmd_trajectory_result, a_win, FLOAT);
216 parse_pgm_token_num_t cmd_trajectory_as = TOKEN_NUM_INITIALIZER(struct cmd_trajectory_result, a_start, FLOAT);
218 prog_char help_trajectory[] = "Set trajectory windows (distance, angle, angle_start)";
219 parse_pgm_inst_t cmd_trajectory = {
220 .f = cmd_trajectory_parsed, /* function to call */
221 .data = NULL, /* 2nd arg of func */
222 .help_str = help_trajectory,
223 .tokens = { /* token list, NULL terminated */
224 (prog_void *)&cmd_trajectory_arg0,
225 (prog_void *)&cmd_trajectory_arg1,
226 (prog_void *)&cmd_trajectory_d,
227 (prog_void *)&cmd_trajectory_a,
228 (prog_void *)&cmd_trajectory_as,
235 prog_char str_trajectory_show_arg[] = "show";
236 parse_pgm_token_string_t cmd_trajectory_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_trajectory_result, arg1, str_trajectory_show_arg);
238 prog_char help_trajectory_show[] = "Show trajectory window configuration";
239 parse_pgm_inst_t cmd_trajectory_show = {
240 .f = cmd_trajectory_parsed, /* function to call */
241 .data = NULL, /* 2nd arg of func */
242 .help_str = help_trajectory_show,
243 .tokens = { /* token list, NULL terminated */
244 (prog_void *)&cmd_trajectory_arg0,
245 (prog_void *)&cmd_trajectory_show_arg,
250 /**********************************************************/
251 /* rs_gains configuration */
253 /* this structure is filled when cmd_rs_gains is parsed successfully */
254 struct cmd_rs_gains_result {
261 /* function called when cmd_rs_gains is parsed successfully */
262 static void cmd_rs_gains_parsed(void * parsed_result, void * data)
265 printf("not implemented\n");
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 "));
276 f64_print(mainboard.rs.left_ext_gain);
278 f64_print(mainboard.rs.right_ext_gain);
279 printf_P(PSTR("\r\n"));
283 prog_char str_rs_gains_arg0[] = "rs_gains";
284 parse_pgm_token_string_t cmd_rs_gains_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_rs_gains_result, arg0, str_rs_gains_arg0);
285 prog_char str_rs_gains_arg1[] = "set";
286 parse_pgm_token_string_t cmd_rs_gains_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_rs_gains_result, arg1, str_rs_gains_arg1);
287 parse_pgm_token_num_t cmd_rs_gains_l = TOKEN_NUM_INITIALIZER(struct cmd_rs_gains_result, left, FLOAT);
288 parse_pgm_token_num_t cmd_rs_gains_r = TOKEN_NUM_INITIALIZER(struct cmd_rs_gains_result, right, FLOAT);
290 prog_char help_rs_gains[] = "Set rs_gains (left, right)";
291 parse_pgm_inst_t cmd_rs_gains = {
292 .f = cmd_rs_gains_parsed, /* function to call */
293 .data = NULL, /* 2nd arg of func */
294 .help_str = help_rs_gains,
295 .tokens = { /* token list, NULL terminated */
296 (prog_void *)&cmd_rs_gains_arg0,
297 (prog_void *)&cmd_rs_gains_arg1,
298 (prog_void *)&cmd_rs_gains_l,
299 (prog_void *)&cmd_rs_gains_r,
306 prog_char str_rs_gains_show_arg[] = "show";
307 parse_pgm_token_string_t cmd_rs_gains_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_rs_gains_result, arg1, str_rs_gains_show_arg);
309 prog_char help_rs_gains_show[] = "Show rs_gains";
310 parse_pgm_inst_t cmd_rs_gains_show = {
311 .f = cmd_rs_gains_parsed, /* function to call */
312 .data = NULL, /* 2nd arg of func */
313 .help_str = help_rs_gains_show,
314 .tokens = { /* token list, NULL terminated */
315 (prog_void *)&cmd_rs_gains_arg0,
316 (prog_void *)&cmd_rs_gains_show_arg,
321 /**********************************************************/
322 /* track configuration */
324 /* this structure is filled when cmd_track is parsed successfully */
325 struct cmd_track_result {
331 /* function called when cmd_track is parsed successfully */
332 static void cmd_track_parsed(void * parsed_result, void * data)
334 struct cmd_track_result * res = parsed_result;
336 if (!strcmp_P(res->arg1, PSTR("set"))) {
337 position_set_physical_params(&mainboard.pos, res->val, DIST_IMP_MM);
339 printf_P(PSTR("track set %f\r\n"), mainboard.pos.phys.track_mm);
342 prog_char str_track_arg0[] = "track";
343 parse_pgm_token_string_t cmd_track_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_track_result, arg0, str_track_arg0);
344 prog_char str_track_arg1[] = "set";
345 parse_pgm_token_string_t cmd_track_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_track_result, arg1, str_track_arg1);
346 parse_pgm_token_num_t cmd_track_val = TOKEN_NUM_INITIALIZER(struct cmd_track_result, val, FLOAT);
348 prog_char help_track[] = "Set track in mm";
349 parse_pgm_inst_t cmd_track = {
350 .f = cmd_track_parsed, /* function to call */
351 .data = NULL, /* 2nd arg of func */
352 .help_str = help_track,
353 .tokens = { /* token list, NULL terminated */
354 (prog_void *)&cmd_track_arg0,
355 (prog_void *)&cmd_track_arg1,
356 (prog_void *)&cmd_track_val,
363 prog_char str_track_show_arg[] = "show";
364 parse_pgm_token_string_t cmd_track_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_track_result, arg1, str_track_show_arg);
366 prog_char help_track_show[] = "Show track";
367 parse_pgm_inst_t cmd_track_show = {
368 .f = cmd_track_parsed, /* function to call */
369 .data = NULL, /* 2nd arg of func */
370 .help_str = help_track_show,
371 .tokens = { /* token list, NULL terminated */
372 (prog_void *)&cmd_track_arg0,
373 (prog_void *)&cmd_track_show_arg,
380 /**********************************************************/
381 /* Pt_Lists for testing traj */
383 #define PT_LIST_SIZE 10
384 static struct xy_point pt_list[PT_LIST_SIZE];
385 static uint16_t pt_list_len = 0;
387 /* this structure is filled when cmd_pt_list is parsed successfully */
388 struct cmd_pt_list_result {
396 /* function called when cmd_pt_list is parsed successfully */
397 static void cmd_pt_list_parsed(void * parsed_result, void * data)
399 struct cmd_pt_list_result * res = parsed_result;
402 if (!strcmp_P(res->arg1, PSTR("append"))) {
403 res->arg2 = pt_list_len;
406 if (!strcmp_P(res->arg1, PSTR("insert")) ||
407 !strcmp_P(res->arg1, PSTR("append"))) {
408 if (res->arg2 > pt_list_len) {
409 printf_P(PSTR("Index too large\r\n"));
412 if (pt_list_len >= PT_LIST_SIZE) {
413 printf_P(PSTR("List is too large\r\n"));
416 memmove(&pt_list[res->arg2+1], &pt_list[res->arg2],
417 PT_LIST_SIZE-1-res->arg2);
418 pt_list[res->arg2].x = res->arg3;
419 pt_list[res->arg2].y = res->arg4;
422 else if (!strcmp_P(res->arg1, PSTR("del"))) {
423 if (pt_list_len <= 0) {
424 printf_P(PSTR("Error: list empty\r\n"));
427 if (res->arg2 > pt_list_len) {
428 printf_P(PSTR("Index too large\r\n"));
431 memmove(&pt_list[res->arg2], &pt_list[res->arg2+1],
432 (PT_LIST_SIZE-1-res->arg2)*sizeof(struct xy_point));
435 else if (!strcmp_P(res->arg1, PSTR("reset"))) {
439 /* else it is a "show" or a "start" */
440 if (pt_list_len == 0) {
441 printf_P(PSTR("List empty\r\n"));
444 for (i=0 ; i<pt_list_len ; i++) {
445 printf_P(PSTR("%d: x=%d y=%d\r\n"), i, pt_list[i].x, pt_list[i].y);
446 if (!strcmp_P(res->arg1, PSTR("start"))) {
447 trajectory_goto_xy_abs(&mainboard.traj, pt_list[i].x, pt_list[i].y);
448 why = wait_traj_end(0xFF); /* all */
450 else if (!strcmp_P(res->arg1, PSTR("avoid_start"))) {
452 why = goto_and_avoid(pt_list[i].x, pt_list[i].y, 0xFF, 0xFF);
453 printf("next point\r\n");
454 if (why != END_OBSTACLE)
458 if (why & (~(END_TRAJ | END_NEAR)))
459 trajectory_stop(&mainboard.traj);
463 prog_char str_pt_list_arg0[] = "pt_list";
464 parse_pgm_token_string_t cmd_pt_list_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg0, str_pt_list_arg0);
465 prog_char str_pt_list_arg1[] = "insert";
466 parse_pgm_token_string_t cmd_pt_list_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_arg1);
467 parse_pgm_token_num_t cmd_pt_list_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_pt_list_result, arg2, UINT16);
468 parse_pgm_token_num_t cmd_pt_list_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_pt_list_result, arg3, INT16);
469 parse_pgm_token_num_t cmd_pt_list_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_pt_list_result, arg4, INT16);
471 prog_char help_pt_list[] = "Insert point in pt_list (idx,x,y)";
472 parse_pgm_inst_t cmd_pt_list = {
473 .f = cmd_pt_list_parsed, /* function to call */
474 .data = NULL, /* 2nd arg of func */
475 .help_str = help_pt_list,
476 .tokens = { /* token list, NULL terminated */
477 (prog_void *)&cmd_pt_list_arg0,
478 (prog_void *)&cmd_pt_list_arg1,
479 (prog_void *)&cmd_pt_list_arg2,
480 (prog_void *)&cmd_pt_list_arg3,
481 (prog_void *)&cmd_pt_list_arg4,
488 prog_char str_pt_list_arg1_append[] = "append";
489 parse_pgm_token_string_t cmd_pt_list_arg1_append = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_arg1_append);
491 prog_char help_pt_list_append[] = "Append point in pt_list (x,y)";
492 parse_pgm_inst_t cmd_pt_list_append = {
493 .f = cmd_pt_list_parsed, /* function to call */
494 .data = NULL, /* 2nd arg of func */
495 .help_str = help_pt_list_append,
496 .tokens = { /* token list, NULL terminated */
497 (prog_void *)&cmd_pt_list_arg0,
498 (prog_void *)&cmd_pt_list_arg1_append,
499 (prog_void *)&cmd_pt_list_arg3,
500 (prog_void *)&cmd_pt_list_arg4,
507 prog_char str_pt_list_del_arg[] = "del";
508 parse_pgm_token_string_t cmd_pt_list_del_arg = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_del_arg);
510 prog_char help_pt_list_del[] = "Del or insert point in pt_list (num)";
511 parse_pgm_inst_t cmd_pt_list_del = {
512 .f = cmd_pt_list_parsed, /* function to call */
513 .data = NULL, /* 2nd arg of func */
514 .help_str = help_pt_list_del,
515 .tokens = { /* token list, NULL terminated */
516 (prog_void *)&cmd_pt_list_arg0,
517 (prog_void *)&cmd_pt_list_del_arg,
518 (prog_void *)&cmd_pt_list_arg2,
524 prog_char str_pt_list_show_arg[] = "show#reset#start#avoid_start";
525 parse_pgm_token_string_t cmd_pt_list_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_show_arg);
527 prog_char help_pt_list_show[] = "Show, start or reset pt_list";
528 parse_pgm_inst_t cmd_pt_list_show = {
529 .f = cmd_pt_list_parsed, /* function to call */
530 .data = NULL, /* 2nd arg of func */
531 .help_str = help_pt_list_show,
532 .tokens = { /* token list, NULL terminated */
533 (prog_void *)&cmd_pt_list_arg0,
534 (prog_void *)&cmd_pt_list_show_arg,
541 /**********************************************************/
544 /* this structure is filled when cmd_goto is parsed successfully */
545 struct cmd_goto_result {
553 /* function called when cmd_goto is parsed successfully */
554 static void cmd_goto_parsed(void * parsed_result, void * data)
556 struct cmd_goto_result * res = parsed_result;
560 interrupt_traj_reset();
561 if (!strcmp_P(res->arg1, PSTR("a_rel"))) {
562 trajectory_a_rel(&mainboard.traj, res->arg2);
564 else if (!strcmp_P(res->arg1, PSTR("d_rel"))) {
565 trajectory_d_rel(&mainboard.traj, res->arg2);
567 else if (!strcmp_P(res->arg1, PSTR("a_abs"))) {
568 trajectory_a_abs(&mainboard.traj, res->arg2);
570 else if (!strcmp_P(res->arg1, PSTR("a_to_xy"))) {
571 trajectory_turnto_xy(&mainboard.traj, res->arg2, res->arg3);
573 else if (!strcmp_P(res->arg1, PSTR("a_behind_xy"))) {
574 trajectory_turnto_xy_behind(&mainboard.traj, res->arg2, res->arg3);
576 else if (!strcmp_P(res->arg1, PSTR("xy_rel"))) {
577 trajectory_goto_xy_rel(&mainboard.traj, res->arg2, res->arg3);
579 else if (!strcmp_P(res->arg1, PSTR("xy_abs"))) {
580 trajectory_goto_xy_abs(&mainboard.traj, res->arg2, res->arg3);
582 else if (!strcmp_P(res->arg1, PSTR("avoid"))) {
583 err = goto_and_avoid_forward(res->arg2, res->arg3, 0xFF, 0xFF);
584 if (err != END_TRAJ && err != END_NEAR)
587 else if (!strcmp_P(res->arg1, PSTR("avoid_bw"))) {
588 err = goto_and_avoid_backward(res->arg2, res->arg3, 0xFF, 0xFF);
589 if (err != END_TRAJ && err != END_NEAR)
592 else if (!strcmp_P(res->arg1, PSTR("xy_abs_fow"))) {
593 trajectory_goto_forward_xy_abs(&mainboard.traj, res->arg2, res->arg3);
595 else if (!strcmp_P(res->arg1, PSTR("xy_abs_back"))) {
596 trajectory_goto_backward_xy_abs(&mainboard.traj, res->arg2, res->arg3);
598 else if (!strcmp_P(res->arg1, PSTR("da_rel"))) {
599 trajectory_d_a_rel(&mainboard.traj, res->arg2, res->arg3);
602 while ((err = test_traj_end(0xFF)) == 0) {
604 if (t2 - t1 > 200000) {
605 dump_cs_debug("angle", &mainboard.angle.cs);
606 dump_cs_debug("distance", &mainboard.distance.cs);
610 if (err != END_TRAJ && err != END_NEAR)
612 printf_P(PSTR("returned %s\r\n"), get_err(err));
615 prog_char str_goto_arg0[] = "goto";
616 parse_pgm_token_string_t cmd_goto_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_goto_result, arg0, str_goto_arg0);
617 prog_char str_goto_arg1_a[] = "d_rel#a_rel#a_abs";
618 parse_pgm_token_string_t cmd_goto_arg1_a = TOKEN_STRING_INITIALIZER(struct cmd_goto_result, arg1, str_goto_arg1_a);
619 parse_pgm_token_num_t cmd_goto_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_goto_result, arg2, INT32);
622 prog_char help_goto1[] = "Change orientation of the mainboard";
623 parse_pgm_inst_t cmd_goto1 = {
624 .f = cmd_goto_parsed, /* function to call */
625 .data = NULL, /* 2nd arg of func */
626 .help_str = help_goto1,
627 .tokens = { /* token list, NULL terminated */
628 (prog_void *)&cmd_goto_arg0,
629 (prog_void *)&cmd_goto_arg1_a,
630 (prog_void *)&cmd_goto_arg2,
635 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";
636 parse_pgm_token_string_t cmd_goto_arg1_b = TOKEN_STRING_INITIALIZER(struct cmd_goto_result, arg1, str_goto_arg1_b);
637 parse_pgm_token_num_t cmd_goto_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_goto_result, arg3, INT32);
640 prog_char help_goto2[] = "Go to a (x,y) or (d,a) position";
641 parse_pgm_inst_t cmd_goto2 = {
642 .f = cmd_goto_parsed, /* function to call */
643 .data = NULL, /* 2nd arg of func */
644 .help_str = help_goto2,
645 .tokens = { /* token list, NULL terminated */
646 (prog_void *)&cmd_goto_arg0,
647 (prog_void *)&cmd_goto_arg1_b,
648 (prog_void *)&cmd_goto_arg2,
649 (prog_void *)&cmd_goto_arg3,
654 /**********************************************************/
657 /* this structure is filled when cmd_position is parsed successfully */
658 struct cmd_position_result {
666 #define AUTOPOS_SPEED_FAST 200
667 static void auto_position(void)
670 uint16_t old_spdd, old_spda;
672 interrupt_traj_reset();
673 strat_get_speed(&old_spdd, &old_spda);
674 strat_set_speed(AUTOPOS_SPEED_FAST, AUTOPOS_SPEED_FAST);
676 trajectory_d_rel(&mainboard.traj, -300);
677 err = wait_traj_end(END_INTR|END_TRAJ|END_BLOCKING);
681 strat_reset_pos(ROBOT_LENGTH/2, 0, 0);
683 trajectory_d_rel(&mainboard.traj, 120);
684 err = wait_traj_end(END_INTR|END_TRAJ);
688 trajectory_a_rel(&mainboard.traj, COLOR_A(90));
689 err = wait_traj_end(END_INTR|END_TRAJ);
693 trajectory_d_rel(&mainboard.traj, -300);
694 err = wait_traj_end(END_INTR|END_TRAJ|END_BLOCKING);
698 strat_reset_pos(DO_NOT_SET_POS, COLOR_Y(ROBOT_LENGTH/2),
701 trajectory_d_rel(&mainboard.traj, 120);
702 err = wait_traj_end(END_INTR|END_TRAJ);
707 trajectory_a_rel(&mainboard.traj, COLOR_A(-40));
708 err = wait_traj_end(END_INTR|END_TRAJ);
713 strat_set_speed(old_spdd, old_spda);
718 strat_set_speed(old_spdd, old_spda);
721 /* function called when cmd_position is parsed successfully */
722 static void cmd_position_parsed(void * parsed_result, void * data)
724 struct cmd_position_result * res = parsed_result;
726 /* display raw position values */
727 if (!strcmp_P(res->arg1, PSTR("reset"))) {
728 position_set(&mainboard.pos, 0, 0, 0);
730 else if (!strcmp_P(res->arg1, PSTR("set"))) {
731 position_set(&mainboard.pos, res->arg2, res->arg3, res->arg4);
733 else if (!strcmp_P(res->arg1, PSTR("autoset_green"))) {
734 mainboard.our_color = I2C_COLOR_GREEN;
736 i2c_set_color(I2C_MECHBOARD_ADDR, I2C_COLOR_GREEN);
737 i2c_set_color(I2C_SENSORBOARD_ADDR, I2C_COLOR_GREEN);
741 else if (!strcmp_P(res->arg1, PSTR("autoset_red"))) {
742 mainboard.our_color = I2C_COLOR_RED;
744 i2c_set_color(I2C_MECHBOARD_ADDR, I2C_COLOR_RED);
745 i2c_set_color(I2C_SENSORBOARD_ADDR, I2C_COLOR_RED);
750 /* else it's just a "show" */
751 printf_P(PSTR("x=%.2f y=%.2f a=%.2f\r\n"),
752 position_get_x_double(&mainboard.pos),
753 position_get_y_double(&mainboard.pos),
754 DEG(position_get_a_rad_double(&mainboard.pos)));
757 prog_char str_position_arg0[] = "position";
758 parse_pgm_token_string_t cmd_position_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg0, str_position_arg0);
759 prog_char str_position_arg1[] = "show#reset#autoset_green#autoset_red";
760 parse_pgm_token_string_t cmd_position_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg1, str_position_arg1);
762 prog_char help_position[] = "Show/reset (x,y,a) position";
763 parse_pgm_inst_t cmd_position = {
764 .f = cmd_position_parsed, /* function to call */
765 .data = NULL, /* 2nd arg of func */
766 .help_str = help_position,
767 .tokens = { /* token list, NULL terminated */
768 (prog_void *)&cmd_position_arg0,
769 (prog_void *)&cmd_position_arg1,
775 prog_char str_position_arg1_set[] = "set";
776 parse_pgm_token_string_t cmd_position_arg1_set = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg1, str_position_arg1_set);
777 parse_pgm_token_num_t cmd_position_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg2, INT32);
778 parse_pgm_token_num_t cmd_position_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg3, INT32);
779 parse_pgm_token_num_t cmd_position_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg4, INT32);
781 prog_char help_position_set[] = "Set (x,y,a) position";
782 parse_pgm_inst_t cmd_position_set = {
783 .f = cmd_position_parsed, /* function to call */
784 .data = NULL, /* 2nd arg of func */
785 .help_str = help_position_set,
786 .tokens = { /* token list, NULL terminated */
787 (prog_void *)&cmd_position_arg0,
788 (prog_void *)&cmd_position_arg1_set,
789 (prog_void *)&cmd_position_arg2,
790 (prog_void *)&cmd_position_arg3,
791 (prog_void *)&cmd_position_arg4,
797 /**********************************************************/
798 /* strat configuration */
800 /* this structure is filled when cmd_strat_infos is parsed successfully */
801 struct cmd_strat_infos_result {
806 /* function called when cmd_strat_infos is parsed successfully */
807 static void cmd_strat_infos_parsed(void *parsed_result, void *data)
809 struct cmd_strat_infos_result *res = parsed_result;
811 if (!strcmp_P(res->arg1, PSTR("reset"))) {
814 strat_infos.dump_enabled = 1;
815 strat_dump_infos(__FUNCTION__);
818 prog_char str_strat_infos_arg0[] = "strat_infos";
819 parse_pgm_token_string_t cmd_strat_infos_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_infos_result, arg0, str_strat_infos_arg0);
820 prog_char str_strat_infos_arg1[] = "show#reset";
821 parse_pgm_token_string_t cmd_strat_infos_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_infos_result, arg1, str_strat_infos_arg1);
823 prog_char help_strat_infos[] = "reset/show strat_infos";
824 parse_pgm_inst_t cmd_strat_infos = {
825 .f = cmd_strat_infos_parsed, /* function to call */
826 .data = NULL, /* 2nd arg of func */
827 .help_str = help_strat_infos,
828 .tokens = { /* token list, NULL terminated */
829 (prog_void *)&cmd_strat_infos_arg0,
830 (prog_void *)&cmd_strat_infos_arg1,
835 /**********************************************************/
836 /* strat configuration */
838 /* this structure is filled when cmd_strat_conf is parsed successfully */
839 struct cmd_strat_conf_result {
844 /* function called when cmd_strat_conf is parsed successfully */
845 static void cmd_strat_conf_parsed(void *parsed_result, void *data)
847 struct cmd_strat_conf_result *res = parsed_result;
849 if (!strcmp_P(res->arg1, PSTR("base"))) {
850 strat_infos.conf.flags = 0;
851 strat_infos.conf.scan_our_min_time = 90;
852 strat_infos.conf.delay_between_our_scan = 90;
853 strat_infos.conf.scan_opp_min_time = 90;
854 strat_infos.conf.delay_between_opp_scan = 90;
856 else if (!strcmp_P(res->arg1, PSTR("big3"))) {
857 strat_infos.conf.flags =
858 STRAT_CONF_STORE_STATIC2 |
859 STRAT_CONF_BIG_3_TEMPLE;
860 strat_infos.conf.scan_our_min_time = 90;
861 strat_infos.conf.delay_between_our_scan = 90;
862 strat_infos.conf.scan_opp_min_time = 90;
863 strat_infos.conf.delay_between_opp_scan = 90;
865 else if (!strcmp_P(res->arg1, PSTR("base_check"))) {
866 strat_infos.conf.flags = 0;
867 strat_infos.conf.scan_our_min_time = 35;
868 strat_infos.conf.delay_between_our_scan = 90;
869 strat_infos.conf.scan_opp_min_time = 90;
870 strat_infos.conf.delay_between_opp_scan = 90;
872 else if (!strcmp_P(res->arg1, PSTR("big3_check"))) {
873 strat_infos.conf.flags =
874 STRAT_CONF_STORE_STATIC2 |
875 STRAT_CONF_BIG_3_TEMPLE;
876 strat_infos.conf.scan_our_min_time = 35;
877 strat_infos.conf.delay_between_our_scan = 90;
878 strat_infos.conf.scan_opp_min_time = 90;
879 strat_infos.conf.delay_between_opp_scan = 90;
881 else if (!strcmp_P(res->arg1, PSTR("offensive_early"))) {
882 strat_infos.conf.flags =
883 STRAT_CONF_TAKE_ONE_LINTEL |
884 STRAT_CONF_STORE_STATIC2 |
885 STRAT_CONF_EARLY_SCAN |
886 STRAT_CONF_PUSH_OPP_COLS;
887 strat_infos.conf.scan_our_min_time = 50;
888 strat_infos.conf.delay_between_our_scan = 90;
889 strat_infos.conf.scan_opp_min_time = 15;
890 strat_infos.conf.delay_between_opp_scan = 90;
891 strat_infos.conf.wait_opponent = 5;
893 else if (!strcmp_P(res->arg1, PSTR("offensive_late"))) {
894 strat_infos.conf.flags = STRAT_CONF_TAKE_ONE_LINTEL;
895 strat_infos.conf.scan_our_min_time = 90;
896 strat_infos.conf.delay_between_our_scan = 90;
897 strat_infos.conf.scan_opp_min_time = 30;
898 strat_infos.conf.delay_between_opp_scan = 90;
900 else if (!strcmp_P(res->arg1, PSTR("one_on_disc"))) {
901 strat_infos.conf.flags =
902 STRAT_CONF_ONLY_ONE_ON_DISC;
903 strat_infos.conf.scan_our_min_time = 90;
904 strat_infos.conf.delay_between_our_scan = 90;
905 strat_infos.conf.scan_opp_min_time = 90;
906 strat_infos.conf.delay_between_opp_scan = 90;
908 strat_infos.dump_enabled = 1;
912 prog_char str_strat_conf_arg0[] = "strat_conf";
913 parse_pgm_token_string_t cmd_strat_conf_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf_result, arg0, str_strat_conf_arg0);
914 prog_char str_strat_conf_arg1[] = "show#base#big3#base_check#big3_check#offensive_early#offensive_late#one_on_disc";
915 parse_pgm_token_string_t cmd_strat_conf_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf_result, arg1, str_strat_conf_arg1);
917 prog_char help_strat_conf[] = "configure strat options";
918 parse_pgm_inst_t cmd_strat_conf = {
919 .f = cmd_strat_conf_parsed, /* function to call */
920 .data = NULL, /* 2nd arg of func */
921 .help_str = help_strat_conf,
922 .tokens = { /* token list, NULL terminated */
923 (prog_void *)&cmd_strat_conf_arg0,
924 (prog_void *)&cmd_strat_conf_arg1,
929 /**********************************************************/
930 /* strat configuration */
932 /* this structure is filled when cmd_strat_conf2 is parsed successfully */
933 struct cmd_strat_conf2_result {
939 /* function called when cmd_strat_conf2 is parsed successfully */
940 static void cmd_strat_conf2_parsed(void *parsed_result, void *data)
942 struct cmd_strat_conf2_result *res = parsed_result;
945 if (!strcmp_P(res->arg2, PSTR("on")))
950 if (!strcmp_P(res->arg1, PSTR("one_temple_on_disc")))
951 bit = STRAT_CONF_ONLY_ONE_ON_DISC;
952 else if (!strcmp_P(res->arg1, PSTR("bypass_static2")))
953 bit = STRAT_CONF_BYPASS_STATIC2;
954 else if (!strcmp_P(res->arg1, PSTR("take_one_lintel")))
955 bit = STRAT_CONF_TAKE_ONE_LINTEL;
956 else if (!strcmp_P(res->arg1, PSTR("skip_when_check_fail")))
957 bit = STRAT_CONF_TAKE_ONE_LINTEL;
958 else if (!strcmp_P(res->arg1, PSTR("store_static2")))
959 bit = STRAT_CONF_STORE_STATIC2;
960 else if (!strcmp_P(res->arg1, PSTR("big3_temple")))
961 bit = STRAT_CONF_BIG_3_TEMPLE;
962 else if (!strcmp_P(res->arg1, PSTR("early_opp_scan")))
963 bit = STRAT_CONF_EARLY_SCAN;
964 else if (!strcmp_P(res->arg1, PSTR("push_opp_cols")))
965 bit = STRAT_CONF_PUSH_OPP_COLS;
968 strat_infos.conf.flags |= bit;
970 strat_infos.conf.flags &= (~bit);
972 strat_infos.dump_enabled = 1;
976 prog_char str_strat_conf2_arg0[] = "strat_conf";
977 parse_pgm_token_string_t cmd_strat_conf2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg0, str_strat_conf2_arg0);
978 prog_char str_strat_conf2_arg1[] = "push_opp_cols#one_temple_on_disc#bypass_static2#take_one_lintel#skip_when_check_fail#store_static2#big3_temple#early_opp_scan";
979 parse_pgm_token_string_t cmd_strat_conf2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg1, str_strat_conf2_arg1);
980 prog_char str_strat_conf2_arg2[] = "on#off";
981 parse_pgm_token_string_t cmd_strat_conf2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg2, str_strat_conf2_arg2);
984 prog_char help_strat_conf2[] = "configure strat options";
985 parse_pgm_inst_t cmd_strat_conf2 = {
986 .f = cmd_strat_conf2_parsed, /* function to call */
987 .data = NULL, /* 2nd arg of func */
988 .help_str = help_strat_conf2,
989 .tokens = { /* token list, NULL terminated */
990 (prog_void *)&cmd_strat_conf2_arg0,
991 (prog_void *)&cmd_strat_conf2_arg1,
992 (prog_void *)&cmd_strat_conf2_arg2,
997 /**********************************************************/
998 /* strat configuration */
1000 /* this structure is filled when cmd_strat_conf3 is parsed successfully */
1001 struct cmd_strat_conf3_result {
1002 fixed_string_t arg0;
1003 fixed_string_t arg1;
1007 /* function called when cmd_strat_conf3 is parsed successfully */
1008 static void cmd_strat_conf3_parsed(void *parsed_result, void *data)
1010 struct cmd_strat_conf3_result *res = parsed_result;
1012 if (!strcmp_P(res->arg1, PSTR("scan_opponent_min_time"))) {
1015 strat_infos.conf.scan_opp_min_time = res->arg2;
1017 else if (!strcmp_P(res->arg1, PSTR("delay_between_opponent_scan"))) {
1020 strat_infos.conf.delay_between_opp_scan = res->arg2;
1022 else if (!strcmp_P(res->arg1, PSTR("scan_our_min_time"))) {
1025 strat_infos.conf.scan_our_min_time = res->arg2;
1027 else if (!strcmp_P(res->arg1, PSTR("delay_between_our_scan"))) {
1030 strat_infos.conf.delay_between_our_scan = res->arg2;
1032 else if (!strcmp_P(res->arg1, PSTR("wait_opponent"))) {
1033 strat_infos.conf.wait_opponent = res->arg2;
1035 else if (!strcmp_P(res->arg1, PSTR("lintel_min_time"))) {
1036 strat_infos.conf.lintel_min_time = res->arg2;
1038 strat_infos.dump_enabled = 1;
1042 prog_char str_strat_conf3_arg0[] = "strat_conf";
1043 parse_pgm_token_string_t cmd_strat_conf3_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf3_result, arg0, str_strat_conf3_arg0);
1044 prog_char str_strat_conf3_arg1[] = "lintel_min_time#scan_opponent_min_time#delay_between_opponent_scan#scan_our_min_time#delay_between_our_scan#wait_opponent";
1045 parse_pgm_token_string_t cmd_strat_conf3_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf3_result, arg1, str_strat_conf3_arg1);
1046 parse_pgm_token_num_t cmd_strat_conf3_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_strat_conf3_result, arg2, UINT16);
1048 prog_char help_strat_conf3[] = "configure strat options";
1049 parse_pgm_inst_t cmd_strat_conf3 = {
1050 .f = cmd_strat_conf3_parsed, /* function to call */
1051 .data = NULL, /* 2nd arg of func */
1052 .help_str = help_strat_conf3,
1053 .tokens = { /* token list, NULL terminated */
1054 (prog_void *)&cmd_strat_conf3_arg0,
1055 (prog_void *)&cmd_strat_conf3_arg1,
1056 (prog_void *)&cmd_strat_conf3_arg2,
1061 /**********************************************************/
1062 /* strat configuration */
1064 /* this structure is filled when cmd_strat_conf4 is parsed successfully */
1065 struct cmd_strat_conf4_result {
1066 fixed_string_t arg0;
1067 fixed_string_t arg1;
1071 /* function called when cmd_strat_conf4 is parsed successfully */
1072 static void cmd_strat_conf4_parsed(void *parsed_result, void *data)
1074 struct cmd_strat_conf4_result *res = parsed_result;
1076 if (!strcmp_P(res->arg1, PSTR("scan_opponent_angle"))) {
1077 strat_infos.conf.scan_opp_angle = res->arg2;
1079 strat_infos.dump_enabled = 1;
1083 prog_char str_strat_conf4_arg0[] = "strat_conf";
1084 parse_pgm_token_string_t cmd_strat_conf4_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf4_result, arg0, str_strat_conf4_arg0);
1085 prog_char str_strat_conf4_arg1[] = "scan_opponent_angle";
1086 parse_pgm_token_string_t cmd_strat_conf4_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf4_result, arg1, str_strat_conf4_arg1);
1087 parse_pgm_token_num_t cmd_strat_conf4_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_strat_conf4_result, arg2, UINT16);
1089 prog_char help_strat_conf4[] = "configure strat options";
1090 parse_pgm_inst_t cmd_strat_conf4 = {
1091 .f = cmd_strat_conf4_parsed, /* function to call */
1092 .data = NULL, /* 2nd arg of func */
1093 .help_str = help_strat_conf4,
1094 .tokens = { /* token list, NULL terminated */
1095 (prog_void *)&cmd_strat_conf4_arg0,
1096 (prog_void *)&cmd_strat_conf4_arg1,
1097 (prog_void *)&cmd_strat_conf4_arg2,
1103 /**********************************************************/
1106 /* this structure is filled when cmd_subtraj is parsed successfully */
1107 struct cmd_subtraj_result {
1108 fixed_string_t arg0;
1109 fixed_string_t arg1;
1116 /* function called when cmd_subtraj is parsed successfully */
1117 static void cmd_subtraj_parsed(void *parsed_result, void *data)
1120 printf("not implemented\n");
1122 struct cmd_subtraj_result *res = parsed_result;
1124 struct column_dispenser *disp;
1126 if (strcmp_P(res->arg1, PSTR("static")) == 0) {
1127 err = strat_static_columns(res->arg2);
1129 else if (strcmp_P(res->arg1, PSTR("static2")) == 0) {
1130 strat_infos.s_cols.configuration = res->arg2;
1131 switch (res->arg2) {
1133 position_set(&mainboard.pos, 1398,
1134 COLOR_Y(1297), COLOR_A(-66));
1137 position_set(&mainboard.pos, 1232,
1138 COLOR_Y(1051), COLOR_A(4));
1141 position_set(&mainboard.pos, 1232,
1142 COLOR_Y(1043), COLOR_A(5));
1145 position_set(&mainboard.pos, 1346,
1146 COLOR_Y(852), COLOR_A(57));
1151 if (res->arg2 == 1 && res->arg3 == 1) {
1152 strat_infos.s_cols.flags = STATIC_COL_LINE1_DONE;
1154 if (res->arg2 == 1 && res->arg3 == 2) {
1155 strat_infos.s_cols.flags = STATIC_COL_LINE2_DONE;
1157 err = strat_static_columns_pass2();
1159 else if (strcmp_P(res->arg1, PSTR("lintel1")) == 0) {
1160 err = strat_goto_lintel_disp(&strat_infos.l1);
1162 else if (strcmp_P(res->arg1, PSTR("lintel2")) == 0) {
1163 err = strat_goto_lintel_disp(&strat_infos.l2);
1165 else if (strcmp_P(res->arg1, PSTR("coldisp1")) == 0) {
1166 disp = &strat_infos.c1;
1167 err = strat_goto_col_disp(&disp);
1169 else if (strcmp_P(res->arg1, PSTR("coldisp2")) == 0) {
1170 disp = &strat_infos.c2;
1171 err = strat_goto_col_disp(&disp);
1173 else if (strcmp_P(res->arg1, PSTR("coldisp3")) == 0) {
1174 disp = &strat_infos.c3;
1175 err = strat_goto_col_disp(&disp);
1177 else if (strcmp_P(res->arg1, PSTR("disc")) == 0) {
1178 if (res->arg2 == 0) {
1179 printf_P(PSTR("bad level\r\n"));
1182 err = strat_goto_disc(res->arg2);
1185 printf_P(PSTR("substrat returned %s\r\n"), get_err(err));
1186 trajectory_hardstop(&mainboard.traj);
1190 prog_char str_subtraj_arg0[] = "subtraj";
1191 parse_pgm_token_string_t cmd_subtraj_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_subtraj_result, arg0, str_subtraj_arg0);
1192 prog_char str_subtraj_arg1[] = "static#disc#lintel1#lintel2#coldisp1#coldisp2#coldisp3#static2";
1193 parse_pgm_token_string_t cmd_subtraj_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_subtraj_result, arg1, str_subtraj_arg1);
1194 parse_pgm_token_num_t cmd_subtraj_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg2, INT32);
1195 parse_pgm_token_num_t cmd_subtraj_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg3, INT32);
1196 parse_pgm_token_num_t cmd_subtraj_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg4, INT32);
1197 parse_pgm_token_num_t cmd_subtraj_arg5 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg5, INT32);
1199 prog_char help_subtraj[] = "Test sub-trajectories (a,b,c,d: specific params)";
1200 parse_pgm_inst_t cmd_subtraj = {
1201 .f = cmd_subtraj_parsed, /* function to call */
1202 .data = NULL, /* 2nd arg of func */
1203 .help_str = help_subtraj,
1204 .tokens = { /* token list, NULL terminated */
1205 (prog_void *)&cmd_subtraj_arg0,
1206 (prog_void *)&cmd_subtraj_arg1,
1207 (prog_void *)&cmd_subtraj_arg2,
1208 (prog_void *)&cmd_subtraj_arg3,
1209 (prog_void *)&cmd_subtraj_arg4,
1210 (prog_void *)&cmd_subtraj_arg5,