merge hostsim in main
[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 <hostsim.h>
27 #include <aversive/pgmspace.h>
28 #include <aversive/wait.h>
29 #include <aversive/error.h>
30
31 #include <ax12.h>
32 #include <uart.h>
33 #include <pwm_ng.h>
34 #include <clock_time.h>
35 #include <spi.h>
36 #include <encoders_spi.h>
37
38 #include <pid.h>
39 #include <quadramp.h>
40 #include <control_system_manager.h>
41 #include <trajectory_manager.h>
42 #include <vect_base.h>
43 #include <lines.h>
44 #include <polygon.h>
45 #include <obstacle_avoidance.h>
46 #include <blocking_detection_manager.h>
47 #include <robot_system.h>
48 #include <position_manager.h>
49
50 #include <rdline.h>
51 #include <parse.h>
52 #include <parse_string.h>
53 #include <parse_num.h>
54
55 #include "main.h"
56 #include "cs.h"
57 #include "cmdline.h"
58 #include "strat_utils.h"
59 #include "strat_base.h"
60 #include "strat.h"
61 #include "../common/i2c_commands.h"
62 #include "i2c_protocol.h"
63
64 /**********************************************************/
65 /* Traj_Speeds for trajectory_manager */
66
67 /* this structure is filled when cmd_traj_speed is parsed successfully */
68 struct cmd_traj_speed_result {
69         fixed_string_t arg0;
70         fixed_string_t arg1;
71         uint16_t s;
72 };
73
74 /* function called when cmd_traj_speed is parsed successfully */
75 static void cmd_traj_speed_parsed(void *parsed_result, void *data)
76 {
77         struct cmd_traj_speed_result * res = parsed_result;
78         
79         if (!strcmp_P(res->arg1, PSTR("angle"))) {
80                 trajectory_set_speed(&mainboard.traj, mainboard.traj.d_speed, res->s);
81         }
82         else if (!strcmp_P(res->arg1, PSTR("distance"))) {
83                 trajectory_set_speed(&mainboard.traj, res->s, mainboard.traj.a_speed);
84         }
85         /* else it is a "show" */
86
87         printf_P(PSTR("angle %u, distance %u\r\n"), 
88                  mainboard.traj.a_speed,
89                  mainboard.traj.d_speed);
90 }
91
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);
97
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, 
107                 NULL,
108         },
109 };
110
111 /* show */
112
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);
115
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,
124                 NULL,
125         },
126 };
127
128 /**********************************************************/
129 /* circle coef configuration */
130
131 /* this structure is filled when cmd_circle_coef is parsed successfully */
132 struct cmd_circle_coef_result {
133         fixed_string_t arg0;
134         fixed_string_t arg1;
135         float circle_coef;
136 };
137
138
139 /* function called when cmd_circle_coef is parsed successfully */
140 static void cmd_circle_coef_parsed(void *parsed_result, void *data)
141 {
142         struct cmd_circle_coef_result *res = parsed_result;
143
144         if (!strcmp_P(res->arg1, PSTR("set"))) {
145                 trajectory_set_circle_coef(&mainboard.traj, res->circle_coef);
146         }
147
148         printf_P(PSTR("circle_coef set %2.2f\r\n"), mainboard.traj.circle_coef);
149 }
150
151 prog_char str_circle_coef_arg0[] = "circle_coef";
152 parse_pgm_token_string_t cmd_circle_coef_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_circle_coef_result, arg0, str_circle_coef_arg0);
153 prog_char str_circle_coef_arg1[] = "set";
154 parse_pgm_token_string_t cmd_circle_coef_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_circle_coef_result, arg1, str_circle_coef_arg1);
155 parse_pgm_token_num_t cmd_circle_coef_val = TOKEN_NUM_INITIALIZER(struct cmd_circle_coef_result, circle_coef, FLOAT);
156
157 prog_char help_circle_coef[] = "Set circle coef";
158 parse_pgm_inst_t cmd_circle_coef = {
159         .f = cmd_circle_coef_parsed,  /* function to call */
160         .data = NULL,      /* 2nd arg of func */
161         .help_str = help_circle_coef,
162         .tokens = {        /* token list, NULL terminated */
163                 (prog_void *)&cmd_circle_coef_arg0,
164                 (prog_void *)&cmd_circle_coef_arg1,
165                 (prog_void *)&cmd_circle_coef_val,
166                 NULL,
167         },
168 };
169
170 /* show */
171
172 prog_char str_circle_coef_show_arg[] = "show";
173 parse_pgm_token_string_t cmd_circle_coef_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_circle_coef_result, arg1, str_circle_coef_show_arg);
174
175 prog_char help_circle_coef_show[] = "Show circle coef";
176 parse_pgm_inst_t cmd_circle_coef_show = {
177         .f = cmd_circle_coef_parsed,  /* function to call */
178         .data = NULL,      /* 2nd arg of func */
179         .help_str = help_circle_coef_show,
180         .tokens = {        /* token list, NULL terminated */
181                 (prog_void *)&cmd_circle_coef_arg0, 
182                 (prog_void *)&cmd_circle_coef_show_arg,
183                 NULL,
184         },
185 };
186
187 /**********************************************************/
188 /* trajectory window configuration */
189
190 /* this structure is filled when cmd_trajectory is parsed successfully */
191 struct cmd_trajectory_result {
192         fixed_string_t arg0;
193         fixed_string_t arg1;
194         float d_win;
195         float a_win;
196         float a_start;
197 };
198
199
200 /* function called when cmd_trajectory is parsed successfully */
201 static void cmd_trajectory_parsed(void * parsed_result, void * data)
202 {
203         struct cmd_trajectory_result * res = parsed_result;
204         
205         if (!strcmp_P(res->arg1, PSTR("set"))) {
206                 trajectory_set_windows(&mainboard.traj, res->d_win,
207                                        res->a_win, res->a_start);
208         }
209
210         printf_P(PSTR("trajectory %2.2f %2.2f %2.2f\r\n"), mainboard.traj.d_win,
211                  DEG(mainboard.traj.a_win_rad), DEG(mainboard.traj.a_start_rad));
212 }
213
214 prog_char str_trajectory_arg0[] = "trajectory";
215 parse_pgm_token_string_t cmd_trajectory_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_trajectory_result, arg0, str_trajectory_arg0);
216 prog_char str_trajectory_arg1[] = "set";
217 parse_pgm_token_string_t cmd_trajectory_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_trajectory_result, arg1, str_trajectory_arg1);
218 parse_pgm_token_num_t cmd_trajectory_d = TOKEN_NUM_INITIALIZER(struct cmd_trajectory_result, d_win, FLOAT);
219 parse_pgm_token_num_t cmd_trajectory_a = TOKEN_NUM_INITIALIZER(struct cmd_trajectory_result, a_win, FLOAT);
220 parse_pgm_token_num_t cmd_trajectory_as = TOKEN_NUM_INITIALIZER(struct cmd_trajectory_result, a_start, FLOAT);
221
222 prog_char help_trajectory[] = "Set trajectory windows (distance, angle, angle_start)";
223 parse_pgm_inst_t cmd_trajectory = {
224         .f = cmd_trajectory_parsed,  /* function to call */
225         .data = NULL,      /* 2nd arg of func */
226         .help_str = help_trajectory,
227         .tokens = {        /* token list, NULL terminated */
228                 (prog_void *)&cmd_trajectory_arg0, 
229                 (prog_void *)&cmd_trajectory_arg1, 
230                 (prog_void *)&cmd_trajectory_d, 
231                 (prog_void *)&cmd_trajectory_a, 
232                 (prog_void *)&cmd_trajectory_as, 
233                 NULL,
234         },
235 };
236
237 /* show */
238
239 prog_char str_trajectory_show_arg[] = "show";
240 parse_pgm_token_string_t cmd_trajectory_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_trajectory_result, arg1, str_trajectory_show_arg);
241
242 prog_char help_trajectory_show[] = "Show trajectory window configuration";
243 parse_pgm_inst_t cmd_trajectory_show = {
244         .f = cmd_trajectory_parsed,  /* function to call */
245         .data = NULL,      /* 2nd arg of func */
246         .help_str = help_trajectory_show,
247         .tokens = {        /* token list, NULL terminated */
248                 (prog_void *)&cmd_trajectory_arg0, 
249                 (prog_void *)&cmd_trajectory_show_arg,
250                 NULL,
251         },
252 };
253
254 /**********************************************************/
255 /* rs_gains configuration */
256
257 /* this structure is filled when cmd_rs_gains is parsed successfully */
258 struct cmd_rs_gains_result {
259         fixed_string_t arg0;
260         fixed_string_t arg1;
261         float left;
262         float right;
263 };
264
265 /* function called when cmd_rs_gains is parsed successfully */
266 static void cmd_rs_gains_parsed(void * parsed_result, void * data)
267 {
268 #ifdef HOST_VERSION
269         printf("not implemented\n");
270 #else
271         struct cmd_rs_gains_result * res = parsed_result;
272
273         if (!strcmp_P(res->arg1, PSTR("set"))) {
274                 rs_set_left_ext_encoder(&mainboard.rs, encoders_spi_get_value, 
275                                         LEFT_ENCODER, res->left); // en augmentant on tourne à gauche
276                 rs_set_right_ext_encoder(&mainboard.rs, encoders_spi_get_value, 
277                                          RIGHT_ENCODER, res->right); //en augmentant on tourne à droite
278         }
279         printf_P(PSTR("rs_gains set %2.2f %2.2f\r\n"),
280                  mainboard.rs.left_ext_gain, mainboard.rs.right_ext_gain);
281 #endif
282 }
283
284 prog_char str_rs_gains_arg0[] = "rs_gains";
285 parse_pgm_token_string_t cmd_rs_gains_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_rs_gains_result, arg0, str_rs_gains_arg0);
286 prog_char str_rs_gains_arg1[] = "set";
287 parse_pgm_token_string_t cmd_rs_gains_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_rs_gains_result, arg1, str_rs_gains_arg1);
288 parse_pgm_token_num_t cmd_rs_gains_l = TOKEN_NUM_INITIALIZER(struct cmd_rs_gains_result, left, FLOAT);
289 parse_pgm_token_num_t cmd_rs_gains_r = TOKEN_NUM_INITIALIZER(struct cmd_rs_gains_result, right, FLOAT);
290
291 prog_char help_rs_gains[] = "Set rs_gains (left, right)";
292 parse_pgm_inst_t cmd_rs_gains = {
293         .f = cmd_rs_gains_parsed,  /* function to call */
294         .data = NULL,      /* 2nd arg of func */
295         .help_str = help_rs_gains,
296         .tokens = {        /* token list, NULL terminated */
297                 (prog_void *)&cmd_rs_gains_arg0, 
298                 (prog_void *)&cmd_rs_gains_arg1, 
299                 (prog_void *)&cmd_rs_gains_l, 
300                 (prog_void *)&cmd_rs_gains_r, 
301                 NULL,
302         },
303 };
304
305 /* show */
306
307 prog_char str_rs_gains_show_arg[] = "show";
308 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
310 prog_char help_rs_gains_show[] = "Show rs_gains";
311 parse_pgm_inst_t cmd_rs_gains_show = {
312         .f = cmd_rs_gains_parsed,  /* function to call */
313         .data = NULL,      /* 2nd arg of func */
314         .help_str = help_rs_gains_show,
315         .tokens = {        /* token list, NULL terminated */
316                 (prog_void *)&cmd_rs_gains_arg0, 
317                 (prog_void *)&cmd_rs_gains_show_arg,
318                 NULL,
319         },
320 };
321
322 /**********************************************************/
323 /* track configuration */
324
325 /* this structure is filled when cmd_track is parsed successfully */
326 struct cmd_track_result {
327         fixed_string_t arg0;
328         fixed_string_t arg1;
329         float val;
330 };
331
332 /* function called when cmd_track is parsed successfully */
333 static void cmd_track_parsed(void * parsed_result, void * data)
334 {
335         struct cmd_track_result * res = parsed_result;
336
337         if (!strcmp_P(res->arg1, PSTR("set"))) {
338                 position_set_physical_params(&mainboard.pos, res->val, DIST_IMP_MM);
339         }
340         printf_P(PSTR("track set %f\r\n"), mainboard.pos.phys.track_mm);
341 }
342
343 prog_char str_track_arg0[] = "track";
344 parse_pgm_token_string_t cmd_track_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_track_result, arg0, str_track_arg0);
345 prog_char str_track_arg1[] = "set";
346 parse_pgm_token_string_t cmd_track_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_track_result, arg1, str_track_arg1);
347 parse_pgm_token_num_t cmd_track_val = TOKEN_NUM_INITIALIZER(struct cmd_track_result, val, FLOAT);
348
349 prog_char help_track[] = "Set track in mm";
350 parse_pgm_inst_t cmd_track = {
351         .f = cmd_track_parsed,  /* function to call */
352         .data = NULL,      /* 2nd arg of func */
353         .help_str = help_track,
354         .tokens = {        /* token list, NULL terminated */
355                 (prog_void *)&cmd_track_arg0, 
356                 (prog_void *)&cmd_track_arg1, 
357                 (prog_void *)&cmd_track_val, 
358                 NULL,
359         },
360 };
361
362 /* show */
363
364 prog_char str_track_show_arg[] = "show";
365 parse_pgm_token_string_t cmd_track_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_track_result, arg1, str_track_show_arg);
366
367 prog_char help_track_show[] = "Show track";
368 parse_pgm_inst_t cmd_track_show = {
369         .f = cmd_track_parsed,  /* function to call */
370         .data = NULL,      /* 2nd arg of func */
371         .help_str = help_track_show,
372         .tokens = {        /* token list, NULL terminated */
373                 (prog_void *)&cmd_track_arg0, 
374                 (prog_void *)&cmd_track_show_arg,
375                 NULL,
376         },
377 };
378
379
380
381 /**********************************************************/
382 /* Pt_Lists for testing traj */
383
384 #define PT_LIST_SIZE 10
385 static struct xy_point pt_list[PT_LIST_SIZE];
386 static uint16_t pt_list_len = 0;
387
388 /* this structure is filled when cmd_pt_list is parsed successfully */
389 struct cmd_pt_list_result {
390         fixed_string_t arg0;
391         fixed_string_t arg1;
392         uint16_t arg2;
393         int16_t arg3;
394         int16_t arg4;
395 };
396
397 /* function called when cmd_pt_list is parsed successfully */
398 static void cmd_pt_list_parsed(void * parsed_result, void * data)
399 {
400         struct cmd_pt_list_result * res = parsed_result;
401         uint8_t i, why=0;
402         
403         if (!strcmp_P(res->arg1, PSTR("append"))) {
404                 res->arg2 = pt_list_len;
405         }
406         if (!strcmp_P(res->arg1, PSTR("avoid_start"))) {
407                 printf_P(PSTR("removed\r\n"));
408                 return;
409         }
410
411         if (!strcmp_P(res->arg1, PSTR("insert")) ||
412             !strcmp_P(res->arg1, PSTR("append"))) {
413                 if (res->arg2 > pt_list_len) {
414                         printf_P(PSTR("Index too large\r\n"));
415                         return;
416                 }
417                 if (pt_list_len >= PT_LIST_SIZE) {
418                         printf_P(PSTR("List is too large\r\n"));
419                         return;
420                 }
421                 memmove(&pt_list[res->arg2+1], &pt_list[res->arg2], 
422                        PT_LIST_SIZE-1-res->arg2);
423                 pt_list[res->arg2].x = res->arg3;
424                 pt_list[res->arg2].y = res->arg4;
425                 pt_list_len++;
426         }
427         else if (!strcmp_P(res->arg1, PSTR("del"))) {
428                 if (pt_list_len <= 0) {
429                         printf_P(PSTR("Error: list empty\r\n"));
430                         return;
431                 }
432                 if (res->arg2 > pt_list_len) {
433                         printf_P(PSTR("Index too large\r\n"));
434                         return;
435                 }
436                 memmove(&pt_list[res->arg2], &pt_list[res->arg2+1], 
437                        (PT_LIST_SIZE-1-res->arg2)*sizeof(struct xy_point));
438                 pt_list_len--;
439         }
440         else if (!strcmp_P(res->arg1, PSTR("reset"))) {
441                 pt_list_len = 0;
442         }
443         
444         /* else it is a "show" or a "start" */
445         if (pt_list_len == 0) {
446                 printf_P(PSTR("List empty\r\n"));
447                 return;
448         }
449  restart:
450         for (i=0 ; i<pt_list_len ; i++) {
451                 printf_P(PSTR("%d: x=%d y=%d\r\n"), i, pt_list[i].x, pt_list[i].y);
452                 if (!strcmp_P(res->arg1, PSTR("start"))) {
453                         trajectory_goto_xy_abs(&mainboard.traj, pt_list[i].x, pt_list[i].y);
454                         why = wait_traj_end(0xFF); /* all */
455                 }
456                 else if (!strcmp_P(res->arg1, PSTR("loop_start"))) {
457                         trajectory_goto_xy_abs(&mainboard.traj, pt_list[i].x, pt_list[i].y);
458                         why = wait_traj_end(0xFF); /* all */
459                 }
460 #if 0
461                 else if (!strcmp_P(res->arg1, PSTR("avoid_start"))) {
462                         while (1) {
463                                 why = goto_and_avoid(pt_list[i].x, pt_list[i].y, 0xFF, 0xFF);
464                                 printf("next point\r\n");
465                                 if (why != END_OBSTACLE)
466                                         break;
467                         }
468                 }
469 #endif
470                 if (why & (~(END_TRAJ | END_NEAR)))
471                         trajectory_stop(&mainboard.traj);
472                 if (why & END_INTR)
473                         break;
474         }
475         if (why & END_INTR)
476                 return;
477         if (!strcmp_P(res->arg1, PSTR("loop_start")))
478                 goto restart;
479 }
480
481 prog_char str_pt_list_arg0[] = "pt_list";
482 parse_pgm_token_string_t cmd_pt_list_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg0, str_pt_list_arg0);
483 prog_char str_pt_list_arg1[] = "insert";
484 parse_pgm_token_string_t cmd_pt_list_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_arg1);
485 parse_pgm_token_num_t cmd_pt_list_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_pt_list_result, arg2, UINT16);
486 parse_pgm_token_num_t cmd_pt_list_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_pt_list_result, arg3, INT16);
487 parse_pgm_token_num_t cmd_pt_list_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_pt_list_result, arg4, INT16);
488
489 prog_char help_pt_list[] = "Insert point in pt_list (idx,x,y)";
490 parse_pgm_inst_t cmd_pt_list = {
491         .f = cmd_pt_list_parsed,  /* function to call */
492         .data = NULL,      /* 2nd arg of func */
493         .help_str = help_pt_list,
494         .tokens = {        /* token list, NULL terminated */
495                 (prog_void *)&cmd_pt_list_arg0, 
496                 (prog_void *)&cmd_pt_list_arg1, 
497                 (prog_void *)&cmd_pt_list_arg2, 
498                 (prog_void *)&cmd_pt_list_arg3, 
499                 (prog_void *)&cmd_pt_list_arg4, 
500                 NULL,
501         },
502 };
503
504 /* append */
505
506 prog_char str_pt_list_arg1_append[] = "append";
507 parse_pgm_token_string_t cmd_pt_list_arg1_append = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_arg1_append);
508
509 prog_char help_pt_list_append[] = "Append point in pt_list (x,y)";
510 parse_pgm_inst_t cmd_pt_list_append = {
511         .f = cmd_pt_list_parsed,  /* function to call */
512         .data = NULL,      /* 2nd arg of func */
513         .help_str = help_pt_list_append,
514         .tokens = {        /* token list, NULL terminated */
515                 (prog_void *)&cmd_pt_list_arg0, 
516                 (prog_void *)&cmd_pt_list_arg1_append, 
517                 (prog_void *)&cmd_pt_list_arg3, 
518                 (prog_void *)&cmd_pt_list_arg4, 
519                 NULL,
520         },
521 };
522
523 /* del */
524
525 prog_char str_pt_list_del_arg[] = "del";
526 parse_pgm_token_string_t cmd_pt_list_del_arg = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_del_arg);
527
528 prog_char help_pt_list_del[] = "Del or insert point in pt_list (num)";
529 parse_pgm_inst_t cmd_pt_list_del = {
530         .f = cmd_pt_list_parsed,  /* function to call */
531         .data = NULL,      /* 2nd arg of func */
532         .help_str = help_pt_list_del,
533         .tokens = {        /* token list, NULL terminated */
534                 (prog_void *)&cmd_pt_list_arg0, 
535                 (prog_void *)&cmd_pt_list_del_arg, 
536                 (prog_void *)&cmd_pt_list_arg2,
537                 NULL,
538         },
539 };
540 /* show */
541
542 prog_char str_pt_list_show_arg[] = "show#reset#start#avoid_start#loop_start";
543 parse_pgm_token_string_t cmd_pt_list_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_show_arg);
544
545 prog_char help_pt_list_show[] = "Show, start or reset pt_list";
546 parse_pgm_inst_t cmd_pt_list_show = {
547         .f = cmd_pt_list_parsed,  /* function to call */
548         .data = NULL,      /* 2nd arg of func */
549         .help_str = help_pt_list_show,
550         .tokens = {        /* token list, NULL terminated */
551                 (prog_void *)&cmd_pt_list_arg0, 
552                 (prog_void *)&cmd_pt_list_show_arg,
553                 NULL,
554         },
555 };
556
557
558
559 /**********************************************************/
560 /* Goto function */
561
562 /* this structure is filled when cmd_goto is parsed successfully */
563 struct cmd_goto_result {
564         fixed_string_t arg0;
565         fixed_string_t arg1;
566         int32_t arg2;
567         int32_t arg3;
568         int32_t arg4;
569 };
570
571 /* function called when cmd_goto is parsed successfully */
572 static void cmd_goto_parsed(void * parsed_result, void * data)
573 {
574         struct cmd_goto_result * res = parsed_result;
575         uint8_t err;
576         microseconds t1, t2;
577
578         interrupt_traj_reset();
579         if (!strcmp_P(res->arg1, PSTR("a_rel"))) {
580                 trajectory_a_rel(&mainboard.traj, res->arg2);
581         }
582         else if (!strcmp_P(res->arg1, PSTR("d_rel"))) {
583                 trajectory_d_rel(&mainboard.traj, res->arg2);
584         }
585         else if (!strcmp_P(res->arg1, PSTR("a_abs"))) {
586                 trajectory_a_abs(&mainboard.traj, res->arg2);
587         }
588         else if (!strcmp_P(res->arg1, PSTR("a_to_xy"))) {
589                 trajectory_turnto_xy(&mainboard.traj, res->arg2, res->arg3);
590         }
591         else if (!strcmp_P(res->arg1, PSTR("a_behind_xy"))) {
592                 trajectory_turnto_xy_behind(&mainboard.traj, res->arg2, res->arg3);
593         }
594         else if (!strcmp_P(res->arg1, PSTR("xy_rel"))) {
595                 trajectory_goto_xy_rel(&mainboard.traj, res->arg2, res->arg3);
596         }
597         else if (!strcmp_P(res->arg1, PSTR("xy_abs"))) {
598                 trajectory_goto_xy_abs(&mainboard.traj, res->arg2, res->arg3);
599         }
600         else if (!strcmp_P(res->arg1, PSTR("avoid"))) {
601 #if 0
602                 err = goto_and_avoid_forward(res->arg2, res->arg3, 0xFF, 0xFF);
603                 if (err != END_TRAJ && err != END_NEAR)
604                         strat_hardstop();
605 #else
606                 printf_P(PSTR("not implemented\r\n"));
607                 return;
608 #endif
609         }
610         else if (!strcmp_P(res->arg1, PSTR("avoid_bw"))) {
611 #if 0
612                 err = goto_and_avoid_backward(res->arg2, res->arg3, 0xFF, 0xFF);
613                 if (err != END_TRAJ && err != END_NEAR)
614                         strat_hardstop();
615 #else
616                 printf_P(PSTR("not implemented\r\n"));
617                 return;
618 #endif
619         }
620         else if (!strcmp_P(res->arg1, PSTR("xy_abs_fow"))) {
621                 trajectory_goto_forward_xy_abs(&mainboard.traj, res->arg2, res->arg3);
622         }
623         else if (!strcmp_P(res->arg1, PSTR("xy_abs_back"))) {
624                 trajectory_goto_backward_xy_abs(&mainboard.traj, res->arg2, res->arg3);
625         }
626         else if (!strcmp_P(res->arg1, PSTR("da_rel"))) {
627                 trajectory_d_a_rel(&mainboard.traj, res->arg2, res->arg3);
628         }
629         t1 = time_get_us2();
630         while ((err = test_traj_end(0xFF)) == 0) {
631                 t2 = time_get_us2();
632                 if (t2 - t1 > 200000) {
633                         dump_cs_debug("angle", &mainboard.angle.cs);
634                         dump_cs_debug("distance", &mainboard.distance.cs);
635                         t1 = t2;
636                 }
637         }
638         if (err != END_TRAJ && err != END_NEAR)
639                 strat_hardstop();
640         printf_P(PSTR("returned %s\r\n"), get_err(err));
641 }
642
643 prog_char str_goto_arg0[] = "goto";
644 parse_pgm_token_string_t cmd_goto_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_goto_result, arg0, str_goto_arg0);
645 prog_char str_goto_arg1_a[] = "d_rel#a_rel#a_abs";
646 parse_pgm_token_string_t cmd_goto_arg1_a = TOKEN_STRING_INITIALIZER(struct cmd_goto_result, arg1, str_goto_arg1_a);
647 parse_pgm_token_num_t cmd_goto_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_goto_result, arg2, INT32);
648
649 /* 1 params */
650 prog_char help_goto1[] = "Change orientation of the mainboard";
651 parse_pgm_inst_t cmd_goto1 = {
652         .f = cmd_goto_parsed,  /* function to call */
653         .data = NULL,      /* 2nd arg of func */
654         .help_str = help_goto1,
655         .tokens = {        /* token list, NULL terminated */
656                 (prog_void *)&cmd_goto_arg0, 
657                 (prog_void *)&cmd_goto_arg1_a, 
658                 (prog_void *)&cmd_goto_arg2, 
659                 NULL,
660         },
661 };
662
663 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";
664 parse_pgm_token_string_t cmd_goto_arg1_b = TOKEN_STRING_INITIALIZER(struct cmd_goto_result, arg1, str_goto_arg1_b);
665 parse_pgm_token_num_t cmd_goto_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_goto_result, arg3, INT32);
666
667 /* 2 params */
668 prog_char help_goto2[] = "Go to a (x,y) or (d,a) position";
669 parse_pgm_inst_t cmd_goto2 = {
670         .f = cmd_goto_parsed,  /* function to call */
671         .data = NULL,      /* 2nd arg of func */
672         .help_str = help_goto2,
673         .tokens = {        /* token list, NULL terminated */
674                 (prog_void *)&cmd_goto_arg0, 
675                 (prog_void *)&cmd_goto_arg1_b, 
676                 (prog_void *)&cmd_goto_arg2,
677                 (prog_void *)&cmd_goto_arg3, 
678                 NULL,
679         },
680 };
681
682 /**********************************************************/
683 /* Position tests */
684
685 /* this structure is filled when cmd_position is parsed successfully */
686 struct cmd_position_result {
687         fixed_string_t arg0;
688         fixed_string_t arg1;
689         int32_t arg2;
690         int32_t arg3;
691         int32_t arg4;
692 };
693
694 #define AUTOPOS_SPEED_FAST 200
695 static void auto_position(void)
696 {
697         uint8_t err;
698         uint16_t old_spdd, old_spda;
699
700         interrupt_traj_reset();
701         strat_get_speed(&old_spdd, &old_spda);
702         strat_set_speed(AUTOPOS_SPEED_FAST, AUTOPOS_SPEED_FAST);
703
704         trajectory_d_rel(&mainboard.traj, -300);
705         err = wait_traj_end(END_INTR|END_TRAJ|END_BLOCKING);
706         if (err == END_INTR)
707                 goto intr;
708         wait_ms(100);
709         strat_reset_pos(ROBOT_LENGTH/2, 0, 0);
710
711         trajectory_d_rel(&mainboard.traj, 120);
712         err = wait_traj_end(END_INTR|END_TRAJ);
713         if (err == END_INTR)
714                 goto intr;
715
716         trajectory_a_rel(&mainboard.traj, COLOR_A(90));
717         err = wait_traj_end(END_INTR|END_TRAJ);
718         if (err == END_INTR)
719                 goto intr;
720
721         trajectory_d_rel(&mainboard.traj, -300);
722         err = wait_traj_end(END_INTR|END_TRAJ|END_BLOCKING);
723         if (err == END_INTR)
724                 goto intr;
725         wait_ms(100);
726         strat_reset_pos(DO_NOT_SET_POS, COLOR_Y(ROBOT_LENGTH/2),
727                         COLOR_A(90));
728
729         trajectory_d_rel(&mainboard.traj, 120);
730         err = wait_traj_end(END_INTR|END_TRAJ);
731         if (err == END_INTR)
732                 goto intr;
733         wait_ms(100);
734         
735         trajectory_a_rel(&mainboard.traj, COLOR_A(-40));
736         err = wait_traj_end(END_INTR|END_TRAJ);
737         if (err == END_INTR)
738                 goto intr;
739         wait_ms(100);
740         
741         strat_set_speed(old_spdd, old_spda);
742         return;
743
744 intr:
745         strat_hardstop();
746         strat_set_speed(old_spdd, old_spda);
747 }
748
749 /* function called when cmd_position is parsed successfully */
750 static void cmd_position_parsed(void * parsed_result, void * data)
751 {
752         struct cmd_position_result * res = parsed_result;
753         
754         /* display raw position values */
755         if (!strcmp_P(res->arg1, PSTR("reset"))) {
756                 position_set(&mainboard.pos, 0, 0, 0);
757         }
758         else if (!strcmp_P(res->arg1, PSTR("set"))) {
759                 position_set(&mainboard.pos, res->arg2, res->arg3, res->arg4);
760         }
761         else if (!strcmp_P(res->arg1, PSTR("autoset_blue"))) {
762                 mainboard.our_color = I2C_COLOR_BLUE;
763 #ifndef HOST_VERSION
764                 i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_BLUE);
765                 i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_BLUE);
766 #endif
767                 auto_position();
768         }
769         else if (!strcmp_P(res->arg1, PSTR("autoset_red"))) {
770                 mainboard.our_color = I2C_COLOR_YELLOW;
771 #ifndef HOST_VERSION
772                 i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_YELLOW);
773                 i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_YELLOW);
774 #endif
775                 auto_position();
776         }
777
778         /* else it's just a "show" */
779         printf_P(PSTR("x=%.2f y=%.2f a=%.2f\r\n"), 
780                  position_get_x_double(&mainboard.pos),
781                  position_get_y_double(&mainboard.pos),
782                  DEG(position_get_a_rad_double(&mainboard.pos)));
783 }
784
785 prog_char str_position_arg0[] = "position";
786 parse_pgm_token_string_t cmd_position_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg0, str_position_arg0);
787 prog_char str_position_arg1[] = "show#reset#autoset_blue#autoset_red";
788 parse_pgm_token_string_t cmd_position_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg1, str_position_arg1);
789
790 prog_char help_position[] = "Show/reset (x,y,a) position";
791 parse_pgm_inst_t cmd_position = {
792         .f = cmd_position_parsed,  /* function to call */
793         .data = NULL,      /* 2nd arg of func */
794         .help_str = help_position,
795         .tokens = {        /* token list, NULL terminated */
796                 (prog_void *)&cmd_position_arg0, 
797                 (prog_void *)&cmd_position_arg1, 
798                 NULL,
799         },
800 };
801
802
803 prog_char str_position_arg1_set[] = "set";
804 parse_pgm_token_string_t cmd_position_arg1_set = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg1, str_position_arg1_set);
805 parse_pgm_token_num_t cmd_position_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg2, INT32);
806 parse_pgm_token_num_t cmd_position_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg3, INT32);
807 parse_pgm_token_num_t cmd_position_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg4, INT32);
808
809 prog_char help_position_set[] = "Set (x,y,a) position";
810 parse_pgm_inst_t cmd_position_set = {
811         .f = cmd_position_parsed,  /* function to call */
812         .data = NULL,      /* 2nd arg of func */
813         .help_str = help_position_set,
814         .tokens = {        /* token list, NULL terminated */
815                 (prog_void *)&cmd_position_arg0, 
816                 (prog_void *)&cmd_position_arg1_set, 
817                 (prog_void *)&cmd_position_arg2, 
818                 (prog_void *)&cmd_position_arg3, 
819                 (prog_void *)&cmd_position_arg4, 
820                 NULL,
821         },
822 };
823
824
825 /**********************************************************/
826 /* strat configuration */
827
828 /* this structure is filled when cmd_strat_infos is parsed successfully */
829 struct cmd_strat_infos_result {
830         fixed_string_t arg0;
831         fixed_string_t arg1;
832 };
833
834 /* function called when cmd_strat_infos is parsed successfully */
835 static void cmd_strat_infos_parsed(void *parsed_result, void *data)
836 {
837         struct cmd_strat_infos_result *res = parsed_result;
838
839         if (!strcmp_P(res->arg1, PSTR("reset"))) {
840                 strat_reset_infos();
841         }
842         strat_infos.dump_enabled = 1;
843         strat_dump_infos(__FUNCTION__);
844 }
845
846 prog_char str_strat_infos_arg0[] = "strat_infos";
847 parse_pgm_token_string_t cmd_strat_infos_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_infos_result, arg0, str_strat_infos_arg0);
848 prog_char str_strat_infos_arg1[] = "show#reset";
849 parse_pgm_token_string_t cmd_strat_infos_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_infos_result, arg1, str_strat_infos_arg1);
850
851 prog_char help_strat_infos[] = "reset/show strat_infos";
852 parse_pgm_inst_t cmd_strat_infos = {
853         .f = cmd_strat_infos_parsed,  /* function to call */
854         .data = NULL,      /* 2nd arg of func */
855         .help_str = help_strat_infos,
856         .tokens = {        /* token list, NULL terminated */
857                 (prog_void *)&cmd_strat_infos_arg0, 
858                 (prog_void *)&cmd_strat_infos_arg1, 
859                 NULL,
860         },
861 };
862
863 /**********************************************************/
864 /* strat configuration */
865
866 /* this structure is filled when cmd_strat_conf is parsed successfully */
867 struct cmd_strat_conf_result {
868         fixed_string_t arg0;
869         fixed_string_t arg1;
870 };
871
872 /* function called when cmd_strat_conf is parsed successfully */
873 static void cmd_strat_conf_parsed(void *parsed_result, void *data)
874 {
875         //      struct cmd_strat_conf_result *res = parsed_result;
876
877         strat_infos.dump_enabled = 1;
878         strat_dump_conf();
879 }
880
881 prog_char str_strat_conf_arg0[] = "strat_conf";
882 parse_pgm_token_string_t cmd_strat_conf_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf_result, arg0, str_strat_conf_arg0);
883 prog_char str_strat_conf_arg1[] = "show#base";
884 parse_pgm_token_string_t cmd_strat_conf_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf_result, arg1, str_strat_conf_arg1);
885
886 prog_char help_strat_conf[] = "configure strat options";
887 parse_pgm_inst_t cmd_strat_conf = {
888         .f = cmd_strat_conf_parsed,  /* function to call */
889         .data = NULL,      /* 2nd arg of func */
890         .help_str = help_strat_conf,
891         .tokens = {        /* token list, NULL terminated */
892                 (prog_void *)&cmd_strat_conf_arg0, 
893                 (prog_void *)&cmd_strat_conf_arg1, 
894                 NULL,
895         },
896 };
897
898 /**********************************************************/
899 /* strat configuration */
900
901 /* this structure is filled when cmd_strat_conf2 is parsed successfully */
902 struct cmd_strat_conf2_result {
903         fixed_string_t arg0;
904         fixed_string_t arg1;
905         fixed_string_t arg2;
906 };
907
908 /* function called when cmd_strat_conf2 is parsed successfully */
909 static void cmd_strat_conf2_parsed(void *parsed_result, void *data)
910 {
911         struct cmd_strat_conf2_result *res = parsed_result;
912         uint8_t on, bit = 0;
913
914         if (!strcmp_P(res->arg2, PSTR("on")))
915                 on = 1;
916         else
917                 on = 0;
918         
919 #if 0
920         if (!strcmp_P(res->arg1, PSTR("one_temple_on_disc")))
921                 bit = STRAT_CONF_ONLY_ONE_ON_DISC;
922         else if (!strcmp_P(res->arg1, PSTR("bypass_static2")))
923                 bit = STRAT_CONF_BYPASS_STATIC2;
924         else if (!strcmp_P(res->arg1, PSTR("take_one_lintel")))
925                 bit = STRAT_CONF_TAKE_ONE_LINTEL;
926         else if (!strcmp_P(res->arg1, PSTR("skip_when_check_fail")))
927                 bit = STRAT_CONF_TAKE_ONE_LINTEL;
928         else if (!strcmp_P(res->arg1, PSTR("store_static2")))
929                 bit = STRAT_CONF_STORE_STATIC2;
930         else if (!strcmp_P(res->arg1, PSTR("big3_temple")))
931                 bit = STRAT_CONF_BIG_3_TEMPLE;
932         else if (!strcmp_P(res->arg1, PSTR("early_opp_scan")))
933                 bit = STRAT_CONF_EARLY_SCAN;
934         else if (!strcmp_P(res->arg1, PSTR("push_opp_cols")))
935                 bit = STRAT_CONF_PUSH_OPP_COLS;
936 #endif
937
938         if (on)
939                 strat_infos.conf.flags |= bit;
940         else
941                 strat_infos.conf.flags &= (~bit);
942
943         strat_infos.dump_enabled = 1;
944         strat_dump_conf();
945 }
946
947 prog_char str_strat_conf2_arg0[] = "strat_conf";
948 parse_pgm_token_string_t cmd_strat_conf2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg0, str_strat_conf2_arg0);
949 prog_char str_strat_conf2_arg1[] = "faux";
950 parse_pgm_token_string_t cmd_strat_conf2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg1, str_strat_conf2_arg1);
951 prog_char str_strat_conf2_arg2[] = "on#off";
952 parse_pgm_token_string_t cmd_strat_conf2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg2, str_strat_conf2_arg2);
953
954
955 prog_char help_strat_conf2[] = "configure strat options";
956 parse_pgm_inst_t cmd_strat_conf2 = {
957         .f = cmd_strat_conf2_parsed,  /* function to call */
958         .data = NULL,      /* 2nd arg of func */
959         .help_str = help_strat_conf2,
960         .tokens = {        /* token list, NULL terminated */
961                 (prog_void *)&cmd_strat_conf2_arg0, 
962                 (prog_void *)&cmd_strat_conf2_arg1, 
963                 (prog_void *)&cmd_strat_conf2_arg2, 
964                 NULL,
965         },
966 };
967
968 /**********************************************************/
969 /* strat configuration */
970
971 /* this structure is filled when cmd_strat_conf3 is parsed successfully */
972 struct cmd_strat_conf3_result {
973         fixed_string_t arg0;
974         fixed_string_t arg1;
975         uint8_t arg2;
976 };
977
978 /* function called when cmd_strat_conf3 is parsed successfully */
979 static void cmd_strat_conf3_parsed(void *parsed_result, void *data)
980 {
981 #if 0
982         struct cmd_strat_conf3_result *res = parsed_result;
983
984         if (!strcmp_P(res->arg1, PSTR("scan_opponent_min_time"))) {
985                 if (res->arg2 > 90)
986                         res->arg2 = 90;
987                 strat_infos.conf.scan_opp_min_time = res->arg2;
988         }
989         else if (!strcmp_P(res->arg1, PSTR("delay_between_opponent_scan"))) {
990                 if (res->arg2 > 90)
991                         res->arg2 = 90;
992                 strat_infos.conf.delay_between_opp_scan = res->arg2;
993         }
994         else if (!strcmp_P(res->arg1, PSTR("scan_our_min_time"))) {
995                 if (res->arg2 > 90)
996                         res->arg2 = 90;
997                 strat_infos.conf.scan_our_min_time = res->arg2;
998         }
999         else if (!strcmp_P(res->arg1, PSTR("delay_between_our_scan"))) {
1000                 if (res->arg2 > 90)
1001                         res->arg2 = 90;
1002                 strat_infos.conf.delay_between_our_scan = res->arg2;
1003         }
1004         else if (!strcmp_P(res->arg1, PSTR("wait_opponent"))) {
1005                 strat_infos.conf.wait_opponent = res->arg2;
1006         }
1007         else if (!strcmp_P(res->arg1, PSTR("lintel_min_time"))) {
1008                 strat_infos.conf.lintel_min_time = res->arg2;
1009         }
1010 #endif
1011         strat_infos.dump_enabled = 1;
1012         strat_dump_conf();
1013 }
1014
1015 prog_char str_strat_conf3_arg0[] = "strat_conf";
1016 parse_pgm_token_string_t cmd_strat_conf3_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf3_result, arg0, str_strat_conf3_arg0);
1017 prog_char str_strat_conf3_arg1[] = "faux2";
1018 parse_pgm_token_string_t cmd_strat_conf3_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf3_result, arg1, str_strat_conf3_arg1);
1019 parse_pgm_token_num_t cmd_strat_conf3_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_strat_conf3_result, arg2, UINT16);
1020
1021 prog_char help_strat_conf3[] = "configure strat options";
1022 parse_pgm_inst_t cmd_strat_conf3 = {
1023         .f = cmd_strat_conf3_parsed,  /* function to call */
1024         .data = NULL,      /* 2nd arg of func */
1025         .help_str = help_strat_conf3,
1026         .tokens = {        /* token list, NULL terminated */
1027                 (prog_void *)&cmd_strat_conf3_arg0, 
1028                 (prog_void *)&cmd_strat_conf3_arg1, 
1029                 (prog_void *)&cmd_strat_conf3_arg2, 
1030                 NULL,
1031         },
1032 };
1033
1034
1035 /**********************************************************/
1036 /* Subtraj */
1037
1038 /* this structure is filled when cmd_subtraj is parsed successfully */
1039 struct cmd_subtraj_result {
1040         fixed_string_t arg0;
1041         fixed_string_t arg1;
1042         int32_t arg2;
1043         int32_t arg3;
1044         int32_t arg4;
1045         int32_t arg5;
1046 };
1047
1048 /* function called when cmd_subtraj is parsed successfully */
1049 static void cmd_subtraj_parsed(void *parsed_result, void *data)
1050 {
1051 /*      struct cmd_subtraj_result *res = parsed_result; */
1052
1053         printf_P(PSTR("TODO\r\n"));
1054         trajectory_hardstop(&mainboard.traj);
1055 }
1056
1057 prog_char str_subtraj_arg0[] = "subtraj";
1058 parse_pgm_token_string_t cmd_subtraj_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_subtraj_result, arg0, str_subtraj_arg0);
1059 prog_char str_subtraj_arg1[] = "faux";
1060 parse_pgm_token_string_t cmd_subtraj_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_subtraj_result, arg1, str_subtraj_arg1);
1061 parse_pgm_token_num_t cmd_subtraj_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg2, INT32);
1062 parse_pgm_token_num_t cmd_subtraj_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg3, INT32);
1063 parse_pgm_token_num_t cmd_subtraj_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg4, INT32);
1064 parse_pgm_token_num_t cmd_subtraj_arg5 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg5, INT32);
1065
1066 prog_char help_subtraj[] = "Test sub-trajectories (a,b,c,d: specific params)";
1067 parse_pgm_inst_t cmd_subtraj = {
1068         .f = cmd_subtraj_parsed,  /* function to call */
1069         .data = NULL,      /* 2nd arg of func */
1070         .help_str = help_subtraj,
1071         .tokens = {        /* token list, NULL terminated */
1072                 (prog_void *)&cmd_subtraj_arg0, 
1073                 (prog_void *)&cmd_subtraj_arg1, 
1074                 (prog_void *)&cmd_subtraj_arg2, 
1075                 (prog_void *)&cmd_subtraj_arg3, 
1076                 (prog_void *)&cmd_subtraj_arg4, 
1077                 (prog_void *)&cmd_subtraj_arg5, 
1078                 NULL,
1079         },
1080 };