restore first circle algo + hostsim work
[aversive.git] / projects / microb2010 / tests / hostsim / 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 <uart.h>
32 #include <clock_time.h>
33
34 #include <pid.h>
35 #include <quadramp.h>
36 #include <control_system_manager.h>
37 #include <trajectory_manager.h>
38 #include <vect_base.h>
39 #include <lines.h>
40 #include <polygon.h>
41 #include <obstacle_avoidance.h>
42 #include <blocking_detection_manager.h>
43 #include <robot_system.h>
44 #include <position_manager.h>
45
46 #include <rdline.h>
47 #include <parse.h>
48 #include <parse_string.h>
49 #include <parse_num.h>
50
51 #include "i2c_commands.h"
52 #include "main.h"
53 #include "cs.h"
54 #include "cmdline.h"
55 #include "strat.h"
56 #include "strat_utils.h"
57 #include "strat_base.h"
58 #include "strat_avoid.h"
59
60 /**********************************************************/
61 /* Traj_Speeds for trajectory_manager */
62
63 /* this structure is filled when cmd_traj_speed is parsed successfully */
64 struct cmd_traj_speed_result {
65         fixed_string_t arg0;
66         fixed_string_t arg1;
67         uint16_t s;
68 };
69
70 /* function called when cmd_traj_speed is parsed successfully */
71 static void cmd_traj_speed_parsed(void *parsed_result, void *data)
72 {
73         struct cmd_traj_speed_result * res = parsed_result;
74         
75         if (!strcmp_P(res->arg1, PSTR("angle"))) {
76                 trajectory_set_speed(&mainboard.traj, mainboard.traj.d_speed, res->s);
77         }
78         else if (!strcmp_P(res->arg1, PSTR("distance"))) {
79                 trajectory_set_speed(&mainboard.traj, res->s, mainboard.traj.a_speed);
80         }
81         /* else it is a "show" */
82
83         printf_P(PSTR("angle %u, distance %u\r\n"), 
84                  mainboard.traj.a_speed,
85                  mainboard.traj.d_speed);
86 }
87
88 prog_char str_traj_speed_arg0[] = "traj_speed";
89 parse_pgm_token_string_t cmd_traj_speed_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_traj_speed_result, arg0, str_traj_speed_arg0);
90 prog_char str_traj_speed_arg1[] = "angle#distance";
91 parse_pgm_token_string_t cmd_traj_speed_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_traj_speed_result, arg1, str_traj_speed_arg1);
92 parse_pgm_token_num_t cmd_traj_speed_s = TOKEN_NUM_INITIALIZER(struct cmd_traj_speed_result, s, UINT16);
93
94 prog_char help_traj_speed[] = "Set traj_speed values for trajectory manager";
95 parse_pgm_inst_t cmd_traj_speed = {
96         .f = cmd_traj_speed_parsed,  /* function to call */
97         .data = NULL,      /* 2nd arg of func */
98         .help_str = help_traj_speed,
99         .tokens = {        /* token list, NULL terminated */
100                 (prog_void *)&cmd_traj_speed_arg0, 
101                 (prog_void *)&cmd_traj_speed_arg1, 
102                 (prog_void *)&cmd_traj_speed_s, 
103                 NULL,
104         },
105 };
106
107 /* show */
108
109 prog_char str_traj_speed_show_arg[] = "show";
110 parse_pgm_token_string_t cmd_traj_speed_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_traj_speed_result, arg1, str_traj_speed_show_arg);
111
112 prog_char help_traj_speed_show[] = "Show traj_speed values for trajectory manager";
113 parse_pgm_inst_t cmd_traj_speed_show = {
114         .f = cmd_traj_speed_parsed,  /* function to call */
115         .data = NULL,      /* 2nd arg of func */
116         .help_str = help_traj_speed_show,
117         .tokens = {        /* token list, NULL terminated */
118                 (prog_void *)&cmd_traj_speed_arg0, 
119                 (prog_void *)&cmd_traj_speed_show_arg,
120                 NULL,
121         },
122 };
123
124 /**********************************************************/
125 /* circle coef configuration */
126
127 /* this structure is filled when cmd_circle_coef is parsed successfully */
128 struct cmd_circle_coef_result {
129         fixed_string_t arg0;
130         fixed_string_t arg1;
131         float circle_coef;
132 };
133
134
135 /* function called when cmd_circle_coef is parsed successfully */
136 static void cmd_circle_coef_parsed(void *parsed_result, void *data)
137 {
138         struct cmd_circle_coef_result *res = parsed_result;
139
140         if (!strcmp_P(res->arg1, PSTR("set"))) {
141                 trajectory_set_circle_coef(&mainboard.traj, res->circle_coef);
142         }
143
144         printf_P(PSTR("circle_coef set %2.2f\r\n"), mainboard.traj.circle_coef);
145 }
146
147 prog_char str_circle_coef_arg0[] = "circle_coef";
148 parse_pgm_token_string_t cmd_circle_coef_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_circle_coef_result, arg0, str_circle_coef_arg0);
149 prog_char str_circle_coef_arg1[] = "set";
150 parse_pgm_token_string_t cmd_circle_coef_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_circle_coef_result, arg1, str_circle_coef_arg1);
151 parse_pgm_token_num_t cmd_circle_coef_val = TOKEN_NUM_INITIALIZER(struct cmd_circle_coef_result, circle_coef, FLOAT);
152
153 prog_char help_circle_coef[] = "Set circle coef";
154 parse_pgm_inst_t cmd_circle_coef = {
155         .f = cmd_circle_coef_parsed,  /* function to call */
156         .data = NULL,      /* 2nd arg of func */
157         .help_str = help_circle_coef,
158         .tokens = {        /* token list, NULL terminated */
159                 (prog_void *)&cmd_circle_coef_arg0,
160                 (prog_void *)&cmd_circle_coef_arg1,
161                 (prog_void *)&cmd_circle_coef_val,
162                 NULL,
163         },
164 };
165
166 /* show */
167
168 prog_char str_circle_coef_show_arg[] = "show";
169 parse_pgm_token_string_t cmd_circle_coef_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_circle_coef_result, arg1, str_circle_coef_show_arg);
170
171 prog_char help_circle_coef_show[] = "Show circle coef";
172 parse_pgm_inst_t cmd_circle_coef_show = {
173         .f = cmd_circle_coef_parsed,  /* function to call */
174         .data = NULL,      /* 2nd arg of func */
175         .help_str = help_circle_coef_show,
176         .tokens = {        /* token list, NULL terminated */
177                 (prog_void *)&cmd_circle_coef_arg0, 
178                 (prog_void *)&cmd_circle_coef_show_arg,
179                 NULL,
180         },
181 };
182
183 /**********************************************************/
184 /* trajectory window configuration */
185
186 /* this structure is filled when cmd_trajectory is parsed successfully */
187 struct cmd_trajectory_result {
188         fixed_string_t arg0;
189         fixed_string_t arg1;
190         float d_win;
191         float a_win;
192         float a_start;
193 };
194
195
196 /* function called when cmd_trajectory is parsed successfully */
197 static void cmd_trajectory_parsed(void * parsed_result, void * data)
198 {
199         struct cmd_trajectory_result * res = parsed_result;
200         
201         if (!strcmp_P(res->arg1, PSTR("set"))) {
202                 trajectory_set_windows(&mainboard.traj, res->d_win,
203                                        res->a_win, res->a_start);
204         }
205
206         printf_P(PSTR("trajectory %2.2f %2.2f %2.2f\r\n"), mainboard.traj.d_win,
207                  DEG(mainboard.traj.a_win_rad), DEG(mainboard.traj.a_start_rad));
208 }
209
210 prog_char str_trajectory_arg0[] = "trajectory";
211 parse_pgm_token_string_t cmd_trajectory_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_trajectory_result, arg0, str_trajectory_arg0);
212 prog_char str_trajectory_arg1[] = "set";
213 parse_pgm_token_string_t cmd_trajectory_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_trajectory_result, arg1, str_trajectory_arg1);
214 parse_pgm_token_num_t cmd_trajectory_d = TOKEN_NUM_INITIALIZER(struct cmd_trajectory_result, d_win, FLOAT);
215 parse_pgm_token_num_t cmd_trajectory_a = TOKEN_NUM_INITIALIZER(struct cmd_trajectory_result, a_win, FLOAT);
216 parse_pgm_token_num_t cmd_trajectory_as = TOKEN_NUM_INITIALIZER(struct cmd_trajectory_result, a_start, FLOAT);
217
218 prog_char help_trajectory[] = "Set trajectory windows (distance, angle, angle_start)";
219 parse_pgm_inst_t cmd_trajectory = {
220         .f = cmd_trajectory_parsed,  /* function to call */
221         .data = NULL,      /* 2nd arg of func */
222         .help_str = help_trajectory,
223         .tokens = {        /* token list, NULL terminated */
224                 (prog_void *)&cmd_trajectory_arg0, 
225                 (prog_void *)&cmd_trajectory_arg1, 
226                 (prog_void *)&cmd_trajectory_d, 
227                 (prog_void *)&cmd_trajectory_a, 
228                 (prog_void *)&cmd_trajectory_as, 
229                 NULL,
230         },
231 };
232
233 /* show */
234
235 prog_char str_trajectory_show_arg[] = "show";
236 parse_pgm_token_string_t cmd_trajectory_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_trajectory_result, arg1, str_trajectory_show_arg);
237
238 prog_char help_trajectory_show[] = "Show trajectory window configuration";
239 parse_pgm_inst_t cmd_trajectory_show = {
240         .f = cmd_trajectory_parsed,  /* function to call */
241         .data = NULL,      /* 2nd arg of func */
242         .help_str = help_trajectory_show,
243         .tokens = {        /* token list, NULL terminated */
244                 (prog_void *)&cmd_trajectory_arg0, 
245                 (prog_void *)&cmd_trajectory_show_arg,
246                 NULL,
247         },
248 };
249
250 /**********************************************************/
251 /* rs_gains configuration */
252
253 /* this structure is filled when cmd_rs_gains is parsed successfully */
254 struct cmd_rs_gains_result {
255         fixed_string_t arg0;
256         fixed_string_t arg1;
257         float left;
258         float right;
259 };
260
261 /* function called when cmd_rs_gains is parsed successfully */
262 static void cmd_rs_gains_parsed(void * parsed_result, void * data)
263 {
264 #ifdef HOST_VERSION
265         printf("not implemented\n");
266 #else
267         struct cmd_rs_gains_result * res = parsed_result;
268
269         if (!strcmp_P(res->arg1, PSTR("set"))) {
270                 rs_set_left_ext_encoder(&mainboard.rs, encoders_spi_get_value, 
271                                         LEFT_ENCODER, res->left); // en augmentant on tourne à gauche
272                 rs_set_right_ext_encoder(&mainboard.rs, encoders_spi_get_value, 
273                                          RIGHT_ENCODER, res->right); //en augmentant on tourne à droite
274         }
275         printf_P(PSTR("rs_gains set "));
276         f64_print(mainboard.rs.left_ext_gain);
277         printf_P(PSTR(" "));
278         f64_print(mainboard.rs.right_ext_gain);
279         printf_P(PSTR("\r\n"));
280 #endif
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  restart:
445         for (i=0 ; i<pt_list_len ; i++) {
446                 printf_P(PSTR("%d: x=%d y=%d\r\n"), i, pt_list[i].x, pt_list[i].y);
447                 if (!strcmp_P(res->arg1, PSTR("start"))) {
448                         trajectory_goto_xy_abs(&mainboard.traj, pt_list[i].x, pt_list[i].y);
449                         why = wait_traj_end(0xFF); /* all */
450                 }
451                 else if (!strcmp_P(res->arg1, PSTR("loop_start"))) {
452                         trajectory_goto_xy_abs(&mainboard.traj, pt_list[i].x, pt_list[i].y);
453                         why = wait_traj_end(0xFF); /* all */
454                 }
455                 else if (!strcmp_P(res->arg1, PSTR("avoid_start"))) {
456                         while (1) {
457                                 why = goto_and_avoid(pt_list[i].x, pt_list[i].y, 0xFF, 0xFF);
458                                 printf("next point\r\n");
459                                 if (why != END_OBSTACLE)
460                                         break;
461                         }
462                 }
463                 if (why & (~(END_TRAJ | END_NEAR)))
464                         trajectory_stop(&mainboard.traj);
465                 if (why & END_INTR)
466                         break;
467         }
468         if (why & END_INTR)
469                 return;
470         if (!strcmp_P(res->arg1, PSTR("loop_start")))
471                 goto restart;
472 }
473
474 prog_char str_pt_list_arg0[] = "pt_list";
475 parse_pgm_token_string_t cmd_pt_list_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg0, str_pt_list_arg0);
476 prog_char str_pt_list_arg1[] = "insert";
477 parse_pgm_token_string_t cmd_pt_list_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_arg1);
478 parse_pgm_token_num_t cmd_pt_list_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_pt_list_result, arg2, UINT16);
479 parse_pgm_token_num_t cmd_pt_list_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_pt_list_result, arg3, INT16);
480 parse_pgm_token_num_t cmd_pt_list_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_pt_list_result, arg4, INT16);
481
482 prog_char help_pt_list[] = "Insert point in pt_list (idx,x,y)";
483 parse_pgm_inst_t cmd_pt_list = {
484         .f = cmd_pt_list_parsed,  /* function to call */
485         .data = NULL,      /* 2nd arg of func */
486         .help_str = help_pt_list,
487         .tokens = {        /* token list, NULL terminated */
488                 (prog_void *)&cmd_pt_list_arg0, 
489                 (prog_void *)&cmd_pt_list_arg1, 
490                 (prog_void *)&cmd_pt_list_arg2, 
491                 (prog_void *)&cmd_pt_list_arg3, 
492                 (prog_void *)&cmd_pt_list_arg4, 
493                 NULL,
494         },
495 };
496
497 /* append */
498
499 prog_char str_pt_list_arg1_append[] = "append";
500 parse_pgm_token_string_t cmd_pt_list_arg1_append = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_arg1_append);
501
502 prog_char help_pt_list_append[] = "Append point in pt_list (x,y)";
503 parse_pgm_inst_t cmd_pt_list_append = {
504         .f = cmd_pt_list_parsed,  /* function to call */
505         .data = NULL,      /* 2nd arg of func */
506         .help_str = help_pt_list_append,
507         .tokens = {        /* token list, NULL terminated */
508                 (prog_void *)&cmd_pt_list_arg0, 
509                 (prog_void *)&cmd_pt_list_arg1_append, 
510                 (prog_void *)&cmd_pt_list_arg3, 
511                 (prog_void *)&cmd_pt_list_arg4, 
512                 NULL,
513         },
514 };
515
516 /* del */
517
518 prog_char str_pt_list_del_arg[] = "del";
519 parse_pgm_token_string_t cmd_pt_list_del_arg = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_del_arg);
520
521 prog_char help_pt_list_del[] = "Del or insert point in pt_list (num)";
522 parse_pgm_inst_t cmd_pt_list_del = {
523         .f = cmd_pt_list_parsed,  /* function to call */
524         .data = NULL,      /* 2nd arg of func */
525         .help_str = help_pt_list_del,
526         .tokens = {        /* token list, NULL terminated */
527                 (prog_void *)&cmd_pt_list_arg0, 
528                 (prog_void *)&cmd_pt_list_del_arg, 
529                 (prog_void *)&cmd_pt_list_arg2,
530                 NULL,
531         },
532 };
533 /* show */
534
535 prog_char str_pt_list_show_arg[] = "show#reset#start#avoid_start#loop_start";
536 parse_pgm_token_string_t cmd_pt_list_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_show_arg);
537
538 prog_char help_pt_list_show[] = "Show, start or reset pt_list";
539 parse_pgm_inst_t cmd_pt_list_show = {
540         .f = cmd_pt_list_parsed,  /* function to call */
541         .data = NULL,      /* 2nd arg of func */
542         .help_str = help_pt_list_show,
543         .tokens = {        /* token list, NULL terminated */
544                 (prog_void *)&cmd_pt_list_arg0, 
545                 (prog_void *)&cmd_pt_list_show_arg,
546                 NULL,
547         },
548 };
549
550
551
552 /**********************************************************/
553 /* Goto function */
554
555 /* this structure is filled when cmd_goto is parsed successfully */
556 struct cmd_goto_result {
557         fixed_string_t arg0;
558         fixed_string_t arg1;
559         int32_t arg2;
560         int32_t arg3;
561         int32_t arg4;
562         int32_t arg5;
563 };
564
565 /* function called when cmd_goto is parsed successfully */
566 static void cmd_goto_parsed(void * parsed_result, void * data)
567 {
568         struct cmd_goto_result * res = parsed_result;
569         uint8_t err;
570         microseconds t1, t2;
571
572         interrupt_traj_reset();
573         if (!strcmp_P(res->arg1, PSTR("a_rel"))) {
574                 trajectory_a_rel(&mainboard.traj, res->arg2);
575         }
576         else if (!strcmp_P(res->arg1, PSTR("d_rel"))) {
577                 trajectory_d_rel(&mainboard.traj, res->arg2);
578         }
579         else if (!strcmp_P(res->arg1, PSTR("a_abs"))) {
580                 trajectory_a_abs(&mainboard.traj, res->arg2);
581         }
582         else if (!strcmp_P(res->arg1, PSTR("a_to_xy"))) {
583                 trajectory_turnto_xy(&mainboard.traj, res->arg2, res->arg3);
584         }
585         else if (!strcmp_P(res->arg1, PSTR("a_behind_xy"))) {
586                 trajectory_turnto_xy_behind(&mainboard.traj, res->arg2, res->arg3);
587         }
588         else if (!strcmp_P(res->arg1, PSTR("xy_rel"))) {
589                 trajectory_goto_xy_rel(&mainboard.traj, res->arg2, res->arg3);
590         }
591         else if (!strcmp_P(res->arg1, PSTR("xy_abs"))) {
592                 trajectory_goto_xy_abs(&mainboard.traj, res->arg2, res->arg3);
593         }
594         else if (!strcmp_P(res->arg1, PSTR("avoid"))) {
595                 err = goto_and_avoid_forward(res->arg2, res->arg3, 0xFF, 0xFF);
596                 if (err != END_TRAJ && err != END_NEAR)
597                         strat_hardstop();
598         }
599         else if (!strcmp_P(res->arg1, PSTR("avoid_bw"))) {
600                 err = goto_and_avoid_backward(res->arg2, res->arg3, 0xFF, 0xFF);
601                 if (err != END_TRAJ && err != END_NEAR)
602                         strat_hardstop();
603         }
604         else if (!strcmp_P(res->arg1, PSTR("xy_abs_fow"))) {
605                 trajectory_goto_forward_xy_abs(&mainboard.traj, res->arg2, res->arg3);
606         }
607         else if (!strcmp_P(res->arg1, PSTR("xy_abs_back"))) {
608                 trajectory_goto_backward_xy_abs(&mainboard.traj, res->arg2, res->arg3);
609         }
610         else if (!strcmp_P(res->arg1, PSTR("da_rel"))) {
611                 trajectory_d_a_rel(&mainboard.traj, res->arg2, res->arg3);
612         }
613         else if (!strcmp_P(res->arg1, PSTR("circle_rel"))) {
614                 trajectory_circle_rel(&mainboard.traj, res->arg2, res->arg3,
615                                       res->arg4, res->arg5, 0);
616                 return; /* XXX */
617         }
618         t1 = time_get_us2();
619         while ((err = test_traj_end(0xFF)) == 0) {
620                 t2 = time_get_us2();
621                 if (t2 - t1 > 200000) {
622                         dump_cs_debug("angle", &mainboard.angle.cs);
623                         dump_cs_debug("distance", &mainboard.distance.cs);
624                         t1 = t2;
625                 }
626         }
627         if (err != END_TRAJ && err != END_NEAR)
628                 strat_hardstop();
629         printf_P(PSTR("returned %s\r\n"), get_err(err));
630 }
631
632 prog_char str_goto_arg0[] = "goto";
633 parse_pgm_token_string_t cmd_goto_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_goto_result, arg0, str_goto_arg0);
634 prog_char str_goto_arg1_a[] = "d_rel#a_rel#a_abs";
635 parse_pgm_token_string_t cmd_goto_arg1_a = TOKEN_STRING_INITIALIZER(struct cmd_goto_result, arg1, str_goto_arg1_a);
636 parse_pgm_token_num_t cmd_goto_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_goto_result, arg2, INT32);
637
638 /* 1 params */
639 prog_char help_goto1[] = "Change orientation of the mainboard";
640 parse_pgm_inst_t cmd_goto1 = {
641         .f = cmd_goto_parsed,  /* function to call */
642         .data = NULL,      /* 2nd arg of func */
643         .help_str = help_goto1,
644         .tokens = {        /* token list, NULL terminated */
645                 (prog_void *)&cmd_goto_arg0, 
646                 (prog_void *)&cmd_goto_arg1_a, 
647                 (prog_void *)&cmd_goto_arg2, 
648                 NULL,
649         },
650 };
651
652 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";
653 parse_pgm_token_string_t cmd_goto_arg1_b = TOKEN_STRING_INITIALIZER(struct cmd_goto_result, arg1, str_goto_arg1_b);
654 parse_pgm_token_num_t cmd_goto_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_goto_result, arg3, INT32);
655
656 /* 2 params */
657 prog_char help_goto2[] = "Go to a (x,y) or (d,a) position";
658 parse_pgm_inst_t cmd_goto2 = {
659         .f = cmd_goto_parsed,  /* function to call */
660         .data = NULL,      /* 2nd arg of func */
661         .help_str = help_goto2,
662         .tokens = {        /* token list, NULL terminated */
663                 (prog_void *)&cmd_goto_arg0, 
664                 (prog_void *)&cmd_goto_arg1_b, 
665                 (prog_void *)&cmd_goto_arg2,
666                 (prog_void *)&cmd_goto_arg3, 
667                 NULL,
668         },
669 };
670
671 prog_char str_goto_arg1_c[] = "circle_rel";
672 parse_pgm_token_string_t cmd_goto_arg1_c = TOKEN_STRING_INITIALIZER(struct cmd_goto_result, arg1, str_goto_arg1_c);
673 parse_pgm_token_num_t cmd_goto_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_goto_result, arg4, INT32);
674 parse_pgm_token_num_t cmd_goto_arg5 = TOKEN_NUM_INITIALIZER(struct cmd_goto_result, arg5, INT32);
675
676 /* 4 params */
677 prog_char help_goto4[] = "Do a circle: (x,y, radius, angle)";
678 parse_pgm_inst_t cmd_goto4 = {
679         .f = cmd_goto_parsed,  /* function to call */
680         .data = NULL,      /* 2nd arg of func */
681         .help_str = help_goto4,
682         .tokens = {        /* token list, NULL terminated */
683                 (prog_void *)&cmd_goto_arg0,
684                 (prog_void *)&cmd_goto_arg1_c,
685                 (prog_void *)&cmd_goto_arg2,
686                 (prog_void *)&cmd_goto_arg3,
687                 (prog_void *)&cmd_goto_arg4,
688                 (prog_void *)&cmd_goto_arg5,
689                 NULL,
690         },
691 };
692
693 /**********************************************************/
694 /* Position tests */
695
696 /* this structure is filled when cmd_position is parsed successfully */
697 struct cmd_position_result {
698         fixed_string_t arg0;
699         fixed_string_t arg1;
700         int32_t arg2;
701         int32_t arg3;
702         int32_t arg4;
703 };
704
705 #define AUTOPOS_SPEED_FAST 200
706 static void auto_position(void)
707 {
708         uint8_t err;
709         uint16_t old_spdd, old_spda;
710
711         interrupt_traj_reset();
712         strat_get_speed(&old_spdd, &old_spda);
713         strat_set_speed(AUTOPOS_SPEED_FAST, AUTOPOS_SPEED_FAST);
714
715         trajectory_d_rel(&mainboard.traj, -300);
716         err = wait_traj_end(END_INTR|END_TRAJ|END_BLOCKING);
717         if (err == END_INTR)
718                 goto intr;
719         wait_ms(100);
720         strat_reset_pos(ROBOT_LENGTH/2, 0, 0);
721
722         trajectory_d_rel(&mainboard.traj, 120);
723         err = wait_traj_end(END_INTR|END_TRAJ);
724         if (err == END_INTR)
725                 goto intr;
726
727         trajectory_a_rel(&mainboard.traj, COLOR_A(90));
728         err = wait_traj_end(END_INTR|END_TRAJ);
729         if (err == END_INTR)
730                 goto intr;
731
732         trajectory_d_rel(&mainboard.traj, -300);
733         err = wait_traj_end(END_INTR|END_TRAJ|END_BLOCKING);
734         if (err == END_INTR)
735                 goto intr;
736         wait_ms(100);
737         strat_reset_pos(DO_NOT_SET_POS, COLOR_Y(ROBOT_LENGTH/2),
738                         COLOR_A(90));
739
740         trajectory_d_rel(&mainboard.traj, 120);
741         err = wait_traj_end(END_INTR|END_TRAJ);
742         if (err == END_INTR)
743                 goto intr;
744         wait_ms(100);
745         
746         trajectory_a_rel(&mainboard.traj, COLOR_A(-40));
747         err = wait_traj_end(END_INTR|END_TRAJ);
748         if (err == END_INTR)
749                 goto intr;
750         wait_ms(100);
751         
752         strat_set_speed(old_spdd, old_spda);
753         return;
754
755 intr:
756         strat_hardstop();
757         strat_set_speed(old_spdd, old_spda);
758 }
759
760 /* function called when cmd_position is parsed successfully */
761 static void cmd_position_parsed(void * parsed_result, void * data)
762 {
763         struct cmd_position_result * res = parsed_result;
764         
765         /* display raw position values */
766         if (!strcmp_P(res->arg1, PSTR("reset"))) {
767                 position_set(&mainboard.pos, 0, 0, 0);
768         }
769         else if (!strcmp_P(res->arg1, PSTR("set"))) {
770                 position_set(&mainboard.pos, res->arg2, res->arg3, res->arg4);
771         }
772         else if (!strcmp_P(res->arg1, PSTR("autoset_green"))) {
773                 mainboard.our_color = I2C_COLOR_GREEN;
774 #ifndef HOST_VERSION
775                 i2c_set_color(I2C_MECHBOARD_ADDR, I2C_COLOR_GREEN);
776                 i2c_set_color(I2C_SENSORBOARD_ADDR, I2C_COLOR_GREEN);
777 #endif
778                 auto_position();
779         }
780         else if (!strcmp_P(res->arg1, PSTR("autoset_red"))) {
781                 mainboard.our_color = I2C_COLOR_RED;
782 #ifndef HOST_VERSION
783                 i2c_set_color(I2C_MECHBOARD_ADDR, I2C_COLOR_RED);
784                 i2c_set_color(I2C_SENSORBOARD_ADDR, I2C_COLOR_RED);
785 #endif
786                 auto_position();
787         }
788
789         /* else it's just a "show" */
790         printf_P(PSTR("x=%.2f y=%.2f a=%.2f\r\n"), 
791                  position_get_x_double(&mainboard.pos),
792                  position_get_y_double(&mainboard.pos),
793                  DEG(position_get_a_rad_double(&mainboard.pos)));
794 }
795
796 prog_char str_position_arg0[] = "position";
797 parse_pgm_token_string_t cmd_position_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg0, str_position_arg0);
798 prog_char str_position_arg1[] = "show#reset#autoset_green#autoset_red";
799 parse_pgm_token_string_t cmd_position_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg1, str_position_arg1);
800
801 prog_char help_position[] = "Show/reset (x,y,a) position";
802 parse_pgm_inst_t cmd_position = {
803         .f = cmd_position_parsed,  /* function to call */
804         .data = NULL,      /* 2nd arg of func */
805         .help_str = help_position,
806         .tokens = {        /* token list, NULL terminated */
807                 (prog_void *)&cmd_position_arg0, 
808                 (prog_void *)&cmd_position_arg1, 
809                 NULL,
810         },
811 };
812
813
814 prog_char str_position_arg1_set[] = "set";
815 parse_pgm_token_string_t cmd_position_arg1_set = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg1, str_position_arg1_set);
816 parse_pgm_token_num_t cmd_position_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg2, INT32);
817 parse_pgm_token_num_t cmd_position_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg3, INT32);
818 parse_pgm_token_num_t cmd_position_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg4, INT32);
819
820 prog_char help_position_set[] = "Set (x,y,a) position";
821 parse_pgm_inst_t cmd_position_set = {
822         .f = cmd_position_parsed,  /* function to call */
823         .data = NULL,      /* 2nd arg of func */
824         .help_str = help_position_set,
825         .tokens = {        /* token list, NULL terminated */
826                 (prog_void *)&cmd_position_arg0, 
827                 (prog_void *)&cmd_position_arg1_set, 
828                 (prog_void *)&cmd_position_arg2, 
829                 (prog_void *)&cmd_position_arg3, 
830                 (prog_void *)&cmd_position_arg4, 
831                 NULL,
832         },
833 };
834
835
836 /**********************************************************/
837 /* strat configuration */
838
839 /* this structure is filled when cmd_strat_infos is parsed successfully */
840 struct cmd_strat_infos_result {
841         fixed_string_t arg0;
842         fixed_string_t arg1;
843 };
844
845 /* function called when cmd_strat_infos is parsed successfully */
846 static void cmd_strat_infos_parsed(void *parsed_result, void *data)
847 {
848         struct cmd_strat_infos_result *res = parsed_result;
849
850         if (!strcmp_P(res->arg1, PSTR("reset"))) {
851                 strat_reset_infos();
852         }
853         strat_infos.dump_enabled = 1;
854         strat_dump_infos(__FUNCTION__);
855 }
856
857 prog_char str_strat_infos_arg0[] = "strat_infos";
858 parse_pgm_token_string_t cmd_strat_infos_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_infos_result, arg0, str_strat_infos_arg0);
859 prog_char str_strat_infos_arg1[] = "show#reset";
860 parse_pgm_token_string_t cmd_strat_infos_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_infos_result, arg1, str_strat_infos_arg1);
861
862 prog_char help_strat_infos[] = "reset/show strat_infos";
863 parse_pgm_inst_t cmd_strat_infos = {
864         .f = cmd_strat_infos_parsed,  /* function to call */
865         .data = NULL,      /* 2nd arg of func */
866         .help_str = help_strat_infos,
867         .tokens = {        /* token list, NULL terminated */
868                 (prog_void *)&cmd_strat_infos_arg0, 
869                 (prog_void *)&cmd_strat_infos_arg1, 
870                 NULL,
871         },
872 };
873
874 /**********************************************************/
875 /* strat configuration */
876
877 /* this structure is filled when cmd_strat_conf is parsed successfully */
878 struct cmd_strat_conf_result {
879         fixed_string_t arg0;
880         fixed_string_t arg1;
881 };
882
883 /* function called when cmd_strat_conf is parsed successfully */
884 static void cmd_strat_conf_parsed(void *parsed_result, void *data)
885 {
886         struct cmd_strat_conf_result *res = parsed_result;
887
888         if (!strcmp_P(res->arg1, PSTR("base"))) {
889                 strat_infos.conf.flags = 0;
890                 strat_infos.conf.scan_our_min_time = 90;
891                 strat_infos.conf.delay_between_our_scan = 90;
892                 strat_infos.conf.scan_opp_min_time = 90;
893                 strat_infos.conf.delay_between_opp_scan = 90;
894         }
895         else if (!strcmp_P(res->arg1, PSTR("big3"))) {
896                 strat_infos.conf.flags = 
897                         STRAT_CONF_STORE_STATIC2 |
898                         STRAT_CONF_BIG_3_TEMPLE;
899                 strat_infos.conf.scan_our_min_time = 90;
900                 strat_infos.conf.delay_between_our_scan = 90;
901                 strat_infos.conf.scan_opp_min_time = 90;
902                 strat_infos.conf.delay_between_opp_scan = 90;
903         }
904         else if (!strcmp_P(res->arg1, PSTR("base_check"))) {
905                 strat_infos.conf.flags = 0;
906                 strat_infos.conf.scan_our_min_time = 35;
907                 strat_infos.conf.delay_between_our_scan = 90;
908                 strat_infos.conf.scan_opp_min_time = 90;
909                 strat_infos.conf.delay_between_opp_scan = 90;
910         }
911         else if (!strcmp_P(res->arg1, PSTR("big3_check"))) {
912                 strat_infos.conf.flags = 
913                         STRAT_CONF_STORE_STATIC2 |
914                         STRAT_CONF_BIG_3_TEMPLE;
915                 strat_infos.conf.scan_our_min_time = 35;
916                 strat_infos.conf.delay_between_our_scan = 90;
917                 strat_infos.conf.scan_opp_min_time = 90;
918                 strat_infos.conf.delay_between_opp_scan = 90;
919         }
920         else if (!strcmp_P(res->arg1, PSTR("offensive_early"))) {
921                 strat_infos.conf.flags = 
922                         STRAT_CONF_TAKE_ONE_LINTEL |
923                         STRAT_CONF_STORE_STATIC2 |
924                         STRAT_CONF_EARLY_SCAN |
925                         STRAT_CONF_PUSH_OPP_COLS;
926                 strat_infos.conf.scan_our_min_time = 50;
927                 strat_infos.conf.delay_between_our_scan = 90;
928                 strat_infos.conf.scan_opp_min_time = 15;
929                 strat_infos.conf.delay_between_opp_scan = 90;
930                 strat_infos.conf.wait_opponent = 5;
931         }
932         else if (!strcmp_P(res->arg1, PSTR("offensive_late"))) {
933                 strat_infos.conf.flags = STRAT_CONF_TAKE_ONE_LINTEL;
934                 strat_infos.conf.scan_our_min_time = 90;
935                 strat_infos.conf.delay_between_our_scan = 90;
936                 strat_infos.conf.scan_opp_min_time = 30;
937                 strat_infos.conf.delay_between_opp_scan = 90;
938         }
939         else if (!strcmp_P(res->arg1, PSTR("one_on_disc"))) {
940                 strat_infos.conf.flags = 
941                         STRAT_CONF_ONLY_ONE_ON_DISC;
942                 strat_infos.conf.scan_our_min_time = 90;
943                 strat_infos.conf.delay_between_our_scan = 90;
944                 strat_infos.conf.scan_opp_min_time = 90;
945                 strat_infos.conf.delay_between_opp_scan = 90;
946         }
947         strat_infos.dump_enabled = 1;
948         strat_dump_conf();
949 }
950
951 prog_char str_strat_conf_arg0[] = "strat_conf";
952 parse_pgm_token_string_t cmd_strat_conf_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf_result, arg0, str_strat_conf_arg0);
953 prog_char str_strat_conf_arg1[] = "show#base#big3#base_check#big3_check#offensive_early#offensive_late#one_on_disc";
954 parse_pgm_token_string_t cmd_strat_conf_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf_result, arg1, str_strat_conf_arg1);
955
956 prog_char help_strat_conf[] = "configure strat options";
957 parse_pgm_inst_t cmd_strat_conf = {
958         .f = cmd_strat_conf_parsed,  /* function to call */
959         .data = NULL,      /* 2nd arg of func */
960         .help_str = help_strat_conf,
961         .tokens = {        /* token list, NULL terminated */
962                 (prog_void *)&cmd_strat_conf_arg0, 
963                 (prog_void *)&cmd_strat_conf_arg1, 
964                 NULL,
965         },
966 };
967
968 /**********************************************************/
969 /* strat configuration */
970
971 /* this structure is filled when cmd_strat_conf2 is parsed successfully */
972 struct cmd_strat_conf2_result {
973         fixed_string_t arg0;
974         fixed_string_t arg1;
975         fixed_string_t arg2;
976 };
977
978 /* function called when cmd_strat_conf2 is parsed successfully */
979 static void cmd_strat_conf2_parsed(void *parsed_result, void *data)
980 {
981         struct cmd_strat_conf2_result *res = parsed_result;
982         uint8_t on, bit = 0;
983
984         if (!strcmp_P(res->arg2, PSTR("on")))
985                 on = 1;
986         else
987                 on = 0;
988         
989         if (!strcmp_P(res->arg1, PSTR("one_temple_on_disc")))
990                 bit = STRAT_CONF_ONLY_ONE_ON_DISC;
991         else if (!strcmp_P(res->arg1, PSTR("bypass_static2")))
992                 bit = STRAT_CONF_BYPASS_STATIC2;
993         else if (!strcmp_P(res->arg1, PSTR("take_one_lintel")))
994                 bit = STRAT_CONF_TAKE_ONE_LINTEL;
995         else if (!strcmp_P(res->arg1, PSTR("skip_when_check_fail")))
996                 bit = STRAT_CONF_TAKE_ONE_LINTEL;
997         else if (!strcmp_P(res->arg1, PSTR("store_static2")))
998                 bit = STRAT_CONF_STORE_STATIC2;
999         else if (!strcmp_P(res->arg1, PSTR("big3_temple")))
1000                 bit = STRAT_CONF_BIG_3_TEMPLE;
1001         else if (!strcmp_P(res->arg1, PSTR("early_opp_scan")))
1002                 bit = STRAT_CONF_EARLY_SCAN;
1003         else if (!strcmp_P(res->arg1, PSTR("push_opp_cols")))
1004                 bit = STRAT_CONF_PUSH_OPP_COLS;
1005
1006         if (on)
1007                 strat_infos.conf.flags |= bit;
1008         else
1009                 strat_infos.conf.flags &= (~bit);
1010
1011         strat_infos.dump_enabled = 1;
1012         strat_dump_conf();
1013 }
1014
1015 prog_char str_strat_conf2_arg0[] = "strat_conf";
1016 parse_pgm_token_string_t cmd_strat_conf2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg0, str_strat_conf2_arg0);
1017 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";
1018 parse_pgm_token_string_t cmd_strat_conf2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg1, str_strat_conf2_arg1);
1019 prog_char str_strat_conf2_arg2[] = "on#off";
1020 parse_pgm_token_string_t cmd_strat_conf2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg2, str_strat_conf2_arg2);
1021
1022
1023 prog_char help_strat_conf2[] = "configure strat options";
1024 parse_pgm_inst_t cmd_strat_conf2 = {
1025         .f = cmd_strat_conf2_parsed,  /* function to call */
1026         .data = NULL,      /* 2nd arg of func */
1027         .help_str = help_strat_conf2,
1028         .tokens = {        /* token list, NULL terminated */
1029                 (prog_void *)&cmd_strat_conf2_arg0, 
1030                 (prog_void *)&cmd_strat_conf2_arg1, 
1031                 (prog_void *)&cmd_strat_conf2_arg2, 
1032                 NULL,
1033         },
1034 };
1035
1036 /**********************************************************/
1037 /* strat configuration */
1038
1039 /* this structure is filled when cmd_strat_conf3 is parsed successfully */
1040 struct cmd_strat_conf3_result {
1041         fixed_string_t arg0;
1042         fixed_string_t arg1;
1043         uint8_t arg2;
1044 };
1045
1046 /* function called when cmd_strat_conf3 is parsed successfully */
1047 static void cmd_strat_conf3_parsed(void *parsed_result, void *data)
1048 {
1049         struct cmd_strat_conf3_result *res = parsed_result;
1050
1051         if (!strcmp_P(res->arg1, PSTR("scan_opponent_min_time"))) {
1052                 if (res->arg2 > 90)
1053                         res->arg2 = 90;
1054                 strat_infos.conf.scan_opp_min_time = res->arg2;
1055         }
1056         else if (!strcmp_P(res->arg1, PSTR("delay_between_opponent_scan"))) {
1057                 if (res->arg2 > 90)
1058                         res->arg2 = 90;
1059                 strat_infos.conf.delay_between_opp_scan = res->arg2;
1060         }
1061         else if (!strcmp_P(res->arg1, PSTR("scan_our_min_time"))) {
1062                 if (res->arg2 > 90)
1063                         res->arg2 = 90;
1064                 strat_infos.conf.scan_our_min_time = res->arg2;
1065         }
1066         else if (!strcmp_P(res->arg1, PSTR("delay_between_our_scan"))) {
1067                 if (res->arg2 > 90)
1068                         res->arg2 = 90;
1069                 strat_infos.conf.delay_between_our_scan = res->arg2;
1070         }
1071         else if (!strcmp_P(res->arg1, PSTR("wait_opponent"))) {
1072                 strat_infos.conf.wait_opponent = res->arg2;
1073         }
1074         else if (!strcmp_P(res->arg1, PSTR("lintel_min_time"))) {
1075                 strat_infos.conf.lintel_min_time = res->arg2;
1076         }
1077         strat_infos.dump_enabled = 1;
1078         strat_dump_conf();
1079 }
1080
1081 prog_char str_strat_conf3_arg0[] = "strat_conf";
1082 parse_pgm_token_string_t cmd_strat_conf3_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf3_result, arg0, str_strat_conf3_arg0);
1083 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";
1084 parse_pgm_token_string_t cmd_strat_conf3_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf3_result, arg1, str_strat_conf3_arg1);
1085 parse_pgm_token_num_t cmd_strat_conf3_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_strat_conf3_result, arg2, UINT16);
1086
1087 prog_char help_strat_conf3[] = "configure strat options";
1088 parse_pgm_inst_t cmd_strat_conf3 = {
1089         .f = cmd_strat_conf3_parsed,  /* function to call */
1090         .data = NULL,      /* 2nd arg of func */
1091         .help_str = help_strat_conf3,
1092         .tokens = {        /* token list, NULL terminated */
1093                 (prog_void *)&cmd_strat_conf3_arg0, 
1094                 (prog_void *)&cmd_strat_conf3_arg1, 
1095                 (prog_void *)&cmd_strat_conf3_arg2, 
1096                 NULL,
1097         },
1098 };
1099
1100 /**********************************************************/
1101 /* strat configuration */
1102
1103 /* this structure is filled when cmd_strat_conf4 is parsed successfully */
1104 struct cmd_strat_conf4_result {
1105         fixed_string_t arg0;
1106         fixed_string_t arg1;
1107         int16_t arg2;
1108 };
1109
1110 /* function called when cmd_strat_conf4 is parsed successfully */
1111 static void cmd_strat_conf4_parsed(void *parsed_result, void *data)
1112 {
1113         struct cmd_strat_conf4_result *res = parsed_result;
1114
1115         if (!strcmp_P(res->arg1, PSTR("scan_opponent_angle"))) {
1116                 strat_infos.conf.scan_opp_angle = res->arg2;
1117         }
1118         strat_infos.dump_enabled = 1;
1119         strat_dump_conf();
1120 }
1121
1122 prog_char str_strat_conf4_arg0[] = "strat_conf";
1123 parse_pgm_token_string_t cmd_strat_conf4_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf4_result, arg0, str_strat_conf4_arg0);
1124 prog_char str_strat_conf4_arg1[] = "scan_opponent_angle";
1125 parse_pgm_token_string_t cmd_strat_conf4_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf4_result, arg1, str_strat_conf4_arg1);
1126 parse_pgm_token_num_t cmd_strat_conf4_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_strat_conf4_result, arg2, UINT16);
1127
1128 prog_char help_strat_conf4[] = "configure strat options";
1129 parse_pgm_inst_t cmd_strat_conf4 = {
1130         .f = cmd_strat_conf4_parsed,  /* function to call */
1131         .data = NULL,      /* 2nd arg of func */
1132         .help_str = help_strat_conf4,
1133         .tokens = {        /* token list, NULL terminated */
1134                 (prog_void *)&cmd_strat_conf4_arg0, 
1135                 (prog_void *)&cmd_strat_conf4_arg1, 
1136                 (prog_void *)&cmd_strat_conf4_arg2, 
1137                 NULL,
1138         },
1139 };
1140
1141
1142 /**********************************************************/
1143 /* Subtraj */
1144
1145 /* this structure is filled when cmd_subtraj is parsed successfully */
1146 struct cmd_subtraj_result {
1147         fixed_string_t arg0;
1148         fixed_string_t arg1;
1149         int32_t arg2;
1150         int32_t arg3;
1151         int32_t arg4;
1152         int32_t arg5;
1153 };
1154
1155 /* function called when cmd_subtraj is parsed successfully */
1156 static void cmd_subtraj_parsed(void *parsed_result, void *data)
1157 {
1158 #ifdef HOST_VERSION
1159         printf("not implemented\n");
1160 #else
1161         struct cmd_subtraj_result *res = parsed_result;
1162         uint8_t err = 0;
1163         struct column_dispenser *disp;
1164
1165         if (strcmp_P(res->arg1, PSTR("static")) == 0) {
1166                 err = strat_static_columns(res->arg2);
1167         }
1168         else if (strcmp_P(res->arg1, PSTR("static2")) == 0) {
1169                 strat_infos.s_cols.configuration = res->arg2;
1170                 switch (res->arg2) {
1171                 case 1:
1172                         position_set(&mainboard.pos, 1398, 
1173                                      COLOR_Y(1297), COLOR_A(-66));
1174                         break;
1175                 case 2:
1176                         position_set(&mainboard.pos, 1232, 
1177                                      COLOR_Y(1051), COLOR_A(4));
1178                         break;
1179                 case 3:
1180                         position_set(&mainboard.pos, 1232, 
1181                                      COLOR_Y(1043), COLOR_A(5));
1182                         break;
1183                 case 4:
1184                         position_set(&mainboard.pos, 1346,
1185                                      COLOR_Y(852), COLOR_A(57));
1186                         break;
1187                 default:
1188                         return;
1189                 }
1190                 if (res->arg2 == 1 && res->arg3 == 1) {
1191                         strat_infos.s_cols.flags = STATIC_COL_LINE1_DONE;
1192                 }
1193                 if (res->arg2 == 1 && res->arg3 == 2) {
1194                         strat_infos.s_cols.flags = STATIC_COL_LINE2_DONE;
1195                 }
1196                 err = strat_static_columns_pass2();
1197         }
1198         else if (strcmp_P(res->arg1, PSTR("lintel1")) == 0) {
1199                 err = strat_goto_lintel_disp(&strat_infos.l1);
1200         }
1201         else if (strcmp_P(res->arg1, PSTR("lintel2")) == 0) {
1202                 err = strat_goto_lintel_disp(&strat_infos.l2);
1203         }
1204         else if (strcmp_P(res->arg1, PSTR("coldisp1")) == 0) {
1205                 disp = &strat_infos.c1;
1206                 err = strat_goto_col_disp(&disp);
1207         }
1208         else if (strcmp_P(res->arg1, PSTR("coldisp2")) == 0) {
1209                 disp = &strat_infos.c2;
1210                 err = strat_goto_col_disp(&disp);
1211         }
1212         else if (strcmp_P(res->arg1, PSTR("coldisp3")) == 0) {
1213                 disp = &strat_infos.c3;
1214                 err = strat_goto_col_disp(&disp);
1215         }
1216         else if (strcmp_P(res->arg1, PSTR("disc")) == 0) {
1217                 if (res->arg2 == 0) {
1218                         printf_P(PSTR("bad level\r\n"));
1219                         return;
1220                 }
1221                 err = strat_goto_disc(res->arg2);
1222         }
1223
1224         printf_P(PSTR("substrat returned %s\r\n"), get_err(err));
1225         trajectory_hardstop(&mainboard.traj);
1226 #endif
1227 }
1228
1229 prog_char str_subtraj_arg0[] = "subtraj";
1230 parse_pgm_token_string_t cmd_subtraj_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_subtraj_result, arg0, str_subtraj_arg0);
1231 prog_char str_subtraj_arg1[] = "static#disc#lintel1#lintel2#coldisp1#coldisp2#coldisp3#static2";
1232 parse_pgm_token_string_t cmd_subtraj_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_subtraj_result, arg1, str_subtraj_arg1);
1233 parse_pgm_token_num_t cmd_subtraj_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg2, INT32);
1234 parse_pgm_token_num_t cmd_subtraj_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg3, INT32);
1235 parse_pgm_token_num_t cmd_subtraj_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg4, INT32);
1236 parse_pgm_token_num_t cmd_subtraj_arg5 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg5, INT32);
1237
1238 prog_char help_subtraj[] = "Test sub-trajectories (a,b,c,d: specific params)";
1239 parse_pgm_inst_t cmd_subtraj = {
1240         .f = cmd_subtraj_parsed,  /* function to call */
1241         .data = NULL,      /* 2nd arg of func */
1242         .help_str = help_subtraj,
1243         .tokens = {        /* token list, NULL terminated */
1244                 (prog_void *)&cmd_subtraj_arg0, 
1245                 (prog_void *)&cmd_subtraj_arg1, 
1246                 (prog_void *)&cmd_subtraj_arg2, 
1247                 (prog_void *)&cmd_subtraj_arg3, 
1248                 (prog_void *)&cmd_subtraj_arg4, 
1249                 (prog_void *)&cmd_subtraj_arg5, 
1250                 NULL,
1251         },
1252 };