beacon from 2009
[aversive.git] / projects / microb2010 / mainboard / commands_traj.c
1 /*
2  *  Copyright Droids Corporation (2009)
3  *
4  *  This program is free software; you can redistribute it and/or modify
5  *  it under the terms of the GNU General Public License as published by
6  *  the Free Software Foundation; either version 2 of the License, or
7  *  (at your option) any later version.
8  *
9  *  This program is distributed in the hope that it will be useful,
10  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
11  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
12  *  GNU General Public License for more details.
13  *
14  *  You should have received a copy of the GNU General Public License
15  *  along with this program; if not, write to the Free Software
16  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
17  *
18  *  Revision : $Id: commands_traj.c,v 1.8 2009-11-08 17:24:33 zer0 Exp $
19  *
20  *  Olivier MATZ <zer0@droids-corp.org>
21  */
22
23 #include <stdio.h>
24 #include <string.h>
25
26 #include <hostsim.h>
27 #include <aversive/pgmspace.h>
28 #include <aversive/wait.h>
29 #include <aversive/error.h>
30
31 #include <ax12.h>
32 #include <uart.h>
33 #include <pwm_ng.h>
34 #include <clock_time.h>
35 #include <spi.h>
36 #include <encoders_spi.h>
37
38 #include <pid.h>
39 #include <quadramp.h>
40 #include <control_system_manager.h>
41 #include <trajectory_manager.h>
42 #include <trajectory_manager_utils.h>
43 #include <vect_base.h>
44 #include <lines.h>
45 #include <polygon.h>
46 #include <obstacle_avoidance.h>
47 #include <blocking_detection_manager.h>
48 #include <robot_system.h>
49 #include <position_manager.h>
50
51 #include <rdline.h>
52 #include <parse.h>
53 #include <parse_string.h>
54 #include <parse_num.h>
55
56 #include "main.h"
57 #include "cs.h"
58 #include "cmdline.h"
59 #include "strat_utils.h"
60 #include "strat_base.h"
61 #include "strat.h"
62 #include "strat_db.h"
63 #include "../common/i2c_commands.h"
64 #include "i2c_protocol.h"
65
66 /**********************************************************/
67 /* Traj_Speeds for trajectory_manager */
68
69 /* this structure is filled when cmd_traj_speed is parsed successfully */
70 struct cmd_traj_speed_result {
71         fixed_string_t arg0;
72         fixed_string_t arg1;
73         float s;
74 };
75
76 /* function called when cmd_traj_speed is parsed successfully */
77 static void cmd_traj_speed_parsed(void *parsed_result, void *data)
78 {
79         struct cmd_traj_speed_result * res = parsed_result;
80
81         if (!strcmp_P(res->arg1, PSTR("angle"))) {
82                 trajectory_set_speed(&mainboard.traj, mainboard.traj.d_speed, res->s);
83         }
84         else if (!strcmp_P(res->arg1, PSTR("distance"))) {
85                 trajectory_set_speed(&mainboard.traj, res->s, mainboard.traj.a_speed);
86         }
87         /* else it is a "show" */
88
89         printf_P(PSTR("angle %2.2f, distance %2.2f\r\n"),
90                  mainboard.traj.a_speed,
91                  mainboard.traj.d_speed);
92 }
93
94 prog_char str_traj_speed_arg0[] = "traj_speed";
95 parse_pgm_token_string_t cmd_traj_speed_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_traj_speed_result, arg0, str_traj_speed_arg0);
96 prog_char str_traj_speed_arg1[] = "angle#distance";
97 parse_pgm_token_string_t cmd_traj_speed_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_traj_speed_result, arg1, str_traj_speed_arg1);
98 parse_pgm_token_num_t cmd_traj_speed_s = TOKEN_NUM_INITIALIZER(struct cmd_traj_speed_result, s, FLOAT);
99
100 prog_char help_traj_speed[] = "Set traj_speed values for trajectory manager";
101 parse_pgm_inst_t cmd_traj_speed = {
102         .f = cmd_traj_speed_parsed,  /* function to call */
103         .data = NULL,      /* 2nd arg of func */
104         .help_str = help_traj_speed,
105         .tokens = {        /* token list, NULL terminated */
106                 (prog_void *)&cmd_traj_speed_arg0,
107                 (prog_void *)&cmd_traj_speed_arg1,
108                 (prog_void *)&cmd_traj_speed_s,
109                 NULL,
110         },
111 };
112
113 /* show */
114
115 prog_char str_traj_speed_show_arg[] = "show";
116 parse_pgm_token_string_t cmd_traj_speed_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_traj_speed_result, arg1, str_traj_speed_show_arg);
117
118 prog_char help_traj_speed_show[] = "Show traj_speed values for trajectory manager";
119 parse_pgm_inst_t cmd_traj_speed_show = {
120         .f = cmd_traj_speed_parsed,  /* function to call */
121         .data = NULL,      /* 2nd arg of func */
122         .help_str = help_traj_speed_show,
123         .tokens = {        /* token list, NULL terminated */
124                 (prog_void *)&cmd_traj_speed_arg0,
125                 (prog_void *)&cmd_traj_speed_show_arg,
126                 NULL,
127         },
128 };
129
130 /**********************************************************/
131 /* Traj_Accs for trajectory_manager */
132
133 /* this structure is filled when cmd_traj_acc is parsed successfully */
134 struct cmd_traj_acc_result {
135         fixed_string_t arg0;
136         fixed_string_t arg1;
137         float s;
138 };
139
140 /* function called when cmd_traj_acc is parsed successfully */
141 static void cmd_traj_acc_parsed(void *parsed_result, void *data)
142 {
143         struct cmd_traj_acc_result * res = parsed_result;
144
145         if (!strcmp_P(res->arg1, PSTR("angle"))) {
146                 trajectory_set_acc(&mainboard.traj, mainboard.traj.d_acc, res->s);
147         }
148         else if (!strcmp_P(res->arg1, PSTR("distance"))) {
149                 trajectory_set_acc(&mainboard.traj, res->s, mainboard.traj.a_acc);
150         }
151         /* else it is a "show" */
152
153         printf_P(PSTR("angle %2.2f, distance %2.2f\r\n"),
154                  mainboard.traj.a_acc,
155                  mainboard.traj.d_acc);
156 }
157
158 prog_char str_traj_acc_arg0[] = "traj_acc";
159 parse_pgm_token_string_t cmd_traj_acc_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_traj_acc_result, arg0, str_traj_acc_arg0);
160 prog_char str_traj_acc_arg1[] = "angle#distance";
161 parse_pgm_token_string_t cmd_traj_acc_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_traj_acc_result, arg1, str_traj_acc_arg1);
162 parse_pgm_token_num_t cmd_traj_acc_s = TOKEN_NUM_INITIALIZER(struct cmd_traj_acc_result, s, FLOAT);
163
164 prog_char help_traj_acc[] = "Set traj_acc values for trajectory manager";
165 parse_pgm_inst_t cmd_traj_acc = {
166         .f = cmd_traj_acc_parsed,  /* function to call */
167         .data = NULL,      /* 2nd arg of func */
168         .help_str = help_traj_acc,
169         .tokens = {        /* token list, NULL terminated */
170                 (prog_void *)&cmd_traj_acc_arg0,
171                 (prog_void *)&cmd_traj_acc_arg1,
172                 (prog_void *)&cmd_traj_acc_s,
173                 NULL,
174         },
175 };
176
177 /* show */
178
179 prog_char str_traj_acc_show_arg[] = "show";
180 parse_pgm_token_string_t cmd_traj_acc_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_traj_acc_result, arg1, str_traj_acc_show_arg);
181
182 prog_char help_traj_acc_show[] = "Show traj_acc values for trajectory manager";
183 parse_pgm_inst_t cmd_traj_acc_show = {
184         .f = cmd_traj_acc_parsed,  /* function to call */
185         .data = NULL,      /* 2nd arg of func */
186         .help_str = help_traj_acc_show,
187         .tokens = {        /* token list, NULL terminated */
188                 (prog_void *)&cmd_traj_acc_arg0,
189                 (prog_void *)&cmd_traj_acc_show_arg,
190                 NULL,
191         },
192 };
193
194 /**********************************************************/
195 /* circle coef configuration */
196
197 /* this structure is filled when cmd_circle_coef is parsed successfully */
198 struct cmd_circle_coef_result {
199         fixed_string_t arg0;
200         fixed_string_t arg1;
201         float circle_coef;
202 };
203
204
205 /* function called when cmd_circle_coef is parsed successfully */
206 static void cmd_circle_coef_parsed(void *parsed_result, void *data)
207 {
208         struct cmd_circle_coef_result *res = parsed_result;
209
210         if (!strcmp_P(res->arg1, PSTR("set"))) {
211                 trajectory_set_circle_coef(&mainboard.traj, res->circle_coef);
212         }
213
214         printf_P(PSTR("circle_coef set %2.2f\r\n"), mainboard.traj.circle_coef);
215 }
216
217 prog_char str_circle_coef_arg0[] = "circle_coef";
218 parse_pgm_token_string_t cmd_circle_coef_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_circle_coef_result, arg0, str_circle_coef_arg0);
219 prog_char str_circle_coef_arg1[] = "set";
220 parse_pgm_token_string_t cmd_circle_coef_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_circle_coef_result, arg1, str_circle_coef_arg1);
221 parse_pgm_token_num_t cmd_circle_coef_val = TOKEN_NUM_INITIALIZER(struct cmd_circle_coef_result, circle_coef, FLOAT);
222
223 prog_char help_circle_coef[] = "Set circle coef";
224 parse_pgm_inst_t cmd_circle_coef = {
225         .f = cmd_circle_coef_parsed,  /* function to call */
226         .data = NULL,      /* 2nd arg of func */
227         .help_str = help_circle_coef,
228         .tokens = {        /* token list, NULL terminated */
229                 (prog_void *)&cmd_circle_coef_arg0,
230                 (prog_void *)&cmd_circle_coef_arg1,
231                 (prog_void *)&cmd_circle_coef_val,
232                 NULL,
233         },
234 };
235
236 /* show */
237
238 prog_char str_circle_coef_show_arg[] = "show";
239 parse_pgm_token_string_t cmd_circle_coef_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_circle_coef_result, arg1, str_circle_coef_show_arg);
240
241 prog_char help_circle_coef_show[] = "Show circle coef";
242 parse_pgm_inst_t cmd_circle_coef_show = {
243         .f = cmd_circle_coef_parsed,  /* function to call */
244         .data = NULL,      /* 2nd arg of func */
245         .help_str = help_circle_coef_show,
246         .tokens = {        /* token list, NULL terminated */
247                 (prog_void *)&cmd_circle_coef_arg0,
248                 (prog_void *)&cmd_circle_coef_show_arg,
249                 NULL,
250         },
251 };
252
253 /**********************************************************/
254 /* trajectory window configuration */
255
256 /* this structure is filled when cmd_trajectory is parsed successfully */
257 struct cmd_trajectory_result {
258         fixed_string_t arg0;
259         fixed_string_t arg1;
260         float d_win;
261         float a_win;
262         float a_start;
263 };
264
265
266 /* function called when cmd_trajectory is parsed successfully */
267 static void cmd_trajectory_parsed(void * parsed_result, void * data)
268 {
269         struct cmd_trajectory_result * res = parsed_result;
270
271         if (!strcmp_P(res->arg1, PSTR("set"))) {
272                 trajectory_set_windows(&mainboard.traj, res->d_win,
273                                        res->a_win, res->a_start);
274         }
275
276         printf_P(PSTR("trajectory %2.2f %2.2f %2.2f\r\n"), mainboard.traj.d_win,
277                  DEG(mainboard.traj.a_win_rad), DEG(mainboard.traj.a_start_rad));
278 }
279
280 prog_char str_trajectory_arg0[] = "trajectory";
281 parse_pgm_token_string_t cmd_trajectory_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_trajectory_result, arg0, str_trajectory_arg0);
282 prog_char str_trajectory_arg1[] = "set";
283 parse_pgm_token_string_t cmd_trajectory_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_trajectory_result, arg1, str_trajectory_arg1);
284 parse_pgm_token_num_t cmd_trajectory_d = TOKEN_NUM_INITIALIZER(struct cmd_trajectory_result, d_win, FLOAT);
285 parse_pgm_token_num_t cmd_trajectory_a = TOKEN_NUM_INITIALIZER(struct cmd_trajectory_result, a_win, FLOAT);
286 parse_pgm_token_num_t cmd_trajectory_as = TOKEN_NUM_INITIALIZER(struct cmd_trajectory_result, a_start, FLOAT);
287
288 prog_char help_trajectory[] = "Set trajectory windows (distance, angle, angle_start)";
289 parse_pgm_inst_t cmd_trajectory = {
290         .f = cmd_trajectory_parsed,  /* function to call */
291         .data = NULL,      /* 2nd arg of func */
292         .help_str = help_trajectory,
293         .tokens = {        /* token list, NULL terminated */
294                 (prog_void *)&cmd_trajectory_arg0,
295                 (prog_void *)&cmd_trajectory_arg1,
296                 (prog_void *)&cmd_trajectory_d,
297                 (prog_void *)&cmd_trajectory_a,
298                 (prog_void *)&cmd_trajectory_as,
299                 NULL,
300         },
301 };
302
303 /* show */
304
305 prog_char str_trajectory_show_arg[] = "show";
306 parse_pgm_token_string_t cmd_trajectory_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_trajectory_result, arg1, str_trajectory_show_arg);
307
308 prog_char help_trajectory_show[] = "Show trajectory window configuration";
309 parse_pgm_inst_t cmd_trajectory_show = {
310         .f = cmd_trajectory_parsed,  /* function to call */
311         .data = NULL,      /* 2nd arg of func */
312         .help_str = help_trajectory_show,
313         .tokens = {        /* token list, NULL terminated */
314                 (prog_void *)&cmd_trajectory_arg0,
315                 (prog_void *)&cmd_trajectory_show_arg,
316                 NULL,
317         },
318 };
319
320 /**********************************************************/
321 /* rs_gains configuration */
322
323 /* this structure is filled when cmd_rs_gains is parsed successfully */
324 struct cmd_rs_gains_result {
325         fixed_string_t arg0;
326         fixed_string_t arg1;
327         float left;
328         float right;
329 };
330
331 /* function called when cmd_rs_gains is parsed successfully */
332 static void cmd_rs_gains_parsed(void * parsed_result, void * data)
333 {
334 #ifdef HOST_VERSION
335         printf("not implemented\n");
336 #else
337         struct cmd_rs_gains_result * res = parsed_result;
338
339         if (!strcmp_P(res->arg1, PSTR("set"))) {
340                 rs_set_left_ext_encoder(&mainboard.rs, encoders_spi_get_value,
341                                         LEFT_ENCODER, res->left); // en augmentant on tourne à gauche
342                 rs_set_right_ext_encoder(&mainboard.rs, encoders_spi_get_value,
343                                          RIGHT_ENCODER, res->right); //en augmentant on tourne à droite
344         }
345         printf_P(PSTR("rs_gains set %2.2f %2.2f\r\n"),
346                  mainboard.rs.left_ext_gain, mainboard.rs.right_ext_gain);
347 #endif
348 }
349
350 prog_char str_rs_gains_arg0[] = "rs_gains";
351 parse_pgm_token_string_t cmd_rs_gains_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_rs_gains_result, arg0, str_rs_gains_arg0);
352 prog_char str_rs_gains_arg1[] = "set";
353 parse_pgm_token_string_t cmd_rs_gains_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_rs_gains_result, arg1, str_rs_gains_arg1);
354 parse_pgm_token_num_t cmd_rs_gains_l = TOKEN_NUM_INITIALIZER(struct cmd_rs_gains_result, left, FLOAT);
355 parse_pgm_token_num_t cmd_rs_gains_r = TOKEN_NUM_INITIALIZER(struct cmd_rs_gains_result, right, FLOAT);
356
357 prog_char help_rs_gains[] = "Set rs_gains (left, right)";
358 parse_pgm_inst_t cmd_rs_gains = {
359         .f = cmd_rs_gains_parsed,  /* function to call */
360         .data = NULL,      /* 2nd arg of func */
361         .help_str = help_rs_gains,
362         .tokens = {        /* token list, NULL terminated */
363                 (prog_void *)&cmd_rs_gains_arg0,
364                 (prog_void *)&cmd_rs_gains_arg1,
365                 (prog_void *)&cmd_rs_gains_l,
366                 (prog_void *)&cmd_rs_gains_r,
367                 NULL,
368         },
369 };
370
371 /* show */
372
373 prog_char str_rs_gains_show_arg[] = "show";
374 parse_pgm_token_string_t cmd_rs_gains_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_rs_gains_result, arg1, str_rs_gains_show_arg);
375
376 prog_char help_rs_gains_show[] = "Show rs_gains";
377 parse_pgm_inst_t cmd_rs_gains_show = {
378         .f = cmd_rs_gains_parsed,  /* function to call */
379         .data = NULL,      /* 2nd arg of func */
380         .help_str = help_rs_gains_show,
381         .tokens = {        /* token list, NULL terminated */
382                 (prog_void *)&cmd_rs_gains_arg0,
383                 (prog_void *)&cmd_rs_gains_show_arg,
384                 NULL,
385         },
386 };
387
388 /**********************************************************/
389 /* track configuration */
390
391 /* this structure is filled when cmd_track is parsed successfully */
392 struct cmd_track_result {
393         fixed_string_t arg0;
394         fixed_string_t arg1;
395         float val;
396 };
397
398 /* function called when cmd_track is parsed successfully */
399 static void cmd_track_parsed(void * parsed_result, void * data)
400 {
401         struct cmd_track_result * res = parsed_result;
402
403         if (!strcmp_P(res->arg1, PSTR("set"))) {
404                 position_set_physical_params(&mainboard.pos, res->val, DIST_IMP_MM);
405         }
406         printf_P(PSTR("track set %f\r\n"), mainboard.pos.phys.track_mm);
407 }
408
409 prog_char str_track_arg0[] = "track";
410 parse_pgm_token_string_t cmd_track_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_track_result, arg0, str_track_arg0);
411 prog_char str_track_arg1[] = "set";
412 parse_pgm_token_string_t cmd_track_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_track_result, arg1, str_track_arg1);
413 parse_pgm_token_num_t cmd_track_val = TOKEN_NUM_INITIALIZER(struct cmd_track_result, val, FLOAT);
414
415 prog_char help_track[] = "Set track in mm";
416 parse_pgm_inst_t cmd_track = {
417         .f = cmd_track_parsed,  /* function to call */
418         .data = NULL,      /* 2nd arg of func */
419         .help_str = help_track,
420         .tokens = {        /* token list, NULL terminated */
421                 (prog_void *)&cmd_track_arg0,
422                 (prog_void *)&cmd_track_arg1,
423                 (prog_void *)&cmd_track_val,
424                 NULL,
425         },
426 };
427
428 /* show */
429
430 prog_char str_track_show_arg[] = "show";
431 parse_pgm_token_string_t cmd_track_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_track_result, arg1, str_track_show_arg);
432
433 prog_char help_track_show[] = "Show track";
434 parse_pgm_inst_t cmd_track_show = {
435         .f = cmd_track_parsed,  /* function to call */
436         .data = NULL,      /* 2nd arg of func */
437         .help_str = help_track_show,
438         .tokens = {        /* token list, NULL terminated */
439                 (prog_void *)&cmd_track_arg0,
440                 (prog_void *)&cmd_track_show_arg,
441                 NULL,
442         },
443 };
444
445 /**********************************************************/
446 /* centrifugal configuration */
447
448 /* this structure is filled when cmd_centrifugal is parsed successfully */
449 struct cmd_centrifugal_result {
450         fixed_string_t arg0;
451         fixed_string_t arg1;
452         float val;
453 };
454
455 /* function called when cmd_centrifugal is parsed successfully */
456 static void cmd_centrifugal_parsed(void * parsed_result, void * data)
457 {
458         struct cmd_centrifugal_result * res = parsed_result;
459
460         if (!strcmp_P(res->arg1, PSTR("set"))) {
461                 position_set_centrifugal_coef(&mainboard.pos, res->val);
462         }
463         printf_P(PSTR("centrifugal set %f\r\n"), mainboard.pos.centrifugal_coef);
464 }
465
466 prog_char str_centrifugal_arg0[] = "centrifugal";
467 parse_pgm_token_string_t cmd_centrifugal_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_centrifugal_result, arg0, str_centrifugal_arg0);
468 prog_char str_centrifugal_arg1[] = "set";
469 parse_pgm_token_string_t cmd_centrifugal_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_centrifugal_result, arg1, str_centrifugal_arg1);
470 parse_pgm_token_num_t cmd_centrifugal_val = TOKEN_NUM_INITIALIZER(struct cmd_centrifugal_result, val, FLOAT);
471
472 prog_char help_centrifugal[] = "Set centrifugal coef";
473 parse_pgm_inst_t cmd_centrifugal = {
474         .f = cmd_centrifugal_parsed,  /* function to call */
475         .data = NULL,      /* 2nd arg of func */
476         .help_str = help_centrifugal,
477         .tokens = {        /* token list, NULL terminated */
478                 (prog_void *)&cmd_centrifugal_arg0,
479                 (prog_void *)&cmd_centrifugal_arg1,
480                 (prog_void *)&cmd_centrifugal_val,
481                 NULL,
482         },
483 };
484
485 /* show */
486
487 prog_char str_centrifugal_show_arg[] = "show";
488 parse_pgm_token_string_t cmd_centrifugal_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_centrifugal_result, arg1, str_centrifugal_show_arg);
489
490 prog_char help_centrifugal_show[] = "Show centrifugal";
491 parse_pgm_inst_t cmd_centrifugal_show = {
492         .f = cmd_centrifugal_parsed,  /* function to call */
493         .data = NULL,      /* 2nd arg of func */
494         .help_str = help_centrifugal_show,
495         .tokens = {        /* token list, NULL terminated */
496                 (prog_void *)&cmd_centrifugal_arg0,
497                 (prog_void *)&cmd_centrifugal_show_arg,
498                 NULL,
499         },
500 };
501
502
503
504 /**********************************************************/
505 /* Pt_Lists for testing traj */
506
507 #define PT_LIST_SIZE 10
508 static struct xy_point pt_list[PT_LIST_SIZE];
509 static uint16_t pt_list_len = 0;
510
511 /* this structure is filled when cmd_pt_list is parsed successfully */
512 struct cmd_pt_list_result {
513         fixed_string_t arg0;
514         fixed_string_t arg1;
515         uint16_t arg2;
516         int16_t arg3;
517         int16_t arg4;
518 };
519
520 /* function called when cmd_pt_list is parsed successfully */
521 static void cmd_pt_list_parsed(void * parsed_result, void * data)
522 {
523         struct cmd_pt_list_result * res = parsed_result;
524         uint8_t i, why=0;
525
526         if (!strcmp_P(res->arg1, PSTR("append"))) {
527                 res->arg2 = pt_list_len;
528         }
529         if (!strcmp_P(res->arg1, PSTR("avoid_start"))) {
530                 printf_P(PSTR("removed\r\n"));
531                 return;
532         }
533
534         if (!strcmp_P(res->arg1, PSTR("insert")) ||
535             !strcmp_P(res->arg1, PSTR("append"))) {
536                 if (res->arg2 > pt_list_len) {
537                         printf_P(PSTR("Index too large\r\n"));
538                         return;
539                 }
540                 if (pt_list_len >= PT_LIST_SIZE) {
541                         printf_P(PSTR("List is too large\r\n"));
542                         return;
543                 }
544                 memmove(&pt_list[res->arg2+1], &pt_list[res->arg2],
545                        PT_LIST_SIZE-1-res->arg2);
546                 pt_list[res->arg2].x = res->arg3;
547                 pt_list[res->arg2].y = res->arg4;
548                 pt_list_len++;
549         }
550         else if (!strcmp_P(res->arg1, PSTR("del"))) {
551                 if (pt_list_len <= 0) {
552                         printf_P(PSTR("Error: list empty\r\n"));
553                         return;
554                 }
555                 if (res->arg2 > pt_list_len) {
556                         printf_P(PSTR("Index too large\r\n"));
557                         return;
558                 }
559                 memmove(&pt_list[res->arg2], &pt_list[res->arg2+1],
560                        (PT_LIST_SIZE-1-res->arg2)*sizeof(struct xy_point));
561                 pt_list_len--;
562         }
563         else if (!strcmp_P(res->arg1, PSTR("reset"))) {
564                 pt_list_len = 0;
565         }
566
567         /* else it is a "show" or a "start" */
568         if (pt_list_len == 0) {
569                 printf_P(PSTR("List empty\r\n"));
570                 return;
571         }
572  restart:
573         for (i=0 ; i<pt_list_len ; i++) {
574                 printf_P(PSTR("%d: x=%d y=%d\r\n"), i, pt_list[i].x, pt_list[i].y);
575                 if (!strcmp_P(res->arg1, PSTR("start"))) {
576                         trajectory_goto_xy_abs(&mainboard.traj, pt_list[i].x, pt_list[i].y);
577                         why = wait_traj_end(0xFF); /* all */
578                 }
579                 else if (!strcmp_P(res->arg1, PSTR("loop_start"))) {
580                         trajectory_goto_xy_abs(&mainboard.traj, pt_list[i].x, pt_list[i].y);
581                         why = wait_traj_end(0xFF); /* all */
582                 }
583 #if 0
584                 else if (!strcmp_P(res->arg1, PSTR("avoid_start"))) {
585                         while (1) {
586                                 why = goto_and_avoid(pt_list[i].x, pt_list[i].y, 0xFF, 0xFF);
587                                 printf("next point\r\n");
588                                 if (why != END_OBSTACLE)
589                                         break;
590                         }
591                 }
592 #endif
593                 if (why & (~(END_TRAJ | END_NEAR)))
594                         trajectory_stop(&mainboard.traj);
595                 if (why & END_INTR)
596                         break;
597         }
598         if (why & END_INTR)
599                 return;
600         if (!strcmp_P(res->arg1, PSTR("loop_start")))
601                 goto restart;
602 }
603
604 prog_char str_pt_list_arg0[] = "pt_list";
605 parse_pgm_token_string_t cmd_pt_list_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg0, str_pt_list_arg0);
606 prog_char str_pt_list_arg1[] = "insert";
607 parse_pgm_token_string_t cmd_pt_list_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_arg1);
608 parse_pgm_token_num_t cmd_pt_list_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_pt_list_result, arg2, UINT16);
609 parse_pgm_token_num_t cmd_pt_list_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_pt_list_result, arg3, INT16);
610 parse_pgm_token_num_t cmd_pt_list_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_pt_list_result, arg4, INT16);
611
612 prog_char help_pt_list[] = "Insert point in pt_list (idx,x,y)";
613 parse_pgm_inst_t cmd_pt_list = {
614         .f = cmd_pt_list_parsed,  /* function to call */
615         .data = NULL,      /* 2nd arg of func */
616         .help_str = help_pt_list,
617         .tokens = {        /* token list, NULL terminated */
618                 (prog_void *)&cmd_pt_list_arg0,
619                 (prog_void *)&cmd_pt_list_arg1,
620                 (prog_void *)&cmd_pt_list_arg2,
621                 (prog_void *)&cmd_pt_list_arg3,
622                 (prog_void *)&cmd_pt_list_arg4,
623                 NULL,
624         },
625 };
626
627 /* append */
628
629 prog_char str_pt_list_arg1_append[] = "append";
630 parse_pgm_token_string_t cmd_pt_list_arg1_append = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_arg1_append);
631
632 prog_char help_pt_list_append[] = "Append point in pt_list (x,y)";
633 parse_pgm_inst_t cmd_pt_list_append = {
634         .f = cmd_pt_list_parsed,  /* function to call */
635         .data = NULL,      /* 2nd arg of func */
636         .help_str = help_pt_list_append,
637         .tokens = {        /* token list, NULL terminated */
638                 (prog_void *)&cmd_pt_list_arg0,
639                 (prog_void *)&cmd_pt_list_arg1_append,
640                 (prog_void *)&cmd_pt_list_arg3,
641                 (prog_void *)&cmd_pt_list_arg4,
642                 NULL,
643         },
644 };
645
646 /* del */
647
648 prog_char str_pt_list_del_arg[] = "del";
649 parse_pgm_token_string_t cmd_pt_list_del_arg = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_del_arg);
650
651 prog_char help_pt_list_del[] = "Del or insert point in pt_list (num)";
652 parse_pgm_inst_t cmd_pt_list_del = {
653         .f = cmd_pt_list_parsed,  /* function to call */
654         .data = NULL,      /* 2nd arg of func */
655         .help_str = help_pt_list_del,
656         .tokens = {        /* token list, NULL terminated */
657                 (prog_void *)&cmd_pt_list_arg0,
658                 (prog_void *)&cmd_pt_list_del_arg,
659                 (prog_void *)&cmd_pt_list_arg2,
660                 NULL,
661         },
662 };
663 /* show */
664
665 prog_char str_pt_list_show_arg[] = "show#reset#start#avoid_start#loop_start";
666 parse_pgm_token_string_t cmd_pt_list_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_show_arg);
667
668 prog_char help_pt_list_show[] = "Show, start or reset pt_list";
669 parse_pgm_inst_t cmd_pt_list_show = {
670         .f = cmd_pt_list_parsed,  /* function to call */
671         .data = NULL,      /* 2nd arg of func */
672         .help_str = help_pt_list_show,
673         .tokens = {        /* token list, NULL terminated */
674                 (prog_void *)&cmd_pt_list_arg0,
675                 (prog_void *)&cmd_pt_list_show_arg,
676                 NULL,
677         },
678 };
679
680
681
682 /**********************************************************/
683 /* Goto function */
684
685 /* this structure is filled when cmd_goto is parsed successfully */
686 struct cmd_goto_result {
687         fixed_string_t arg0;
688         fixed_string_t arg1;
689         int32_t arg2;
690         int32_t arg3;
691         int32_t arg4;
692 };
693
694 /* function called when cmd_goto is parsed successfully */
695 static void cmd_goto_parsed(void * parsed_result, void * data)
696 {
697         struct cmd_goto_result * res = parsed_result;
698         uint8_t err;
699         microseconds t1, t2;
700
701         interrupt_traj_reset();
702         if (!strcmp_P(res->arg1, PSTR("a_rel"))) {
703                 trajectory_a_rel(&mainboard.traj, res->arg2);
704         }
705         else if (!strcmp_P(res->arg1, PSTR("d_rel"))) {
706                 trajectory_d_rel(&mainboard.traj, res->arg2);
707         }
708         else if (!strcmp_P(res->arg1, PSTR("a_abs"))) {
709                 trajectory_a_abs(&mainboard.traj, res->arg2);
710         }
711         else if (!strcmp_P(res->arg1, PSTR("a_to_xy"))) {
712                 trajectory_turnto_xy(&mainboard.traj, res->arg2, res->arg3);
713         }
714         else if (!strcmp_P(res->arg1, PSTR("a_behind_xy"))) {
715                 trajectory_turnto_xy_behind(&mainboard.traj, res->arg2, res->arg3);
716         }
717         else if (!strcmp_P(res->arg1, PSTR("xy_rel"))) {
718                 trajectory_goto_xy_rel(&mainboard.traj, res->arg2, res->arg3);
719         }
720         else if (!strcmp_P(res->arg1, PSTR("xy_abs"))) {
721                 trajectory_goto_xy_abs(&mainboard.traj, res->arg2, res->arg3);
722         }
723         else if (!strcmp_P(res->arg1, PSTR("avoid"))) {
724 #if 0
725                 err = goto_and_avoid_forward(res->arg2, res->arg3, 0xFF, 0xFF);
726                 if (err != END_TRAJ && err != END_NEAR)
727                         strat_hardstop();
728 #else
729                 printf_P(PSTR("not implemented\r\n"));
730                 return;
731 #endif
732         }
733         else if (!strcmp_P(res->arg1, PSTR("avoid_bw"))) {
734 #if 0
735                 err = goto_and_avoid_backward(res->arg2, res->arg3, 0xFF, 0xFF);
736                 if (err != END_TRAJ && err != END_NEAR)
737                         strat_hardstop();
738 #else
739                 printf_P(PSTR("not implemented\r\n"));
740                 return;
741 #endif
742         }
743         else if (!strcmp_P(res->arg1, PSTR("xy_abs_fow"))) {
744                 trajectory_goto_forward_xy_abs(&mainboard.traj, res->arg2, res->arg3);
745         }
746         else if (!strcmp_P(res->arg1, PSTR("xy_abs_back"))) {
747                 trajectory_goto_backward_xy_abs(&mainboard.traj, res->arg2, res->arg3);
748         }
749         else if (!strcmp_P(res->arg1, PSTR("da_rel"))) {
750                 trajectory_d_a_rel(&mainboard.traj, res->arg2, res->arg3);
751         }
752         t1 = time_get_us2();
753         while ((err = test_traj_end(0xFF)) == 0) {
754                 t2 = time_get_us2();
755                 if (t2 - t1 > 200000) {
756                         dump_cs_debug("angle", &mainboard.angle.cs);
757                         dump_cs_debug("distance", &mainboard.distance.cs);
758                         t1 = t2;
759                 }
760         }
761         if (err != END_TRAJ && err != END_NEAR)
762                 strat_hardstop();
763         printf_P(PSTR("returned %s\r\n"), get_err(err));
764 }
765
766 prog_char str_goto_arg0[] = "goto";
767 parse_pgm_token_string_t cmd_goto_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_goto_result, arg0, str_goto_arg0);
768 prog_char str_goto_arg1_a[] = "d_rel#a_rel#a_abs";
769 parse_pgm_token_string_t cmd_goto_arg1_a = TOKEN_STRING_INITIALIZER(struct cmd_goto_result, arg1, str_goto_arg1_a);
770 parse_pgm_token_num_t cmd_goto_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_goto_result, arg2, INT32);
771
772 /* 1 params */
773 prog_char help_goto1[] = "Change orientation of the mainboard";
774 parse_pgm_inst_t cmd_goto1 = {
775         .f = cmd_goto_parsed,  /* function to call */
776         .data = NULL,      /* 2nd arg of func */
777         .help_str = help_goto1,
778         .tokens = {        /* token list, NULL terminated */
779                 (prog_void *)&cmd_goto_arg0,
780                 (prog_void *)&cmd_goto_arg1_a,
781                 (prog_void *)&cmd_goto_arg2,
782                 NULL,
783         },
784 };
785
786 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";
787 parse_pgm_token_string_t cmd_goto_arg1_b = TOKEN_STRING_INITIALIZER(struct cmd_goto_result, arg1, str_goto_arg1_b);
788 parse_pgm_token_num_t cmd_goto_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_goto_result, arg3, INT32);
789
790 /* 2 params */
791 prog_char help_goto2[] = "Go to a (x,y) or (d,a) position";
792 parse_pgm_inst_t cmd_goto2 = {
793         .f = cmd_goto_parsed,  /* function to call */
794         .data = NULL,      /* 2nd arg of func */
795         .help_str = help_goto2,
796         .tokens = {        /* token list, NULL terminated */
797                 (prog_void *)&cmd_goto_arg0,
798                 (prog_void *)&cmd_goto_arg1_b,
799                 (prog_void *)&cmd_goto_arg2,
800                 (prog_void *)&cmd_goto_arg3,
801                 NULL,
802         },
803 };
804
805 /**********************************************************/
806 /* Position tests */
807
808 /* this structure is filled when cmd_position is parsed successfully */
809 struct cmd_position_result {
810         fixed_string_t arg0;
811         fixed_string_t arg1;
812         int32_t arg2;
813         int32_t arg3;
814         int32_t arg4;
815 };
816
817 #define AUTOPOS_SPEED_FAST 200
818 static void auto_position(void)
819 {
820         uint8_t err;
821         uint16_t old_spdd, old_spda;
822
823         interrupt_traj_reset();
824         strat_get_speed(&old_spdd, &old_spda);
825         strat_set_speed(AUTOPOS_SPEED_FAST, AUTOPOS_SPEED_FAST);
826         strat_set_acc(3, 3);
827
828         err = strat_calib(300, END_INTR|END_TRAJ|END_BLOCKING);
829         if (err == END_INTR)
830                 goto intr;
831         strat_reset_pos(ROBOT_WIDTH/2 + 100,
832                         COLOR_Y(ROBOT_HALF_LENGTH_FRONT),
833                         COLOR_A(-90) + ROBOT_ANGLE_FRONT);
834         strat_hardstop();
835
836         trajectory_d_rel(&mainboard.traj, -180);
837         err = wait_traj_end(END_INTR|END_TRAJ);
838         if (err == END_INTR)
839                 goto intr;
840
841         time_wait_ms(250);
842         trajectory_a_rel(&mainboard.traj, COLOR_A(-90));
843         err = wait_traj_end(END_INTR|END_TRAJ);
844         if (err == END_INTR)
845                 goto intr;
846
847         time_wait_ms(250);
848         err = strat_calib(300, END_INTR|END_TRAJ|END_BLOCKING);
849         if (err == END_INTR)
850                 goto intr;
851         strat_reset_pos(ROBOT_HALF_LENGTH_FRONT,
852                         DO_NOT_SET_POS,
853                         180 + ROBOT_ANGLE_FRONT);
854         strat_hardstop();
855
856         trajectory_d_rel(&mainboard.traj, -170);
857         err = wait_traj_end(END_INTR|END_TRAJ);
858         if (err == END_INTR)
859                 goto intr;
860         time_wait_ms(250);
861
862         trajectory_a_rel(&mainboard.traj, COLOR_A(-110));
863         err = wait_traj_end(END_INTR|END_TRAJ);
864         if (err == END_INTR)
865                 goto intr;
866         time_wait_ms(250);
867
868         strat_set_speed(old_spdd, old_spda);
869         return;
870
871 intr:
872         strat_hardstop();
873         strat_set_speed(old_spdd, old_spda);
874 }
875
876 /* function called when cmd_position is parsed successfully */
877 static void cmd_position_parsed(void * parsed_result, void * data)
878 {
879         struct cmd_position_result * res = parsed_result;
880
881         /* display raw position values */
882         if (!strcmp_P(res->arg1, PSTR("reset"))) {
883                 position_set(&mainboard.pos, 0, 0, 0);
884         }
885         else if (!strcmp_P(res->arg1, PSTR("set"))) {
886                 position_set(&mainboard.pos, res->arg2, res->arg3, res->arg4);
887         }
888         else if (!strcmp_P(res->arg1, PSTR("autoset_blue")) ||
889                  !strcmp_P(res->arg1, PSTR("autoset_or_blue"))) {
890                 mainboard.our_color = I2C_COLOR_BLUE;
891 #ifndef HOST_VERSION
892                 i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_BLUE);
893                 i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_BLUE);
894 #endif
895                 auto_position();
896         }
897         else if (!strcmp_P(res->arg1, PSTR("autoset_yellow")) ||
898                  !strcmp_P(res->arg1, PSTR("autoset_or_yellow"))) {
899                 mainboard.our_color = I2C_COLOR_YELLOW;
900 #ifndef HOST_VERSION
901                 i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_YELLOW);
902                 i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_YELLOW);
903 #endif
904                 auto_position();
905         }
906
907         if (!strcmp_P(res->arg1, PSTR("autoset_or_blue"))) {
908                 strat_conf.flags |= STRAT_CONF_OUR_ORANGE;
909                 prepare_hill(I2C_COLOR_BLUE, 340);
910         }
911         else if (!strcmp_P(res->arg1, PSTR("autoset_or_yellow"))) {
912                 strat_conf.flags |= STRAT_CONF_OUR_ORANGE;
913                 prepare_hill(I2C_COLOR_YELLOW, 340);
914         }
915
916         /* else it's just a "show" */
917         printf_P(PSTR("x=%.2f y=%.2f a=%.2f\r\n"),
918                  position_get_x_double(&mainboard.pos),
919                  position_get_y_double(&mainboard.pos),
920                  DEG(position_get_a_rad_double(&mainboard.pos)));
921 }
922
923 prog_char str_position_arg0[] = "position";
924 parse_pgm_token_string_t cmd_position_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg0, str_position_arg0);
925 prog_char str_position_arg1[] = "show#reset#autoset_blue#autoset_yellow#autoset_or_blue#autoset_or_yellow";
926 parse_pgm_token_string_t cmd_position_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg1, str_position_arg1);
927
928 prog_char help_position[] = "Show/reset (x,y,a) position";
929 parse_pgm_inst_t cmd_position = {
930         .f = cmd_position_parsed,  /* function to call */
931         .data = NULL,      /* 2nd arg of func */
932         .help_str = help_position,
933         .tokens = {        /* token list, NULL terminated */
934                 (prog_void *)&cmd_position_arg0,
935                 (prog_void *)&cmd_position_arg1,
936                 NULL,
937         },
938 };
939
940
941 prog_char str_position_arg1_set[] = "set";
942 parse_pgm_token_string_t cmd_position_arg1_set = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg1, str_position_arg1_set);
943 parse_pgm_token_num_t cmd_position_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg2, INT32);
944 parse_pgm_token_num_t cmd_position_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg3, INT32);
945 parse_pgm_token_num_t cmd_position_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg4, INT32);
946
947 prog_char help_position_set[] = "Set (x,y,a) position";
948 parse_pgm_inst_t cmd_position_set = {
949         .f = cmd_position_parsed,  /* function to call */
950         .data = NULL,      /* 2nd arg of func */
951         .help_str = help_position_set,
952         .tokens = {        /* token list, NULL terminated */
953                 (prog_void *)&cmd_position_arg0,
954                 (prog_void *)&cmd_position_arg1_set,
955                 (prog_void *)&cmd_position_arg2,
956                 (prog_void *)&cmd_position_arg3,
957                 (prog_void *)&cmd_position_arg4,
958                 NULL,
959         },
960 };
961
962
963 /**********************************************************/
964 /* strat configuration */
965
966 /* this structure is filled when cmd_strat_db is parsed successfully */
967 struct cmd_strat_db_result {
968         fixed_string_t arg0;
969         fixed_string_t arg1;
970 };
971
972 /* function called when cmd_strat_db is parsed successfully */
973 static void cmd_strat_db_parsed(void *parsed_result, void *data)
974 {
975         struct cmd_strat_db_result *res = parsed_result;
976
977         if (!strcmp_P(res->arg1, PSTR("reset"))) {
978                 strat_db_init();
979         }
980         strat_db.dump_enabled = 1;
981         strat_db_dump(__FUNCTION__);
982 }
983
984 prog_char str_strat_db_arg0[] = "strat_db";
985 parse_pgm_token_string_t cmd_strat_db_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_db_result, arg0, str_strat_db_arg0);
986 prog_char str_strat_db_arg1[] = "show#reset";
987 parse_pgm_token_string_t cmd_strat_db_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_db_result, arg1, str_strat_db_arg1);
988
989 prog_char help_strat_db[] = "reset/show strat_db";
990 parse_pgm_inst_t cmd_strat_db = {
991         .f = cmd_strat_db_parsed,  /* function to call */
992         .data = NULL,      /* 2nd arg of func */
993         .help_str = help_strat_db,
994         .tokens = {        /* token list, NULL terminated */
995                 (prog_void *)&cmd_strat_db_arg0,
996                 (prog_void *)&cmd_strat_db_arg1,
997                 NULL,
998         },
999 };
1000
1001 /**********************************************************/
1002 /* strat configuration */
1003
1004 /* this structure is filled when cmd_strat_conf is parsed successfully */
1005 struct cmd_strat_conf_result {
1006         fixed_string_t arg0;
1007         fixed_string_t arg1;
1008 };
1009
1010 /* function called when cmd_strat_conf is parsed successfully */
1011 static void cmd_strat_conf_parsed(void *parsed_result, void *data)
1012 {
1013         //      struct cmd_strat_conf_result *res = parsed_result;
1014         strat_conf.dump_enabled = 1;
1015         strat_conf_dump(__FUNCTION__);
1016 }
1017
1018 prog_char str_strat_conf_arg0[] = "strat_conf";
1019 parse_pgm_token_string_t cmd_strat_conf_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf_result, arg0, str_strat_conf_arg0);
1020 prog_char str_strat_conf_arg1[] = "show#base";
1021 parse_pgm_token_string_t cmd_strat_conf_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf_result, arg1, str_strat_conf_arg1);
1022
1023 prog_char help_strat_conf[] = "configure strat options";
1024 parse_pgm_inst_t cmd_strat_conf = {
1025         .f = cmd_strat_conf_parsed,  /* function to call */
1026         .data = NULL,      /* 2nd arg of func */
1027         .help_str = help_strat_conf,
1028         .tokens = {        /* token list, NULL terminated */
1029                 (prog_void *)&cmd_strat_conf_arg0,
1030                 (prog_void *)&cmd_strat_conf_arg1,
1031                 NULL,
1032         },
1033 };
1034
1035 /**********************************************************/
1036 /* strat configuration */
1037
1038 /* this structure is filled when cmd_strat_conf2 is parsed successfully */
1039 struct cmd_strat_conf2_result {
1040         fixed_string_t arg0;
1041         fixed_string_t arg1;
1042         fixed_string_t arg2;
1043 };
1044
1045 /* function called when cmd_strat_conf2 is parsed successfully */
1046 static void cmd_strat_conf2_parsed(void *parsed_result, void *data)
1047 {
1048         struct cmd_strat_conf2_result *res = parsed_result;
1049         uint8_t on, bit = 0;
1050
1051         if (!strcmp_P(res->arg2, PSTR("on")))
1052                 on = 1;
1053         else
1054                 on = 0;
1055
1056         if (!strcmp_P(res->arg1, PSTR("our_orange")))
1057                 bit = STRAT_CONF_OUR_ORANGE;
1058         else if (!strcmp_P(res->arg1, PSTR("wait_obstacle")))
1059                 bit = STRAT_CONF_WAIT_OBSTACLE;
1060         else if (!strcmp_P(res->arg1, PSTR("straight_begin")))
1061                 bit = STRAT_CONF_STRAIGHT_BEGIN;
1062
1063         if (on)
1064                 strat_conf.flags |= bit;
1065         else
1066                 strat_conf.flags &= (~bit);
1067
1068         strat_conf.dump_enabled = 1;
1069         strat_conf_dump(__FUNCTION__);
1070 }
1071
1072 prog_char str_strat_conf2_arg0[] = "strat_conf";
1073 parse_pgm_token_string_t cmd_strat_conf2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg0, str_strat_conf2_arg0);
1074 prog_char str_strat_conf2_arg1[] = "our_orange#wait_obstacle#straight_begin";
1075 parse_pgm_token_string_t cmd_strat_conf2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg1, str_strat_conf2_arg1);
1076 prog_char str_strat_conf2_arg2[] = "on#off";
1077 parse_pgm_token_string_t cmd_strat_conf2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg2, str_strat_conf2_arg2);
1078
1079
1080 prog_char help_strat_conf2[] = "configure strat options";
1081 parse_pgm_inst_t cmd_strat_conf2 = {
1082         .f = cmd_strat_conf2_parsed,  /* function to call */
1083         .data = NULL,      /* 2nd arg of func */
1084         .help_str = help_strat_conf2,
1085         .tokens = {        /* token list, NULL terminated */
1086                 (prog_void *)&cmd_strat_conf2_arg0,
1087                 (prog_void *)&cmd_strat_conf2_arg1,
1088                 (prog_void *)&cmd_strat_conf2_arg2,
1089                 NULL,
1090         },
1091 };
1092
1093 /**********************************************************/
1094 /* strat configuration */
1095
1096 /* this structure is filled when cmd_strat_conf3 is parsed successfully */
1097 struct cmd_strat_conf3_result {
1098         fixed_string_t arg0;
1099         fixed_string_t arg1;
1100         uint8_t arg2;
1101 };
1102
1103 /* function called when cmd_strat_conf3 is parsed successfully */
1104 static void cmd_strat_conf3_parsed(void *parsed_result, void *data)
1105 {
1106         struct cmd_strat_conf3_result *res = parsed_result;
1107
1108         if (!strcmp_P(res->arg1, PSTR("orphan_tomato"))) {
1109                 if (res->arg2 > 90)
1110                         res->arg2 = 90;
1111                 strat_conf.orphan_tomato = res->arg2;
1112         }
1113         else if (!strcmp_P(res->arg1, PSTR("opp_orange"))) {
1114                 if (res->arg2 > 90)
1115                         res->arg2 = 90;
1116                 strat_conf.opp_orange = res->arg2;
1117         }
1118         strat_conf.dump_enabled = 1;
1119         strat_conf_dump(__FUNCTION__);
1120 }
1121
1122 prog_char str_strat_conf3_arg0[] = "strat_conf";
1123 parse_pgm_token_string_t cmd_strat_conf3_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf3_result, arg0, str_strat_conf3_arg0);
1124 prog_char str_strat_conf3_arg1[] = "orphan_tomato#opp_orange";
1125 parse_pgm_token_string_t cmd_strat_conf3_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf3_result, arg1, str_strat_conf3_arg1);
1126 parse_pgm_token_num_t cmd_strat_conf3_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_strat_conf3_result, arg2, UINT8);
1127
1128 prog_char help_strat_conf3[] = "configure strat options";
1129 parse_pgm_inst_t cmd_strat_conf3 = {
1130         .f = cmd_strat_conf3_parsed,  /* function to call */
1131         .data = NULL,      /* 2nd arg of func */
1132         .help_str = help_strat_conf3,
1133         .tokens = {        /* token list, NULL terminated */
1134                 (prog_void *)&cmd_strat_conf3_arg0,
1135                 (prog_void *)&cmd_strat_conf3_arg1,
1136                 (prog_void *)&cmd_strat_conf3_arg2,
1137                 NULL,
1138         },
1139 };
1140
1141
1142 /**********************************************************/
1143 /* Subtraj */
1144
1145
1146 /* function called when cmd_test is parsed successfully */
1147 static void subtraj_test(void)
1148 {
1149 }
1150
1151
1152 /* this structure is filled when cmd_subtraj is parsed successfully */
1153 struct cmd_subtraj_result {
1154         fixed_string_t arg0;
1155         fixed_string_t arg1;
1156         int32_t arg2;
1157         int32_t arg3;
1158         int32_t arg4;
1159         int32_t arg5;
1160 };
1161
1162 /* function called when cmd_subtraj is parsed successfully */
1163 static void cmd_subtraj_parsed(void *parsed_result, void *data)
1164 {
1165         struct cmd_subtraj_result *res = parsed_result;
1166
1167         if (!strcmp_P(res->arg1, PSTR("test")))
1168                 subtraj_test();
1169         else if (!strcmp_P(res->arg1, PSTR("orange_yellow")))
1170                 run_to_the_hills(I2C_COLOR_YELLOW);
1171         else if (!strcmp_P(res->arg1, PSTR("orange_blue")))
1172                 run_to_the_hills(I2C_COLOR_BLUE);
1173         else if (!strcmp_P(res->arg1, PSTR("tomato"))) {
1174                 position_set(&mainboard.pos, 2625,
1175                              COLOR_Y(1847), COLOR_A(0.00));
1176                 get_orphan_tomatoes();
1177         }
1178
1179         trajectory_hardstop(&mainboard.traj);
1180 }
1181
1182 prog_char str_subtraj_arg0[] = "subtraj";
1183 parse_pgm_token_string_t cmd_subtraj_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_subtraj_result, arg0, str_subtraj_arg0);
1184 prog_char str_subtraj_arg1[] = "test#orange_yellow#orange_blue#tomato";
1185 parse_pgm_token_string_t cmd_subtraj_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_subtraj_result, arg1, str_subtraj_arg1);
1186 parse_pgm_token_num_t cmd_subtraj_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg2, INT32);
1187 parse_pgm_token_num_t cmd_subtraj_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg3, INT32);
1188 parse_pgm_token_num_t cmd_subtraj_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg4, INT32);
1189 parse_pgm_token_num_t cmd_subtraj_arg5 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg5, INT32);
1190
1191 prog_char help_subtraj[] = "Test sub-trajectories (a,b,c,d: specific params)";
1192 parse_pgm_inst_t cmd_subtraj = {
1193         .f = cmd_subtraj_parsed,  /* function to call */
1194         .data = NULL,      /* 2nd arg of func */
1195         .help_str = help_subtraj,
1196         .tokens = {        /* token list, NULL terminated */
1197                 (prog_void *)&cmd_subtraj_arg0,
1198                 (prog_void *)&cmd_subtraj_arg1,
1199                 (prog_void *)&cmd_subtraj_arg2,
1200                 (prog_void *)&cmd_subtraj_arg3,
1201                 (prog_void *)&cmd_subtraj_arg4,
1202                 (prog_void *)&cmd_subtraj_arg5,
1203                 NULL,
1204         },
1205 };
1206