0e237983bcbd66be3386bc028604f5b69a14bae9
[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 %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         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 };
552
553 /* function called when cmd_goto is parsed successfully */
554 static void cmd_goto_parsed(void * parsed_result, void * data)
555 {
556         struct cmd_goto_result * res = parsed_result;
557         uint8_t err;
558         microseconds t1, t2;
559
560         interrupt_traj_reset();
561         if (!strcmp_P(res->arg1, PSTR("a_rel"))) {
562                 trajectory_a_rel(&mainboard.traj, res->arg2);
563         }
564         else if (!strcmp_P(res->arg1, PSTR("d_rel"))) {
565                 trajectory_d_rel(&mainboard.traj, res->arg2);
566         }
567         else if (!strcmp_P(res->arg1, PSTR("a_abs"))) {
568                 trajectory_a_abs(&mainboard.traj, res->arg2);
569         }
570         else if (!strcmp_P(res->arg1, PSTR("a_to_xy"))) {
571                 trajectory_turnto_xy(&mainboard.traj, res->arg2, res->arg3);
572         }
573         else if (!strcmp_P(res->arg1, PSTR("a_behind_xy"))) {
574                 trajectory_turnto_xy_behind(&mainboard.traj, res->arg2, res->arg3);
575         }
576         else if (!strcmp_P(res->arg1, PSTR("xy_rel"))) {
577                 trajectory_goto_xy_rel(&mainboard.traj, res->arg2, res->arg3);
578         }
579         else if (!strcmp_P(res->arg1, PSTR("xy_abs"))) {
580                 trajectory_goto_xy_abs(&mainboard.traj, res->arg2, res->arg3);
581         }
582         else if (!strcmp_P(res->arg1, PSTR("avoid"))) {
583                 err = goto_and_avoid_forward(res->arg2, res->arg3, 0xFF, 0xFF);
584                 if (err != END_TRAJ && err != END_NEAR)
585                         strat_hardstop();
586         }
587         else if (!strcmp_P(res->arg1, PSTR("avoid_bw"))) {
588                 err = goto_and_avoid_backward(res->arg2, res->arg3, 0xFF, 0xFF);
589                 if (err != END_TRAJ && err != END_NEAR)
590                         strat_hardstop();
591         }
592         else if (!strcmp_P(res->arg1, PSTR("xy_abs_fow"))) {
593                 trajectory_goto_forward_xy_abs(&mainboard.traj, res->arg2, res->arg3);
594         }
595         else if (!strcmp_P(res->arg1, PSTR("xy_abs_back"))) {
596                 trajectory_goto_backward_xy_abs(&mainboard.traj, res->arg2, res->arg3);
597         }
598         else if (!strcmp_P(res->arg1, PSTR("da_rel"))) {
599                 trajectory_d_a_rel(&mainboard.traj, res->arg2, res->arg3);
600         }
601         t1 = time_get_us2();
602         while ((err = test_traj_end(0xFF)) == 0) {
603                 t2 = time_get_us2();
604                 if (t2 - t1 > 200000) {
605                         dump_cs_debug("angle", &mainboard.angle.cs);
606                         dump_cs_debug("distance", &mainboard.distance.cs);
607                         t1 = t2;
608                 }
609         }
610         if (err != END_TRAJ && err != END_NEAR)
611                 strat_hardstop();
612         printf_P(PSTR("returned %s\r\n"), get_err(err));
613 }
614
615 prog_char str_goto_arg0[] = "goto";
616 parse_pgm_token_string_t cmd_goto_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_goto_result, arg0, str_goto_arg0);
617 prog_char str_goto_arg1_a[] = "d_rel#a_rel#a_abs";
618 parse_pgm_token_string_t cmd_goto_arg1_a = TOKEN_STRING_INITIALIZER(struct cmd_goto_result, arg1, str_goto_arg1_a);
619 parse_pgm_token_num_t cmd_goto_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_goto_result, arg2, INT32);
620
621 /* 1 params */
622 prog_char help_goto1[] = "Change orientation of the mainboard";
623 parse_pgm_inst_t cmd_goto1 = {
624         .f = cmd_goto_parsed,  /* function to call */
625         .data = NULL,      /* 2nd arg of func */
626         .help_str = help_goto1,
627         .tokens = {        /* token list, NULL terminated */
628                 (prog_void *)&cmd_goto_arg0, 
629                 (prog_void *)&cmd_goto_arg1_a, 
630                 (prog_void *)&cmd_goto_arg2, 
631                 NULL,
632         },
633 };
634
635 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";
636 parse_pgm_token_string_t cmd_goto_arg1_b = TOKEN_STRING_INITIALIZER(struct cmd_goto_result, arg1, str_goto_arg1_b);
637 parse_pgm_token_num_t cmd_goto_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_goto_result, arg3, INT32);
638
639 /* 2 params */
640 prog_char help_goto2[] = "Go to a (x,y) or (d,a) position";
641 parse_pgm_inst_t cmd_goto2 = {
642         .f = cmd_goto_parsed,  /* function to call */
643         .data = NULL,      /* 2nd arg of func */
644         .help_str = help_goto2,
645         .tokens = {        /* token list, NULL terminated */
646                 (prog_void *)&cmd_goto_arg0, 
647                 (prog_void *)&cmd_goto_arg1_b, 
648                 (prog_void *)&cmd_goto_arg2,
649                 (prog_void *)&cmd_goto_arg3, 
650                 NULL,
651         },
652 };
653
654 /**********************************************************/
655 /* Position tests */
656
657 /* this structure is filled when cmd_position is parsed successfully */
658 struct cmd_position_result {
659         fixed_string_t arg0;
660         fixed_string_t arg1;
661         int32_t arg2;
662         int32_t arg3;
663         int32_t arg4;
664 };
665
666 #define AUTOPOS_SPEED_FAST 200
667 static void auto_position(void)
668 {
669         uint8_t err;
670         uint16_t old_spdd, old_spda;
671
672         interrupt_traj_reset();
673         strat_get_speed(&old_spdd, &old_spda);
674         strat_set_speed(AUTOPOS_SPEED_FAST, AUTOPOS_SPEED_FAST);
675
676         trajectory_d_rel(&mainboard.traj, -300);
677         err = wait_traj_end(END_INTR|END_TRAJ|END_BLOCKING);
678         if (err == END_INTR)
679                 goto intr;
680         wait_ms(100);
681         strat_reset_pos(ROBOT_LENGTH/2, 0, 0);
682
683         trajectory_d_rel(&mainboard.traj, 120);
684         err = wait_traj_end(END_INTR|END_TRAJ);
685         if (err == END_INTR)
686                 goto intr;
687
688         trajectory_a_rel(&mainboard.traj, COLOR_A(90));
689         err = wait_traj_end(END_INTR|END_TRAJ);
690         if (err == END_INTR)
691                 goto intr;
692
693         trajectory_d_rel(&mainboard.traj, -300);
694         err = wait_traj_end(END_INTR|END_TRAJ|END_BLOCKING);
695         if (err == END_INTR)
696                 goto intr;
697         wait_ms(100);
698         strat_reset_pos(DO_NOT_SET_POS, COLOR_Y(ROBOT_LENGTH/2),
699                         COLOR_A(90));
700
701         trajectory_d_rel(&mainboard.traj, 120);
702         err = wait_traj_end(END_INTR|END_TRAJ);
703         if (err == END_INTR)
704                 goto intr;
705         wait_ms(100);
706         
707         trajectory_a_rel(&mainboard.traj, COLOR_A(-40));
708         err = wait_traj_end(END_INTR|END_TRAJ);
709         if (err == END_INTR)
710                 goto intr;
711         wait_ms(100);
712         
713         strat_set_speed(old_spdd, old_spda);
714         return;
715
716 intr:
717         strat_hardstop();
718         strat_set_speed(old_spdd, old_spda);
719 }
720
721 /* function called when cmd_position is parsed successfully */
722 static void cmd_position_parsed(void * parsed_result, void * data)
723 {
724         struct cmd_position_result * res = parsed_result;
725         
726         /* display raw position values */
727         if (!strcmp_P(res->arg1, PSTR("reset"))) {
728                 position_set(&mainboard.pos, 0, 0, 0);
729         }
730         else if (!strcmp_P(res->arg1, PSTR("set"))) {
731                 position_set(&mainboard.pos, res->arg2, res->arg3, res->arg4);
732         }
733         else if (!strcmp_P(res->arg1, PSTR("autoset_green"))) {
734                 mainboard.our_color = I2C_COLOR_GREEN;
735 #ifndef HOST_VERSION
736                 i2c_set_color(I2C_MECHBOARD_ADDR, I2C_COLOR_GREEN);
737                 i2c_set_color(I2C_SENSORBOARD_ADDR, I2C_COLOR_GREEN);
738 #endif
739                 auto_position();
740         }
741         else if (!strcmp_P(res->arg1, PSTR("autoset_red"))) {
742                 mainboard.our_color = I2C_COLOR_RED;
743 #ifndef HOST_VERSION
744                 i2c_set_color(I2C_MECHBOARD_ADDR, I2C_COLOR_RED);
745                 i2c_set_color(I2C_SENSORBOARD_ADDR, I2C_COLOR_RED);
746 #endif
747                 auto_position();
748         }
749
750         /* else it's just a "show" */
751         printf_P(PSTR("x=%.2f y=%.2f a=%.2f\r\n"), 
752                  position_get_x_double(&mainboard.pos),
753                  position_get_y_double(&mainboard.pos),
754                  DEG(position_get_a_rad_double(&mainboard.pos)));
755 }
756
757 prog_char str_position_arg0[] = "position";
758 parse_pgm_token_string_t cmd_position_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg0, str_position_arg0);
759 prog_char str_position_arg1[] = "show#reset#autoset_green#autoset_red";
760 parse_pgm_token_string_t cmd_position_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg1, str_position_arg1);
761
762 prog_char help_position[] = "Show/reset (x,y,a) position";
763 parse_pgm_inst_t cmd_position = {
764         .f = cmd_position_parsed,  /* function to call */
765         .data = NULL,      /* 2nd arg of func */
766         .help_str = help_position,
767         .tokens = {        /* token list, NULL terminated */
768                 (prog_void *)&cmd_position_arg0, 
769                 (prog_void *)&cmd_position_arg1, 
770                 NULL,
771         },
772 };
773
774
775 prog_char str_position_arg1_set[] = "set";
776 parse_pgm_token_string_t cmd_position_arg1_set = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg1, str_position_arg1_set);
777 parse_pgm_token_num_t cmd_position_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg2, INT32);
778 parse_pgm_token_num_t cmd_position_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg3, INT32);
779 parse_pgm_token_num_t cmd_position_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg4, INT32);
780
781 prog_char help_position_set[] = "Set (x,y,a) position";
782 parse_pgm_inst_t cmd_position_set = {
783         .f = cmd_position_parsed,  /* function to call */
784         .data = NULL,      /* 2nd arg of func */
785         .help_str = help_position_set,
786         .tokens = {        /* token list, NULL terminated */
787                 (prog_void *)&cmd_position_arg0, 
788                 (prog_void *)&cmd_position_arg1_set, 
789                 (prog_void *)&cmd_position_arg2, 
790                 (prog_void *)&cmd_position_arg3, 
791                 (prog_void *)&cmd_position_arg4, 
792                 NULL,
793         },
794 };
795
796
797 /**********************************************************/
798 /* strat configuration */
799
800 /* this structure is filled when cmd_strat_infos is parsed successfully */
801 struct cmd_strat_infos_result {
802         fixed_string_t arg0;
803         fixed_string_t arg1;
804 };
805
806 /* function called when cmd_strat_infos is parsed successfully */
807 static void cmd_strat_infos_parsed(void *parsed_result, void *data)
808 {
809         struct cmd_strat_infos_result *res = parsed_result;
810
811         if (!strcmp_P(res->arg1, PSTR("reset"))) {
812                 strat_reset_infos();
813         }
814         strat_infos.dump_enabled = 1;
815         strat_dump_infos(__FUNCTION__);
816 }
817
818 prog_char str_strat_infos_arg0[] = "strat_infos";
819 parse_pgm_token_string_t cmd_strat_infos_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_infos_result, arg0, str_strat_infos_arg0);
820 prog_char str_strat_infos_arg1[] = "show#reset";
821 parse_pgm_token_string_t cmd_strat_infos_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_infos_result, arg1, str_strat_infos_arg1);
822
823 prog_char help_strat_infos[] = "reset/show strat_infos";
824 parse_pgm_inst_t cmd_strat_infos = {
825         .f = cmd_strat_infos_parsed,  /* function to call */
826         .data = NULL,      /* 2nd arg of func */
827         .help_str = help_strat_infos,
828         .tokens = {        /* token list, NULL terminated */
829                 (prog_void *)&cmd_strat_infos_arg0, 
830                 (prog_void *)&cmd_strat_infos_arg1, 
831                 NULL,
832         },
833 };
834
835 /**********************************************************/
836 /* strat configuration */
837
838 /* this structure is filled when cmd_strat_conf is parsed successfully */
839 struct cmd_strat_conf_result {
840         fixed_string_t arg0;
841         fixed_string_t arg1;
842 };
843
844 /* function called when cmd_strat_conf is parsed successfully */
845 static void cmd_strat_conf_parsed(void *parsed_result, void *data)
846 {
847         struct cmd_strat_conf_result *res = parsed_result;
848
849         if (!strcmp_P(res->arg1, PSTR("base"))) {
850                 strat_infos.conf.flags = 0;
851                 strat_infos.conf.scan_our_min_time = 90;
852                 strat_infos.conf.delay_between_our_scan = 90;
853                 strat_infos.conf.scan_opp_min_time = 90;
854                 strat_infos.conf.delay_between_opp_scan = 90;
855         }
856         else if (!strcmp_P(res->arg1, PSTR("big3"))) {
857                 strat_infos.conf.flags = 
858                         STRAT_CONF_STORE_STATIC2 |
859                         STRAT_CONF_BIG_3_TEMPLE;
860                 strat_infos.conf.scan_our_min_time = 90;
861                 strat_infos.conf.delay_between_our_scan = 90;
862                 strat_infos.conf.scan_opp_min_time = 90;
863                 strat_infos.conf.delay_between_opp_scan = 90;
864         }
865         else if (!strcmp_P(res->arg1, PSTR("base_check"))) {
866                 strat_infos.conf.flags = 0;
867                 strat_infos.conf.scan_our_min_time = 35;
868                 strat_infos.conf.delay_between_our_scan = 90;
869                 strat_infos.conf.scan_opp_min_time = 90;
870                 strat_infos.conf.delay_between_opp_scan = 90;
871         }
872         else if (!strcmp_P(res->arg1, PSTR("big3_check"))) {
873                 strat_infos.conf.flags = 
874                         STRAT_CONF_STORE_STATIC2 |
875                         STRAT_CONF_BIG_3_TEMPLE;
876                 strat_infos.conf.scan_our_min_time = 35;
877                 strat_infos.conf.delay_between_our_scan = 90;
878                 strat_infos.conf.scan_opp_min_time = 90;
879                 strat_infos.conf.delay_between_opp_scan = 90;
880         }
881         else if (!strcmp_P(res->arg1, PSTR("offensive_early"))) {
882                 strat_infos.conf.flags = 
883                         STRAT_CONF_TAKE_ONE_LINTEL |
884                         STRAT_CONF_STORE_STATIC2 |
885                         STRAT_CONF_EARLY_SCAN |
886                         STRAT_CONF_PUSH_OPP_COLS;
887                 strat_infos.conf.scan_our_min_time = 50;
888                 strat_infos.conf.delay_between_our_scan = 90;
889                 strat_infos.conf.scan_opp_min_time = 15;
890                 strat_infos.conf.delay_between_opp_scan = 90;
891                 strat_infos.conf.wait_opponent = 5;
892         }
893         else if (!strcmp_P(res->arg1, PSTR("offensive_late"))) {
894                 strat_infos.conf.flags = STRAT_CONF_TAKE_ONE_LINTEL;
895                 strat_infos.conf.scan_our_min_time = 90;
896                 strat_infos.conf.delay_between_our_scan = 90;
897                 strat_infos.conf.scan_opp_min_time = 30;
898                 strat_infos.conf.delay_between_opp_scan = 90;
899         }
900         else if (!strcmp_P(res->arg1, PSTR("one_on_disc"))) {
901                 strat_infos.conf.flags = 
902                         STRAT_CONF_ONLY_ONE_ON_DISC;
903                 strat_infos.conf.scan_our_min_time = 90;
904                 strat_infos.conf.delay_between_our_scan = 90;
905                 strat_infos.conf.scan_opp_min_time = 90;
906                 strat_infos.conf.delay_between_opp_scan = 90;
907         }
908         strat_infos.dump_enabled = 1;
909         strat_dump_conf();
910 }
911
912 prog_char str_strat_conf_arg0[] = "strat_conf";
913 parse_pgm_token_string_t cmd_strat_conf_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf_result, arg0, str_strat_conf_arg0);
914 prog_char str_strat_conf_arg1[] = "show#base#big3#base_check#big3_check#offensive_early#offensive_late#one_on_disc";
915 parse_pgm_token_string_t cmd_strat_conf_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf_result, arg1, str_strat_conf_arg1);
916
917 prog_char help_strat_conf[] = "configure strat options";
918 parse_pgm_inst_t cmd_strat_conf = {
919         .f = cmd_strat_conf_parsed,  /* function to call */
920         .data = NULL,      /* 2nd arg of func */
921         .help_str = help_strat_conf,
922         .tokens = {        /* token list, NULL terminated */
923                 (prog_void *)&cmd_strat_conf_arg0, 
924                 (prog_void *)&cmd_strat_conf_arg1, 
925                 NULL,
926         },
927 };
928
929 /**********************************************************/
930 /* strat configuration */
931
932 /* this structure is filled when cmd_strat_conf2 is parsed successfully */
933 struct cmd_strat_conf2_result {
934         fixed_string_t arg0;
935         fixed_string_t arg1;
936         fixed_string_t arg2;
937 };
938
939 /* function called when cmd_strat_conf2 is parsed successfully */
940 static void cmd_strat_conf2_parsed(void *parsed_result, void *data)
941 {
942         struct cmd_strat_conf2_result *res = parsed_result;
943         uint8_t on, bit = 0;
944
945         if (!strcmp_P(res->arg2, PSTR("on")))
946                 on = 1;
947         else
948                 on = 0;
949         
950         if (!strcmp_P(res->arg1, PSTR("one_temple_on_disc")))
951                 bit = STRAT_CONF_ONLY_ONE_ON_DISC;
952         else if (!strcmp_P(res->arg1, PSTR("bypass_static2")))
953                 bit = STRAT_CONF_BYPASS_STATIC2;
954         else if (!strcmp_P(res->arg1, PSTR("take_one_lintel")))
955                 bit = STRAT_CONF_TAKE_ONE_LINTEL;
956         else if (!strcmp_P(res->arg1, PSTR("skip_when_check_fail")))
957                 bit = STRAT_CONF_TAKE_ONE_LINTEL;
958         else if (!strcmp_P(res->arg1, PSTR("store_static2")))
959                 bit = STRAT_CONF_STORE_STATIC2;
960         else if (!strcmp_P(res->arg1, PSTR("big3_temple")))
961                 bit = STRAT_CONF_BIG_3_TEMPLE;
962         else if (!strcmp_P(res->arg1, PSTR("early_opp_scan")))
963                 bit = STRAT_CONF_EARLY_SCAN;
964         else if (!strcmp_P(res->arg1, PSTR("push_opp_cols")))
965                 bit = STRAT_CONF_PUSH_OPP_COLS;
966
967         if (on)
968                 strat_infos.conf.flags |= bit;
969         else
970                 strat_infos.conf.flags &= (~bit);
971
972         strat_infos.dump_enabled = 1;
973         strat_dump_conf();
974 }
975
976 prog_char str_strat_conf2_arg0[] = "strat_conf";
977 parse_pgm_token_string_t cmd_strat_conf2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg0, str_strat_conf2_arg0);
978 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";
979 parse_pgm_token_string_t cmd_strat_conf2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg1, str_strat_conf2_arg1);
980 prog_char str_strat_conf2_arg2[] = "on#off";
981 parse_pgm_token_string_t cmd_strat_conf2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg2, str_strat_conf2_arg2);
982
983
984 prog_char help_strat_conf2[] = "configure strat options";
985 parse_pgm_inst_t cmd_strat_conf2 = {
986         .f = cmd_strat_conf2_parsed,  /* function to call */
987         .data = NULL,      /* 2nd arg of func */
988         .help_str = help_strat_conf2,
989         .tokens = {        /* token list, NULL terminated */
990                 (prog_void *)&cmd_strat_conf2_arg0, 
991                 (prog_void *)&cmd_strat_conf2_arg1, 
992                 (prog_void *)&cmd_strat_conf2_arg2, 
993                 NULL,
994         },
995 };
996
997 /**********************************************************/
998 /* strat configuration */
999
1000 /* this structure is filled when cmd_strat_conf3 is parsed successfully */
1001 struct cmd_strat_conf3_result {
1002         fixed_string_t arg0;
1003         fixed_string_t arg1;
1004         uint8_t arg2;
1005 };
1006
1007 /* function called when cmd_strat_conf3 is parsed successfully */
1008 static void cmd_strat_conf3_parsed(void *parsed_result, void *data)
1009 {
1010         struct cmd_strat_conf3_result *res = parsed_result;
1011
1012         if (!strcmp_P(res->arg1, PSTR("scan_opponent_min_time"))) {
1013                 if (res->arg2 > 90)
1014                         res->arg2 = 90;
1015                 strat_infos.conf.scan_opp_min_time = res->arg2;
1016         }
1017         else if (!strcmp_P(res->arg1, PSTR("delay_between_opponent_scan"))) {
1018                 if (res->arg2 > 90)
1019                         res->arg2 = 90;
1020                 strat_infos.conf.delay_between_opp_scan = res->arg2;
1021         }
1022         else if (!strcmp_P(res->arg1, PSTR("scan_our_min_time"))) {
1023                 if (res->arg2 > 90)
1024                         res->arg2 = 90;
1025                 strat_infos.conf.scan_our_min_time = res->arg2;
1026         }
1027         else if (!strcmp_P(res->arg1, PSTR("delay_between_our_scan"))) {
1028                 if (res->arg2 > 90)
1029                         res->arg2 = 90;
1030                 strat_infos.conf.delay_between_our_scan = res->arg2;
1031         }
1032         else if (!strcmp_P(res->arg1, PSTR("wait_opponent"))) {
1033                 strat_infos.conf.wait_opponent = res->arg2;
1034         }
1035         else if (!strcmp_P(res->arg1, PSTR("lintel_min_time"))) {
1036                 strat_infos.conf.lintel_min_time = res->arg2;
1037         }
1038         strat_infos.dump_enabled = 1;
1039         strat_dump_conf();
1040 }
1041
1042 prog_char str_strat_conf3_arg0[] = "strat_conf";
1043 parse_pgm_token_string_t cmd_strat_conf3_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf3_result, arg0, str_strat_conf3_arg0);
1044 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";
1045 parse_pgm_token_string_t cmd_strat_conf3_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf3_result, arg1, str_strat_conf3_arg1);
1046 parse_pgm_token_num_t cmd_strat_conf3_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_strat_conf3_result, arg2, UINT16);
1047
1048 prog_char help_strat_conf3[] = "configure strat options";
1049 parse_pgm_inst_t cmd_strat_conf3 = {
1050         .f = cmd_strat_conf3_parsed,  /* function to call */
1051         .data = NULL,      /* 2nd arg of func */
1052         .help_str = help_strat_conf3,
1053         .tokens = {        /* token list, NULL terminated */
1054                 (prog_void *)&cmd_strat_conf3_arg0, 
1055                 (prog_void *)&cmd_strat_conf3_arg1, 
1056                 (prog_void *)&cmd_strat_conf3_arg2, 
1057                 NULL,
1058         },
1059 };
1060
1061 /**********************************************************/
1062 /* strat configuration */
1063
1064 /* this structure is filled when cmd_strat_conf4 is parsed successfully */
1065 struct cmd_strat_conf4_result {
1066         fixed_string_t arg0;
1067         fixed_string_t arg1;
1068         int16_t arg2;
1069 };
1070
1071 /* function called when cmd_strat_conf4 is parsed successfully */
1072 static void cmd_strat_conf4_parsed(void *parsed_result, void *data)
1073 {
1074         struct cmd_strat_conf4_result *res = parsed_result;
1075
1076         if (!strcmp_P(res->arg1, PSTR("scan_opponent_angle"))) {
1077                 strat_infos.conf.scan_opp_angle = res->arg2;
1078         }
1079         strat_infos.dump_enabled = 1;
1080         strat_dump_conf();
1081 }
1082
1083 prog_char str_strat_conf4_arg0[] = "strat_conf";
1084 parse_pgm_token_string_t cmd_strat_conf4_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf4_result, arg0, str_strat_conf4_arg0);
1085 prog_char str_strat_conf4_arg1[] = "scan_opponent_angle";
1086 parse_pgm_token_string_t cmd_strat_conf4_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf4_result, arg1, str_strat_conf4_arg1);
1087 parse_pgm_token_num_t cmd_strat_conf4_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_strat_conf4_result, arg2, UINT16);
1088
1089 prog_char help_strat_conf4[] = "configure strat options";
1090 parse_pgm_inst_t cmd_strat_conf4 = {
1091         .f = cmd_strat_conf4_parsed,  /* function to call */
1092         .data = NULL,      /* 2nd arg of func */
1093         .help_str = help_strat_conf4,
1094         .tokens = {        /* token list, NULL terminated */
1095                 (prog_void *)&cmd_strat_conf4_arg0, 
1096                 (prog_void *)&cmd_strat_conf4_arg1, 
1097                 (prog_void *)&cmd_strat_conf4_arg2, 
1098                 NULL,
1099         },
1100 };
1101
1102
1103 /**********************************************************/
1104 /* Subtraj */
1105
1106 /* this structure is filled when cmd_subtraj is parsed successfully */
1107 struct cmd_subtraj_result {
1108         fixed_string_t arg0;
1109         fixed_string_t arg1;
1110         int32_t arg2;
1111         int32_t arg3;
1112         int32_t arg4;
1113         int32_t arg5;
1114 };
1115
1116 /* function called when cmd_subtraj is parsed successfully */
1117 static void cmd_subtraj_parsed(void *parsed_result, void *data)
1118 {
1119 #ifdef HOST_VERSION
1120         printf("not implemented\n");
1121 #else
1122         struct cmd_subtraj_result *res = parsed_result;
1123         uint8_t err = 0;
1124         struct column_dispenser *disp;
1125
1126         if (strcmp_P(res->arg1, PSTR("static")) == 0) {
1127                 err = strat_static_columns(res->arg2);
1128         }
1129         else if (strcmp_P(res->arg1, PSTR("static2")) == 0) {
1130                 strat_infos.s_cols.configuration = res->arg2;
1131                 switch (res->arg2) {
1132                 case 1:
1133                         position_set(&mainboard.pos, 1398, 
1134                                      COLOR_Y(1297), COLOR_A(-66));
1135                         break;
1136                 case 2:
1137                         position_set(&mainboard.pos, 1232, 
1138                                      COLOR_Y(1051), COLOR_A(4));
1139                         break;
1140                 case 3:
1141                         position_set(&mainboard.pos, 1232, 
1142                                      COLOR_Y(1043), COLOR_A(5));
1143                         break;
1144                 case 4:
1145                         position_set(&mainboard.pos, 1346,
1146                                      COLOR_Y(852), COLOR_A(57));
1147                         break;
1148                 default:
1149                         return;
1150                 }
1151                 if (res->arg2 == 1 && res->arg3 == 1) {
1152                         strat_infos.s_cols.flags = STATIC_COL_LINE1_DONE;
1153                 }
1154                 if (res->arg2 == 1 && res->arg3 == 2) {
1155                         strat_infos.s_cols.flags = STATIC_COL_LINE2_DONE;
1156                 }
1157                 err = strat_static_columns_pass2();
1158         }
1159         else if (strcmp_P(res->arg1, PSTR("lintel1")) == 0) {
1160                 err = strat_goto_lintel_disp(&strat_infos.l1);
1161         }
1162         else if (strcmp_P(res->arg1, PSTR("lintel2")) == 0) {
1163                 err = strat_goto_lintel_disp(&strat_infos.l2);
1164         }
1165         else if (strcmp_P(res->arg1, PSTR("coldisp1")) == 0) {
1166                 disp = &strat_infos.c1;
1167                 err = strat_goto_col_disp(&disp);
1168         }
1169         else if (strcmp_P(res->arg1, PSTR("coldisp2")) == 0) {
1170                 disp = &strat_infos.c2;
1171                 err = strat_goto_col_disp(&disp);
1172         }
1173         else if (strcmp_P(res->arg1, PSTR("coldisp3")) == 0) {
1174                 disp = &strat_infos.c3;
1175                 err = strat_goto_col_disp(&disp);
1176         }
1177         else if (strcmp_P(res->arg1, PSTR("disc")) == 0) {
1178                 if (res->arg2 == 0) {
1179                         printf_P(PSTR("bad level\r\n"));
1180                         return;
1181                 }
1182                 err = strat_goto_disc(res->arg2);
1183         }
1184
1185         printf_P(PSTR("substrat returned %s\r\n"), get_err(err));
1186         trajectory_hardstop(&mainboard.traj);
1187 #endif
1188 }
1189
1190 prog_char str_subtraj_arg0[] = "subtraj";
1191 parse_pgm_token_string_t cmd_subtraj_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_subtraj_result, arg0, str_subtraj_arg0);
1192 prog_char str_subtraj_arg1[] = "static#disc#lintel1#lintel2#coldisp1#coldisp2#coldisp3#static2";
1193 parse_pgm_token_string_t cmd_subtraj_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_subtraj_result, arg1, str_subtraj_arg1);
1194 parse_pgm_token_num_t cmd_subtraj_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg2, INT32);
1195 parse_pgm_token_num_t cmd_subtraj_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg3, INT32);
1196 parse_pgm_token_num_t cmd_subtraj_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg4, INT32);
1197 parse_pgm_token_num_t cmd_subtraj_arg5 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg5, INT32);
1198
1199 prog_char help_subtraj[] = "Test sub-trajectories (a,b,c,d: specific params)";
1200 parse_pgm_inst_t cmd_subtraj = {
1201         .f = cmd_subtraj_parsed,  /* function to call */
1202         .data = NULL,      /* 2nd arg of func */
1203         .help_str = help_subtraj,
1204         .tokens = {        /* token list, NULL terminated */
1205                 (prog_void *)&cmd_subtraj_arg0, 
1206                 (prog_void *)&cmd_subtraj_arg1, 
1207                 (prog_void *)&cmd_subtraj_arg2, 
1208                 (prog_void *)&cmd_subtraj_arg3, 
1209                 (prog_void *)&cmd_subtraj_arg4, 
1210                 (prog_void *)&cmd_subtraj_arg5, 
1211                 NULL,
1212         },
1213 };