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