code 2010
[aversive.git] / projects / microb2010 / mainboard / commands_traj.c
1 /*
2  *  Copyright Droids Corporation (2009)
3  * 
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.
8  *
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.
13  *
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
17  *
18  *  Revision : $Id: commands_traj.c,v 1.8 2009-11-08 17:24:33 zer0 Exp $
19  *
20  *  Olivier MATZ <zer0@droids-corp.org> 
21  */
22
23 #include <stdio.h>
24 #include <string.h>
25
26 #include <aversive/pgmspace.h>
27 #include <aversive/wait.h>
28 #include <aversive/error.h>
29
30 #include <ax12.h>
31 #include <uart.h>
32 #include <pwm_ng.h>
33 #include <clock_time.h>
34 #include <spi.h>
35 #include <encoders_spi.h>
36
37 #include <pid.h>
38 #include <quadramp.h>
39 #include <control_system_manager.h>
40 #include <trajectory_manager.h>
41 #include <vect_base.h>
42 #include <lines.h>
43 #include <polygon.h>
44 #include <obstacle_avoidance.h>
45 #include <blocking_detection_manager.h>
46 #include <robot_system.h>
47 #include <position_manager.h>
48
49 #include <rdline.h>
50 #include <parse.h>
51 #include <parse_string.h>
52 #include <parse_num.h>
53
54 #include "main.h"
55 #include "cs.h"
56 #include "cmdline.h"
57 #include "strat_utils.h"
58 #include "strat_base.h"
59 #include "strat.h"
60 #include "../common/i2c_commands.h"
61 #include "i2c_protocol.h"
62
63 /**********************************************************/
64 /* Traj_Speeds for trajectory_manager */
65
66 /* this structure is filled when cmd_traj_speed is parsed successfully */
67 struct cmd_traj_speed_result {
68         fixed_string_t arg0;
69         fixed_string_t arg1;
70         uint16_t s;
71 };
72
73 /* function called when cmd_traj_speed is parsed successfully */
74 static void cmd_traj_speed_parsed(void *parsed_result, void *data)
75 {
76         struct cmd_traj_speed_result * res = parsed_result;
77         
78         if (!strcmp_P(res->arg1, PSTR("angle"))) {
79                 trajectory_set_speed(&mainboard.traj, mainboard.traj.d_speed, res->s);
80         }
81         else if (!strcmp_P(res->arg1, PSTR("distance"))) {
82                 trajectory_set_speed(&mainboard.traj, res->s, mainboard.traj.a_speed);
83         }
84         /* else it is a "show" */
85
86         printf_P(PSTR("angle %u, distance %u\r\n"), 
87                  mainboard.traj.a_speed,
88                  mainboard.traj.d_speed);
89 }
90
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);
96
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, 
106                 NULL,
107         },
108 };
109
110 /* show */
111
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);
114
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,
123                 NULL,
124         },
125 };
126
127 /**********************************************************/
128 /* circle coef configuration */
129
130 /* this structure is filled when cmd_circle_coef is parsed successfully */
131 struct cmd_circle_coef_result {
132         fixed_string_t arg0;
133         fixed_string_t arg1;
134         float circle_coef;
135 };
136
137
138 /* function called when cmd_circle_coef is parsed successfully */
139 static void cmd_circle_coef_parsed(void *parsed_result, void *data)
140 {
141         struct cmd_circle_coef_result *res = parsed_result;
142
143         if (!strcmp_P(res->arg1, PSTR("set"))) {
144                 trajectory_set_circle_coef(&mainboard.traj, res->circle_coef);
145         }
146
147         printf_P(PSTR("circle_coef %2.2f\r\n"), mainboard.traj.circle_coef);
148 }
149
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);
155
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,
165                 NULL,
166         },
167 };
168
169 /* show */
170
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);
173
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,
182                 NULL,
183         },
184 };
185
186 /**********************************************************/
187 /* trajectory window configuration */
188
189 /* this structure is filled when cmd_trajectory is parsed successfully */
190 struct cmd_trajectory_result {
191         fixed_string_t arg0;
192         fixed_string_t arg1;
193         float d_win;
194         float a_win;
195         float a_start;
196 };
197
198
199 /* function called when cmd_trajectory is parsed successfully */
200 static void cmd_trajectory_parsed(void * parsed_result, void * data)
201 {
202         struct cmd_trajectory_result * res = parsed_result;
203         
204         if (!strcmp_P(res->arg1, PSTR("set"))) {
205                 trajectory_set_windows(&mainboard.traj, res->d_win,
206                                        res->a_win, res->a_start);
207         }
208
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));
211 }
212
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);
220
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, 
232                 NULL,
233         },
234 };
235
236 /* show */
237
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);
240
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,
249                 NULL,
250         },
251 };
252
253 /**********************************************************/
254 /* rs_gains configuration */
255
256 /* this structure is filled when cmd_rs_gains is parsed successfully */
257 struct cmd_rs_gains_result {
258         fixed_string_t arg0;
259         fixed_string_t arg1;
260         float left;
261         float right;
262 };
263
264 /* function called when cmd_rs_gains is parsed successfully */
265 static void cmd_rs_gains_parsed(void * parsed_result, void * data)
266 {
267         struct cmd_rs_gains_result * res = parsed_result;
268
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
274         }
275         printf_P(PSTR("rs_gains set %2.2f %2.2f\r\n"),
276                  mainboard.rs.left_ext_gain, mainboard.rs.right_ext_gain);
277 }
278
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);
285
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, 
296                 NULL,
297         },
298 };
299
300 /* show */
301
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);
304
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,
313                 NULL,
314         },
315 };
316
317 /**********************************************************/
318 /* track configuration */
319
320 /* this structure is filled when cmd_track is parsed successfully */
321 struct cmd_track_result {
322         fixed_string_t arg0;
323         fixed_string_t arg1;
324         float val;
325 };
326
327 /* function called when cmd_track is parsed successfully */
328 static void cmd_track_parsed(void * parsed_result, void * data)
329 {
330         struct cmd_track_result * res = parsed_result;
331
332         if (!strcmp_P(res->arg1, PSTR("set"))) {
333                 position_set_physical_params(&mainboard.pos, res->val, DIST_IMP_MM);
334         }
335         printf_P(PSTR("track set %f\r\n"), mainboard.pos.phys.track_mm);
336 }
337
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);
343
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, 
353                 NULL,
354         },
355 };
356
357 /* show */
358
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);
361
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,
370                 NULL,
371         },
372 };
373
374
375
376 /**********************************************************/
377 /* Pt_Lists for testing traj */
378
379 #define PT_LIST_SIZE 10
380 static struct xy_point pt_list[PT_LIST_SIZE];
381 static uint16_t pt_list_len = 0;
382
383 /* this structure is filled when cmd_pt_list is parsed successfully */
384 struct cmd_pt_list_result {
385         fixed_string_t arg0;
386         fixed_string_t arg1;
387         uint16_t arg2;
388         int16_t arg3;
389         int16_t arg4;
390 };
391
392 /* function called when cmd_pt_list is parsed successfully */
393 static void cmd_pt_list_parsed(void * parsed_result, void * data)
394 {
395         struct cmd_pt_list_result * res = parsed_result;
396         uint8_t i, why=0;
397         
398         if (!strcmp_P(res->arg1, PSTR("avoid_start"))) {
399                 printf_P(PSTR("not implemented\r\n"));
400                 return;
401         }
402
403         if (!strcmp_P(res->arg1, PSTR("append"))) {
404                 res->arg2 = pt_list_len;
405         }
406
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"));
411                         return;
412                 }
413                 if (pt_list_len >= PT_LIST_SIZE) {
414                         printf_P(PSTR("List is too large\r\n"));
415                         return;
416                 }
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;
421                 pt_list_len++;
422         }
423         else if (!strcmp_P(res->arg1, PSTR("del"))) {
424                 if (pt_list_len <= 0) {
425                         printf_P(PSTR("Error: list empty\r\n"));
426                         return;
427                 }
428                 if (res->arg2 > pt_list_len) {
429                         printf_P(PSTR("Index too large\r\n"));
430                         return;
431                 }
432                 memmove(&pt_list[res->arg2], &pt_list[res->arg2+1], 
433                        (PT_LIST_SIZE-1-res->arg2)*sizeof(struct xy_point));
434                 pt_list_len--;
435         }
436         else if (!strcmp_P(res->arg1, PSTR("reset"))) {
437                 pt_list_len = 0;
438         }
439         
440         /* else it is a "show" or a "start" */
441         if (pt_list_len == 0) {
442                 printf_P(PSTR("List empty\r\n"));
443                 return;
444         }
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 */
450                 }
451 #if 0
452                 else if (!strcmp_P(res->arg1, PSTR("avoid_start"))) {
453                         while (1) {
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)
457                                         break;
458                         }
459                 }
460 #endif
461                 if (why & (~(END_TRAJ | END_NEAR)))
462                         trajectory_stop(&mainboard.traj);
463         }
464 }
465
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);
473
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, 
485                 NULL,
486         },
487 };
488
489 /* append */
490
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);
493
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, 
504                 NULL,
505         },
506 };
507
508 /* del */
509
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);
512
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,
522                 NULL,
523         },
524 };
525 /* show */
526
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);
529
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,
538                 NULL,
539         },
540 };
541
542
543
544 /**********************************************************/
545 /* Goto function */
546
547 /* this structure is filled when cmd_goto is parsed successfully */
548 struct cmd_goto_result {
549         fixed_string_t arg0;
550         fixed_string_t arg1;
551         int32_t arg2;
552         int32_t arg3;
553         int32_t arg4;
554 };
555
556 /* function called when cmd_goto is parsed successfully */
557 static void cmd_goto_parsed(void * parsed_result, void * data)
558 {
559         struct cmd_goto_result * res = parsed_result;
560         uint8_t err;
561         microseconds t1, t2;
562
563         interrupt_traj_reset();
564         if (!strcmp_P(res->arg1, PSTR("a_rel"))) {
565                 trajectory_a_rel(&mainboard.traj, res->arg2);
566         }
567         else if (!strcmp_P(res->arg1, PSTR("d_rel"))) {
568                 trajectory_d_rel(&mainboard.traj, res->arg2);
569         }
570         else if (!strcmp_P(res->arg1, PSTR("a_abs"))) {
571                 trajectory_a_abs(&mainboard.traj, res->arg2);
572         }
573         else if (!strcmp_P(res->arg1, PSTR("a_to_xy"))) {
574                 trajectory_turnto_xy(&mainboard.traj, res->arg2, res->arg3);
575         }
576         else if (!strcmp_P(res->arg1, PSTR("a_behind_xy"))) {
577                 trajectory_turnto_xy_behind(&mainboard.traj, res->arg2, res->arg3);
578         }
579         else if (!strcmp_P(res->arg1, PSTR("xy_rel"))) {
580                 trajectory_goto_xy_rel(&mainboard.traj, res->arg2, res->arg3);
581         }
582         else if (!strcmp_P(res->arg1, PSTR("xy_abs"))) {
583                 trajectory_goto_xy_abs(&mainboard.traj, res->arg2, res->arg3);
584         }
585         else if (!strcmp_P(res->arg1, PSTR("avoid"))) {
586 #if 0
587                 err = goto_and_avoid_forward(res->arg2, res->arg3, 0xFF, 0xFF);
588                 if (err != END_TRAJ && err != END_NEAR)
589                         strat_hardstop();
590 #else
591                 printf_P(PSTR("not implemented\r\n"));
592                 return;
593 #endif
594         }
595         else if (!strcmp_P(res->arg1, PSTR("avoid_bw"))) {
596 #if 0
597                 err = goto_and_avoid_backward(res->arg2, res->arg3, 0xFF, 0xFF);
598                 if (err != END_TRAJ && err != END_NEAR)
599                         strat_hardstop();
600 #else
601                 printf_P(PSTR("not implemented\r\n"));
602                 return;
603 #endif
604         }
605         else if (!strcmp_P(res->arg1, PSTR("xy_abs_fow"))) {
606                 trajectory_goto_forward_xy_abs(&mainboard.traj, res->arg2, res->arg3);
607         }
608         else if (!strcmp_P(res->arg1, PSTR("xy_abs_back"))) {
609                 trajectory_goto_backward_xy_abs(&mainboard.traj, res->arg2, res->arg3);
610         }
611         else if (!strcmp_P(res->arg1, PSTR("da_rel"))) {
612                 trajectory_d_a_rel(&mainboard.traj, res->arg2, res->arg3);
613         }
614         t1 = time_get_us2();
615         while ((err = test_traj_end(0xFF)) == 0) {
616                 t2 = time_get_us2();
617                 if (t2 - t1 > 200000) {
618                         dump_cs_debug("angle", &mainboard.angle.cs);
619                         dump_cs_debug("distance", &mainboard.distance.cs);
620                         t1 = t2;
621                 }
622         }
623         if (err != END_TRAJ && err != END_NEAR)
624                 strat_hardstop();
625         printf_P(PSTR("returned %s\r\n"), get_err(err));
626 }
627
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);
633
634 /* 1 params */
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, 
644                 NULL,
645         },
646 };
647
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);
651
652 /* 2 params */
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, 
663                 NULL,
664         },
665 };
666
667 /**********************************************************/
668 /* Position tests */
669
670 /* this structure is filled when cmd_position is parsed successfully */
671 struct cmd_position_result {
672         fixed_string_t arg0;
673         fixed_string_t arg1;
674         int32_t arg2;
675         int32_t arg3;
676         int32_t arg4;
677 };
678
679 #define AUTOPOS_SPEED_FAST 200
680 static void auto_position(void)
681 {
682         uint8_t err;
683         uint16_t old_spdd, old_spda;
684
685         interrupt_traj_reset();
686         strat_get_speed(&old_spdd, &old_spda);
687         strat_set_speed(AUTOPOS_SPEED_FAST, AUTOPOS_SPEED_FAST);
688
689         trajectory_d_rel(&mainboard.traj, -300);
690         err = wait_traj_end(END_INTR|END_TRAJ|END_BLOCKING);
691         if (err == END_INTR)
692                 goto intr;
693         wait_ms(100);
694         strat_reset_pos(ROBOT_LENGTH/2, 0, 0);
695
696         trajectory_d_rel(&mainboard.traj, 120);
697         err = wait_traj_end(END_INTR|END_TRAJ);
698         if (err == END_INTR)
699                 goto intr;
700
701         trajectory_a_rel(&mainboard.traj, COLOR_A(90));
702         err = wait_traj_end(END_INTR|END_TRAJ);
703         if (err == END_INTR)
704                 goto intr;
705
706         trajectory_d_rel(&mainboard.traj, -300);
707         err = wait_traj_end(END_INTR|END_TRAJ|END_BLOCKING);
708         if (err == END_INTR)
709                 goto intr;
710         wait_ms(100);
711         strat_reset_pos(DO_NOT_SET_POS, COLOR_Y(ROBOT_LENGTH/2),
712                         COLOR_A(90));
713
714         trajectory_d_rel(&mainboard.traj, 120);
715         err = wait_traj_end(END_INTR|END_TRAJ);
716         if (err == END_INTR)
717                 goto intr;
718         wait_ms(100);
719         
720         trajectory_a_rel(&mainboard.traj, COLOR_A(-40));
721         err = wait_traj_end(END_INTR|END_TRAJ);
722         if (err == END_INTR)
723                 goto intr;
724         wait_ms(100);
725         
726         strat_set_speed(old_spdd, old_spda);
727         return;
728
729 intr:
730         strat_hardstop();
731         strat_set_speed(old_spdd, old_spda);
732 }
733
734 /* function called when cmd_position is parsed successfully */
735 static void cmd_position_parsed(void * parsed_result, void * data)
736 {
737         struct cmd_position_result * res = parsed_result;
738         
739         /* display raw position values */
740         if (!strcmp_P(res->arg1, PSTR("reset"))) {
741                 position_set(&mainboard.pos, 0, 0, 0);
742         }
743         else if (!strcmp_P(res->arg1, PSTR("set"))) {
744                 position_set(&mainboard.pos, res->arg2, res->arg3, res->arg4);
745         }
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);
750                 auto_position();
751         }
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);
756                 auto_position();
757         }
758
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)));
764 }
765
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);
770
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, 
779                 NULL,
780         },
781 };
782
783
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);
789
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, 
801                 NULL,
802         },
803 };
804
805
806 /**********************************************************/
807 /* strat configuration */
808
809 /* this structure is filled when cmd_strat_infos is parsed successfully */
810 struct cmd_strat_infos_result {
811         fixed_string_t arg0;
812         fixed_string_t arg1;
813 };
814
815 /* function called when cmd_strat_infos is parsed successfully */
816 static void cmd_strat_infos_parsed(void *parsed_result, void *data)
817 {
818         struct cmd_strat_infos_result *res = parsed_result;
819
820         if (!strcmp_P(res->arg1, PSTR("reset"))) {
821                 strat_reset_infos();
822         }
823         strat_infos.dump_enabled = 1;
824         strat_dump_infos(__FUNCTION__);
825 }
826
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);
831
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, 
840                 NULL,
841         },
842 };
843
844 /**********************************************************/
845 /* strat configuration */
846
847 /* this structure is filled when cmd_strat_conf is parsed successfully */
848 struct cmd_strat_conf_result {
849         fixed_string_t arg0;
850         fixed_string_t arg1;
851 };
852
853 /* function called when cmd_strat_conf is parsed successfully */
854 static void cmd_strat_conf_parsed(void *parsed_result, void *data)
855 {
856         //      struct cmd_strat_conf_result *res = parsed_result;
857
858         strat_infos.dump_enabled = 1;
859         strat_dump_conf();
860 }
861
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);
866
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, 
875                 NULL,
876         },
877 };
878
879 /**********************************************************/
880 /* strat configuration */
881
882 /* this structure is filled when cmd_strat_conf2 is parsed successfully */
883 struct cmd_strat_conf2_result {
884         fixed_string_t arg0;
885         fixed_string_t arg1;
886         fixed_string_t arg2;
887 };
888
889 /* function called when cmd_strat_conf2 is parsed successfully */
890 static void cmd_strat_conf2_parsed(void *parsed_result, void *data)
891 {
892         struct cmd_strat_conf2_result *res = parsed_result;
893         uint8_t on, bit = 0;
894
895         if (!strcmp_P(res->arg2, PSTR("on")))
896                 on = 1;
897         else
898                 on = 0;
899         
900 #if 0
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;
917 #endif
918
919         if (on)
920                 strat_infos.conf.flags |= bit;
921         else
922                 strat_infos.conf.flags &= (~bit);
923
924         strat_infos.dump_enabled = 1;
925         strat_dump_conf();
926 }
927
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);
934
935
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, 
945                 NULL,
946         },
947 };
948
949 /**********************************************************/
950 /* strat configuration */
951
952 /* this structure is filled when cmd_strat_conf3 is parsed successfully */
953 struct cmd_strat_conf3_result {
954         fixed_string_t arg0;
955         fixed_string_t arg1;
956         uint8_t arg2;
957 };
958
959 /* function called when cmd_strat_conf3 is parsed successfully */
960 static void cmd_strat_conf3_parsed(void *parsed_result, void *data)
961 {
962 #if 0
963         struct cmd_strat_conf3_result *res = parsed_result;
964
965         if (!strcmp_P(res->arg1, PSTR("scan_opponent_min_time"))) {
966                 if (res->arg2 > 90)
967                         res->arg2 = 90;
968                 strat_infos.conf.scan_opp_min_time = res->arg2;
969         }
970         else if (!strcmp_P(res->arg1, PSTR("delay_between_opponent_scan"))) {
971                 if (res->arg2 > 90)
972                         res->arg2 = 90;
973                 strat_infos.conf.delay_between_opp_scan = res->arg2;
974         }
975         else if (!strcmp_P(res->arg1, PSTR("scan_our_min_time"))) {
976                 if (res->arg2 > 90)
977                         res->arg2 = 90;
978                 strat_infos.conf.scan_our_min_time = res->arg2;
979         }
980         else if (!strcmp_P(res->arg1, PSTR("delay_between_our_scan"))) {
981                 if (res->arg2 > 90)
982                         res->arg2 = 90;
983                 strat_infos.conf.delay_between_our_scan = res->arg2;
984         }
985         else if (!strcmp_P(res->arg1, PSTR("wait_opponent"))) {
986                 strat_infos.conf.wait_opponent = res->arg2;
987         }
988         else if (!strcmp_P(res->arg1, PSTR("lintel_min_time"))) {
989                 strat_infos.conf.lintel_min_time = res->arg2;
990         }
991 #endif
992         strat_infos.dump_enabled = 1;
993         strat_dump_conf();
994 }
995
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);
1001
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, 
1011                 NULL,
1012         },
1013 };
1014
1015
1016 /**********************************************************/
1017 /* Subtraj */
1018
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;
1023         int32_t arg2;
1024         int32_t arg3;
1025         int32_t arg4;
1026         int32_t arg5;
1027 };
1028
1029 /* function called when cmd_subtraj is parsed successfully */
1030 static void cmd_subtraj_parsed(void *parsed_result, void *data)
1031 {
1032 /*      struct cmd_subtraj_result *res = parsed_result; */
1033
1034         printf_P(PSTR("TODO\r\n"));
1035         trajectory_hardstop(&mainboard.traj);
1036 }
1037
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);
1046
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, 
1059                 NULL,
1060         },
1061 };