test
[aversive.git] / projects / microb2010 / tests / test_board2008 / 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.6 2009-05-02 10:08:09 zer0 Exp $
19  *
20  *  Olivier MATZ <zer0@droids-corp.org> 
21  */
22
23 #include <stdio.h>
24 #include <string.h>
25
26 #include <aversive/pgmspace.h>
27 #include <aversive/wait.h>
28 #include <aversive/error.h>
29
30 #include <uart.h>
31 #include <pwm_ng.h>
32 #include <time.h>
33 #include <encoders_microb.h>
34
35 #include <pid.h>
36 #include <quadramp.h>
37 #include <control_system_manager.h>
38 #include <trajectory_manager.h>
39 #include <blocking_detection_manager.h>
40 #include <robot_system.h>
41 #include <position_manager.h>
42
43 #include <rdline.h>
44 #include <parse.h>
45 #include <parse_string.h>
46 #include <parse_num.h>
47
48 #include "main.h"
49 #include "strat_base.h"
50 #include "strat_utils.h"
51 #include "cs.h"
52 #include "cmdline.h"
53
54 /**********************************************************/
55 /* Traj_Speeds for trajectory_manager */
56
57 /* this structure is filled when cmd_traj_speed is parsed successfully */
58 struct cmd_traj_speed_result {
59         fixed_string_t arg0;
60         fixed_string_t arg1;
61         uint16_t s;
62 };
63
64 /* function called when cmd_traj_speed is parsed successfully */
65 static void cmd_traj_speed_parsed(void *parsed_result, void *data)
66 {
67         struct cmd_traj_speed_result * res = parsed_result;
68         
69         if (!strcmp_P(res->arg1, PSTR("angle"))) {
70                 trajectory_set_speed(&mainboard.traj, mainboard.traj.d_speed, res->s);
71         }
72         else if (!strcmp_P(res->arg1, PSTR("distance"))) {
73                 trajectory_set_speed(&mainboard.traj, res->s, mainboard.traj.a_speed);
74         }
75         /* else it is a "show" */
76
77         printf_P(PSTR("angle %u, distance %u\r\n"), 
78                  mainboard.traj.a_speed,
79                  mainboard.traj.d_speed);
80 }
81
82 prog_char str_traj_speed_arg0[] = "traj_speed";
83 parse_pgm_token_string_t cmd_traj_speed_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_traj_speed_result, arg0, str_traj_speed_arg0);
84 prog_char str_traj_speed_arg1[] = "angle#distance";
85 parse_pgm_token_string_t cmd_traj_speed_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_traj_speed_result, arg1, str_traj_speed_arg1);
86 parse_pgm_token_num_t cmd_traj_speed_s = TOKEN_NUM_INITIALIZER(struct cmd_traj_speed_result, s, UINT16);
87
88 prog_char help_traj_speed[] = "Set traj_speed values for trajectory manager";
89 parse_pgm_inst_t cmd_traj_speed = {
90         .f = cmd_traj_speed_parsed,  /* function to call */
91         .data = NULL,      /* 2nd arg of func */
92         .help_str = help_traj_speed,
93         .tokens = {        /* token list, NULL terminated */
94                 (prog_void *)&cmd_traj_speed_arg0, 
95                 (prog_void *)&cmd_traj_speed_arg1, 
96                 (prog_void *)&cmd_traj_speed_s, 
97                 NULL,
98         },
99 };
100
101 /* show */
102
103 prog_char str_traj_speed_show_arg[] = "show";
104 parse_pgm_token_string_t cmd_traj_speed_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_traj_speed_result, arg1, str_traj_speed_show_arg);
105
106 prog_char help_traj_speed_show[] = "Show traj_speed values for trajectory manager";
107 parse_pgm_inst_t cmd_traj_speed_show = {
108         .f = cmd_traj_speed_parsed,  /* function to call */
109         .data = NULL,      /* 2nd arg of func */
110         .help_str = help_traj_speed_show,
111         .tokens = {        /* token list, NULL terminated */
112                 (prog_void *)&cmd_traj_speed_arg0, 
113                 (prog_void *)&cmd_traj_speed_show_arg,
114                 NULL,
115         },
116 };
117
118 /**********************************************************/
119 /* trajectory window configuration */
120
121 /* this structure is filled when cmd_trajectory is parsed successfully */
122 struct cmd_trajectory_result {
123         fixed_string_t arg0;
124         fixed_string_t arg1;
125         float d_win;
126         float a_win;
127         float a_start;
128 };
129
130
131 /* function called when cmd_trajectory is parsed successfully */
132 static void cmd_trajectory_parsed(void * parsed_result, void * data)
133 {
134         struct cmd_trajectory_result * res = parsed_result;
135         
136         if (!strcmp_P(res->arg1, PSTR("set"))) {
137                 trajectory_set_windows(&mainboard.traj, res->d_win,
138                                        res->a_win, res->a_start);
139         }
140
141         printf_P(PSTR("trajectory %2.2f %2.2f %2.2f\r\n"), mainboard.traj.d_win,
142                  DEG(mainboard.traj.a_win_rad), DEG(mainboard.traj.a_start_rad));
143 }
144
145 prog_char str_trajectory_arg0[] = "trajectory";
146 parse_pgm_token_string_t cmd_trajectory_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_trajectory_result, arg0, str_trajectory_arg0);
147 prog_char str_trajectory_arg1[] = "set";
148 parse_pgm_token_string_t cmd_trajectory_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_trajectory_result, arg1, str_trajectory_arg1);
149 parse_pgm_token_num_t cmd_trajectory_d = TOKEN_NUM_INITIALIZER(struct cmd_trajectory_result, d_win, FLOAT);
150 parse_pgm_token_num_t cmd_trajectory_a = TOKEN_NUM_INITIALIZER(struct cmd_trajectory_result, a_win, FLOAT);
151 parse_pgm_token_num_t cmd_trajectory_as = TOKEN_NUM_INITIALIZER(struct cmd_trajectory_result, a_start, FLOAT);
152
153 prog_char help_trajectory[] = "Set trajectory windows (distance, angle, angle_start)";
154 parse_pgm_inst_t cmd_trajectory = {
155         .f = cmd_trajectory_parsed,  /* function to call */
156         .data = NULL,      /* 2nd arg of func */
157         .help_str = help_trajectory,
158         .tokens = {        /* token list, NULL terminated */
159                 (prog_void *)&cmd_trajectory_arg0, 
160                 (prog_void *)&cmd_trajectory_arg1, 
161                 (prog_void *)&cmd_trajectory_d, 
162                 (prog_void *)&cmd_trajectory_a, 
163                 (prog_void *)&cmd_trajectory_as, 
164                 NULL,
165         },
166 };
167
168 /* show */
169
170 prog_char str_trajectory_show_arg[] = "show";
171 parse_pgm_token_string_t cmd_trajectory_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_trajectory_result, arg1, str_trajectory_show_arg);
172
173 prog_char help_trajectory_show[] = "Show trajectory window configuration";
174 parse_pgm_inst_t cmd_trajectory_show = {
175         .f = cmd_trajectory_parsed,  /* function to call */
176         .data = NULL,      /* 2nd arg of func */
177         .help_str = help_trajectory_show,
178         .tokens = {        /* token list, NULL terminated */
179                 (prog_void *)&cmd_trajectory_arg0, 
180                 (prog_void *)&cmd_trajectory_show_arg,
181                 NULL,
182         },
183 };
184
185 /**********************************************************/
186 /* rs_gains configuration */
187
188 /* this structure is filled when cmd_rs_gains is parsed successfully */
189 struct cmd_rs_gains_result {
190         fixed_string_t arg0;
191         fixed_string_t arg1;
192         float left;
193         float right;
194 };
195
196 /* function called when cmd_rs_gains is parsed successfully */
197 static void cmd_rs_gains_parsed(void * parsed_result, void * data)
198 {
199         struct cmd_rs_gains_result * res = parsed_result;
200
201         if (!strcmp_P(res->arg1, PSTR("set"))) {
202                 rs_set_left_ext_encoder(&mainboard.rs, encoders_microb_get_value, 
203                                         LEFT_ENCODER, res->left); // en augmentant on tourne à gauche
204                 rs_set_right_ext_encoder(&mainboard.rs, encoders_microb_get_value, 
205                                          RIGHT_ENCODER, res->right); //en augmentant on tourne à droite
206         }
207         printf_P(PSTR("rs_gains set %f %f"),
208                  mainboard.rs.left_ext_gain,
209                  mainboard.rs.right_ext_gain);
210 /*      f64_print(mainboard.rs.left_ext_gain); */
211 /*      printf_P(PSTR(" ")); */
212 /*      f64_print(mainboard.rs.right_ext_gain); */
213         printf_P(PSTR("\r\n"));
214 }
215
216 prog_char str_rs_gains_arg0[] = "rs_gains";
217 parse_pgm_token_string_t cmd_rs_gains_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_rs_gains_result, arg0, str_rs_gains_arg0);
218 prog_char str_rs_gains_arg1[] = "set";
219 parse_pgm_token_string_t cmd_rs_gains_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_rs_gains_result, arg1, str_rs_gains_arg1);
220 parse_pgm_token_num_t cmd_rs_gains_l = TOKEN_NUM_INITIALIZER(struct cmd_rs_gains_result, left, FLOAT);
221 parse_pgm_token_num_t cmd_rs_gains_r = TOKEN_NUM_INITIALIZER(struct cmd_rs_gains_result, right, FLOAT);
222
223 prog_char help_rs_gains[] = "Set rs_gains (left, right)";
224 parse_pgm_inst_t cmd_rs_gains = {
225         .f = cmd_rs_gains_parsed,  /* function to call */
226         .data = NULL,      /* 2nd arg of func */
227         .help_str = help_rs_gains,
228         .tokens = {        /* token list, NULL terminated */
229                 (prog_void *)&cmd_rs_gains_arg0, 
230                 (prog_void *)&cmd_rs_gains_arg1, 
231                 (prog_void *)&cmd_rs_gains_l, 
232                 (prog_void *)&cmd_rs_gains_r, 
233                 NULL,
234         },
235 };
236
237 /* show */
238
239 prog_char str_rs_gains_show_arg[] = "show";
240 parse_pgm_token_string_t cmd_rs_gains_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_rs_gains_result, arg1, str_rs_gains_show_arg);
241
242 prog_char help_rs_gains_show[] = "Show rs_gains";
243 parse_pgm_inst_t cmd_rs_gains_show = {
244         .f = cmd_rs_gains_parsed,  /* function to call */
245         .data = NULL,      /* 2nd arg of func */
246         .help_str = help_rs_gains_show,
247         .tokens = {        /* token list, NULL terminated */
248                 (prog_void *)&cmd_rs_gains_arg0, 
249                 (prog_void *)&cmd_rs_gains_show_arg,
250                 NULL,
251         },
252 };
253
254 /**********************************************************/
255 /* track configuration */
256
257 /* this structure is filled when cmd_track is parsed successfully */
258 struct cmd_track_result {
259         fixed_string_t arg0;
260         fixed_string_t arg1;
261         float val;
262 };
263
264 /* function called when cmd_track is parsed successfully */
265 static void cmd_track_parsed(void * parsed_result, void * data)
266 {
267         struct cmd_track_result * res = parsed_result;
268
269         if (!strcmp_P(res->arg1, PSTR("set"))) {
270                 position_set_physical_params(&mainboard.pos, res->val, DIST_IMP_MM);
271         }
272         printf_P(PSTR("track set %f\r\n"), mainboard.pos.phys.track_mm);
273 }
274
275 prog_char str_track_arg0[] = "track";
276 parse_pgm_token_string_t cmd_track_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_track_result, arg0, str_track_arg0);
277 prog_char str_track_arg1[] = "set";
278 parse_pgm_token_string_t cmd_track_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_track_result, arg1, str_track_arg1);
279 parse_pgm_token_num_t cmd_track_val = TOKEN_NUM_INITIALIZER(struct cmd_track_result, val, FLOAT);
280
281 prog_char help_track[] = "Set track in mm";
282 parse_pgm_inst_t cmd_track = {
283         .f = cmd_track_parsed,  /* function to call */
284         .data = NULL,      /* 2nd arg of func */
285         .help_str = help_track,
286         .tokens = {        /* token list, NULL terminated */
287                 (prog_void *)&cmd_track_arg0, 
288                 (prog_void *)&cmd_track_arg1, 
289                 (prog_void *)&cmd_track_val, 
290                 NULL,
291         },
292 };
293
294 /* show */
295
296 prog_char str_track_show_arg[] = "show";
297 parse_pgm_token_string_t cmd_track_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_track_result, arg1, str_track_show_arg);
298
299 prog_char help_track_show[] = "Show track";
300 parse_pgm_inst_t cmd_track_show = {
301         .f = cmd_track_parsed,  /* function to call */
302         .data = NULL,      /* 2nd arg of func */
303         .help_str = help_track_show,
304         .tokens = {        /* token list, NULL terminated */
305                 (prog_void *)&cmd_track_arg0, 
306                 (prog_void *)&cmd_track_show_arg,
307                 NULL,
308         },
309 };
310
311
312
313 /**********************************************************/
314 /* Pt_Lists for testing traj */
315
316 #define PT_LIST_SIZE 10
317 static struct xy_point pt_list[PT_LIST_SIZE];
318 static uint16_t pt_list_len = 0;
319
320 /* this structure is filled when cmd_pt_list is parsed successfully */
321 struct cmd_pt_list_result {
322         fixed_string_t arg0;
323         fixed_string_t arg1;
324         uint16_t arg2;
325         int16_t arg3;
326         int16_t arg4;
327 };
328
329 /* function called when cmd_pt_list is parsed successfully */
330 static void cmd_pt_list_parsed(void * parsed_result, void * data)
331 {
332         struct cmd_pt_list_result * res = parsed_result;
333         uint8_t i, why=0;
334         
335         if (!strcmp_P(res->arg1, PSTR("append"))) {
336                 res->arg2 = pt_list_len;
337         }
338
339         if (!strcmp_P(res->arg1, PSTR("insert")) ||
340             !strcmp_P(res->arg1, PSTR("append"))) {
341                 if (res->arg2 > pt_list_len) {
342                         printf_P(PSTR("Index too large\r\n"));
343                         return;
344                 }
345                 if (pt_list_len >= PT_LIST_SIZE) {
346                         printf_P(PSTR("List is too large\r\n"));
347                         return;
348                 }
349                 memmove(&pt_list[res->arg2+1], &pt_list[res->arg2], 
350                        PT_LIST_SIZE-1-res->arg2);
351                 pt_list[res->arg2].x = res->arg3;
352                 pt_list[res->arg2].y = res->arg4;
353                 pt_list_len++;
354         }
355         else if (!strcmp_P(res->arg1, PSTR("del"))) {
356                 if (pt_list_len <= 0) {
357                         printf_P(PSTR("Error: list empty\r\n"));
358                         return;
359                 }
360                 if (res->arg2 > pt_list_len) {
361                         printf_P(PSTR("Index too large\r\n"));
362                         return;
363                 }
364                 memmove(&pt_list[res->arg2], &pt_list[res->arg2+1], 
365                        (PT_LIST_SIZE-1-res->arg2)*sizeof(struct xy_point));
366                 pt_list_len--;
367         }
368         else if (!strcmp_P(res->arg1, PSTR("reset"))) {
369                 pt_list_len = 0;
370         }
371         
372         /* else it is a "show" or a "start" */
373         if (pt_list_len == 0) {
374                 printf_P(PSTR("List empty\r\n"));
375                 return;
376         }
377         for (i=0 ; i<pt_list_len ; i++) {
378                 printf_P(PSTR("%d: x=%d y=%d\r\n"), i, pt_list[i].x, pt_list[i].y);
379                 if (!strcmp_P(res->arg1, PSTR("start"))) {
380                         trajectory_goto_xy_abs(&mainboard.traj, pt_list[i].x, pt_list[i].y);
381                         why = wait_traj_end(0xFF); /* all */
382                 }
383                 else if (!strcmp_P(res->arg1, PSTR("avoid_start"))) {
384                         while (1) {
385                                 //why = goto_and_avoid(pt_list[i].x, pt_list[i].y, 0xFF, 0xFF);
386                                 printf("next point\r\n");
387                                 if (why != END_OBSTACLE)
388                                         break;
389                         }
390                 }
391                 if (why & (~(END_TRAJ | END_NEAR)))
392                         trajectory_stop(&mainboard.traj);
393         }
394 }
395
396 prog_char str_pt_list_arg0[] = "pt_list";
397 parse_pgm_token_string_t cmd_pt_list_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg0, str_pt_list_arg0);
398 prog_char str_pt_list_arg1[] = "insert";
399 parse_pgm_token_string_t cmd_pt_list_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_arg1);
400 parse_pgm_token_num_t cmd_pt_list_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_pt_list_result, arg2, UINT16);
401 parse_pgm_token_num_t cmd_pt_list_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_pt_list_result, arg3, INT16);
402 parse_pgm_token_num_t cmd_pt_list_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_pt_list_result, arg4, INT16);
403
404 prog_char help_pt_list[] = "Insert point in pt_list (idx,x,y)";
405 parse_pgm_inst_t cmd_pt_list = {
406         .f = cmd_pt_list_parsed,  /* function to call */
407         .data = NULL,      /* 2nd arg of func */
408         .help_str = help_pt_list,
409         .tokens = {        /* token list, NULL terminated */
410                 (prog_void *)&cmd_pt_list_arg0, 
411                 (prog_void *)&cmd_pt_list_arg1, 
412                 (prog_void *)&cmd_pt_list_arg2, 
413                 (prog_void *)&cmd_pt_list_arg3, 
414                 (prog_void *)&cmd_pt_list_arg4, 
415                 NULL,
416         },
417 };
418
419 /* append */
420
421 prog_char str_pt_list_arg1_append[] = "append";
422 parse_pgm_token_string_t cmd_pt_list_arg1_append = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_arg1_append);
423
424 prog_char help_pt_list_append[] = "Append point in pt_list (x,y)";
425 parse_pgm_inst_t cmd_pt_list_append = {
426         .f = cmd_pt_list_parsed,  /* function to call */
427         .data = NULL,      /* 2nd arg of func */
428         .help_str = help_pt_list_append,
429         .tokens = {        /* token list, NULL terminated */
430                 (prog_void *)&cmd_pt_list_arg0, 
431                 (prog_void *)&cmd_pt_list_arg1_append, 
432                 (prog_void *)&cmd_pt_list_arg3, 
433                 (prog_void *)&cmd_pt_list_arg4, 
434                 NULL,
435         },
436 };
437
438 /* del */
439
440 prog_char str_pt_list_del_arg[] = "del";
441 parse_pgm_token_string_t cmd_pt_list_del_arg = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_del_arg);
442
443 prog_char help_pt_list_del[] = "Del or insert point in pt_list (num)";
444 parse_pgm_inst_t cmd_pt_list_del = {
445         .f = cmd_pt_list_parsed,  /* function to call */
446         .data = NULL,      /* 2nd arg of func */
447         .help_str = help_pt_list_del,
448         .tokens = {        /* token list, NULL terminated */
449                 (prog_void *)&cmd_pt_list_arg0, 
450                 (prog_void *)&cmd_pt_list_del_arg, 
451                 (prog_void *)&cmd_pt_list_arg2,
452                 NULL,
453         },
454 };
455 /* show */
456
457 prog_char str_pt_list_show_arg[] = "show#reset#start#avoid_start";
458 parse_pgm_token_string_t cmd_pt_list_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_show_arg);
459
460 prog_char help_pt_list_show[] = "Show, start or reset pt_list";
461 parse_pgm_inst_t cmd_pt_list_show = {
462         .f = cmd_pt_list_parsed,  /* function to call */
463         .data = NULL,      /* 2nd arg of func */
464         .help_str = help_pt_list_show,
465         .tokens = {        /* token list, NULL terminated */
466                 (prog_void *)&cmd_pt_list_arg0, 
467                 (prog_void *)&cmd_pt_list_show_arg,
468                 NULL,
469         },
470 };
471
472
473
474 /**********************************************************/
475 /* Goto function */
476
477 /* this structure is filled when cmd_goto is parsed successfully */
478 struct cmd_goto_result {
479         fixed_string_t arg0;
480         fixed_string_t arg1;
481         int32_t arg2;
482         int32_t arg3;
483         int32_t arg4;
484 };
485
486 /* function called when cmd_goto is parsed successfully */
487 static void cmd_goto_parsed(void * parsed_result, void * data)
488 {
489         struct cmd_goto_result * res = parsed_result;
490         uint8_t err;
491         microseconds t1, t2;
492
493         interrupt_traj_reset();
494         if (!strcmp_P(res->arg1, PSTR("a_rel"))) {
495                 trajectory_a_rel(&mainboard.traj, res->arg2);
496         }
497         else if (!strcmp_P(res->arg1, PSTR("d_rel"))) {
498                 trajectory_d_rel(&mainboard.traj, res->arg2);
499         }
500         else if (!strcmp_P(res->arg1, PSTR("a_abs"))) {
501                 trajectory_a_abs(&mainboard.traj, res->arg2);
502         }
503         else if (!strcmp_P(res->arg1, PSTR("a_to_xy"))) {
504                 trajectory_turnto_xy(&mainboard.traj, res->arg2, res->arg3);
505         }
506         else if (!strcmp_P(res->arg1, PSTR("a_behind_xy"))) {
507                 trajectory_turnto_xy_behind(&mainboard.traj, res->arg2, res->arg3);
508         }
509         else if (!strcmp_P(res->arg1, PSTR("xy_rel"))) {
510                 trajectory_goto_xy_rel(&mainboard.traj, res->arg2, res->arg3);
511         }
512         else if (!strcmp_P(res->arg1, PSTR("xy_abs"))) {
513                 trajectory_goto_xy_abs(&mainboard.traj, res->arg2, res->arg3);
514         }
515         else if (!strcmp_P(res->arg1, PSTR("avoid"))) {
516                 //err = goto_and_avoid_forward(res->arg2, res->arg3, 0xFF, 0xFF);
517                 err=0;
518                 if (err != END_TRAJ && err != END_NEAR)
519                         strat_hardstop();
520         }
521         else if (!strcmp_P(res->arg1, PSTR("avoid_bw"))) {
522                 //err = goto_and_avoid_backward(res->arg2, res->arg3, 0xFF, 0xFF);
523                 err=0;
524                 if (err != END_TRAJ && err != END_NEAR)
525                         strat_hardstop();
526         }
527         else if (!strcmp_P(res->arg1, PSTR("xy_abs_fow"))) {
528                 trajectory_goto_forward_xy_abs(&mainboard.traj, res->arg2, res->arg3);
529         }
530         else if (!strcmp_P(res->arg1, PSTR("xy_abs_back"))) {
531                 trajectory_goto_backward_xy_abs(&mainboard.traj, res->arg2, res->arg3);
532         }
533         else if (!strcmp_P(res->arg1, PSTR("da_rel"))) {
534                 trajectory_d_a_rel(&mainboard.traj, res->arg2, res->arg3);
535         }
536         t1 = time_get_us2();
537         while ((err = test_traj_end(0xFF)) == 0) {
538                 t2 = time_get_us2();
539                 if (t2 - t1 > 200000) {
540                         dump_cs_debug("angle", &mainboard.angle.cs);
541                         dump_cs_debug("distance", &mainboard.distance.cs);
542                         t1 = t2;
543                 }
544         }
545         if (err != END_TRAJ && err != END_NEAR)
546                 strat_hardstop();
547         printf_P(PSTR("returned %s\r\n"), get_err(err));
548 }
549
550 prog_char str_goto_arg0[] = "goto";
551 parse_pgm_token_string_t cmd_goto_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_goto_result, arg0, str_goto_arg0);
552 prog_char str_goto_arg1_a[] = "d_rel#a_rel#a_abs";
553 parse_pgm_token_string_t cmd_goto_arg1_a = TOKEN_STRING_INITIALIZER(struct cmd_goto_result, arg1, str_goto_arg1_a);
554 parse_pgm_token_num_t cmd_goto_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_goto_result, arg2, INT32);
555
556 /* 1 params */
557 prog_char help_goto1[] = "Change orientation of the mainboard";
558 parse_pgm_inst_t cmd_goto1 = {
559         .f = cmd_goto_parsed,  /* function to call */
560         .data = NULL,      /* 2nd arg of func */
561         .help_str = help_goto1,
562         .tokens = {        /* token list, NULL terminated */
563                 (prog_void *)&cmd_goto_arg0, 
564                 (prog_void *)&cmd_goto_arg1_a, 
565                 (prog_void *)&cmd_goto_arg2, 
566                 NULL,
567         },
568 };
569
570 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";
571 parse_pgm_token_string_t cmd_goto_arg1_b = TOKEN_STRING_INITIALIZER(struct cmd_goto_result, arg1, str_goto_arg1_b);
572 parse_pgm_token_num_t cmd_goto_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_goto_result, arg3, INT32);
573
574 /* 2 params */
575 prog_char help_goto2[] = "Go to a (x,y) or (d,a) position";
576 parse_pgm_inst_t cmd_goto2 = {
577         .f = cmd_goto_parsed,  /* function to call */
578         .data = NULL,      /* 2nd arg of func */
579         .help_str = help_goto2,
580         .tokens = {        /* token list, NULL terminated */
581                 (prog_void *)&cmd_goto_arg0, 
582                 (prog_void *)&cmd_goto_arg1_b, 
583                 (prog_void *)&cmd_goto_arg2,
584                 (prog_void *)&cmd_goto_arg3, 
585                 NULL,
586         },
587 };
588
589 /**********************************************************/
590 /* Position tests */
591
592 /* this structure is filled when cmd_position is parsed successfully */
593 struct cmd_position_result {
594         fixed_string_t arg0;
595         fixed_string_t arg1;
596         int32_t arg2;
597         int32_t arg3;
598         int32_t arg4;
599 };
600
601 #define AUTOPOS_SPEED_FAST 200
602 static void auto_position(void)
603 {
604         uint8_t err;
605         uint16_t old_spdd, old_spda;
606
607         interrupt_traj_reset();
608         strat_get_speed(&old_spdd, &old_spda);
609         strat_set_speed(AUTOPOS_SPEED_FAST, AUTOPOS_SPEED_FAST);
610
611         trajectory_d_rel(&mainboard.traj, -300);
612         err = wait_traj_end(END_INTR|END_TRAJ|END_BLOCKING);
613         if (err == END_INTR)
614                 goto intr;
615         wait_ms(100);
616         strat_reset_pos(ROBOT_LENGTH/2, 0, 0);
617
618         trajectory_d_rel(&mainboard.traj, 120);
619         err = wait_traj_end(END_INTR|END_TRAJ);
620         if (err == END_INTR)
621                 goto intr;
622
623         trajectory_a_rel(&mainboard.traj, 90);
624         err = wait_traj_end(END_INTR|END_TRAJ);
625         if (err == END_INTR)
626                 goto intr;
627
628         trajectory_d_rel(&mainboard.traj, -300);
629         err = wait_traj_end(END_INTR|END_TRAJ|END_BLOCKING);
630         if (err == END_INTR)
631                 goto intr;
632         wait_ms(100);
633         strat_reset_pos(DO_NOT_SET_POS, ROBOT_LENGTH/2,
634                         90);
635
636         trajectory_d_rel(&mainboard.traj, 120);
637         err = wait_traj_end(END_INTR|END_TRAJ);
638         if (err == END_INTR)
639                 goto intr;
640         wait_ms(100);
641         
642         trajectory_a_rel(&mainboard.traj, -40);
643         err = wait_traj_end(END_INTR|END_TRAJ);
644         if (err == END_INTR)
645                 goto intr;
646         wait_ms(100);
647         
648         strat_set_speed(old_spdd, old_spda);
649         return;
650
651 intr:
652         strat_hardstop();
653         strat_set_speed(old_spdd, old_spda);
654 }
655
656 /* function called when cmd_position is parsed successfully */
657 static void cmd_position_parsed(void * parsed_result, void * data)
658 {
659         struct cmd_position_result * res = parsed_result;
660         
661         /* display raw position values */
662         if (!strcmp_P(res->arg1, PSTR("reset"))) {
663                 position_set(&mainboard.pos, 0, 0, 0);
664         }
665         else if (!strcmp_P(res->arg1, PSTR("set"))) {
666                 position_set(&mainboard.pos, res->arg2, res->arg3, res->arg4);
667         }
668         else if (!strcmp_P(res->arg1, PSTR("autoset_green"))) {
669                 auto_position();
670         }
671         else if (!strcmp_P(res->arg1, PSTR("autoset_red"))) {
672                 auto_position();
673         }
674
675         /* else it's just a "show" */
676         printf_P(PSTR("x=%.2f y=%.2f a=%.2f\r\n"), 
677                  position_get_x_double(&mainboard.pos),
678                  position_get_y_double(&mainboard.pos),
679                  DEG(position_get_a_rad_double(&mainboard.pos)));
680 }
681
682 prog_char str_position_arg0[] = "position";
683 parse_pgm_token_string_t cmd_position_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg0, str_position_arg0);
684 prog_char str_position_arg1[] = "show#reset#autoset_green#autoset_red";
685 parse_pgm_token_string_t cmd_position_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg1, str_position_arg1);
686
687 prog_char help_position[] = "Show/reset (x,y,a) position";
688 parse_pgm_inst_t cmd_position = {
689         .f = cmd_position_parsed,  /* function to call */
690         .data = NULL,      /* 2nd arg of func */
691         .help_str = help_position,
692         .tokens = {        /* token list, NULL terminated */
693                 (prog_void *)&cmd_position_arg0, 
694                 (prog_void *)&cmd_position_arg1, 
695                 NULL,
696         },
697 };
698
699
700 prog_char str_position_arg1_set[] = "set";
701 parse_pgm_token_string_t cmd_position_arg1_set = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg1, str_position_arg1_set);
702 parse_pgm_token_num_t cmd_position_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg2, INT32);
703 parse_pgm_token_num_t cmd_position_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg3, INT32);
704 parse_pgm_token_num_t cmd_position_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg4, INT32);
705
706 prog_char help_position_set[] = "Set (x,y,a) position";
707 parse_pgm_inst_t cmd_position_set = {
708         .f = cmd_position_parsed,  /* function to call */
709         .data = NULL,      /* 2nd arg of func */
710         .help_str = help_position_set,
711         .tokens = {        /* token list, NULL terminated */
712                 (prog_void *)&cmd_position_arg0, 
713                 (prog_void *)&cmd_position_arg1_set, 
714                 (prog_void *)&cmd_position_arg2, 
715                 (prog_void *)&cmd_position_arg3, 
716                 (prog_void *)&cmd_position_arg4, 
717                 NULL,
718         },
719 };
720