vt100: include pgmspace.h as we use PROGMEM macro
[aversive.git] / projects / microb2009 / mainboard / commands_traj.c
1 /*
2  *  Copyright Droids Corporation (2009)
3  * 
4  *  This program is free software; you can redistribute it and/or modify
5  *  it under the terms of the GNU General Public License as published by
6  *  the Free Software Foundation; either version 2 of the License, or
7  *  (at your option) any later version.
8  *
9  *  This program is distributed in the hope that it will be useful,
10  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
11  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
12  *  GNU General Public License for more details.
13  *
14  *  You should have received a copy of the GNU General Public License
15  *  along with this program; if not, write to the Free Software
16  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
17  *
18  *  Revision : $Id: commands_traj.c,v 1.8 2009-11-08 17:24:33 zer0 Exp $
19  *
20  *  Olivier MATZ <zer0@droids-corp.org> 
21  */
22
23 #include <stdio.h>
24 #include <string.h>
25
26 #include <aversive/pgmspace.h>
27 #include <aversive/wait.h>
28 #include <aversive/error.h>
29
30 #include <ax12.h>
31 #include <uart.h>
32 #include <pwm_ng.h>
33 #include <time.h>
34 #include <spi.h>
35 #include <encoders_spi.h>
36
37 #include <pid.h>
38 #include <quadramp.h>
39 #include <control_system_manager.h>
40 #include <trajectory_manager.h>
41 #include <vect_base.h>
42 #include <lines.h>
43 #include <polygon.h>
44 #include <obstacle_avoidance.h>
45 #include <blocking_detection_manager.h>
46 #include <robot_system.h>
47 #include <position_manager.h>
48
49 #include <rdline.h>
50 #include <parse.h>
51 #include <parse_string.h>
52 #include <parse_num.h>
53
54 #include "main.h"
55 #include "cs.h"
56 #include "cmdline.h"
57 #include "strat_utils.h"
58 #include "strat_base.h"
59 #include "strat_avoid.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         struct cmd_rs_gains_result * res = parsed_result;
269
270         if (!strcmp_P(res->arg1, PSTR("set"))) {
271                 rs_set_left_ext_encoder(&mainboard.rs, encoders_spi_get_value, 
272                                         LEFT_ENCODER, res->left); // en augmentant on tourne à gauche
273                 rs_set_right_ext_encoder(&mainboard.rs, encoders_spi_get_value, 
274                                          RIGHT_ENCODER, res->right); //en augmentant on tourne à droite
275         }
276         printf_P(PSTR("rs_gains set "));
277         //f64_print(mainboard.rs.left_ext_gain);
278         printf_P(PSTR(" "));
279         //f64_print(mainboard.rs.right_ext_gain);
280         printf_P(PSTR("\r\n"));
281 }
282
283 prog_char str_rs_gains_arg0[] = "rs_gains";
284 parse_pgm_token_string_t cmd_rs_gains_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_rs_gains_result, arg0, str_rs_gains_arg0);
285 prog_char str_rs_gains_arg1[] = "set";
286 parse_pgm_token_string_t cmd_rs_gains_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_rs_gains_result, arg1, str_rs_gains_arg1);
287 parse_pgm_token_num_t cmd_rs_gains_l = TOKEN_NUM_INITIALIZER(struct cmd_rs_gains_result, left, FLOAT);
288 parse_pgm_token_num_t cmd_rs_gains_r = TOKEN_NUM_INITIALIZER(struct cmd_rs_gains_result, right, FLOAT);
289
290 prog_char help_rs_gains[] = "Set rs_gains (left, right)";
291 parse_pgm_inst_t cmd_rs_gains = {
292         .f = cmd_rs_gains_parsed,  /* function to call */
293         .data = NULL,      /* 2nd arg of func */
294         .help_str = help_rs_gains,
295         .tokens = {        /* token list, NULL terminated */
296                 (prog_void *)&cmd_rs_gains_arg0, 
297                 (prog_void *)&cmd_rs_gains_arg1, 
298                 (prog_void *)&cmd_rs_gains_l, 
299                 (prog_void *)&cmd_rs_gains_r, 
300                 NULL,
301         },
302 };
303
304 /* show */
305
306 prog_char str_rs_gains_show_arg[] = "show";
307 parse_pgm_token_string_t cmd_rs_gains_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_rs_gains_result, arg1, str_rs_gains_show_arg);
308
309 prog_char help_rs_gains_show[] = "Show rs_gains";
310 parse_pgm_inst_t cmd_rs_gains_show = {
311         .f = cmd_rs_gains_parsed,  /* function to call */
312         .data = NULL,      /* 2nd arg of func */
313         .help_str = help_rs_gains_show,
314         .tokens = {        /* token list, NULL terminated */
315                 (prog_void *)&cmd_rs_gains_arg0, 
316                 (prog_void *)&cmd_rs_gains_show_arg,
317                 NULL,
318         },
319 };
320
321 /**********************************************************/
322 /* track configuration */
323
324 /* this structure is filled when cmd_track is parsed successfully */
325 struct cmd_track_result {
326         fixed_string_t arg0;
327         fixed_string_t arg1;
328         float val;
329 };
330
331 /* function called when cmd_track is parsed successfully */
332 static void cmd_track_parsed(void * parsed_result, void * data)
333 {
334         struct cmd_track_result * res = parsed_result;
335
336         if (!strcmp_P(res->arg1, PSTR("set"))) {
337                 position_set_physical_params(&mainboard.pos, res->val, DIST_IMP_MM);
338         }
339         printf_P(PSTR("track set %f\r\n"), mainboard.pos.phys.track_mm);
340 }
341
342 prog_char str_track_arg0[] = "track";
343 parse_pgm_token_string_t cmd_track_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_track_result, arg0, str_track_arg0);
344 prog_char str_track_arg1[] = "set";
345 parse_pgm_token_string_t cmd_track_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_track_result, arg1, str_track_arg1);
346 parse_pgm_token_num_t cmd_track_val = TOKEN_NUM_INITIALIZER(struct cmd_track_result, val, FLOAT);
347
348 prog_char help_track[] = "Set track in mm";
349 parse_pgm_inst_t cmd_track = {
350         .f = cmd_track_parsed,  /* function to call */
351         .data = NULL,      /* 2nd arg of func */
352         .help_str = help_track,
353         .tokens = {        /* token list, NULL terminated */
354                 (prog_void *)&cmd_track_arg0, 
355                 (prog_void *)&cmd_track_arg1, 
356                 (prog_void *)&cmd_track_val, 
357                 NULL,
358         },
359 };
360
361 /* show */
362
363 prog_char str_track_show_arg[] = "show";
364 parse_pgm_token_string_t cmd_track_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_track_result, arg1, str_track_show_arg);
365
366 prog_char help_track_show[] = "Show track";
367 parse_pgm_inst_t cmd_track_show = {
368         .f = cmd_track_parsed,  /* function to call */
369         .data = NULL,      /* 2nd arg of func */
370         .help_str = help_track_show,
371         .tokens = {        /* token list, NULL terminated */
372                 (prog_void *)&cmd_track_arg0, 
373                 (prog_void *)&cmd_track_show_arg,
374                 NULL,
375         },
376 };
377
378
379
380 /**********************************************************/
381 /* Pt_Lists for testing traj */
382
383 #define PT_LIST_SIZE 10
384 static struct xy_point pt_list[PT_LIST_SIZE];
385 static uint16_t pt_list_len = 0;
386
387 /* this structure is filled when cmd_pt_list is parsed successfully */
388 struct cmd_pt_list_result {
389         fixed_string_t arg0;
390         fixed_string_t arg1;
391         uint16_t arg2;
392         int16_t arg3;
393         int16_t arg4;
394 };
395
396 /* function called when cmd_pt_list is parsed successfully */
397 static void cmd_pt_list_parsed(void * parsed_result, void * data)
398 {
399         struct cmd_pt_list_result * res = parsed_result;
400         uint8_t i, why=0;
401         
402         if (!strcmp_P(res->arg1, PSTR("append"))) {
403                 res->arg2 = pt_list_len;
404         }
405
406         if (!strcmp_P(res->arg1, PSTR("insert")) ||
407             !strcmp_P(res->arg1, PSTR("append"))) {
408                 if (res->arg2 > pt_list_len) {
409                         printf_P(PSTR("Index too large\r\n"));
410                         return;
411                 }
412                 if (pt_list_len >= PT_LIST_SIZE) {
413                         printf_P(PSTR("List is too large\r\n"));
414                         return;
415                 }
416                 memmove(&pt_list[res->arg2+1], &pt_list[res->arg2], 
417                        PT_LIST_SIZE-1-res->arg2);
418                 pt_list[res->arg2].x = res->arg3;
419                 pt_list[res->arg2].y = res->arg4;
420                 pt_list_len++;
421         }
422         else if (!strcmp_P(res->arg1, PSTR("del"))) {
423                 if (pt_list_len <= 0) {
424                         printf_P(PSTR("Error: list empty\r\n"));
425                         return;
426                 }
427                 if (res->arg2 > pt_list_len) {
428                         printf_P(PSTR("Index too large\r\n"));
429                         return;
430                 }
431                 memmove(&pt_list[res->arg2], &pt_list[res->arg2+1], 
432                        (PT_LIST_SIZE-1-res->arg2)*sizeof(struct xy_point));
433                 pt_list_len--;
434         }
435         else if (!strcmp_P(res->arg1, PSTR("reset"))) {
436                 pt_list_len = 0;
437         }
438         
439         /* else it is a "show" or a "start" */
440         if (pt_list_len == 0) {
441                 printf_P(PSTR("List empty\r\n"));
442                 return;
443         }
444         for (i=0 ; i<pt_list_len ; i++) {
445                 printf_P(PSTR("%d: x=%d y=%d\r\n"), i, pt_list[i].x, pt_list[i].y);
446                 if (!strcmp_P(res->arg1, PSTR("start"))) {
447                         trajectory_goto_xy_abs(&mainboard.traj, pt_list[i].x, pt_list[i].y);
448                         why = wait_traj_end(0xFF); /* all */
449                 }
450                 else if (!strcmp_P(res->arg1, PSTR("avoid_start"))) {
451                         while (1) {
452                                 why = goto_and_avoid(pt_list[i].x, pt_list[i].y, 0xFF, 0xFF);
453                                 printf("next point\r\n");
454                                 if (why != END_OBSTACLE)
455                                         break;
456                         }
457                 }
458                 if (why & (~(END_TRAJ | END_NEAR)))
459                         trajectory_stop(&mainboard.traj);
460         }
461 }
462
463 prog_char str_pt_list_arg0[] = "pt_list";
464 parse_pgm_token_string_t cmd_pt_list_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg0, str_pt_list_arg0);
465 prog_char str_pt_list_arg1[] = "insert";
466 parse_pgm_token_string_t cmd_pt_list_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_arg1);
467 parse_pgm_token_num_t cmd_pt_list_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_pt_list_result, arg2, UINT16);
468 parse_pgm_token_num_t cmd_pt_list_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_pt_list_result, arg3, INT16);
469 parse_pgm_token_num_t cmd_pt_list_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_pt_list_result, arg4, INT16);
470
471 prog_char help_pt_list[] = "Insert point in pt_list (idx,x,y)";
472 parse_pgm_inst_t cmd_pt_list = {
473         .f = cmd_pt_list_parsed,  /* function to call */
474         .data = NULL,      /* 2nd arg of func */
475         .help_str = help_pt_list,
476         .tokens = {        /* token list, NULL terminated */
477                 (prog_void *)&cmd_pt_list_arg0, 
478                 (prog_void *)&cmd_pt_list_arg1, 
479                 (prog_void *)&cmd_pt_list_arg2, 
480                 (prog_void *)&cmd_pt_list_arg3, 
481                 (prog_void *)&cmd_pt_list_arg4, 
482                 NULL,
483         },
484 };
485
486 /* append */
487
488 prog_char str_pt_list_arg1_append[] = "append";
489 parse_pgm_token_string_t cmd_pt_list_arg1_append = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_arg1_append);
490
491 prog_char help_pt_list_append[] = "Append point in pt_list (x,y)";
492 parse_pgm_inst_t cmd_pt_list_append = {
493         .f = cmd_pt_list_parsed,  /* function to call */
494         .data = NULL,      /* 2nd arg of func */
495         .help_str = help_pt_list_append,
496         .tokens = {        /* token list, NULL terminated */
497                 (prog_void *)&cmd_pt_list_arg0, 
498                 (prog_void *)&cmd_pt_list_arg1_append, 
499                 (prog_void *)&cmd_pt_list_arg3, 
500                 (prog_void *)&cmd_pt_list_arg4, 
501                 NULL,
502         },
503 };
504
505 /* del */
506
507 prog_char str_pt_list_del_arg[] = "del";
508 parse_pgm_token_string_t cmd_pt_list_del_arg = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_del_arg);
509
510 prog_char help_pt_list_del[] = "Del or insert point in pt_list (num)";
511 parse_pgm_inst_t cmd_pt_list_del = {
512         .f = cmd_pt_list_parsed,  /* function to call */
513         .data = NULL,      /* 2nd arg of func */
514         .help_str = help_pt_list_del,
515         .tokens = {        /* token list, NULL terminated */
516                 (prog_void *)&cmd_pt_list_arg0, 
517                 (prog_void *)&cmd_pt_list_del_arg, 
518                 (prog_void *)&cmd_pt_list_arg2,
519                 NULL,
520         },
521 };
522 /* show */
523
524 prog_char str_pt_list_show_arg[] = "show#reset#start#avoid_start";
525 parse_pgm_token_string_t cmd_pt_list_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_show_arg);
526
527 prog_char help_pt_list_show[] = "Show, start or reset pt_list";
528 parse_pgm_inst_t cmd_pt_list_show = {
529         .f = cmd_pt_list_parsed,  /* function to call */
530         .data = NULL,      /* 2nd arg of func */
531         .help_str = help_pt_list_show,
532         .tokens = {        /* token list, NULL terminated */
533                 (prog_void *)&cmd_pt_list_arg0, 
534                 (prog_void *)&cmd_pt_list_show_arg,
535                 NULL,
536         },
537 };
538
539
540
541 /**********************************************************/
542 /* Goto function */
543
544 /* this structure is filled when cmd_goto is parsed successfully */
545 struct cmd_goto_result {
546         fixed_string_t arg0;
547         fixed_string_t arg1;
548         int32_t arg2;
549         int32_t arg3;
550         int32_t arg4;
551         int32_t arg5;
552 };
553
554 /* function called when cmd_goto is parsed successfully */
555 static void cmd_goto_parsed(void * parsed_result, void * data)
556 {
557         struct cmd_goto_result * res = parsed_result;
558         uint8_t err;
559         microseconds t1, t2;
560
561         interrupt_traj_reset();
562         if (!strcmp_P(res->arg1, PSTR("a_rel"))) {
563                 trajectory_a_rel(&mainboard.traj, res->arg2);
564         }
565         else if (!strcmp_P(res->arg1, PSTR("d_rel"))) {
566                 trajectory_d_rel(&mainboard.traj, res->arg2);
567         }
568         else if (!strcmp_P(res->arg1, PSTR("a_abs"))) {
569                 trajectory_a_abs(&mainboard.traj, res->arg2);
570         }
571         else if (!strcmp_P(res->arg1, PSTR("a_to_xy"))) {
572                 trajectory_turnto_xy(&mainboard.traj, res->arg2, res->arg3);
573         }
574         else if (!strcmp_P(res->arg1, PSTR("a_behind_xy"))) {
575                 trajectory_turnto_xy_behind(&mainboard.traj, res->arg2, res->arg3);
576         }
577         else if (!strcmp_P(res->arg1, PSTR("xy_rel"))) {
578                 trajectory_goto_xy_rel(&mainboard.traj, res->arg2, res->arg3);
579         }
580         else if (!strcmp_P(res->arg1, PSTR("xy_abs"))) {
581                 trajectory_goto_xy_abs(&mainboard.traj, res->arg2, res->arg3);
582         }
583         else if (!strcmp_P(res->arg1, PSTR("avoid"))) {
584                 err = goto_and_avoid_forward(res->arg2, res->arg3, 0xFF, 0xFF);
585                 if (err != END_TRAJ && err != END_NEAR)
586                         strat_hardstop();
587         }
588         else if (!strcmp_P(res->arg1, PSTR("avoid_bw"))) {
589                 err = goto_and_avoid_backward(res->arg2, res->arg3, 0xFF, 0xFF);
590                 if (err != END_TRAJ && err != END_NEAR)
591                         strat_hardstop();
592         }
593         else if (!strcmp_P(res->arg1, PSTR("xy_abs_fow"))) {
594                 trajectory_goto_forward_xy_abs(&mainboard.traj, res->arg2, res->arg3);
595         }
596         else if (!strcmp_P(res->arg1, PSTR("xy_abs_back"))) {
597                 trajectory_goto_backward_xy_abs(&mainboard.traj, res->arg2, res->arg3);
598         }
599         else if (!strcmp_P(res->arg1, PSTR("da_rel"))) {
600                 trajectory_d_a_rel(&mainboard.traj, res->arg2, res->arg3);
601         }
602         else if (!strcmp_P(res->arg1, PSTR("circle_rel"))) {
603                 trajectory_circle_rel(&mainboard.traj, res->arg2, res->arg3,
604                                       res->arg4, res->arg5, 0);
605                 return; /* XXX */
606         }
607         t1 = time_get_us2();
608         while ((err = test_traj_end(0xFF)) == 0) {
609                 t2 = time_get_us2();
610                 if (t2 - t1 > 200000) {
611                         dump_cs_debug("angle", &mainboard.angle.cs);
612                         dump_cs_debug("distance", &mainboard.distance.cs);
613                         t1 = t2;
614                 }
615         }
616         if (err != END_TRAJ && err != END_NEAR)
617                 strat_hardstop();
618         printf_P(PSTR("returned %s\r\n"), get_err(err));
619 }
620
621 prog_char str_goto_arg0[] = "goto";
622 parse_pgm_token_string_t cmd_goto_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_goto_result, arg0, str_goto_arg0);
623 prog_char str_goto_arg1_a[] = "d_rel#a_rel#a_abs";
624 parse_pgm_token_string_t cmd_goto_arg1_a = TOKEN_STRING_INITIALIZER(struct cmd_goto_result, arg1, str_goto_arg1_a);
625 parse_pgm_token_num_t cmd_goto_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_goto_result, arg2, INT32);
626
627 /* 1 params */
628 prog_char help_goto1[] = "Change orientation of the mainboard";
629 parse_pgm_inst_t cmd_goto1 = {
630         .f = cmd_goto_parsed,  /* function to call */
631         .data = NULL,      /* 2nd arg of func */
632         .help_str = help_goto1,
633         .tokens = {        /* token list, NULL terminated */
634                 (prog_void *)&cmd_goto_arg0, 
635                 (prog_void *)&cmd_goto_arg1_a, 
636                 (prog_void *)&cmd_goto_arg2, 
637                 NULL,
638         },
639 };
640
641 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";
642 parse_pgm_token_string_t cmd_goto_arg1_b = TOKEN_STRING_INITIALIZER(struct cmd_goto_result, arg1, str_goto_arg1_b);
643 parse_pgm_token_num_t cmd_goto_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_goto_result, arg3, INT32);
644
645 /* 2 params */
646 prog_char help_goto2[] = "Go to a (x,y) or (d,a) position";
647 parse_pgm_inst_t cmd_goto2 = {
648         .f = cmd_goto_parsed,  /* function to call */
649         .data = NULL,      /* 2nd arg of func */
650         .help_str = help_goto2,
651         .tokens = {        /* token list, NULL terminated */
652                 (prog_void *)&cmd_goto_arg0, 
653                 (prog_void *)&cmd_goto_arg1_b, 
654                 (prog_void *)&cmd_goto_arg2,
655                 (prog_void *)&cmd_goto_arg3, 
656                 NULL,
657         },
658 };
659
660 prog_char str_goto_arg1_c[] = "circle_rel";
661 parse_pgm_token_string_t cmd_goto_arg1_c = TOKEN_STRING_INITIALIZER(struct cmd_goto_result, arg1, str_goto_arg1_c);
662 parse_pgm_token_num_t cmd_goto_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_goto_result, arg4, INT32);
663 parse_pgm_token_num_t cmd_goto_arg5 = TOKEN_NUM_INITIALIZER(struct cmd_goto_result, arg5, INT32);
664
665 /* 4 params */
666 prog_char help_goto4[] = "Do a circle: (x,y, radius, angle)";
667 parse_pgm_inst_t cmd_goto4 = {
668         .f = cmd_goto_parsed,  /* function to call */
669         .data = NULL,      /* 2nd arg of func */
670         .help_str = help_goto4,
671         .tokens = {        /* token list, NULL terminated */
672                 (prog_void *)&cmd_goto_arg0,
673                 (prog_void *)&cmd_goto_arg1_c,
674                 (prog_void *)&cmd_goto_arg2,
675                 (prog_void *)&cmd_goto_arg3,
676                 (prog_void *)&cmd_goto_arg4,
677                 (prog_void *)&cmd_goto_arg5,
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_green"))) {
762                 mainboard.our_color = I2C_COLOR_GREEN;
763                 i2c_set_color(I2C_MECHBOARD_ADDR, I2C_COLOR_GREEN);
764                 i2c_set_color(I2C_SENSORBOARD_ADDR, I2C_COLOR_GREEN);
765                 auto_position();
766         }
767         else if (!strcmp_P(res->arg1, PSTR("autoset_red"))) {
768                 mainboard.our_color = I2C_COLOR_RED;
769                 i2c_set_color(I2C_MECHBOARD_ADDR, I2C_COLOR_RED);
770                 i2c_set_color(I2C_SENSORBOARD_ADDR, I2C_COLOR_RED);
771                 auto_position();
772         }
773
774         /* else it's just a "show" */
775         printf_P(PSTR("x=%.2f y=%.2f a=%.2f\r\n"), 
776                  position_get_x_double(&mainboard.pos),
777                  position_get_y_double(&mainboard.pos),
778                  DEG(position_get_a_rad_double(&mainboard.pos)));
779 }
780
781 prog_char str_position_arg0[] = "position";
782 parse_pgm_token_string_t cmd_position_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg0, str_position_arg0);
783 prog_char str_position_arg1[] = "show#reset#autoset_green#autoset_red";
784 parse_pgm_token_string_t cmd_position_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg1, str_position_arg1);
785
786 prog_char help_position[] = "Show/reset (x,y,a) position";
787 parse_pgm_inst_t cmd_position = {
788         .f = cmd_position_parsed,  /* function to call */
789         .data = NULL,      /* 2nd arg of func */
790         .help_str = help_position,
791         .tokens = {        /* token list, NULL terminated */
792                 (prog_void *)&cmd_position_arg0, 
793                 (prog_void *)&cmd_position_arg1, 
794                 NULL,
795         },
796 };
797
798
799 prog_char str_position_arg1_set[] = "set";
800 parse_pgm_token_string_t cmd_position_arg1_set = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg1, str_position_arg1_set);
801 parse_pgm_token_num_t cmd_position_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg2, INT32);
802 parse_pgm_token_num_t cmd_position_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg3, INT32);
803 parse_pgm_token_num_t cmd_position_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg4, INT32);
804
805 prog_char help_position_set[] = "Set (x,y,a) position";
806 parse_pgm_inst_t cmd_position_set = {
807         .f = cmd_position_parsed,  /* function to call */
808         .data = NULL,      /* 2nd arg of func */
809         .help_str = help_position_set,
810         .tokens = {        /* token list, NULL terminated */
811                 (prog_void *)&cmd_position_arg0, 
812                 (prog_void *)&cmd_position_arg1_set, 
813                 (prog_void *)&cmd_position_arg2, 
814                 (prog_void *)&cmd_position_arg3, 
815                 (prog_void *)&cmd_position_arg4, 
816                 NULL,
817         },
818 };
819
820
821 /**********************************************************/
822 /* strat configuration */
823
824 /* this structure is filled when cmd_strat_infos is parsed successfully */
825 struct cmd_strat_infos_result {
826         fixed_string_t arg0;
827         fixed_string_t arg1;
828 };
829
830 /* function called when cmd_strat_infos is parsed successfully */
831 static void cmd_strat_infos_parsed(void *parsed_result, void *data)
832 {
833         struct cmd_strat_infos_result *res = parsed_result;
834
835         if (!strcmp_P(res->arg1, PSTR("reset"))) {
836                 strat_reset_infos();
837         }
838         strat_infos.dump_enabled = 1;
839         strat_dump_infos(__FUNCTION__);
840 }
841
842 prog_char str_strat_infos_arg0[] = "strat_infos";
843 parse_pgm_token_string_t cmd_strat_infos_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_infos_result, arg0, str_strat_infos_arg0);
844 prog_char str_strat_infos_arg1[] = "show#reset";
845 parse_pgm_token_string_t cmd_strat_infos_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_infos_result, arg1, str_strat_infos_arg1);
846
847 prog_char help_strat_infos[] = "reset/show strat_infos";
848 parse_pgm_inst_t cmd_strat_infos = {
849         .f = cmd_strat_infos_parsed,  /* function to call */
850         .data = NULL,      /* 2nd arg of func */
851         .help_str = help_strat_infos,
852         .tokens = {        /* token list, NULL terminated */
853                 (prog_void *)&cmd_strat_infos_arg0, 
854                 (prog_void *)&cmd_strat_infos_arg1, 
855                 NULL,
856         },
857 };
858
859 /**********************************************************/
860 /* strat configuration */
861
862 /* this structure is filled when cmd_strat_conf is parsed successfully */
863 struct cmd_strat_conf_result {
864         fixed_string_t arg0;
865         fixed_string_t arg1;
866 };
867
868 /* function called when cmd_strat_conf is parsed successfully */
869 static void cmd_strat_conf_parsed(void *parsed_result, void *data)
870 {
871         struct cmd_strat_conf_result *res = parsed_result;
872
873         if (!strcmp_P(res->arg1, PSTR("base"))) {
874                 strat_infos.conf.flags = 0;
875                 strat_infos.conf.scan_our_min_time = 90;
876                 strat_infos.conf.delay_between_our_scan = 90;
877                 strat_infos.conf.scan_opp_min_time = 90;
878                 strat_infos.conf.delay_between_opp_scan = 90;
879         }
880         else if (!strcmp_P(res->arg1, PSTR("big3"))) {
881                 strat_infos.conf.flags = 
882                         STRAT_CONF_STORE_STATIC2 |
883                         STRAT_CONF_BIG_3_TEMPLE;
884                 strat_infos.conf.scan_our_min_time = 90;
885                 strat_infos.conf.delay_between_our_scan = 90;
886                 strat_infos.conf.scan_opp_min_time = 90;
887                 strat_infos.conf.delay_between_opp_scan = 90;
888         }
889         else if (!strcmp_P(res->arg1, PSTR("base_check"))) {
890                 strat_infos.conf.flags = 0;
891                 strat_infos.conf.scan_our_min_time = 35;
892                 strat_infos.conf.delay_between_our_scan = 90;
893                 strat_infos.conf.scan_opp_min_time = 90;
894                 strat_infos.conf.delay_between_opp_scan = 90;
895         }
896         else if (!strcmp_P(res->arg1, PSTR("big3_check"))) {
897                 strat_infos.conf.flags = 
898                         STRAT_CONF_STORE_STATIC2 |
899                         STRAT_CONF_BIG_3_TEMPLE;
900                 strat_infos.conf.scan_our_min_time = 35;
901                 strat_infos.conf.delay_between_our_scan = 90;
902                 strat_infos.conf.scan_opp_min_time = 90;
903                 strat_infos.conf.delay_between_opp_scan = 90;
904         }
905         else if (!strcmp_P(res->arg1, PSTR("offensive_early"))) {
906                 strat_infos.conf.flags = 
907                         STRAT_CONF_TAKE_ONE_LINTEL |
908                         STRAT_CONF_STORE_STATIC2 |
909                         STRAT_CONF_EARLY_SCAN |
910                         STRAT_CONF_PUSH_OPP_COLS;
911                 strat_infos.conf.scan_our_min_time = 50;
912                 strat_infos.conf.delay_between_our_scan = 90;
913                 strat_infos.conf.scan_opp_min_time = 15;
914                 strat_infos.conf.delay_between_opp_scan = 90;
915                 strat_infos.conf.wait_opponent = 5;
916         }
917         else if (!strcmp_P(res->arg1, PSTR("offensive_late"))) {
918                 strat_infos.conf.flags = STRAT_CONF_TAKE_ONE_LINTEL;
919                 strat_infos.conf.scan_our_min_time = 90;
920                 strat_infos.conf.delay_between_our_scan = 90;
921                 strat_infos.conf.scan_opp_min_time = 30;
922                 strat_infos.conf.delay_between_opp_scan = 90;
923         }
924         else if (!strcmp_P(res->arg1, PSTR("one_on_disc"))) {
925                 strat_infos.conf.flags = 
926                         STRAT_CONF_ONLY_ONE_ON_DISC;
927                 strat_infos.conf.scan_our_min_time = 90;
928                 strat_infos.conf.delay_between_our_scan = 90;
929                 strat_infos.conf.scan_opp_min_time = 90;
930                 strat_infos.conf.delay_between_opp_scan = 90;
931         }
932         strat_infos.dump_enabled = 1;
933         strat_dump_conf();
934 }
935
936 prog_char str_strat_conf_arg0[] = "strat_conf";
937 parse_pgm_token_string_t cmd_strat_conf_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf_result, arg0, str_strat_conf_arg0);
938 prog_char str_strat_conf_arg1[] = "show#base#big3#base_check#big3_check#offensive_early#offensive_late#one_on_disc";
939 parse_pgm_token_string_t cmd_strat_conf_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf_result, arg1, str_strat_conf_arg1);
940
941 prog_char help_strat_conf[] = "configure strat options";
942 parse_pgm_inst_t cmd_strat_conf = {
943         .f = cmd_strat_conf_parsed,  /* function to call */
944         .data = NULL,      /* 2nd arg of func */
945         .help_str = help_strat_conf,
946         .tokens = {        /* token list, NULL terminated */
947                 (prog_void *)&cmd_strat_conf_arg0, 
948                 (prog_void *)&cmd_strat_conf_arg1, 
949                 NULL,
950         },
951 };
952
953 /**********************************************************/
954 /* strat configuration */
955
956 /* this structure is filled when cmd_strat_conf2 is parsed successfully */
957 struct cmd_strat_conf2_result {
958         fixed_string_t arg0;
959         fixed_string_t arg1;
960         fixed_string_t arg2;
961 };
962
963 /* function called when cmd_strat_conf2 is parsed successfully */
964 static void cmd_strat_conf2_parsed(void *parsed_result, void *data)
965 {
966         struct cmd_strat_conf2_result *res = parsed_result;
967         uint8_t on, bit = 0;
968
969         if (!strcmp_P(res->arg2, PSTR("on")))
970                 on = 1;
971         else
972                 on = 0;
973         
974         if (!strcmp_P(res->arg1, PSTR("one_temple_on_disc")))
975                 bit = STRAT_CONF_ONLY_ONE_ON_DISC;
976         else if (!strcmp_P(res->arg1, PSTR("bypass_static2")))
977                 bit = STRAT_CONF_BYPASS_STATIC2;
978         else if (!strcmp_P(res->arg1, PSTR("take_one_lintel")))
979                 bit = STRAT_CONF_TAKE_ONE_LINTEL;
980         else if (!strcmp_P(res->arg1, PSTR("skip_when_check_fail")))
981                 bit = STRAT_CONF_TAKE_ONE_LINTEL;
982         else if (!strcmp_P(res->arg1, PSTR("store_static2")))
983                 bit = STRAT_CONF_STORE_STATIC2;
984         else if (!strcmp_P(res->arg1, PSTR("big3_temple")))
985                 bit = STRAT_CONF_BIG_3_TEMPLE;
986         else if (!strcmp_P(res->arg1, PSTR("early_opp_scan")))
987                 bit = STRAT_CONF_EARLY_SCAN;
988         else if (!strcmp_P(res->arg1, PSTR("push_opp_cols")))
989                 bit = STRAT_CONF_PUSH_OPP_COLS;
990
991         if (on)
992                 strat_infos.conf.flags |= bit;
993         else
994                 strat_infos.conf.flags &= (~bit);
995
996         strat_infos.dump_enabled = 1;
997         strat_dump_conf();
998 }
999
1000 prog_char str_strat_conf2_arg0[] = "strat_conf";
1001 parse_pgm_token_string_t cmd_strat_conf2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg0, str_strat_conf2_arg0);
1002 prog_char str_strat_conf2_arg1[] = "push_opp_cols#one_temple_on_disc#bypass_static2#take_one_lintel#skip_when_check_fail#store_static2#big3_temple#early_opp_scan";
1003 parse_pgm_token_string_t cmd_strat_conf2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg1, str_strat_conf2_arg1);
1004 prog_char str_strat_conf2_arg2[] = "on#off";
1005 parse_pgm_token_string_t cmd_strat_conf2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg2, str_strat_conf2_arg2);
1006
1007
1008 prog_char help_strat_conf2[] = "configure strat options";
1009 parse_pgm_inst_t cmd_strat_conf2 = {
1010         .f = cmd_strat_conf2_parsed,  /* function to call */
1011         .data = NULL,      /* 2nd arg of func */
1012         .help_str = help_strat_conf2,
1013         .tokens = {        /* token list, NULL terminated */
1014                 (prog_void *)&cmd_strat_conf2_arg0, 
1015                 (prog_void *)&cmd_strat_conf2_arg1, 
1016                 (prog_void *)&cmd_strat_conf2_arg2, 
1017                 NULL,
1018         },
1019 };
1020
1021 /**********************************************************/
1022 /* strat configuration */
1023
1024 /* this structure is filled when cmd_strat_conf3 is parsed successfully */
1025 struct cmd_strat_conf3_result {
1026         fixed_string_t arg0;
1027         fixed_string_t arg1;
1028         uint8_t arg2;
1029 };
1030
1031 /* function called when cmd_strat_conf3 is parsed successfully */
1032 static void cmd_strat_conf3_parsed(void *parsed_result, void *data)
1033 {
1034         struct cmd_strat_conf3_result *res = parsed_result;
1035
1036         if (!strcmp_P(res->arg1, PSTR("scan_opponent_min_time"))) {
1037                 if (res->arg2 > 90)
1038                         res->arg2 = 90;
1039                 strat_infos.conf.scan_opp_min_time = res->arg2;
1040         }
1041         else if (!strcmp_P(res->arg1, PSTR("delay_between_opponent_scan"))) {
1042                 if (res->arg2 > 90)
1043                         res->arg2 = 90;
1044                 strat_infos.conf.delay_between_opp_scan = res->arg2;
1045         }
1046         else if (!strcmp_P(res->arg1, PSTR("scan_our_min_time"))) {
1047                 if (res->arg2 > 90)
1048                         res->arg2 = 90;
1049                 strat_infos.conf.scan_our_min_time = res->arg2;
1050         }
1051         else if (!strcmp_P(res->arg1, PSTR("delay_between_our_scan"))) {
1052                 if (res->arg2 > 90)
1053                         res->arg2 = 90;
1054                 strat_infos.conf.delay_between_our_scan = res->arg2;
1055         }
1056         else if (!strcmp_P(res->arg1, PSTR("wait_opponent"))) {
1057                 strat_infos.conf.wait_opponent = res->arg2;
1058         }
1059         else if (!strcmp_P(res->arg1, PSTR("lintel_min_time"))) {
1060                 strat_infos.conf.lintel_min_time = res->arg2;
1061         }
1062         strat_infos.dump_enabled = 1;
1063         strat_dump_conf();
1064 }
1065
1066 prog_char str_strat_conf3_arg0[] = "strat_conf";
1067 parse_pgm_token_string_t cmd_strat_conf3_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf3_result, arg0, str_strat_conf3_arg0);
1068 prog_char str_strat_conf3_arg1[] = "lintel_min_time#scan_opponent_min_time#delay_between_opponent_scan#scan_our_min_time#delay_between_our_scan#wait_opponent";
1069 parse_pgm_token_string_t cmd_strat_conf3_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf3_result, arg1, str_strat_conf3_arg1);
1070 parse_pgm_token_num_t cmd_strat_conf3_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_strat_conf3_result, arg2, UINT16);
1071
1072 prog_char help_strat_conf3[] = "configure strat options";
1073 parse_pgm_inst_t cmd_strat_conf3 = {
1074         .f = cmd_strat_conf3_parsed,  /* function to call */
1075         .data = NULL,      /* 2nd arg of func */
1076         .help_str = help_strat_conf3,
1077         .tokens = {        /* token list, NULL terminated */
1078                 (prog_void *)&cmd_strat_conf3_arg0, 
1079                 (prog_void *)&cmd_strat_conf3_arg1, 
1080                 (prog_void *)&cmd_strat_conf3_arg2, 
1081                 NULL,
1082         },
1083 };
1084
1085 /**********************************************************/
1086 /* strat configuration */
1087
1088 /* this structure is filled when cmd_strat_conf4 is parsed successfully */
1089 struct cmd_strat_conf4_result {
1090         fixed_string_t arg0;
1091         fixed_string_t arg1;
1092         int16_t arg2;
1093 };
1094
1095 /* function called when cmd_strat_conf4 is parsed successfully */
1096 static void cmd_strat_conf4_parsed(void *parsed_result, void *data)
1097 {
1098         struct cmd_strat_conf4_result *res = parsed_result;
1099
1100         if (!strcmp_P(res->arg1, PSTR("scan_opponent_angle"))) {
1101                 strat_infos.conf.scan_opp_angle = res->arg2;
1102         }
1103         strat_infos.dump_enabled = 1;
1104         strat_dump_conf();
1105 }
1106
1107 prog_char str_strat_conf4_arg0[] = "strat_conf";
1108 parse_pgm_token_string_t cmd_strat_conf4_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf4_result, arg0, str_strat_conf4_arg0);
1109 prog_char str_strat_conf4_arg1[] = "scan_opponent_angle";
1110 parse_pgm_token_string_t cmd_strat_conf4_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf4_result, arg1, str_strat_conf4_arg1);
1111 parse_pgm_token_num_t cmd_strat_conf4_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_strat_conf4_result, arg2, UINT16);
1112
1113 prog_char help_strat_conf4[] = "configure strat options";
1114 parse_pgm_inst_t cmd_strat_conf4 = {
1115         .f = cmd_strat_conf4_parsed,  /* function to call */
1116         .data = NULL,      /* 2nd arg of func */
1117         .help_str = help_strat_conf4,
1118         .tokens = {        /* token list, NULL terminated */
1119                 (prog_void *)&cmd_strat_conf4_arg0, 
1120                 (prog_void *)&cmd_strat_conf4_arg1, 
1121                 (prog_void *)&cmd_strat_conf4_arg2, 
1122                 NULL,
1123         },
1124 };
1125
1126
1127 /**********************************************************/
1128 /* Subtraj */
1129
1130 /* this structure is filled when cmd_subtraj is parsed successfully */
1131 struct cmd_subtraj_result {
1132         fixed_string_t arg0;
1133         fixed_string_t arg1;
1134         int32_t arg2;
1135         int32_t arg3;
1136         int32_t arg4;
1137         int32_t arg5;
1138 };
1139
1140 /* function called when cmd_subtraj is parsed successfully */
1141 static void cmd_subtraj_parsed(void *parsed_result, void *data)
1142 {
1143         struct cmd_subtraj_result *res = parsed_result;
1144         uint8_t err = 0;
1145         struct column_dispenser *disp;
1146
1147         if (strcmp_P(res->arg1, PSTR("static")) == 0) {
1148                 err = strat_static_columns(res->arg2);
1149         }
1150         else if (strcmp_P(res->arg1, PSTR("static2")) == 0) {
1151                 strat_infos.s_cols.configuration = res->arg2;
1152                 switch (res->arg2) {
1153                 case 1:
1154                         position_set(&mainboard.pos, 1398, 
1155                                      COLOR_Y(1297), COLOR_A(-66));
1156                         break;
1157                 case 2:
1158                         position_set(&mainboard.pos, 1232, 
1159                                      COLOR_Y(1051), COLOR_A(4));
1160                         break;
1161                 case 3:
1162                         position_set(&mainboard.pos, 1232, 
1163                                      COLOR_Y(1043), COLOR_A(5));
1164                         break;
1165                 case 4:
1166                         position_set(&mainboard.pos, 1346,
1167                                      COLOR_Y(852), COLOR_A(57));
1168                         break;
1169                 default:
1170                         return;
1171                 }
1172                 if (res->arg2 == 1 && res->arg3 == 1) {
1173                         strat_infos.s_cols.flags = STATIC_COL_LINE1_DONE;
1174                 }
1175                 if (res->arg2 == 1 && res->arg3 == 2) {
1176                         strat_infos.s_cols.flags = STATIC_COL_LINE2_DONE;
1177                 }
1178                 err = strat_static_columns_pass2();
1179         }
1180         else if (strcmp_P(res->arg1, PSTR("lintel1")) == 0) {
1181                 err = strat_goto_lintel_disp(&strat_infos.l1);
1182         }
1183         else if (strcmp_P(res->arg1, PSTR("lintel2")) == 0) {
1184                 err = strat_goto_lintel_disp(&strat_infos.l2);
1185         }
1186         else if (strcmp_P(res->arg1, PSTR("coldisp1")) == 0) {
1187                 disp = &strat_infos.c1;
1188                 err = strat_goto_col_disp(&disp);
1189         }
1190         else if (strcmp_P(res->arg1, PSTR("coldisp2")) == 0) {
1191                 disp = &strat_infos.c2;
1192                 err = strat_goto_col_disp(&disp);
1193         }
1194         else if (strcmp_P(res->arg1, PSTR("coldisp3")) == 0) {
1195                 disp = &strat_infos.c3;
1196                 err = strat_goto_col_disp(&disp);
1197         }
1198         else if (strcmp_P(res->arg1, PSTR("disc")) == 0) {
1199                 if (res->arg2 == 0) {
1200                         printf_P(PSTR("bad level\r\n"));
1201                         return;
1202                 }
1203                 err = strat_goto_disc(res->arg2);
1204         }
1205
1206         printf_P(PSTR("substrat returned %s\r\n"), get_err(err));
1207         trajectory_hardstop(&mainboard.traj);
1208 }
1209
1210 prog_char str_subtraj_arg0[] = "subtraj";
1211 parse_pgm_token_string_t cmd_subtraj_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_subtraj_result, arg0, str_subtraj_arg0);
1212 prog_char str_subtraj_arg1[] = "static#disc#lintel1#lintel2#coldisp1#coldisp2#coldisp3#static2";
1213 parse_pgm_token_string_t cmd_subtraj_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_subtraj_result, arg1, str_subtraj_arg1);
1214 parse_pgm_token_num_t cmd_subtraj_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg2, INT32);
1215 parse_pgm_token_num_t cmd_subtraj_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg3, INT32);
1216 parse_pgm_token_num_t cmd_subtraj_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg4, INT32);
1217 parse_pgm_token_num_t cmd_subtraj_arg5 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg5, INT32);
1218
1219 prog_char help_subtraj[] = "Test sub-trajectories (a,b,c,d: specific params)";
1220 parse_pgm_inst_t cmd_subtraj = {
1221         .f = cmd_subtraj_parsed,  /* function to call */
1222         .data = NULL,      /* 2nd arg of func */
1223         .help_str = help_subtraj,
1224         .tokens = {        /* token list, NULL terminated */
1225                 (prog_void *)&cmd_subtraj_arg0, 
1226                 (prog_void *)&cmd_subtraj_arg1, 
1227                 (prog_void *)&cmd_subtraj_arg2, 
1228                 (prog_void *)&cmd_subtraj_arg3, 
1229                 (prog_void *)&cmd_subtraj_arg4, 
1230                 (prog_void *)&cmd_subtraj_arg5, 
1231                 NULL,
1232         },
1233 };