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>
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"
59 #include "strat_avoid.h"
61 #include "../common/i2c_commands.h"
62 #include "i2c_protocol.h"
64 /**********************************************************/
65 /* Traj_Speeds for trajectory_manager */
67 /* this structure is filled when cmd_traj_speed is parsed successfully */
68 struct cmd_traj_speed_result {
74 /* function called when cmd_traj_speed is parsed successfully */
75 static void cmd_traj_speed_parsed(void *parsed_result, void *data)
77 struct cmd_traj_speed_result * res = parsed_result;
79 if (!strcmp_P(res->arg1, PSTR("angle"))) {
80 trajectory_set_speed(&mainboard.traj, mainboard.traj.d_speed, res->s);
82 else if (!strcmp_P(res->arg1, PSTR("distance"))) {
83 trajectory_set_speed(&mainboard.traj, res->s, mainboard.traj.a_speed);
85 /* else it is a "show" */
87 printf_P(PSTR("angle %u, distance %u\r\n"),
88 mainboard.traj.a_speed,
89 mainboard.traj.d_speed);
92 prog_char str_traj_speed_arg0[] = "traj_speed";
93 parse_pgm_token_string_t cmd_traj_speed_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_traj_speed_result, arg0, str_traj_speed_arg0);
94 prog_char str_traj_speed_arg1[] = "angle#distance";
95 parse_pgm_token_string_t cmd_traj_speed_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_traj_speed_result, arg1, str_traj_speed_arg1);
96 parse_pgm_token_num_t cmd_traj_speed_s = TOKEN_NUM_INITIALIZER(struct cmd_traj_speed_result, s, UINT16);
98 prog_char help_traj_speed[] = "Set traj_speed values for trajectory manager";
99 parse_pgm_inst_t cmd_traj_speed = {
100 .f = cmd_traj_speed_parsed, /* function to call */
101 .data = NULL, /* 2nd arg of func */
102 .help_str = help_traj_speed,
103 .tokens = { /* token list, NULL terminated */
104 (prog_void *)&cmd_traj_speed_arg0,
105 (prog_void *)&cmd_traj_speed_arg1,
106 (prog_void *)&cmd_traj_speed_s,
113 prog_char str_traj_speed_show_arg[] = "show";
114 parse_pgm_token_string_t cmd_traj_speed_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_traj_speed_result, arg1, str_traj_speed_show_arg);
116 prog_char help_traj_speed_show[] = "Show traj_speed values for trajectory manager";
117 parse_pgm_inst_t cmd_traj_speed_show = {
118 .f = cmd_traj_speed_parsed, /* function to call */
119 .data = NULL, /* 2nd arg of func */
120 .help_str = help_traj_speed_show,
121 .tokens = { /* token list, NULL terminated */
122 (prog_void *)&cmd_traj_speed_arg0,
123 (prog_void *)&cmd_traj_speed_show_arg,
128 /**********************************************************/
129 /* trajectory window configuration */
131 /* this structure is filled when cmd_trajectory is parsed successfully */
132 struct cmd_trajectory_result {
141 /* function called when cmd_trajectory is parsed successfully */
142 static void cmd_trajectory_parsed(void * parsed_result, void * data)
144 struct cmd_trajectory_result * res = parsed_result;
146 if (!strcmp_P(res->arg1, PSTR("set"))) {
147 trajectory_set_windows(&mainboard.traj, res->d_win,
148 res->a_win, res->a_start);
151 printf_P(PSTR("trajectory %2.2f %2.2f %2.2f\r\n"), mainboard.traj.d_win,
152 DEG(mainboard.traj.a_win_rad), DEG(mainboard.traj.a_start_rad));
155 prog_char str_trajectory_arg0[] = "trajectory";
156 parse_pgm_token_string_t cmd_trajectory_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_trajectory_result, arg0, str_trajectory_arg0);
157 prog_char str_trajectory_arg1[] = "set";
158 parse_pgm_token_string_t cmd_trajectory_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_trajectory_result, arg1, str_trajectory_arg1);
159 parse_pgm_token_num_t cmd_trajectory_d = TOKEN_NUM_INITIALIZER(struct cmd_trajectory_result, d_win, FLOAT);
160 parse_pgm_token_num_t cmd_trajectory_a = TOKEN_NUM_INITIALIZER(struct cmd_trajectory_result, a_win, FLOAT);
161 parse_pgm_token_num_t cmd_trajectory_as = TOKEN_NUM_INITIALIZER(struct cmd_trajectory_result, a_start, FLOAT);
163 prog_char help_trajectory[] = "Set trajectory windows (distance, angle, angle_start)";
164 parse_pgm_inst_t cmd_trajectory = {
165 .f = cmd_trajectory_parsed, /* function to call */
166 .data = NULL, /* 2nd arg of func */
167 .help_str = help_trajectory,
168 .tokens = { /* token list, NULL terminated */
169 (prog_void *)&cmd_trajectory_arg0,
170 (prog_void *)&cmd_trajectory_arg1,
171 (prog_void *)&cmd_trajectory_d,
172 (prog_void *)&cmd_trajectory_a,
173 (prog_void *)&cmd_trajectory_as,
180 prog_char str_trajectory_show_arg[] = "show";
181 parse_pgm_token_string_t cmd_trajectory_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_trajectory_result, arg1, str_trajectory_show_arg);
183 prog_char help_trajectory_show[] = "Show trajectory window configuration";
184 parse_pgm_inst_t cmd_trajectory_show = {
185 .f = cmd_trajectory_parsed, /* function to call */
186 .data = NULL, /* 2nd arg of func */
187 .help_str = help_trajectory_show,
188 .tokens = { /* token list, NULL terminated */
189 (prog_void *)&cmd_trajectory_arg0,
190 (prog_void *)&cmd_trajectory_show_arg,
195 /**********************************************************/
196 /* rs_gains configuration */
198 /* this structure is filled when cmd_rs_gains is parsed successfully */
199 struct cmd_rs_gains_result {
206 /* function called when cmd_rs_gains is parsed successfully */
207 static void cmd_rs_gains_parsed(void * parsed_result, void * data)
209 struct cmd_rs_gains_result * res = parsed_result;
211 if (!strcmp_P(res->arg1, PSTR("set"))) {
212 rs_set_left_ext_encoder(&mainboard.rs, encoders_spi_get_value,
213 LEFT_ENCODER, res->left); // en augmentant on tourne à gauche
214 rs_set_right_ext_encoder(&mainboard.rs, encoders_spi_get_value,
215 RIGHT_ENCODER, res->right); //en augmentant on tourne à droite
217 printf_P(PSTR("rs_gains set "));
218 //f64_print(mainboard.rs.left_ext_gain);
220 //f64_print(mainboard.rs.right_ext_gain);
221 printf_P(PSTR("\r\n"));
224 prog_char str_rs_gains_arg0[] = "rs_gains";
225 parse_pgm_token_string_t cmd_rs_gains_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_rs_gains_result, arg0, str_rs_gains_arg0);
226 prog_char str_rs_gains_arg1[] = "set";
227 parse_pgm_token_string_t cmd_rs_gains_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_rs_gains_result, arg1, str_rs_gains_arg1);
228 parse_pgm_token_num_t cmd_rs_gains_l = TOKEN_NUM_INITIALIZER(struct cmd_rs_gains_result, left, FLOAT);
229 parse_pgm_token_num_t cmd_rs_gains_r = TOKEN_NUM_INITIALIZER(struct cmd_rs_gains_result, right, FLOAT);
231 prog_char help_rs_gains[] = "Set rs_gains (left, right)";
232 parse_pgm_inst_t cmd_rs_gains = {
233 .f = cmd_rs_gains_parsed, /* function to call */
234 .data = NULL, /* 2nd arg of func */
235 .help_str = help_rs_gains,
236 .tokens = { /* token list, NULL terminated */
237 (prog_void *)&cmd_rs_gains_arg0,
238 (prog_void *)&cmd_rs_gains_arg1,
239 (prog_void *)&cmd_rs_gains_l,
240 (prog_void *)&cmd_rs_gains_r,
247 prog_char str_rs_gains_show_arg[] = "show";
248 parse_pgm_token_string_t cmd_rs_gains_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_rs_gains_result, arg1, str_rs_gains_show_arg);
250 prog_char help_rs_gains_show[] = "Show rs_gains";
251 parse_pgm_inst_t cmd_rs_gains_show = {
252 .f = cmd_rs_gains_parsed, /* function to call */
253 .data = NULL, /* 2nd arg of func */
254 .help_str = help_rs_gains_show,
255 .tokens = { /* token list, NULL terminated */
256 (prog_void *)&cmd_rs_gains_arg0,
257 (prog_void *)&cmd_rs_gains_show_arg,
262 /**********************************************************/
263 /* track configuration */
265 /* this structure is filled when cmd_track is parsed successfully */
266 struct cmd_track_result {
272 /* function called when cmd_track is parsed successfully */
273 static void cmd_track_parsed(void * parsed_result, void * data)
275 struct cmd_track_result * res = parsed_result;
277 if (!strcmp_P(res->arg1, PSTR("set"))) {
278 position_set_physical_params(&mainboard.pos, res->val, DIST_IMP_MM);
280 printf_P(PSTR("track set %f\r\n"), mainboard.pos.phys.track_mm);
283 prog_char str_track_arg0[] = "track";
284 parse_pgm_token_string_t cmd_track_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_track_result, arg0, str_track_arg0);
285 prog_char str_track_arg1[] = "set";
286 parse_pgm_token_string_t cmd_track_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_track_result, arg1, str_track_arg1);
287 parse_pgm_token_num_t cmd_track_val = TOKEN_NUM_INITIALIZER(struct cmd_track_result, val, FLOAT);
289 prog_char help_track[] = "Set track in mm";
290 parse_pgm_inst_t cmd_track = {
291 .f = cmd_track_parsed, /* function to call */
292 .data = NULL, /* 2nd arg of func */
293 .help_str = help_track,
294 .tokens = { /* token list, NULL terminated */
295 (prog_void *)&cmd_track_arg0,
296 (prog_void *)&cmd_track_arg1,
297 (prog_void *)&cmd_track_val,
304 prog_char str_track_show_arg[] = "show";
305 parse_pgm_token_string_t cmd_track_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_track_result, arg1, str_track_show_arg);
307 prog_char help_track_show[] = "Show track";
308 parse_pgm_inst_t cmd_track_show = {
309 .f = cmd_track_parsed, /* function to call */
310 .data = NULL, /* 2nd arg of func */
311 .help_str = help_track_show,
312 .tokens = { /* token list, NULL terminated */
313 (prog_void *)&cmd_track_arg0,
314 (prog_void *)&cmd_track_show_arg,
321 /**********************************************************/
322 /* Pt_Lists for testing traj */
324 #define PT_LIST_SIZE 10
325 static struct xy_point pt_list[PT_LIST_SIZE];
326 static uint16_t pt_list_len = 0;
328 /* this structure is filled when cmd_pt_list is parsed successfully */
329 struct cmd_pt_list_result {
337 /* function called when cmd_pt_list is parsed successfully */
338 static void cmd_pt_list_parsed(void * parsed_result, void * data)
340 struct cmd_pt_list_result * res = parsed_result;
343 if (!strcmp_P(res->arg1, PSTR("append"))) {
344 res->arg2 = pt_list_len;
347 if (!strcmp_P(res->arg1, PSTR("insert")) ||
348 !strcmp_P(res->arg1, PSTR("append"))) {
349 if (res->arg2 > pt_list_len) {
350 printf_P(PSTR("Index too large\r\n"));
353 if (pt_list_len >= PT_LIST_SIZE) {
354 printf_P(PSTR("List is too large\r\n"));
357 memmove(&pt_list[res->arg2+1], &pt_list[res->arg2],
358 PT_LIST_SIZE-1-res->arg2);
359 pt_list[res->arg2].x = res->arg3;
360 pt_list[res->arg2].y = res->arg4;
363 else if (!strcmp_P(res->arg1, PSTR("del"))) {
364 if (pt_list_len <= 0) {
365 printf_P(PSTR("Error: list empty\r\n"));
368 if (res->arg2 > pt_list_len) {
369 printf_P(PSTR("Index too large\r\n"));
372 memmove(&pt_list[res->arg2], &pt_list[res->arg2+1],
373 (PT_LIST_SIZE-1-res->arg2)*sizeof(struct xy_point));
376 else if (!strcmp_P(res->arg1, PSTR("reset"))) {
380 /* else it is a "show" or a "start" */
381 if (pt_list_len == 0) {
382 printf_P(PSTR("List empty\r\n"));
385 for (i=0 ; i<pt_list_len ; i++) {
386 printf_P(PSTR("%d: x=%d y=%d\r\n"), i, pt_list[i].x, pt_list[i].y);
387 if (!strcmp_P(res->arg1, PSTR("start"))) {
388 trajectory_goto_xy_abs(&mainboard.traj, pt_list[i].x, pt_list[i].y);
389 why = wait_traj_end(0xFF); /* all */
391 else if (!strcmp_P(res->arg1, PSTR("avoid_start"))) {
393 why = goto_and_avoid(pt_list[i].x, pt_list[i].y, 0xFF, 0xFF);
394 printf("next point\r\n");
395 if (why != END_OBSTACLE)
399 if (why & (~(END_TRAJ | END_NEAR)))
400 trajectory_stop(&mainboard.traj);
404 prog_char str_pt_list_arg0[] = "pt_list";
405 parse_pgm_token_string_t cmd_pt_list_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg0, str_pt_list_arg0);
406 prog_char str_pt_list_arg1[] = "insert";
407 parse_pgm_token_string_t cmd_pt_list_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_arg1);
408 parse_pgm_token_num_t cmd_pt_list_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_pt_list_result, arg2, UINT16);
409 parse_pgm_token_num_t cmd_pt_list_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_pt_list_result, arg3, INT16);
410 parse_pgm_token_num_t cmd_pt_list_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_pt_list_result, arg4, INT16);
412 prog_char help_pt_list[] = "Insert point in pt_list (idx,x,y)";
413 parse_pgm_inst_t cmd_pt_list = {
414 .f = cmd_pt_list_parsed, /* function to call */
415 .data = NULL, /* 2nd arg of func */
416 .help_str = help_pt_list,
417 .tokens = { /* token list, NULL terminated */
418 (prog_void *)&cmd_pt_list_arg0,
419 (prog_void *)&cmd_pt_list_arg1,
420 (prog_void *)&cmd_pt_list_arg2,
421 (prog_void *)&cmd_pt_list_arg3,
422 (prog_void *)&cmd_pt_list_arg4,
429 prog_char str_pt_list_arg1_append[] = "append";
430 parse_pgm_token_string_t cmd_pt_list_arg1_append = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_arg1_append);
432 prog_char help_pt_list_append[] = "Append point in pt_list (x,y)";
433 parse_pgm_inst_t cmd_pt_list_append = {
434 .f = cmd_pt_list_parsed, /* function to call */
435 .data = NULL, /* 2nd arg of func */
436 .help_str = help_pt_list_append,
437 .tokens = { /* token list, NULL terminated */
438 (prog_void *)&cmd_pt_list_arg0,
439 (prog_void *)&cmd_pt_list_arg1_append,
440 (prog_void *)&cmd_pt_list_arg3,
441 (prog_void *)&cmd_pt_list_arg4,
448 prog_char str_pt_list_del_arg[] = "del";
449 parse_pgm_token_string_t cmd_pt_list_del_arg = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_del_arg);
451 prog_char help_pt_list_del[] = "Del or insert point in pt_list (num)";
452 parse_pgm_inst_t cmd_pt_list_del = {
453 .f = cmd_pt_list_parsed, /* function to call */
454 .data = NULL, /* 2nd arg of func */
455 .help_str = help_pt_list_del,
456 .tokens = { /* token list, NULL terminated */
457 (prog_void *)&cmd_pt_list_arg0,
458 (prog_void *)&cmd_pt_list_del_arg,
459 (prog_void *)&cmd_pt_list_arg2,
465 prog_char str_pt_list_show_arg[] = "show#reset#start#avoid_start";
466 parse_pgm_token_string_t cmd_pt_list_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_show_arg);
468 prog_char help_pt_list_show[] = "Show, start or reset pt_list";
469 parse_pgm_inst_t cmd_pt_list_show = {
470 .f = cmd_pt_list_parsed, /* function to call */
471 .data = NULL, /* 2nd arg of func */
472 .help_str = help_pt_list_show,
473 .tokens = { /* token list, NULL terminated */
474 (prog_void *)&cmd_pt_list_arg0,
475 (prog_void *)&cmd_pt_list_show_arg,
482 /**********************************************************/
485 /* this structure is filled when cmd_goto is parsed successfully */
486 struct cmd_goto_result {
495 /* function called when cmd_goto is parsed successfully */
496 static void cmd_goto_parsed(void * parsed_result, void * data)
498 struct cmd_goto_result * res = parsed_result;
502 interrupt_traj_reset();
503 if (!strcmp_P(res->arg1, PSTR("a_rel"))) {
504 trajectory_a_rel(&mainboard.traj, res->arg2);
506 else if (!strcmp_P(res->arg1, PSTR("d_rel"))) {
507 trajectory_d_rel(&mainboard.traj, res->arg2);
509 else if (!strcmp_P(res->arg1, PSTR("a_abs"))) {
510 trajectory_a_abs(&mainboard.traj, res->arg2);
512 else if (!strcmp_P(res->arg1, PSTR("a_to_xy"))) {
513 trajectory_turnto_xy(&mainboard.traj, res->arg2, res->arg3);
515 else if (!strcmp_P(res->arg1, PSTR("a_behind_xy"))) {
516 trajectory_turnto_xy_behind(&mainboard.traj, res->arg2, res->arg3);
518 else if (!strcmp_P(res->arg1, PSTR("xy_rel"))) {
519 trajectory_goto_xy_rel(&mainboard.traj, res->arg2, res->arg3);
521 else if (!strcmp_P(res->arg1, PSTR("xy_abs"))) {
522 trajectory_goto_xy_abs(&mainboard.traj, res->arg2, res->arg3);
524 else if (!strcmp_P(res->arg1, PSTR("avoid"))) {
525 err = goto_and_avoid_forward(res->arg2, res->arg3, 0xFF, 0xFF);
526 if (err != END_TRAJ && err != END_NEAR)
529 else if (!strcmp_P(res->arg1, PSTR("avoid_bw"))) {
530 err = goto_and_avoid_backward(res->arg2, res->arg3, 0xFF, 0xFF);
531 if (err != END_TRAJ && err != END_NEAR)
534 else if (!strcmp_P(res->arg1, PSTR("xy_abs_fow"))) {
535 trajectory_goto_forward_xy_abs(&mainboard.traj, res->arg2, res->arg3);
537 else if (!strcmp_P(res->arg1, PSTR("xy_abs_back"))) {
538 trajectory_goto_backward_xy_abs(&mainboard.traj, res->arg2, res->arg3);
540 else if (!strcmp_P(res->arg1, PSTR("da_rel"))) {
541 trajectory_d_a_rel(&mainboard.traj, res->arg2, res->arg3);
543 else if (!strcmp_P(res->arg1, PSTR("circle_rel"))) {
544 trajectory_circle_rel(&mainboard.traj, res->arg2, res->arg3,
545 res->arg4, res->arg5, 0);
549 while ((err = test_traj_end(0xFF)) == 0) {
551 if (t2 - t1 > 200000) {
552 dump_cs_debug("angle", &mainboard.angle.cs);
553 dump_cs_debug("distance", &mainboard.distance.cs);
557 if (err != END_TRAJ && err != END_NEAR)
559 printf_P(PSTR("returned %s\r\n"), get_err(err));
562 prog_char str_goto_arg0[] = "goto";
563 parse_pgm_token_string_t cmd_goto_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_goto_result, arg0, str_goto_arg0);
564 prog_char str_goto_arg1_a[] = "d_rel#a_rel#a_abs";
565 parse_pgm_token_string_t cmd_goto_arg1_a = TOKEN_STRING_INITIALIZER(struct cmd_goto_result, arg1, str_goto_arg1_a);
566 parse_pgm_token_num_t cmd_goto_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_goto_result, arg2, INT32);
569 prog_char help_goto1[] = "Change orientation of the mainboard";
570 parse_pgm_inst_t cmd_goto1 = {
571 .f = cmd_goto_parsed, /* function to call */
572 .data = NULL, /* 2nd arg of func */
573 .help_str = help_goto1,
574 .tokens = { /* token list, NULL terminated */
575 (prog_void *)&cmd_goto_arg0,
576 (prog_void *)&cmd_goto_arg1_a,
577 (prog_void *)&cmd_goto_arg2,
582 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";
583 parse_pgm_token_string_t cmd_goto_arg1_b = TOKEN_STRING_INITIALIZER(struct cmd_goto_result, arg1, str_goto_arg1_b);
584 parse_pgm_token_num_t cmd_goto_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_goto_result, arg3, INT32);
587 prog_char help_goto2[] = "Go to a (x,y) or (d,a) position";
588 parse_pgm_inst_t cmd_goto2 = {
589 .f = cmd_goto_parsed, /* function to call */
590 .data = NULL, /* 2nd arg of func */
591 .help_str = help_goto2,
592 .tokens = { /* token list, NULL terminated */
593 (prog_void *)&cmd_goto_arg0,
594 (prog_void *)&cmd_goto_arg1_b,
595 (prog_void *)&cmd_goto_arg2,
596 (prog_void *)&cmd_goto_arg3,
601 prog_char str_goto_arg1_c[] = "circle_rel";
602 parse_pgm_token_string_t cmd_goto_arg1_c = TOKEN_STRING_INITIALIZER(struct cmd_goto_result, arg1, str_goto_arg1_c);
603 parse_pgm_token_num_t cmd_goto_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_goto_result, arg4, INT32);
604 parse_pgm_token_num_t cmd_goto_arg5 = TOKEN_NUM_INITIALIZER(struct cmd_goto_result, arg5, INT32);
607 prog_char help_goto4[] = "Do a circle: (x,y, radius, angle)";
608 parse_pgm_inst_t cmd_goto4 = {
609 .f = cmd_goto_parsed, /* function to call */
610 .data = NULL, /* 2nd arg of func */
611 .help_str = help_goto4,
612 .tokens = { /* token list, NULL terminated */
613 (prog_void *)&cmd_goto_arg0,
614 (prog_void *)&cmd_goto_arg1_c,
615 (prog_void *)&cmd_goto_arg2,
616 (prog_void *)&cmd_goto_arg3,
617 (prog_void *)&cmd_goto_arg4,
618 (prog_void *)&cmd_goto_arg5,
623 /**********************************************************/
626 /* this structure is filled when cmd_position is parsed successfully */
627 struct cmd_position_result {
635 #define AUTOPOS_SPEED_FAST 200
636 static void auto_position(void)
639 uint16_t old_spdd, old_spda;
641 interrupt_traj_reset();
642 strat_get_speed(&old_spdd, &old_spda);
643 strat_set_speed(AUTOPOS_SPEED_FAST, AUTOPOS_SPEED_FAST);
645 trajectory_d_rel(&mainboard.traj, -300);
646 err = wait_traj_end(END_INTR|END_TRAJ|END_BLOCKING);
650 strat_reset_pos(ROBOT_LENGTH/2, 0, 0);
652 trajectory_d_rel(&mainboard.traj, 120);
653 err = wait_traj_end(END_INTR|END_TRAJ);
657 trajectory_a_rel(&mainboard.traj, COLOR_A(90));
658 err = wait_traj_end(END_INTR|END_TRAJ);
662 trajectory_d_rel(&mainboard.traj, -300);
663 err = wait_traj_end(END_INTR|END_TRAJ|END_BLOCKING);
667 strat_reset_pos(DO_NOT_SET_POS, COLOR_Y(ROBOT_LENGTH/2),
670 trajectory_d_rel(&mainboard.traj, 120);
671 err = wait_traj_end(END_INTR|END_TRAJ);
676 trajectory_a_rel(&mainboard.traj, COLOR_A(-40));
677 err = wait_traj_end(END_INTR|END_TRAJ);
682 strat_set_speed(old_spdd, old_spda);
687 strat_set_speed(old_spdd, old_spda);
690 /* function called when cmd_position is parsed successfully */
691 static void cmd_position_parsed(void * parsed_result, void * data)
693 struct cmd_position_result * res = parsed_result;
695 /* display raw position values */
696 if (!strcmp_P(res->arg1, PSTR("reset"))) {
697 position_set(&mainboard.pos, 0, 0, 0);
699 else if (!strcmp_P(res->arg1, PSTR("set"))) {
700 position_set(&mainboard.pos, res->arg2, res->arg3, res->arg4);
702 else if (!strcmp_P(res->arg1, PSTR("autoset_green"))) {
703 mainboard.our_color = I2C_COLOR_GREEN;
704 i2c_set_color(I2C_MECHBOARD_ADDR, I2C_COLOR_GREEN);
705 i2c_set_color(I2C_SENSORBOARD_ADDR, I2C_COLOR_GREEN);
708 else if (!strcmp_P(res->arg1, PSTR("autoset_red"))) {
709 mainboard.our_color = I2C_COLOR_RED;
710 i2c_set_color(I2C_MECHBOARD_ADDR, I2C_COLOR_RED);
711 i2c_set_color(I2C_SENSORBOARD_ADDR, I2C_COLOR_RED);
715 /* else it's just a "show" */
716 printf_P(PSTR("x=%.2f y=%.2f a=%.2f\r\n"),
717 position_get_x_double(&mainboard.pos),
718 position_get_y_double(&mainboard.pos),
719 DEG(position_get_a_rad_double(&mainboard.pos)));
722 prog_char str_position_arg0[] = "position";
723 parse_pgm_token_string_t cmd_position_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg0, str_position_arg0);
724 prog_char str_position_arg1[] = "show#reset#autoset_green#autoset_red";
725 parse_pgm_token_string_t cmd_position_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg1, str_position_arg1);
727 prog_char help_position[] = "Show/reset (x,y,a) position";
728 parse_pgm_inst_t cmd_position = {
729 .f = cmd_position_parsed, /* function to call */
730 .data = NULL, /* 2nd arg of func */
731 .help_str = help_position,
732 .tokens = { /* token list, NULL terminated */
733 (prog_void *)&cmd_position_arg0,
734 (prog_void *)&cmd_position_arg1,
740 prog_char str_position_arg1_set[] = "set";
741 parse_pgm_token_string_t cmd_position_arg1_set = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg1, str_position_arg1_set);
742 parse_pgm_token_num_t cmd_position_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg2, INT32);
743 parse_pgm_token_num_t cmd_position_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg3, INT32);
744 parse_pgm_token_num_t cmd_position_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg4, INT32);
746 prog_char help_position_set[] = "Set (x,y,a) position";
747 parse_pgm_inst_t cmd_position_set = {
748 .f = cmd_position_parsed, /* function to call */
749 .data = NULL, /* 2nd arg of func */
750 .help_str = help_position_set,
751 .tokens = { /* token list, NULL terminated */
752 (prog_void *)&cmd_position_arg0,
753 (prog_void *)&cmd_position_arg1_set,
754 (prog_void *)&cmd_position_arg2,
755 (prog_void *)&cmd_position_arg3,
756 (prog_void *)&cmd_position_arg4,
762 /**********************************************************/
763 /* strat configuration */
765 /* this structure is filled when cmd_strat_infos is parsed successfully */
766 struct cmd_strat_infos_result {
771 /* function called when cmd_strat_infos is parsed successfully */
772 static void cmd_strat_infos_parsed(void *parsed_result, void *data)
774 struct cmd_strat_infos_result *res = parsed_result;
776 if (!strcmp_P(res->arg1, PSTR("reset"))) {
779 strat_infos.dump_enabled = 1;
780 strat_dump_infos(__FUNCTION__);
783 prog_char str_strat_infos_arg0[] = "strat_infos";
784 parse_pgm_token_string_t cmd_strat_infos_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_infos_result, arg0, str_strat_infos_arg0);
785 prog_char str_strat_infos_arg1[] = "show#reset";
786 parse_pgm_token_string_t cmd_strat_infos_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_infos_result, arg1, str_strat_infos_arg1);
788 prog_char help_strat_infos[] = "reset/show strat_infos";
789 parse_pgm_inst_t cmd_strat_infos = {
790 .f = cmd_strat_infos_parsed, /* function to call */
791 .data = NULL, /* 2nd arg of func */
792 .help_str = help_strat_infos,
793 .tokens = { /* token list, NULL terminated */
794 (prog_void *)&cmd_strat_infos_arg0,
795 (prog_void *)&cmd_strat_infos_arg1,
800 /**********************************************************/
801 /* strat configuration */
803 /* this structure is filled when cmd_strat_conf is parsed successfully */
804 struct cmd_strat_conf_result {
809 /* function called when cmd_strat_conf is parsed successfully */
810 static void cmd_strat_conf_parsed(void *parsed_result, void *data)
812 struct cmd_strat_conf_result *res = parsed_result;
814 if (!strcmp_P(res->arg1, PSTR("base"))) {
815 strat_infos.conf.flags = 0;
816 strat_infos.conf.scan_our_min_time = 90;
817 strat_infos.conf.delay_between_our_scan = 90;
818 strat_infos.conf.scan_opp_min_time = 90;
819 strat_infos.conf.delay_between_opp_scan = 90;
821 else if (!strcmp_P(res->arg1, PSTR("big3"))) {
822 strat_infos.conf.flags =
823 STRAT_CONF_STORE_STATIC2 |
824 STRAT_CONF_BIG_3_TEMPLE;
825 strat_infos.conf.scan_our_min_time = 90;
826 strat_infos.conf.delay_between_our_scan = 90;
827 strat_infos.conf.scan_opp_min_time = 90;
828 strat_infos.conf.delay_between_opp_scan = 90;
830 else if (!strcmp_P(res->arg1, PSTR("base_check"))) {
831 strat_infos.conf.flags = 0;
832 strat_infos.conf.scan_our_min_time = 35;
833 strat_infos.conf.delay_between_our_scan = 90;
834 strat_infos.conf.scan_opp_min_time = 90;
835 strat_infos.conf.delay_between_opp_scan = 90;
837 else if (!strcmp_P(res->arg1, PSTR("big3_check"))) {
838 strat_infos.conf.flags =
839 STRAT_CONF_STORE_STATIC2 |
840 STRAT_CONF_BIG_3_TEMPLE;
841 strat_infos.conf.scan_our_min_time = 35;
842 strat_infos.conf.delay_between_our_scan = 90;
843 strat_infos.conf.scan_opp_min_time = 90;
844 strat_infos.conf.delay_between_opp_scan = 90;
846 else if (!strcmp_P(res->arg1, PSTR("offensive_early"))) {
847 strat_infos.conf.flags =
848 STRAT_CONF_TAKE_ONE_LINTEL |
849 STRAT_CONF_STORE_STATIC2 |
850 STRAT_CONF_EARLY_SCAN |
851 STRAT_CONF_PUSH_OPP_COLS;
852 strat_infos.conf.scan_our_min_time = 50;
853 strat_infos.conf.delay_between_our_scan = 90;
854 strat_infos.conf.scan_opp_min_time = 15;
855 strat_infos.conf.delay_between_opp_scan = 90;
856 strat_infos.conf.wait_opponent = 5;
858 else if (!strcmp_P(res->arg1, PSTR("offensive_late"))) {
859 strat_infos.conf.flags = STRAT_CONF_TAKE_ONE_LINTEL;
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 = 30;
863 strat_infos.conf.delay_between_opp_scan = 90;
865 else if (!strcmp_P(res->arg1, PSTR("one_on_disc"))) {
866 strat_infos.conf.flags =
867 STRAT_CONF_ONLY_ONE_ON_DISC;
868 strat_infos.conf.scan_our_min_time = 90;
869 strat_infos.conf.delay_between_our_scan = 90;
870 strat_infos.conf.scan_opp_min_time = 90;
871 strat_infos.conf.delay_between_opp_scan = 90;
873 strat_infos.dump_enabled = 1;
877 prog_char str_strat_conf_arg0[] = "strat_conf";
878 parse_pgm_token_string_t cmd_strat_conf_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf_result, arg0, str_strat_conf_arg0);
879 prog_char str_strat_conf_arg1[] = "show#base#big3#base_check#big3_check#offensive_early#offensive_late#one_on_disc";
880 parse_pgm_token_string_t cmd_strat_conf_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf_result, arg1, str_strat_conf_arg1);
882 prog_char help_strat_conf[] = "configure strat options";
883 parse_pgm_inst_t cmd_strat_conf = {
884 .f = cmd_strat_conf_parsed, /* function to call */
885 .data = NULL, /* 2nd arg of func */
886 .help_str = help_strat_conf,
887 .tokens = { /* token list, NULL terminated */
888 (prog_void *)&cmd_strat_conf_arg0,
889 (prog_void *)&cmd_strat_conf_arg1,
894 /**********************************************************/
895 /* strat configuration */
897 /* this structure is filled when cmd_strat_conf2 is parsed successfully */
898 struct cmd_strat_conf2_result {
904 /* function called when cmd_strat_conf2 is parsed successfully */
905 static void cmd_strat_conf2_parsed(void *parsed_result, void *data)
907 struct cmd_strat_conf2_result *res = parsed_result;
910 if (!strcmp_P(res->arg2, PSTR("on")))
915 if (!strcmp_P(res->arg1, PSTR("one_temple_on_disc")))
916 bit = STRAT_CONF_ONLY_ONE_ON_DISC;
917 else if (!strcmp_P(res->arg1, PSTR("bypass_static2")))
918 bit = STRAT_CONF_BYPASS_STATIC2;
919 else if (!strcmp_P(res->arg1, PSTR("take_one_lintel")))
920 bit = STRAT_CONF_TAKE_ONE_LINTEL;
921 else if (!strcmp_P(res->arg1, PSTR("skip_when_check_fail")))
922 bit = STRAT_CONF_TAKE_ONE_LINTEL;
923 else if (!strcmp_P(res->arg1, PSTR("store_static2")))
924 bit = STRAT_CONF_STORE_STATIC2;
925 else if (!strcmp_P(res->arg1, PSTR("big3_temple")))
926 bit = STRAT_CONF_BIG_3_TEMPLE;
927 else if (!strcmp_P(res->arg1, PSTR("early_opp_scan")))
928 bit = STRAT_CONF_EARLY_SCAN;
929 else if (!strcmp_P(res->arg1, PSTR("push_opp_cols")))
930 bit = STRAT_CONF_PUSH_OPP_COLS;
933 strat_infos.conf.flags |= bit;
935 strat_infos.conf.flags &= (~bit);
937 strat_infos.dump_enabled = 1;
941 prog_char str_strat_conf2_arg0[] = "strat_conf";
942 parse_pgm_token_string_t cmd_strat_conf2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg0, str_strat_conf2_arg0);
943 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";
944 parse_pgm_token_string_t cmd_strat_conf2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg1, str_strat_conf2_arg1);
945 prog_char str_strat_conf2_arg2[] = "on#off";
946 parse_pgm_token_string_t cmd_strat_conf2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg2, str_strat_conf2_arg2);
949 prog_char help_strat_conf2[] = "configure strat options";
950 parse_pgm_inst_t cmd_strat_conf2 = {
951 .f = cmd_strat_conf2_parsed, /* function to call */
952 .data = NULL, /* 2nd arg of func */
953 .help_str = help_strat_conf2,
954 .tokens = { /* token list, NULL terminated */
955 (prog_void *)&cmd_strat_conf2_arg0,
956 (prog_void *)&cmd_strat_conf2_arg1,
957 (prog_void *)&cmd_strat_conf2_arg2,
962 /**********************************************************/
963 /* strat configuration */
965 /* this structure is filled when cmd_strat_conf3 is parsed successfully */
966 struct cmd_strat_conf3_result {
972 /* function called when cmd_strat_conf3 is parsed successfully */
973 static void cmd_strat_conf3_parsed(void *parsed_result, void *data)
975 struct cmd_strat_conf3_result *res = parsed_result;
977 if (!strcmp_P(res->arg1, PSTR("scan_opponent_min_time"))) {
980 strat_infos.conf.scan_opp_min_time = res->arg2;
982 else if (!strcmp_P(res->arg1, PSTR("delay_between_opponent_scan"))) {
985 strat_infos.conf.delay_between_opp_scan = res->arg2;
987 else if (!strcmp_P(res->arg1, PSTR("scan_our_min_time"))) {
990 strat_infos.conf.scan_our_min_time = res->arg2;
992 else if (!strcmp_P(res->arg1, PSTR("delay_between_our_scan"))) {
995 strat_infos.conf.delay_between_our_scan = res->arg2;
997 else if (!strcmp_P(res->arg1, PSTR("wait_opponent"))) {
998 strat_infos.conf.wait_opponent = res->arg2;
1000 else if (!strcmp_P(res->arg1, PSTR("lintel_min_time"))) {
1001 strat_infos.conf.lintel_min_time = res->arg2;
1003 strat_infos.dump_enabled = 1;
1007 prog_char str_strat_conf3_arg0[] = "strat_conf";
1008 parse_pgm_token_string_t cmd_strat_conf3_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf3_result, arg0, str_strat_conf3_arg0);
1009 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";
1010 parse_pgm_token_string_t cmd_strat_conf3_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf3_result, arg1, str_strat_conf3_arg1);
1011 parse_pgm_token_num_t cmd_strat_conf3_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_strat_conf3_result, arg2, UINT16);
1013 prog_char help_strat_conf3[] = "configure strat options";
1014 parse_pgm_inst_t cmd_strat_conf3 = {
1015 .f = cmd_strat_conf3_parsed, /* function to call */
1016 .data = NULL, /* 2nd arg of func */
1017 .help_str = help_strat_conf3,
1018 .tokens = { /* token list, NULL terminated */
1019 (prog_void *)&cmd_strat_conf3_arg0,
1020 (prog_void *)&cmd_strat_conf3_arg1,
1021 (prog_void *)&cmd_strat_conf3_arg2,
1026 /**********************************************************/
1027 /* strat configuration */
1029 /* this structure is filled when cmd_strat_conf4 is parsed successfully */
1030 struct cmd_strat_conf4_result {
1031 fixed_string_t arg0;
1032 fixed_string_t arg1;
1036 /* function called when cmd_strat_conf4 is parsed successfully */
1037 static void cmd_strat_conf4_parsed(void *parsed_result, void *data)
1039 struct cmd_strat_conf4_result *res = parsed_result;
1041 if (!strcmp_P(res->arg1, PSTR("scan_opponent_angle"))) {
1042 strat_infos.conf.scan_opp_angle = res->arg2;
1044 strat_infos.dump_enabled = 1;
1048 prog_char str_strat_conf4_arg0[] = "strat_conf";
1049 parse_pgm_token_string_t cmd_strat_conf4_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf4_result, arg0, str_strat_conf4_arg0);
1050 prog_char str_strat_conf4_arg1[] = "scan_opponent_angle";
1051 parse_pgm_token_string_t cmd_strat_conf4_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf4_result, arg1, str_strat_conf4_arg1);
1052 parse_pgm_token_num_t cmd_strat_conf4_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_strat_conf4_result, arg2, UINT16);
1054 prog_char help_strat_conf4[] = "configure strat options";
1055 parse_pgm_inst_t cmd_strat_conf4 = {
1056 .f = cmd_strat_conf4_parsed, /* function to call */
1057 .data = NULL, /* 2nd arg of func */
1058 .help_str = help_strat_conf4,
1059 .tokens = { /* token list, NULL terminated */
1060 (prog_void *)&cmd_strat_conf4_arg0,
1061 (prog_void *)&cmd_strat_conf4_arg1,
1062 (prog_void *)&cmd_strat_conf4_arg2,
1068 /**********************************************************/
1071 /* this structure is filled when cmd_subtraj is parsed successfully */
1072 struct cmd_subtraj_result {
1073 fixed_string_t arg0;
1074 fixed_string_t arg1;
1081 /* function called when cmd_subtraj is parsed successfully */
1082 static void cmd_subtraj_parsed(void *parsed_result, void *data)
1084 struct cmd_subtraj_result *res = parsed_result;
1086 struct column_dispenser *disp;
1088 if (strcmp_P(res->arg1, PSTR("static")) == 0) {
1089 err = strat_static_columns(res->arg2);
1091 else if (strcmp_P(res->arg1, PSTR("static2")) == 0) {
1092 strat_infos.s_cols.configuration = res->arg2;
1093 switch (res->arg2) {
1095 position_set(&mainboard.pos, 1398,
1096 COLOR_Y(1297), COLOR_A(-66));
1099 position_set(&mainboard.pos, 1232,
1100 COLOR_Y(1051), COLOR_A(4));
1103 position_set(&mainboard.pos, 1232,
1104 COLOR_Y(1043), COLOR_A(5));
1107 position_set(&mainboard.pos, 1346,
1108 COLOR_Y(852), COLOR_A(57));
1113 if (res->arg2 == 1 && res->arg3 == 1) {
1114 strat_infos.s_cols.flags = STATIC_COL_LINE1_DONE;
1116 if (res->arg2 == 1 && res->arg3 == 2) {
1117 strat_infos.s_cols.flags = STATIC_COL_LINE2_DONE;
1119 err = strat_static_columns_pass2();
1121 else if (strcmp_P(res->arg1, PSTR("lintel1")) == 0) {
1122 err = strat_goto_lintel_disp(&strat_infos.l1);
1124 else if (strcmp_P(res->arg1, PSTR("lintel2")) == 0) {
1125 err = strat_goto_lintel_disp(&strat_infos.l2);
1127 else if (strcmp_P(res->arg1, PSTR("coldisp1")) == 0) {
1128 disp = &strat_infos.c1;
1129 err = strat_goto_col_disp(&disp);
1131 else if (strcmp_P(res->arg1, PSTR("coldisp2")) == 0) {
1132 disp = &strat_infos.c2;
1133 err = strat_goto_col_disp(&disp);
1135 else if (strcmp_P(res->arg1, PSTR("coldisp3")) == 0) {
1136 disp = &strat_infos.c3;
1137 err = strat_goto_col_disp(&disp);
1139 else if (strcmp_P(res->arg1, PSTR("disc")) == 0) {
1140 if (res->arg2 == 0) {
1141 printf_P(PSTR("bad level\r\n"));
1144 err = strat_goto_disc(res->arg2);
1147 printf_P(PSTR("substrat returned %s\r\n"), get_err(err));
1148 trajectory_hardstop(&mainboard.traj);
1151 prog_char str_subtraj_arg0[] = "subtraj";
1152 parse_pgm_token_string_t cmd_subtraj_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_subtraj_result, arg0, str_subtraj_arg0);
1153 prog_char str_subtraj_arg1[] = "static#disc#lintel1#lintel2#coldisp1#coldisp2#coldisp3#static2";
1154 parse_pgm_token_string_t cmd_subtraj_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_subtraj_result, arg1, str_subtraj_arg1);
1155 parse_pgm_token_num_t cmd_subtraj_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg2, INT32);
1156 parse_pgm_token_num_t cmd_subtraj_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg3, INT32);
1157 parse_pgm_token_num_t cmd_subtraj_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg4, INT32);
1158 parse_pgm_token_num_t cmd_subtraj_arg5 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg5, INT32);
1160 prog_char help_subtraj[] = "Test sub-trajectories (a,b,c,d: specific params)";
1161 parse_pgm_inst_t cmd_subtraj = {
1162 .f = cmd_subtraj_parsed, /* function to call */
1163 .data = NULL, /* 2nd arg of func */
1164 .help_str = help_subtraj,
1165 .tokens = { /* token list, NULL terminated */
1166 (prog_void *)&cmd_subtraj_arg0,
1167 (prog_void *)&cmd_subtraj_arg1,
1168 (prog_void *)&cmd_subtraj_arg2,
1169 (prog_void *)&cmd_subtraj_arg3,
1170 (prog_void *)&cmd_subtraj_arg4,
1171 (prog_void *)&cmd_subtraj_arg5,