hostsim test
[aversive.git] / projects / microb2009 / mainboard / commands_mainboard.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_mainboard.c,v 1.9 2009-11-08 17:24:33 zer0 Exp $
19  *
20  *  Olivier MATZ <zer0@droids-corp.org> 
21  */
22
23 #include <stdio.h>
24 #include <string.h>
25
26 #include <aversive/pgmspace.h>
27 #include <aversive/wait.h>
28 #include <aversive/error.h>
29 #include <avr/eeprom.h>
30
31 #include <ax12.h>
32 #include <uart.h>
33 #include <pwm_ng.h>
34 #include <time.h>
35 #include <spi.h>
36 #include <i2c.h>
37
38 #include <pid.h>
39 #include <quadramp.h>
40 #include <control_system_manager.h>
41 #include <trajectory_manager.h>
42 #include <vect_base.h>
43 #include <lines.h>
44 #include <polygon.h>
45 #include <obstacle_avoidance.h>
46 #include <blocking_detection_manager.h>
47 #include <robot_system.h>
48 #include <position_manager.h>
49
50 #include <rdline.h>
51 #include <parse.h>
52 #include <parse_string.h>
53 #include <parse_num.h>
54
55 #include "../common/i2c_commands.h"
56 #include "../common/eeprom_mapping.h"
57
58 #include "main.h"
59 #include "sensor.h"
60 #include "cmdline.h"
61 #include "strat.h"
62 #include "strat_utils.h"
63 #include "strat_base.h"
64 #include "i2c_protocol.h"
65 #include "actuator.h"
66
67 struct cmd_event_result {
68         fixed_string_t arg0;
69         fixed_string_t arg1;
70         fixed_string_t arg2;
71 };
72
73
74 /* function called when cmd_event is parsed successfully */
75 static void cmd_event_parsed(void *parsed_result, void *data)
76 {
77         u08 bit=0;
78
79         struct cmd_event_result * res = parsed_result;
80         
81         if (!strcmp_P(res->arg1, PSTR("all"))) {
82                 bit = DO_ENCODERS | DO_CS | DO_RS | DO_POS |
83                         DO_BD | DO_TIMER | DO_POWER;
84                 if (!strcmp_P(res->arg2, PSTR("on")))
85                         mainboard.flags |= bit;
86                 else if (!strcmp_P(res->arg2, PSTR("off")))
87                         mainboard.flags &= bit;
88                 else { /* show */
89                         printf_P(PSTR("encoders is %s\r\n"), 
90                                  (DO_ENCODERS & mainboard.flags) ? "on":"off");
91                         printf_P(PSTR("cs is %s\r\n"), 
92                                  (DO_CS & mainboard.flags) ? "on":"off");
93                         printf_P(PSTR("rs is %s\r\n"), 
94                                  (DO_RS & mainboard.flags) ? "on":"off");
95                         printf_P(PSTR("pos is %s\r\n"), 
96                                  (DO_POS & mainboard.flags) ? "on":"off");
97                         printf_P(PSTR("bd is %s\r\n"), 
98                                  (DO_BD & mainboard.flags) ? "on":"off");
99                         printf_P(PSTR("timer is %s\r\n"), 
100                                  (DO_TIMER & mainboard.flags) ? "on":"off");
101                         printf_P(PSTR("power is %s\r\n"), 
102                                  (DO_POWER & mainboard.flags) ? "on":"off");
103                 }
104                 return;
105         }
106
107         if (!strcmp_P(res->arg1, PSTR("encoders")))
108                 bit = DO_ENCODERS;
109         else if (!strcmp_P(res->arg1, PSTR("cs"))) {
110                 strat_hardstop();
111                 bit = DO_CS;
112         }
113         else if (!strcmp_P(res->arg1, PSTR("rs")))
114                 bit = DO_RS;
115         else if (!strcmp_P(res->arg1, PSTR("pos")))
116                 bit = DO_POS;
117         else if (!strcmp_P(res->arg1, PSTR("bd")))
118                 bit = DO_BD;
119         else if (!strcmp_P(res->arg1, PSTR("timer"))) {
120                 time_reset();
121                 bit = DO_TIMER;
122         }
123         else if (!strcmp_P(res->arg1, PSTR("power")))
124                 bit = DO_POWER;
125
126         if (!strcmp_P(res->arg2, PSTR("on")))
127                 mainboard.flags |= bit;
128         else if (!strcmp_P(res->arg2, PSTR("off"))) {
129                 if (!strcmp_P(res->arg1, PSTR("cs"))) {
130                         pwm_ng_set(LEFT_PWM, 0);
131                         pwm_ng_set(RIGHT_PWM, 0);
132                 }
133                 mainboard.flags &= (~bit);
134         }
135         printf_P(PSTR("%s is %s\r\n"), res->arg1, 
136                       (bit & mainboard.flags) ? "on":"off");
137 }
138
139 prog_char str_event_arg0[] = "event";
140 parse_pgm_token_string_t cmd_event_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg0, str_event_arg0);
141 prog_char str_event_arg1[] = "all#encoders#cs#rs#pos#bd#timer#power";
142 parse_pgm_token_string_t cmd_event_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg1, str_event_arg1);
143 prog_char str_event_arg2[] = "on#off#show";
144 parse_pgm_token_string_t cmd_event_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg2, str_event_arg2);
145
146 prog_char help_event[] = "Enable/disable events";
147 parse_pgm_inst_t cmd_event = {
148         .f = cmd_event_parsed,  /* function to call */
149         .data = NULL,      /* 2nd arg of func */
150         .help_str = help_event,
151         .tokens = {        /* token list, NULL terminated */
152                 (prog_void *)&cmd_event_arg0, 
153                 (prog_void *)&cmd_event_arg1, 
154                 (prog_void *)&cmd_event_arg2, 
155                 NULL,
156         },
157 };
158
159
160 /**********************************************************/
161 /* Spi_Test */
162
163 /* this structure is filled when cmd_spi_test is parsed successfully */
164 struct cmd_spi_test_result {
165         fixed_string_t arg0;
166 };
167
168 /* function called when cmd_spi_test is parsed successfully */
169 static void cmd_spi_test_parsed(void * parsed_result, void * data)
170 {
171         uint16_t i = 0, ret = 0, ret2 = 0;
172         
173         if (mainboard.flags & DO_ENCODERS) {
174                 printf_P(PSTR("Disable encoder event first\r\n"));
175                 return;
176         }
177
178         do {
179                 spi_slave_select(0);
180                 ret = spi_send_and_receive_byte(i);
181                 ret2 = spi_send_and_receive_byte(i);
182                 spi_slave_deselect(0);
183
184                 if ((i & 0x7ff) == 0)
185                         printf_P(PSTR("Sent %.4x twice, received %x %x\r\n"),
186                                  i, ret, ret2);
187
188                 i++;
189         } while(!cmdline_keypressed());
190 }
191
192 prog_char str_spi_test_arg0[] = "spi_test";
193 parse_pgm_token_string_t cmd_spi_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_spi_test_result, arg0, str_spi_test_arg0);
194
195 prog_char help_spi_test[] = "Test the SPI";
196 parse_pgm_inst_t cmd_spi_test = {
197         .f = cmd_spi_test_parsed,  /* function to call */
198         .data = NULL,      /* 2nd arg of func */
199         .help_str = help_spi_test,
200         .tokens = {        /* token list, NULL terminated */
201                 (prog_void *)&cmd_spi_test_arg0, 
202                 NULL,
203         },
204 };
205
206
207
208 /**********************************************************/
209 /* Opponent tests */
210
211 /* this structure is filled when cmd_opponent is parsed successfully */
212 struct cmd_opponent_result {
213         fixed_string_t arg0;
214         fixed_string_t arg1;
215         int32_t arg2;
216         int32_t arg3;
217 };
218
219 /* function called when cmd_opponent is parsed successfully */
220 static void cmd_opponent_parsed(void *parsed_result, void *data)
221 {
222         int16_t x,y,d,a;
223
224         if (get_opponent_xyda(&x, &y, &d, &a))
225                 printf_P(PSTR("No opponent\r\n"));
226         else
227                 printf_P(PSTR("x=%d y=%d, d=%d a=%d\r\n"), x, y, d, a);
228 }
229
230 prog_char str_opponent_arg0[] = "opponent";
231 parse_pgm_token_string_t cmd_opponent_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_opponent_result, arg0, str_opponent_arg0);
232 prog_char str_opponent_arg1[] = "show";
233 parse_pgm_token_string_t cmd_opponent_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_opponent_result, arg1, str_opponent_arg1);
234
235 prog_char help_opponent[] = "Show (x,y) opponent";
236 parse_pgm_inst_t cmd_opponent = {
237         .f = cmd_opponent_parsed,  /* function to call */
238         .data = NULL,      /* 2nd arg of func */
239         .help_str = help_opponent,
240         .tokens = {        /* token list, NULL terminated */
241                 (prog_void *)&cmd_opponent_arg0, 
242                 (prog_void *)&cmd_opponent_arg1, 
243                 NULL,
244         },
245 };
246
247
248 prog_char str_opponent_arg1_set[] = "set";
249 parse_pgm_token_string_t cmd_opponent_arg1_set = TOKEN_STRING_INITIALIZER(struct cmd_opponent_result, arg1, str_opponent_arg1_set);
250 parse_pgm_token_num_t cmd_opponent_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_opponent_result, arg2, INT32);
251 parse_pgm_token_num_t cmd_opponent_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_opponent_result, arg3, INT32);
252
253 prog_char help_opponent_set[] = "Set (x,y) opponent";
254 parse_pgm_inst_t cmd_opponent_set = {
255         .f = cmd_opponent_parsed,  /* function to call */
256         .data = NULL,      /* 2nd arg of func */
257         .help_str = help_opponent_set,
258         .tokens = {        /* token list, NULL terminated */
259                 (prog_void *)&cmd_opponent_arg0, 
260                 (prog_void *)&cmd_opponent_arg1_set,
261                 (prog_void *)&cmd_opponent_arg2, 
262                 (prog_void *)&cmd_opponent_arg3, 
263                 NULL,
264         },
265 };
266
267
268 /**********************************************************/
269 /* Start */
270
271 /* this structure is filled when cmd_start is parsed successfully */
272 struct cmd_start_result {
273         fixed_string_t arg0;
274         fixed_string_t color;
275         fixed_string_t debug;
276 };
277
278 /* function called when cmd_start is parsed successfully */
279 static void cmd_start_parsed(void *parsed_result, void *data)
280 {
281         struct cmd_start_result *res = parsed_result;
282         uint8_t old_level = gen.log_level;
283
284         gen.logs[NB_LOGS] = E_USER_STRAT;
285         if (!strcmp_P(res->debug, PSTR("debug"))) {
286                 strat_infos.dump_enabled = 1;
287                 gen.log_level = 5;
288         }
289         else {
290                 strat_infos.dump_enabled = 0;
291                 gen.log_level = 0;
292         }
293
294         if (!strcmp_P(res->color, PSTR("red"))) {
295                 mainboard.our_color = I2C_COLOR_RED;
296                 i2c_set_color(I2C_MECHBOARD_ADDR, I2C_COLOR_RED);
297                 i2c_set_color(I2C_SENSORBOARD_ADDR, I2C_COLOR_RED);
298         }
299         else if (!strcmp_P(res->color, PSTR("green"))) {
300                 mainboard.our_color = I2C_COLOR_GREEN;
301                 i2c_set_color(I2C_MECHBOARD_ADDR, I2C_COLOR_GREEN);
302                 i2c_set_color(I2C_SENSORBOARD_ADDR, I2C_COLOR_GREEN);
303         }
304
305         printf_P(PSTR("Check that lintel is loaded\r\n"));
306         while(!cmdline_keypressed());
307
308         printf_P(PSTR("Press a key when beacon ready\r\n"));
309         i2c_sensorboard_set_beacon(0);
310         while(!cmdline_keypressed());
311         i2c_sensorboard_set_beacon(1);
312
313         strat_start();
314
315         gen.logs[NB_LOGS] = 0;
316         gen.log_level = old_level;
317 }
318
319 prog_char str_start_arg0[] = "start";
320 parse_pgm_token_string_t cmd_start_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_start_result, arg0, str_start_arg0);
321 prog_char str_start_color[] = "green#red";
322 parse_pgm_token_string_t cmd_start_color = TOKEN_STRING_INITIALIZER(struct cmd_start_result, color, str_start_color);
323 prog_char str_start_debug[] = "debug#match";
324 parse_pgm_token_string_t cmd_start_debug = TOKEN_STRING_INITIALIZER(struct cmd_start_result, debug, str_start_debug);
325
326 prog_char help_start[] = "Start the robot";
327 parse_pgm_inst_t cmd_start = {
328         .f = cmd_start_parsed,  /* function to call */
329         .data = NULL,      /* 2nd arg of func */
330         .help_str = help_start,
331         .tokens = {        /* token list, NULL terminated */
332                 (prog_void *)&cmd_start_arg0, 
333                 (prog_void *)&cmd_start_color, 
334                 (prog_void *)&cmd_start_debug, 
335                 NULL,
336         },
337 };
338
339
340
341 /**********************************************************/
342 /* Interact */
343
344 /* this structure is filled when cmd_interact is parsed successfully */
345 struct cmd_interact_result {
346         fixed_string_t arg0;
347 };
348
349 static void print_cs(void)
350 {
351         printf_P(PSTR("cons_d=% .8ld cons_a=% .8ld fil_d=% .8ld fil_a=% .8ld "
352                       "err_d=% .8ld err_a=% .8ld out_d=% .8ld out_a=% .8ld\r\n"), 
353                  cs_get_consign(&mainboard.distance.cs),
354                  cs_get_consign(&mainboard.angle.cs),
355                  cs_get_filtered_consign(&mainboard.distance.cs),
356                  cs_get_filtered_consign(&mainboard.angle.cs),
357                  cs_get_error(&mainboard.distance.cs),
358                  cs_get_error(&mainboard.angle.cs),
359                  cs_get_out(&mainboard.distance.cs),
360                  cs_get_out(&mainboard.angle.cs));
361 }
362
363 static void print_pos(void)
364 {
365         printf_P(PSTR("x=% .8d y=% .8d a=% .8d\r\n"), 
366                  position_get_x_s16(&mainboard.pos),
367                  position_get_y_s16(&mainboard.pos),
368                  position_get_a_deg_s16(&mainboard.pos));
369 }
370
371 static void print_time(void)
372 {
373         printf_P(PSTR("time %d\r\n"),time_get_s());
374 }
375
376
377 static void print_sensors(void)
378 {
379 #ifdef notyet
380         if (sensor_start_switch())
381                 printf_P(PSTR("Start switch | "));
382         else
383                 printf_P(PSTR("             | "));
384
385         if (IR_DISP_SENSOR())
386                 printf_P(PSTR("IR disp | "));
387         else
388                 printf_P(PSTR("        | "));
389
390         printf_P(PSTR("\r\n"));
391 #endif
392 }
393
394 static void print_pid(void)
395 {
396         printf_P(PSTR("P=% .8ld I=% .8ld D=% .8ld out=% .8ld | "
397                       "P=% .8ld I=% .8ld D=% .8ld out=% .8ld\r\n"),
398                  pid_get_value_in(&mainboard.distance.pid) * pid_get_gain_P(&mainboard.distance.pid),
399                  pid_get_value_I(&mainboard.distance.pid) * pid_get_gain_I(&mainboard.distance.pid),
400                  pid_get_value_D(&mainboard.distance.pid) * pid_get_gain_D(&mainboard.distance.pid),
401                  pid_get_value_out(&mainboard.distance.pid),
402                  pid_get_value_in(&mainboard.angle.pid) * pid_get_gain_P(&mainboard.angle.pid),
403                  pid_get_value_I(&mainboard.angle.pid) * pid_get_gain_I(&mainboard.angle.pid),
404                  pid_get_value_D(&mainboard.angle.pid) * pid_get_gain_D(&mainboard.angle.pid),
405                  pid_get_value_out(&mainboard.angle.pid));
406 }
407
408 #define PRINT_POS       (1<<0)
409 #define PRINT_PID       (1<<1)
410 #define PRINT_CS        (1<<2)
411 #define PRINT_SENSORS   (1<<3)
412 #define PRINT_TIME      (1<<4)
413 #define PRINT_BLOCKING  (1<<5)
414
415 static void cmd_interact_parsed(void * parsed_result, void * data)
416 {
417         int c;
418         int8_t cmd;
419         uint8_t print = 0;
420         struct vt100 vt100;
421
422         vt100_init(&vt100);
423
424         printf_P(PSTR("Display debugs:\r\n"
425                       "  1:pos\r\n"
426                       "  2:pid\r\n"
427                       "  3:cs\r\n"
428                       "  4:sensors\r\n"
429                       "  5:time\r\n"
430                       /* "  6:blocking\r\n" */
431                       "Commands:\r\n"
432                       "  arrows:move\r\n"
433                       "  space:stop\r\n"
434                       "  q:quit\r\n"));
435
436         /* stop motors */
437         mainboard.flags &= (~DO_CS);
438         pwm_set_and_save(LEFT_PWM, 0);
439         pwm_set_and_save(RIGHT_PWM, 0);
440
441         while(1) {
442                 if (print & PRINT_POS) {
443                         print_pos();
444                 }
445
446                 if (print & PRINT_PID) {
447                         print_pid();
448                 }
449
450                 if (print & PRINT_CS) {
451                         print_cs();
452                 }
453
454                 if (print & PRINT_SENSORS) {
455                         print_sensors();
456                 }
457
458                 if (print & PRINT_TIME) {
459                         print_time();
460                 }
461 /*              if (print & PRINT_BLOCKING) { */
462 /*                      printf_P(PSTR("%s %s blocking=%d\r\n"),  */
463 /*                               mainboard.blocking ? "BLOCK1":"      ", */
464 /*                               rs_is_blocked(&mainboard.rs) ? "BLOCK2":"      ", */
465 /*                               rs_get_blocking(&mainboard.rs)); */
466 /*              } */
467
468                 c = cmdline_getchar();
469                 if (c == -1) {
470                         wait_ms(10);
471                         continue;
472                 }
473                 cmd = vt100_parser(&vt100, c);
474                 if (cmd == -2) {
475                         wait_ms(10);
476                         continue;
477                 }
478                 
479                 if (cmd == -1) {
480                         switch(c) {
481                         case '1': print ^= PRINT_POS; break;
482                         case '2': print ^= PRINT_PID; break;
483                         case '3': print ^= PRINT_CS; break;
484                         case '4': print ^= PRINT_SENSORS; break;
485                         case '5': print ^= PRINT_TIME; break;
486                         case '6': print ^= PRINT_BLOCKING; break;
487
488                         case 'q': 
489                                 if (mainboard.flags & DO_CS)
490                                         strat_hardstop();
491                                 pwm_set_and_save(LEFT_PWM, 0);
492                                 pwm_set_and_save(RIGHT_PWM, 0);
493                                 return;
494                         case ' ':
495                                 pwm_set_and_save(LEFT_PWM, 0);
496                                 pwm_set_and_save(RIGHT_PWM, 0);
497                                 break;
498                         default: 
499                                 break;
500                         }
501                 }
502                 else {
503                         switch(cmd) {
504                         case KEY_UP_ARR: 
505                                 pwm_set_and_save(LEFT_PWM, 1200);
506                                 pwm_set_and_save(RIGHT_PWM, 1200);
507                                 break;
508                         case KEY_LEFT_ARR: 
509                                 pwm_set_and_save(LEFT_PWM, -1200);
510                                 pwm_set_and_save(RIGHT_PWM, 1200);
511                                 break;
512                         case KEY_DOWN_ARR: 
513                                 pwm_set_and_save(LEFT_PWM, -1200);
514                                 pwm_set_and_save(RIGHT_PWM, -1200);
515                                 break;
516                         case KEY_RIGHT_ARR:
517                                 pwm_set_and_save(LEFT_PWM, 1200);
518                                 pwm_set_and_save(RIGHT_PWM, -1200);
519                                 break;
520                         }
521                 }
522                 wait_ms(10);
523         }
524 }
525
526 prog_char str_interact_arg0[] = "interact";
527 parse_pgm_token_string_t cmd_interact_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_interact_result, arg0, str_interact_arg0);
528
529 prog_char help_interact[] = "Interactive mode";
530 parse_pgm_inst_t cmd_interact = {
531         .f = cmd_interact_parsed,  /* function to call */
532         .data = NULL,      /* 2nd arg of func */
533         .help_str = help_interact,
534         .tokens = {        /* token list, NULL terminated */
535                 (prog_void *)&cmd_interact_arg0, 
536                 NULL,
537         },
538 };
539
540
541 /**********************************************************/
542 /* Color */
543
544 /* this structure is filled when cmd_color is parsed successfully */
545 struct cmd_color_result {
546         fixed_string_t arg0;
547         fixed_string_t color;
548 };
549
550 /* function called when cmd_color is parsed successfully */
551 static void cmd_color_parsed(void *parsed_result, void *data)
552 {
553         struct cmd_color_result *res = (struct cmd_color_result *) parsed_result;
554         if (!strcmp_P(res->color, PSTR("red"))) {
555                 mainboard.our_color = I2C_COLOR_RED;
556                 i2c_set_color(I2C_MECHBOARD_ADDR, I2C_COLOR_RED);
557                 i2c_set_color(I2C_SENSORBOARD_ADDR, I2C_COLOR_RED);
558         }
559         else if (!strcmp_P(res->color, PSTR("green"))) {
560                 mainboard.our_color = I2C_COLOR_GREEN;
561                 i2c_set_color(I2C_MECHBOARD_ADDR, I2C_COLOR_GREEN);
562                 i2c_set_color(I2C_SENSORBOARD_ADDR, I2C_COLOR_GREEN);
563         }
564         printf_P(PSTR("Done\r\n"));
565 }
566
567 prog_char str_color_arg0[] = "color";
568 parse_pgm_token_string_t cmd_color_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_color_result, arg0, str_color_arg0);
569 prog_char str_color_color[] = "green#red";
570 parse_pgm_token_string_t cmd_color_color = TOKEN_STRING_INITIALIZER(struct cmd_color_result, color, str_color_color);
571
572 prog_char help_color[] = "Set our color";
573 parse_pgm_inst_t cmd_color = {
574         .f = cmd_color_parsed,  /* function to call */
575         .data = NULL,      /* 2nd arg of func */
576         .help_str = help_color,
577         .tokens = {        /* token list, NULL terminated */
578                 (prog_void *)&cmd_color_arg0, 
579                 (prog_void *)&cmd_color_color, 
580                 NULL,
581         },
582 };
583
584
585 /**********************************************************/
586 /* Rs tests */
587
588 /* this structure is filled when cmd_rs is parsed successfully */
589 struct cmd_rs_result {
590         fixed_string_t arg0;
591         fixed_string_t arg1;
592 };
593
594 /* function called when cmd_rs is parsed successfully */
595 static void cmd_rs_parsed(void *parsed_result, void *data)
596 {
597         //      struct cmd_rs_result *res = parsed_result;
598         do {
599                 printf_P(PSTR("angle cons=% .6ld in=% .6ld out=% .6ld / "), 
600                          cs_get_consign(&mainboard.angle.cs),
601                          cs_get_filtered_feedback(&mainboard.angle.cs),
602                          cs_get_out(&mainboard.angle.cs));
603                 printf_P(PSTR("distance cons=% .6ld in=% .6ld out=% .6ld / "), 
604                          cs_get_consign(&mainboard.distance.cs),
605                          cs_get_filtered_feedback(&mainboard.distance.cs),
606                          cs_get_out(&mainboard.distance.cs));
607                 printf_P(PSTR("l=% .4ld r=% .4ld\r\n"), mainboard.pwm_l,
608                          mainboard.pwm_r);
609                 wait_ms(100);
610         } while(!cmdline_keypressed());
611 }
612
613 prog_char str_rs_arg0[] = "rs";
614 parse_pgm_token_string_t cmd_rs_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_rs_result, arg0, str_rs_arg0);
615 prog_char str_rs_arg1[] = "show";
616 parse_pgm_token_string_t cmd_rs_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_rs_result, arg1, str_rs_arg1);
617
618 prog_char help_rs[] = "Show rs (robot system) values";
619 parse_pgm_inst_t cmd_rs = {
620         .f = cmd_rs_parsed,  /* function to call */
621         .data = NULL,      /* 2nd arg of func */
622         .help_str = help_rs,
623         .tokens = {        /* token list, NULL terminated */
624                 (prog_void *)&cmd_rs_arg0, 
625                 (prog_void *)&cmd_rs_arg1, 
626                 NULL,
627         },
628 };
629
630 /**********************************************************/
631 /* I2cdebug */
632
633 /* this structure is filled when cmd_i2cdebug is parsed successfully */
634 struct cmd_i2cdebug_result {
635         fixed_string_t arg0;
636 };
637
638 /* function called when cmd_i2cdebug is parsed successfully */
639 static void cmd_i2cdebug_parsed(void * parsed_result, void * data)
640 {
641         i2c_debug();
642         i2c_protocol_debug();
643 }
644
645 prog_char str_i2cdebug_arg0[] = "i2cdebug";
646 parse_pgm_token_string_t cmd_i2cdebug_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_i2cdebug_result, arg0, str_i2cdebug_arg0);
647
648 prog_char help_i2cdebug[] = "I2c debug infos";
649 parse_pgm_inst_t cmd_i2cdebug = {
650         .f = cmd_i2cdebug_parsed,  /* function to call */
651         .data = NULL,      /* 2nd arg of func */
652         .help_str = help_i2cdebug,
653         .tokens = {        /* token list, NULL terminated */
654                 (prog_void *)&cmd_i2cdebug_arg0, 
655                 NULL,
656         },
657 };
658
659 /**********************************************************/
660 /* Mechboard_Show */
661
662 /* this structure is filled when cmd_mechboard_show is parsed successfully */
663 struct cmd_mechboard_show_result {
664         fixed_string_t arg0;
665         fixed_string_t arg1;
666 };
667
668 /* function called when cmd_mechboard_show is parsed successfully */
669 static void cmd_mechboard_show_parsed(void * parsed_result, void * data)
670 {
671         printf_P(PSTR("mode = %x\r\n"), mechboard.mode);
672         printf_P(PSTR("status = %x\r\n"), mechboard.status);
673         printf_P(PSTR("lintel_count = %d\r\n"), mechboard.lintel_count);
674
675         printf_P(PSTR("column_count = %d\r\n"), get_column_count());
676         printf_P(PSTR("left1=%d left2=%d right1=%d right2=%d\r\n"),
677                  pump_left1_is_full(), pump_left2_is_full(),
678                  pump_right1_is_full(), pump_right2_is_full());
679         
680         printf_P(PSTR("pump_left1 = %d\r\n"), mechboard.pump_left1);
681         printf_P(PSTR("pump_left2 = %d\r\n"), mechboard.pump_left2);
682         printf_P(PSTR("pump_right1 = %d\r\n"), mechboard.pump_right1);
683         printf_P(PSTR("pump_right2 = %d\r\n"), mechboard.pump_right2);
684
685         printf_P(PSTR("servo_lintel_left = %d\r\n"), mechboard.servo_lintel_left);
686         printf_P(PSTR("servo_lintel_right = %d\r\n"), mechboard.servo_lintel_right);
687
688 }
689
690 prog_char str_mechboard_show_arg0[] = "mechboard";
691 parse_pgm_token_string_t cmd_mechboard_show_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_show_result, arg0, str_mechboard_show_arg0);
692 prog_char str_mechboard_show_arg1[] = "show";
693 parse_pgm_token_string_t cmd_mechboard_show_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_show_result, arg1, str_mechboard_show_arg1);
694
695 prog_char help_mechboard_show[] = "show mechboard status";
696 parse_pgm_inst_t cmd_mechboard_show = {
697         .f = cmd_mechboard_show_parsed,  /* function to call */
698         .data = NULL,      /* 2nd arg of func */
699         .help_str = help_mechboard_show,
700         .tokens = {        /* token list, NULL terminated */
701                 (prog_void *)&cmd_mechboard_show_arg0, 
702                 (prog_void *)&cmd_mechboard_show_arg1, 
703                 NULL,
704         },
705 };
706
707 /**********************************************************/
708 /* Mechboard_Setmode1 */
709
710 /* this structure is filled when cmd_mechboard_setmode1 is parsed successfully */
711 struct cmd_mechboard_setmode1_result {
712         fixed_string_t arg0;
713         fixed_string_t arg1;
714 };
715
716 /* function called when cmd_mechboard_setmode1 is parsed successfully */
717 static void cmd_mechboard_setmode1_parsed(void *parsed_result, void *data)
718 {
719         struct cmd_mechboard_setmode1_result *res = parsed_result;
720
721         if (!strcmp_P(res->arg1, PSTR("init")))
722                 i2c_mechboard_mode_init();
723         else if (!strcmp_P(res->arg1, PSTR("manual")))
724                 i2c_mechboard_mode_manual();
725         else if (!strcmp_P(res->arg1, PSTR("pickup")))
726                 i2c_mechboard_mode_pickup();
727         else if (!strcmp_P(res->arg1, PSTR("lazy_harvest")))
728                 i2c_mechboard_mode_lazy_harvest();
729         else if (!strcmp_P(res->arg1, PSTR("harvest")))
730                 i2c_mechboard_mode_harvest();
731         else if (!strcmp_P(res->arg1, PSTR("prepare_get_lintel")))
732                 i2c_mechboard_mode_prepare_get_lintel();
733         else if (!strcmp_P(res->arg1, PSTR("get_lintel")))
734                 i2c_mechboard_mode_get_lintel();
735         else if (!strcmp_P(res->arg1, PSTR("put_lintel")))
736                 i2c_mechboard_mode_put_lintel();
737         else if (!strcmp_P(res->arg1, PSTR("init")))
738                 i2c_mechboard_mode_init();
739         else if (!strcmp_P(res->arg1, PSTR("eject")))
740                 i2c_mechboard_mode_init();
741         else if (!strcmp_P(res->arg1, PSTR("clear")))
742                 i2c_mechboard_mode_clear();
743         else if (!strcmp_P(res->arg1, PSTR("loaded")))
744                 i2c_mechboard_mode_loaded();
745         else if (!strcmp_P(res->arg1, PSTR("store")))
746                 i2c_mechboard_mode_store();
747         else if (!strcmp_P(res->arg1, PSTR("manivelle")))
748                 i2c_mechboard_mode_manivelle();
749         else if (!strcmp_P(res->arg1, PSTR("lazy_pickup")))
750                 i2c_mechboard_mode_lazy_pickup();
751 }
752
753 prog_char str_mechboard_setmode1_arg0[] = "mechboard";
754 parse_pgm_token_string_t cmd_mechboard_setmode1_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode1_result, arg0, str_mechboard_setmode1_arg0);
755 prog_char str_mechboard_setmode1_arg1[] = "manivelle#init#manual#pickup#prepare_get_lintel#get_lintel#put_lintel1#eject#clear#harvest#lazy_harvest#store#lazy_pickup";
756 parse_pgm_token_string_t cmd_mechboard_setmode1_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode1_result, arg1, str_mechboard_setmode1_arg1);
757
758 prog_char help_mechboard_setmode1[] = "set mechboard mode (mode)";
759 parse_pgm_inst_t cmd_mechboard_setmode1 = {
760         .f = cmd_mechboard_setmode1_parsed,  /* function to call */
761         .data = NULL,      /* 2nd arg of func */
762         .help_str = help_mechboard_setmode1,
763         .tokens = {        /* token list, NULL terminated */
764                 (prog_void *)&cmd_mechboard_setmode1_arg0, 
765                 (prog_void *)&cmd_mechboard_setmode1_arg1, 
766                 NULL,
767         },
768 };
769
770 /**********************************************************/
771 /* Mechboard_Setmode2 */
772
773 /* this structure is filled when cmd_mechboard_setmode2 is parsed successfully */
774 struct cmd_mechboard_setmode2_result {
775         fixed_string_t arg0;
776         fixed_string_t arg1;
777         fixed_string_t arg2;
778 };
779
780 /* function called when cmd_mechboard_setmode2 is parsed successfully */
781 static void cmd_mechboard_setmode2_parsed(void * parsed_result, void * data)
782 {
783         struct cmd_mechboard_setmode2_result *res = parsed_result;
784         uint8_t side = I2C_LEFT_SIDE;
785
786         if (!strcmp_P(res->arg2, PSTR("left")))
787                 side = I2C_LEFT_SIDE;
788         else if (!strcmp_P(res->arg2, PSTR("right")))
789                 side = I2C_RIGHT_SIDE;
790         else if (!strcmp_P(res->arg2, PSTR("center")))
791                 side = I2C_CENTER_SIDE;
792         else if (!strcmp_P(res->arg2, PSTR("auto")))
793                 side = I2C_AUTO_SIDE;
794
795         if (!strcmp_P(res->arg1, PSTR("prepare_pickup")))
796                 i2c_mechboard_mode_prepare_pickup(side);
797         else if (!strcmp_P(res->arg1, PSTR("push_temple_disc")))
798                 i2c_mechboard_mode_push_temple_disc(side);
799 }
800
801 prog_char str_mechboard_setmode2_arg0[] = "mechboard";
802 parse_pgm_token_string_t cmd_mechboard_setmode2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode2_result, arg0, str_mechboard_setmode2_arg0);
803 prog_char str_mechboard_setmode2_arg1[] = "prepare_pickup#push_temple_disc";
804 parse_pgm_token_string_t cmd_mechboard_setmode2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode2_result, arg1, str_mechboard_setmode2_arg1);
805 prog_char str_mechboard_setmode2_arg2[] = "left#right#auto#center";
806 parse_pgm_token_string_t cmd_mechboard_setmode2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode2_result, arg2, str_mechboard_setmode2_arg2);
807
808 prog_char help_mechboard_setmode2[] = "set mechboard mode (more, side)";
809 parse_pgm_inst_t cmd_mechboard_setmode2 = {
810         .f = cmd_mechboard_setmode2_parsed,  /* function to call */
811         .data = NULL,      /* 2nd arg of func */
812         .help_str = help_mechboard_setmode2,
813         .tokens = {        /* token list, NULL terminated */
814                 (prog_void *)&cmd_mechboard_setmode2_arg0, 
815                 (prog_void *)&cmd_mechboard_setmode2_arg1, 
816                 (prog_void *)&cmd_mechboard_setmode2_arg2, 
817                 NULL,
818         },
819 };
820
821 /**********************************************************/
822 /* Mechboard_Setmode3 */
823
824 /* this structure is filled when cmd_mechboard_setmode3 is parsed successfully */
825 struct cmd_mechboard_setmode3_result {
826         fixed_string_t arg0;
827         fixed_string_t arg1;
828         uint8_t level;
829 };
830
831 /* function called when cmd_mechboard_setmode3 is parsed successfully */
832 static void cmd_mechboard_setmode3_parsed(void *parsed_result, void *data)
833 {
834         struct cmd_mechboard_setmode3_result *res = parsed_result;
835         if (!strcmp_P(res->arg1, PSTR("autobuild")))
836                 i2c_mechboard_mode_simple_autobuild(res->level);
837         else if (!strcmp_P(res->arg1, PSTR("prepare_build")))
838                 i2c_mechboard_mode_prepare_build_both(res->level);
839         else if (!strcmp_P(res->arg1, PSTR("prepare_inside")))
840                 i2c_mechboard_mode_prepare_inside_both(res->level);
841         else if (!strcmp_P(res->arg1, PSTR("push_temple")))
842                 i2c_mechboard_mode_push_temple(res->level);
843 }
844
845 prog_char str_mechboard_setmode3_arg0[] = "mechboard";
846 parse_pgm_token_string_t cmd_mechboard_setmode3_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode3_result, arg0, str_mechboard_setmode3_arg0);
847 prog_char str_mechboard_setmode3_arg1[] = "autobuild#prepare_build#prepare_inside#push_temple";
848 parse_pgm_token_string_t cmd_mechboard_setmode3_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode3_result, arg1, str_mechboard_setmode3_arg1);
849 parse_pgm_token_num_t cmd_mechboard_setmode3_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_mechboard_setmode3_result, level, UINT8);
850
851 prog_char help_mechboard_setmode3[] = "set mechboard mode (mode, level)";
852 parse_pgm_inst_t cmd_mechboard_setmode3 = {
853         .f = cmd_mechboard_setmode3_parsed,  /* function to call */
854         .data = NULL,      /* 2nd arg of func */
855         .help_str = help_mechboard_setmode3,
856         .tokens = {        /* token list, NULL terminated */
857                 (prog_void *)&cmd_mechboard_setmode3_arg0, 
858                 (prog_void *)&cmd_mechboard_setmode3_arg1, 
859                 (prog_void *)&cmd_mechboard_setmode3_arg2, 
860                 NULL,
861         },
862 };
863
864 /**********************************************************/
865 /* Mechboard_Setmode4 */
866
867 /* this structure is filled when cmd_mechboard_setmode4 is parsed successfully */
868 struct cmd_mechboard_setmode4_result {
869         fixed_string_t arg0;
870         fixed_string_t arg1;
871         uint8_t level_l;
872         uint8_t count_l;
873         uint8_t dist_l;
874         uint8_t level_r;
875         uint8_t count_r;
876         uint8_t dist_r;
877         uint8_t do_lintel;
878 };
879
880 /* function called when cmd_mechboard_setmode4 is parsed successfully */
881 static void cmd_mechboard_setmode4_parsed(void *parsed_result, void *data)
882 {
883         struct cmd_mechboard_setmode4_result *res = parsed_result;
884         i2c_mechboard_mode_autobuild(res->level_l, res->count_l, res->dist_l,
885                                      res->level_r, res->count_r, res->dist_r,
886                                      res->do_lintel);
887 }
888
889 prog_char str_mechboard_setmode4_arg0[] = "mechboard";
890 parse_pgm_token_string_t cmd_mechboard_setmode4_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode4_result, arg0, str_mechboard_setmode4_arg0);
891 prog_char str_mechboard_setmode4_arg1[] = "autobuild";
892 parse_pgm_token_string_t cmd_mechboard_setmode4_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode4_result, arg1, str_mechboard_setmode4_arg1);
893 parse_pgm_token_num_t cmd_mechboard_setmode4_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_mechboard_setmode4_result, level_l, UINT8);
894 parse_pgm_token_num_t cmd_mechboard_setmode4_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_mechboard_setmode4_result, count_l, UINT8);
895 parse_pgm_token_num_t cmd_mechboard_setmode4_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_mechboard_setmode4_result, dist_l, UINT8);
896 parse_pgm_token_num_t cmd_mechboard_setmode4_arg5 = TOKEN_NUM_INITIALIZER(struct cmd_mechboard_setmode4_result, level_r, UINT8);
897 parse_pgm_token_num_t cmd_mechboard_setmode4_arg6 = TOKEN_NUM_INITIALIZER(struct cmd_mechboard_setmode4_result, count_r, UINT8);
898 parse_pgm_token_num_t cmd_mechboard_setmode4_arg7 = TOKEN_NUM_INITIALIZER(struct cmd_mechboard_setmode4_result, dist_r, UINT8);
899 parse_pgm_token_num_t cmd_mechboard_setmode4_arg8 = TOKEN_NUM_INITIALIZER(struct cmd_mechboard_setmode4_result, do_lintel, UINT8);
900
901 prog_char help_mechboard_setmode4[] = "set mechboard mode (autobuild level_l count_l dist_l level_r count_r dist_r lintel)";
902 parse_pgm_inst_t cmd_mechboard_setmode4 = {
903         .f = cmd_mechboard_setmode4_parsed,  /* function to call */
904         .data = NULL,      /* 2nd arg of func */
905         .help_str = help_mechboard_setmode4,
906         .tokens = {        /* token list, NULL terminated */
907                 (prog_void *)&cmd_mechboard_setmode4_arg0, 
908                 (prog_void *)&cmd_mechboard_setmode4_arg1, 
909                 (prog_void *)&cmd_mechboard_setmode4_arg2, 
910                 (prog_void *)&cmd_mechboard_setmode4_arg3, 
911                 (prog_void *)&cmd_mechboard_setmode4_arg4, 
912                 (prog_void *)&cmd_mechboard_setmode4_arg5, 
913                 (prog_void *)&cmd_mechboard_setmode4_arg6, 
914                 (prog_void *)&cmd_mechboard_setmode4_arg7, 
915                 (prog_void *)&cmd_mechboard_setmode4_arg8, 
916                 NULL,
917         },
918 };
919
920 /**********************************************************/
921 /* Mechboard_Setmode5 */
922
923 /* this structure is filled when cmd_mechboard_setmode5 is parsed successfully */
924 struct cmd_mechboard_setmode5_result {
925         fixed_string_t arg0;
926         fixed_string_t arg1;
927         fixed_string_t arg2;
928         fixed_string_t arg3;
929 };
930
931 /* function called when cmd_mechboard_setmode5 is parsed successfully */
932 static void cmd_mechboard_setmode5_parsed(void *parsed_result, void * data)
933 {
934         struct cmd_mechboard_setmode5_result *res = parsed_result;
935         uint8_t side = I2C_LEFT_SIDE, next_mode = I2C_MECHBOARD_MODE_PREPARE_PICKUP;
936
937         if (!strcmp_P(res->arg2, PSTR("left")))
938                 side = I2C_LEFT_SIDE;
939         else if (!strcmp_P(res->arg2, PSTR("right")))
940                 side = I2C_RIGHT_SIDE;
941         else if (!strcmp_P(res->arg2, PSTR("center")))
942                 side = I2C_CENTER_SIDE;
943         else if (!strcmp_P(res->arg2, PSTR("auto")))
944                 side = I2C_AUTO_SIDE;
945
946         if (!strcmp_P(res->arg3, PSTR("harvest")))
947                 next_mode = I2C_MECHBOARD_MODE_HARVEST;
948         else if (!strcmp_P(res->arg3, PSTR("lazy_harvest")))
949                 next_mode = I2C_MECHBOARD_MODE_LAZY_HARVEST;
950         else if (!strcmp_P(res->arg3, PSTR("pickup")))
951                 next_mode = I2C_MECHBOARD_MODE_PICKUP;
952         else if (!strcmp_P(res->arg3, PSTR("clear")))
953                 next_mode = I2C_MECHBOARD_MODE_CLEAR;
954         else if (!strcmp_P(res->arg3, PSTR("store")))
955                 next_mode = I2C_MECHBOARD_MODE_STORE;
956         else if (!strcmp_P(res->arg3, PSTR("lazy_pickup")))
957                 next_mode = I2C_MECHBOARD_MODE_LAZY_PICKUP;
958
959         if (!strcmp_P(res->arg1, PSTR("prepare_pickup")))
960                 i2c_mechboard_mode_prepare_pickup_next(side, next_mode);
961 }
962
963 prog_char str_mechboard_setmode5_arg0[] = "mechboard";
964 parse_pgm_token_string_t cmd_mechboard_setmode5_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode5_result, arg0, str_mechboard_setmode5_arg0);
965 prog_char str_mechboard_setmode5_arg1[] = "prepare_pickup";
966 parse_pgm_token_string_t cmd_mechboard_setmode5_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode5_result, arg1, str_mechboard_setmode5_arg1);
967 prog_char str_mechboard_setmode5_arg2[] = "left#right#auto#center";
968 parse_pgm_token_string_t cmd_mechboard_setmode5_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode5_result, arg2, str_mechboard_setmode5_arg2);
969 prog_char str_mechboard_setmode5_arg3[] = "harvest#pickup#store#lazy_harvest#lazy_pickup#clear";
970 parse_pgm_token_string_t cmd_mechboard_setmode5_arg3 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode5_result, arg3, str_mechboard_setmode5_arg3);
971
972 prog_char help_mechboard_setmode5[] = "set mechboard mode (more, side)";
973 parse_pgm_inst_t cmd_mechboard_setmode5 = {
974         .f = cmd_mechboard_setmode5_parsed,  /* function to call */
975         .data = NULL,      /* 2nd arg of func */
976         .help_str = help_mechboard_setmode5,
977         .tokens = {        /* token list, NULL terminated */
978                 (prog_void *)&cmd_mechboard_setmode5_arg0, 
979                 (prog_void *)&cmd_mechboard_setmode5_arg1, 
980                 (prog_void *)&cmd_mechboard_setmode5_arg2, 
981                 (prog_void *)&cmd_mechboard_setmode5_arg3, 
982                 NULL,
983         },
984 };
985
986 /**********************************************************/
987 /* pickup wheels */
988
989 /* this structure is filled when cmd_pickup_wheels is parsed successfully */
990 struct cmd_pickup_wheels_result {
991         fixed_string_t arg0;
992         fixed_string_t arg1;
993 };
994
995 /* function called when cmd_pickup_wheels is parsed successfully */
996 static void cmd_pickup_wheels_parsed(void *parsed_result, void *data)
997 {
998         struct cmd_pickup_wheels_result *res = parsed_result;
999         
1000         if (!strcmp_P(res->arg1, PSTR("on")))
1001                 pickup_wheels_on();     
1002         else
1003                 pickup_wheels_off();
1004 }
1005
1006 prog_char str_pickup_wheels_arg0[] = "pickup_wheels";
1007 parse_pgm_token_string_t cmd_pickup_wheels_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_pickup_wheels_result, arg0, str_pickup_wheels_arg0);
1008 prog_char str_pickup_wheels_arg1[] = "on#off";
1009 parse_pgm_token_string_t cmd_pickup_wheels_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_pickup_wheels_result, arg1, str_pickup_wheels_arg1);
1010
1011 prog_char help_pickup_wheels[] = "Enable/disable pickup wheels";
1012 parse_pgm_inst_t cmd_pickup_wheels = {
1013         .f = cmd_pickup_wheels_parsed,  /* function to call */
1014         .data = NULL,      /* 2nd arg of func */
1015         .help_str = help_pickup_wheels,
1016         .tokens = {        /* token list, NULL terminated */
1017                 (prog_void *)&cmd_pickup_wheels_arg0,
1018                 (prog_void *)&cmd_pickup_wheels_arg1,
1019                 NULL,
1020         },
1021 };
1022
1023 /**********************************************************/
1024 /* Beacon_Start */
1025
1026 /* this structure is filled when cmd_beacon_start is parsed successfully */
1027 struct cmd_beacon_start_result {
1028         fixed_string_t arg0;
1029         fixed_string_t arg1;
1030 };
1031
1032 /* function called when cmd_beacon_start is parsed successfully */
1033 static void cmd_beacon_start_parsed(void *parsed_result, void *data)
1034 {
1035         struct cmd_beacon_start_result *res = parsed_result;
1036
1037         if (!strcmp_P(res->arg1, PSTR("start")))
1038                 i2c_sensorboard_set_beacon(1);
1039         else
1040                 i2c_sensorboard_set_beacon(0);
1041 }
1042
1043 prog_char str_beacon_start_arg0[] = "beacon";
1044 parse_pgm_token_string_t cmd_beacon_start_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_beacon_start_result, arg0, str_beacon_start_arg0);
1045 prog_char str_beacon_start_arg1[] = "start#stop";
1046 parse_pgm_token_string_t cmd_beacon_start_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_beacon_start_result, arg1, str_beacon_start_arg1);
1047
1048 prog_char help_beacon_start[] = "Beacon enabled/disable";
1049 parse_pgm_inst_t cmd_beacon_start = {
1050         .f = cmd_beacon_start_parsed,  /* function to call */
1051         .data = NULL,      /* 2nd arg of func */
1052         .help_str = help_beacon_start,
1053         .tokens = {        /* token list, NULL terminated */
1054                 (prog_void *)&cmd_beacon_start_arg0, 
1055                 (prog_void *)&cmd_beacon_start_arg1, 
1056                 NULL,
1057         },
1058 };
1059
1060 /**********************************************************/
1061 /* Pump_Current */
1062
1063 /* this structure is filled when cmd_pump_current is parsed successfully */
1064 struct cmd_pump_current_result {
1065         fixed_string_t arg0;
1066         fixed_string_t arg1;
1067 };
1068
1069 /* function called when cmd_pump_current is parsed successfully */
1070 static void cmd_pump_current_parsed(__attribute__((unused)) void *parsed_result,
1071                                     __attribute__((unused)) void *data)
1072 {
1073         printf_P(PSTR("l1=%d l2=%d r1=%d r2=%d\r\n"),
1074                  sensor_get_adc(ADC_CSENSE3), sensor_get_adc(ADC_CSENSE4),
1075                  mechboard.pump_right1_current, mechboard.pump_right2_current);
1076 }
1077
1078 prog_char str_pump_current_arg0[] = "pump_current";
1079 parse_pgm_token_string_t cmd_pump_current_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_pump_current_result, arg0, str_pump_current_arg0);
1080 prog_char str_pump_current_arg1[] = "show";
1081 parse_pgm_token_string_t cmd_pump_current_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_pump_current_result, arg1, str_pump_current_arg1);
1082
1083 prog_char help_pump_current[] = "dump pump current";
1084 parse_pgm_inst_t cmd_pump_current = {
1085         .f = cmd_pump_current_parsed,  /* function to call */
1086         .data = NULL,      /* 2nd arg of func */
1087         .help_str = help_pump_current,
1088         .tokens = {        /* token list, NULL terminated */
1089                 (prog_void *)&cmd_pump_current_arg0, 
1090                 (prog_void *)&cmd_pump_current_arg1, 
1091                 NULL,
1092         },
1093 };
1094
1095 /**********************************************************/
1096 /* Build_Test */
1097
1098 /* this structure is filled when cmd_build_test is parsed successfully */
1099 struct cmd_build_test_result {
1100         fixed_string_t arg0;
1101 };
1102
1103 /* function called when cmd_build_test is parsed successfully */
1104 static void cmd_build_test_parsed(void *parsed_result, void *data)
1105 {
1106         //struct cmd_build_test_result *res = parsed_result;
1107
1108         printf_P(PSTR("lintel must be there\r\n"));
1109         i2c_mechboard_mode_prepare_pickup_next(I2C_LEFT_SIDE,
1110                                                I2C_MECHBOARD_MODE_HARVEST);
1111         wait_ms(500);
1112
1113         printf_P(PSTR("Insert 4 colums\r\n"));
1114         while (get_column_count() != 4);
1115
1116         i2c_mechboard_mode_prepare_build_both(0);
1117         trajectory_d_rel(&mainboard.traj, 200);
1118         wait_traj_end(TRAJ_FLAGS_NO_NEAR);
1119         wait_ms(500);
1120
1121         i2c_mechboard_mode_simple_autobuild(0);
1122         wait_ms(100);
1123         while (get_mechboard_mode() == I2C_MECHBOARD_MODE_AUTOBUILD);
1124
1125         trajectory_d_rel(&mainboard.traj, -200);
1126         wait_traj_end(TRAJ_FLAGS_NO_NEAR);
1127         i2c_mechboard_mode_prepare_pickup_next(I2C_LEFT_SIDE,
1128                                                I2C_MECHBOARD_MODE_HARVEST);
1129
1130         while (get_column_count() != 3);
1131
1132         i2c_mechboard_mode_prepare_build_both(3);
1133         trajectory_d_rel(&mainboard.traj, 200);
1134         wait_traj_end(TRAJ_FLAGS_NO_NEAR);
1135         wait_ms(500);
1136
1137         i2c_mechboard_mode_autobuild(3, 1, I2C_AUTOBUILD_DEFAULT_DIST,
1138                                      3, 2,I2C_AUTOBUILD_DEFAULT_DIST, 0);
1139         i2cproto_wait_update();
1140         while (get_mechboard_mode() == I2C_MECHBOARD_MODE_AUTOBUILD);
1141
1142         trajectory_d_rel(&mainboard.traj, -200);
1143         wait_traj_end(TRAJ_FLAGS_NO_NEAR);
1144         i2c_mechboard_mode_prepare_pickup(I2C_RIGHT_SIDE);
1145         wait_ms(500);
1146
1147         i2c_mechboard_mode_harvest();
1148         while (get_column_count() != 3);
1149
1150         i2c_mechboard_mode_prepare_build_both(5);
1151         trajectory_d_rel(&mainboard.traj, 200);
1152         wait_traj_end(TRAJ_FLAGS_NO_NEAR);
1153         wait_ms(1000);
1154
1155         i2c_mechboard_mode_autobuild(4, 2, I2C_AUTOBUILD_DEFAULT_DIST,
1156                                      5, 1, I2C_AUTOBUILD_DEFAULT_DIST, 0);
1157         i2cproto_wait_update();
1158         while (get_mechboard_mode() == I2C_MECHBOARD_MODE_AUTOBUILD);
1159
1160         trajectory_d_rel(&mainboard.traj, -200);
1161 }
1162
1163 prog_char str_build_test_arg0[] = "build_test";
1164 parse_pgm_token_string_t cmd_build_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_build_test_result, arg0, str_build_test_arg0);
1165
1166 prog_char help_build_test[] = "Build_Test function";
1167 parse_pgm_inst_t cmd_build_test = {
1168         .f = cmd_build_test_parsed,  /* function to call */
1169         .data = NULL,      /* 2nd arg of func */
1170         .help_str = help_build_test,
1171         .tokens = {        /* token list, NULL terminated */
1172                 (prog_void *)&cmd_build_test_arg0, 
1173                 NULL,
1174         },
1175 };
1176
1177
1178 /**********************************************************/
1179 /* Column_Test */
1180
1181 /* this structure is filled when cmd_column_test is parsed successfully */
1182 struct cmd_column_test_result {
1183         fixed_string_t arg0;
1184         uint8_t level;
1185         int16_t dist;
1186         int8_t a1;
1187         int8_t a2;
1188         int8_t a3;
1189         int16_t arm_dist;
1190         int8_t nb_col;
1191 };
1192
1193 /* function called when cmd_column_test is parsed successfully */
1194 static void cmd_column_test_parsed(void *parsed_result, void *data)
1195 {
1196         struct cmd_column_test_result *res = parsed_result;
1197         uint8_t level = res->level, debug = 0;
1198         uint8_t c, push = 0;
1199
1200         /* default conf */
1201         if (data) {
1202                 res->dist = 70;
1203                 res->a1 = -20;
1204                 res->a2 =  40;
1205                 res->a3 = -20;
1206                 res->arm_dist = 220;
1207                 res->nb_col = 2;
1208         }
1209
1210         if (!strcmp_P(res->arg0, PSTR("column_test_debug")))
1211                 debug = 1;
1212         if (!strcmp_P(res->arg0, PSTR("column_test_push")))
1213                 push = 1;
1214
1215         strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_VERY_SLOW);
1216         
1217         /* Go to disc */
1218
1219         trajectory_d_rel(&mainboard.traj, 200);
1220         wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
1221         
1222         /* go back, insert colums */
1223
1224         trajectory_d_rel(&mainboard.traj, -200);
1225         wait_traj_end(TRAJ_FLAGS_NO_NEAR);
1226
1227         i2c_mechboard_mode_prepare_pickup_next(I2C_LEFT_SIDE,
1228                                                I2C_MECHBOARD_MODE_HARVEST);
1229         printf_P(PSTR("Insert 4 colums\r\n"));
1230         while (get_column_count() != 4);
1231
1232         /* build with left arm */
1233
1234         i2c_mechboard_mode_prepare_inside_both(level);
1235         trajectory_d_rel(&mainboard.traj, 200-(res->dist));
1236         wait_traj_end(TRAJ_FLAGS_NO_NEAR);
1237
1238         if (debug)
1239                 c = cmdline_getchar_wait();
1240
1241         trajectory_a_rel(&mainboard.traj, res->a1);
1242         wait_traj_end(TRAJ_FLAGS_NO_NEAR);
1243
1244         if (debug)
1245                 c = cmdline_getchar_wait();
1246
1247         i2c_mechboard_mode_prepare_build_select(level, -1);
1248         time_wait_ms(200);
1249         if (debug)
1250                 c = cmdline_getchar_wait();
1251         i2c_mechboard_mode_autobuild(level, res->nb_col, res->arm_dist,
1252                                      0, 0, res->arm_dist, 0);
1253         while (get_mechboard_mode() != I2C_MECHBOARD_MODE_AUTOBUILD);
1254         while (get_mechboard_mode() == I2C_MECHBOARD_MODE_AUTOBUILD);
1255
1256         if (debug)
1257                 c = cmdline_getchar_wait();
1258         i2c_mechboard_mode_prepare_inside_select(level+res->nb_col, -1);
1259
1260         if (debug)
1261                 c = cmdline_getchar_wait();
1262         /* build with right arm */
1263
1264         trajectory_a_rel(&mainboard.traj, res->a2);
1265         wait_traj_end(TRAJ_FLAGS_NO_NEAR);
1266
1267         if (debug)
1268                 c = cmdline_getchar_wait();
1269         /* only ok for nb_col == 2 */
1270         if ((level + res->nb_col) >= 7)
1271                 i2c_mechboard_mode_prepare_build_select(-1, level + res->nb_col + 1);
1272         else
1273                 i2c_mechboard_mode_prepare_build_select(-1, level + res->nb_col);
1274         time_wait_ms(200);
1275         if (debug)
1276                 c = cmdline_getchar_wait();
1277         i2c_mechboard_mode_autobuild(0, 0, res->arm_dist,
1278                                      level + res->nb_col, res->nb_col,
1279                                      res->arm_dist, 0);
1280         while (get_mechboard_mode() != I2C_MECHBOARD_MODE_AUTOBUILD);
1281         while (get_mechboard_mode() == I2C_MECHBOARD_MODE_AUTOBUILD);
1282
1283                 
1284         if (push) {
1285                 strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST);
1286                 trajectory_d_rel(&mainboard.traj, -100);
1287                 wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
1288                 i2c_mechboard_mode_push_temple_disc(I2C_RIGHT_SIDE);
1289                 time_wait_ms(500);
1290                 trajectory_d_rel(&mainboard.traj, 100);
1291                 wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
1292         }
1293         else if (level == 1 || level == 0) {
1294                 trajectory_d_rel(&mainboard.traj, -100);
1295                 wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
1296                 i2c_mechboard_mode_push_temple(level);
1297                 time_wait_ms(400);
1298                 strat_set_speed(200, SPEED_ANGLE_SLOW);
1299                 trajectory_d_rel(&mainboard.traj, 120);
1300                 wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
1301         }
1302
1303         strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
1304
1305         if (debug)
1306                 c = cmdline_getchar_wait();
1307         i2c_mechboard_mode_prepare_inside_select(-1, level+res->nb_col*2);
1308
1309         if (debug)
1310                 c = cmdline_getchar_wait();
1311
1312         trajectory_a_rel(&mainboard.traj, res->a3);
1313         wait_traj_end(TRAJ_FLAGS_NO_NEAR);
1314
1315         if (debug)
1316                 c = cmdline_getchar_wait();
1317         /* go back, insert colums */
1318
1319         trajectory_d_rel(&mainboard.traj, -100);
1320
1321         return;
1322 }
1323
1324 prog_char str_column_test_arg0[] = "column_test#column_test_debug#column_test_push";
1325 parse_pgm_token_string_t cmd_column_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_column_test_result, arg0, str_column_test_arg0);
1326 parse_pgm_token_num_t cmd_column_test_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_column_test_result, level, UINT8);
1327
1328 prog_char help_column_test[] = "Column_Test function (level)";
1329 parse_pgm_inst_t cmd_column_test = {
1330         .f = cmd_column_test_parsed,  /* function to call */
1331         .data = (void *)1,      /* 2nd arg of func */
1332         .help_str = help_column_test,
1333         .tokens = {        /* token list, NULL terminated */
1334                 (prog_void *)&cmd_column_test_arg0, 
1335                 (prog_void *)&cmd_column_test_arg1, 
1336                 NULL,
1337         },
1338 };
1339
1340 parse_pgm_token_num_t cmd_column_test_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_column_test_result, dist, INT16);
1341 parse_pgm_token_num_t cmd_column_test_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_column_test_result, a1, INT8);
1342 parse_pgm_token_num_t cmd_column_test_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_column_test_result, a2, INT8);
1343 parse_pgm_token_num_t cmd_column_test_arg5 = TOKEN_NUM_INITIALIZER(struct cmd_column_test_result, a3, INT8);
1344 parse_pgm_token_num_t cmd_column_test_arg6 = TOKEN_NUM_INITIALIZER(struct cmd_column_test_result, arm_dist, INT16);
1345 parse_pgm_token_num_t cmd_column_test_arg7 = TOKEN_NUM_INITIALIZER(struct cmd_column_test_result, nb_col, INT8);
1346
1347 prog_char help_column_test2[] = "Column_Test function (level, dist, a1, a2, a3, arm_dist, nb_col)";
1348 parse_pgm_inst_t cmd_column_test2 = {
1349         .f = cmd_column_test_parsed,  /* function to call */
1350         .data = NULL,      /* 2nd arg of func */
1351         .help_str = help_column_test2,
1352         .tokens = {        /* token list, NULL terminated */
1353                 (prog_void *)&cmd_column_test_arg0, 
1354                 (prog_void *)&cmd_column_test_arg1, 
1355                 (prog_void *)&cmd_column_test_arg2, 
1356                 (prog_void *)&cmd_column_test_arg3, 
1357                 (prog_void *)&cmd_column_test_arg4, 
1358                 (prog_void *)&cmd_column_test_arg5, 
1359                 (prog_void *)&cmd_column_test_arg6, 
1360                 (prog_void *)&cmd_column_test_arg7, 
1361                 NULL,
1362         },
1363 };
1364
1365
1366 /**********************************************************/
1367 /* Pickup_Test */
1368
1369 /* this structure is filled when cmd_pickup_test is parsed successfully */
1370 struct cmd_pickup_test_result {
1371         fixed_string_t arg0;
1372         fixed_string_t arg1;
1373         int16_t dist;
1374 };
1375
1376 /* return red or green sensor */
1377 #define COLOR_IR_SENSOR()                                               \
1378         ({                                                              \
1379                 uint8_t __ret = 0;                                      \
1380                 if (side == I2C_RIGHT_SIDE)                             \
1381                         __ret = sensor_get(S_DISP_RIGHT);               \
1382                 else                                                    \
1383                         __ret = sensor_get(S_DISP_LEFT);                \
1384                 __ret;                                                  \
1385         })                                                              \
1386 /* column dispensers */
1387 #define COL_SCAN_MARGIN 200
1388 /* distance between the wheel axis and the IR sensor */
1389
1390 /* function called when cmd_pickup_test is parsed successfully */
1391 static void cmd_pickup_test_parsed(void *parsed_result, void *data)
1392 {
1393         uint8_t err, side, first_try = 1;
1394         int8_t cols_count_before, cols_count_after, cols;
1395         struct cmd_pickup_test_result *res = parsed_result;
1396         int16_t pos1, pos2, pos;
1397         microseconds us;
1398         int16_t dist = res->dist;
1399         uint8_t timeout = 0;
1400
1401         if (!strcmp_P(res->arg1, PSTR("left")))
1402                 side = I2C_LEFT_SIDE;
1403         else
1404                 side = I2C_RIGHT_SIDE;
1405
1406         i2c_mechboard_mode_prepare_pickup(I2C_AUTO_SIDE);
1407         cols_count_before = get_column_count();
1408         position_set(&mainboard.pos, 0, 0, 0);
1409
1410         strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_FAST);
1411         trajectory_d_rel(&mainboard.traj, -1000);
1412         err = WAIT_COND_OR_TRAJ_END(!COLOR_IR_SENSOR(), TRAJ_FLAGS_NO_NEAR);
1413         if (err) /* we should not reach end */
1414                 goto fail;
1415         pos1 = position_get_x_s16(&mainboard.pos);
1416         printf_P(PSTR("pos1 = %d\r\n"), pos1);
1417
1418         err = WAIT_COND_OR_TRAJ_END(COLOR_IR_SENSOR(), TRAJ_FLAGS_NO_NEAR);
1419         if (err)
1420                 goto fail;
1421         pos2 = position_get_x_s16(&mainboard.pos);
1422         printf_P(PSTR("pos2 = %d\r\n"), pos2);
1423
1424         pos = ABS(pos1 - pos2);
1425         printf_P(PSTR("pos = %d\r\n"), pos);
1426
1427         trajectory_d_rel(&mainboard.traj, -dist + pos/2);
1428         err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
1429
1430         if (side == I2C_LEFT_SIDE)
1431                 trajectory_a_rel(&mainboard.traj, 90);
1432         else
1433                 trajectory_a_rel(&mainboard.traj, -90);
1434         err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
1435
1436         pickup_wheels_on();     
1437  retry:
1438         if (first_try)
1439                 i2c_mechboard_mode_lazy_harvest();
1440         else
1441                 i2c_mechboard_mode_prepare_pickup(I2C_AUTO_SIDE);
1442         first_try = 0;
1443
1444         strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_FAST);
1445         trajectory_d_rel(&mainboard.traj, 300);
1446         err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST | END_NEAR);
1447         strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST);
1448         err = strat_calib(600, TRAJ_FLAGS_SMALL_DIST);
1449
1450         trajectory_d_rel(&mainboard.traj, -DIST_BACK_DISPENSER);
1451         err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
1452         if (!TRAJ_SUCCESS(err))
1453                 goto fail;
1454
1455         position_set(&mainboard.pos, 0, 0, 0);
1456         if (get_mechboard_mode() == I2C_MECHBOARD_MODE_PREPARE_EJECT) {
1457                 strat_eject_col(90, 0);
1458                 goto retry;
1459         }
1460
1461         /* start to pickup with finger / arms */
1462
1463         printf_P(PSTR("%s pickup now\r\n"), __FUNCTION__);
1464         i2c_mechboard_mode_pickup();
1465         WAIT_COND_OR_TIMEOUT(get_mechboard_mode() == 
1466                              I2C_MECHBOARD_MODE_PICKUP, 100);
1467         us = time_get_us2();
1468         cols = get_column_count();
1469         while (get_mechboard_mode() == I2C_MECHBOARD_MODE_PICKUP) {
1470                 if (get_column_count() != cols) {
1471                         cols = get_column_count();
1472                         us = time_get_us2();
1473                 }
1474                 if ((get_column_count() - cols_count_before) >= 4) {
1475                         printf_P(PSTR("%s no more cols in disp\r\n"), __FUNCTION__);
1476                         break;
1477                 }
1478                 /* 1 second timeout */
1479                 if (time_get_us2() - us > 1500000L) {
1480                         printf_P(PSTR("%s timeout\r\n"), __FUNCTION__);
1481                         timeout = 1;
1482                         break;
1483                 }
1484         }
1485
1486         /* eject if we found a bad color column */
1487         
1488         if (get_mechboard_mode() == I2C_MECHBOARD_MODE_PREPARE_EJECT) {
1489                 strat_eject_col(90, 0);
1490                 goto retry;
1491         }
1492
1493         strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
1494         trajectory_d_rel(&mainboard.traj, -250);
1495         wait_traj_end(TRAJ_FLAGS_SMALL_DIST | END_NEAR);
1496
1497         cols_count_after = get_column_count();
1498         cols = cols_count_after - cols_count_before;
1499         DEBUG(E_USER_STRAT, "%s we got %d cols", __FUNCTION__, cols);
1500
1501         pickup_wheels_off();
1502         i2c_mechboard_mode_clear();
1503
1504         wait_ms(1000);
1505         return;
1506  fail:
1507         printf_P(PSTR("failed\r\n"));
1508         strat_hardstop();
1509 }
1510
1511 prog_char str_pickup_test_arg0[] = "pickup_test";
1512 parse_pgm_token_string_t cmd_pickup_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_pickup_test_result, arg0, str_pickup_test_arg0);
1513 prog_char str_pickup_test_arg1[] = "left#right";
1514 parse_pgm_token_string_t cmd_pickup_test_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_pickup_test_result, arg1, str_pickup_test_arg1);
1515 parse_pgm_token_num_t cmd_pickup_test_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_pickup_test_result, dist, INT16);
1516
1517 prog_char help_pickup_test[] = "Pickup_Test function";
1518 parse_pgm_inst_t cmd_pickup_test = {
1519         .f = cmd_pickup_test_parsed,  /* function to call */
1520         .data = NULL,      /* 2nd arg of func */
1521         .help_str = help_pickup_test,
1522         .tokens = {        /* token list, NULL terminated */
1523                 (prog_void *)&cmd_pickup_test_arg0,
1524                 (prog_void *)&cmd_pickup_test_arg1,
1525                 (prog_void *)&cmd_pickup_test_arg2,
1526                 NULL,
1527         },
1528 };
1529
1530 /**********************************************************/
1531 /* Lintel_Test */
1532
1533 /* this structure is filled when cmd_lintel_test is parsed successfully */
1534 struct cmd_lintel_test_result {
1535         fixed_string_t arg0;
1536 };
1537
1538 /* function called when cmd_lintel_test is parsed successfully */
1539 static void cmd_lintel_test_parsed(void *parsed_result, void *data)
1540 {
1541         uint8_t err, first_try = 1, right_ok, left_ok;
1542         int16_t left_cur, right_cur;
1543         
1544         i2c_mechboard_mode_prepare_get_lintel();
1545         time_wait_ms(500);
1546  retry:
1547         strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST);
1548         trajectory_d_rel(&mainboard.traj, 500);
1549         err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
1550         if (!TRAJ_SUCCESS(err) && err != END_BLOCKING)
1551                 goto fail;
1552         
1553         i2c_mechboard_mode_get_lintel();
1554         time_wait_ms(500);
1555
1556         left_cur = sensor_get_adc(ADC_CSENSE3);
1557         left_ok = (left_cur > I2C_MECHBOARD_CURRENT_COLUMN);
1558         right_cur = mechboard.pump_right1_current;
1559         right_ok = (right_cur > I2C_MECHBOARD_CURRENT_COLUMN);
1560
1561         printf_P(PSTR("left_ok=%d (%d), right_ok=%d (%d)\r\n"),
1562                  left_ok, left_cur, right_ok, right_cur);
1563         if (first_try) {
1564                 if (!right_ok && !left_ok) {
1565                         i2c_mechboard_mode_prepare_get_lintel();
1566                         time_wait_ms(300);
1567                 }
1568                 else if (right_ok && !left_ok) {
1569                         i2c_mechboard_mode_prepare_get_lintel();
1570                         time_wait_ms(300);
1571                         strat_set_speed(500, 500);
1572                         trajectory_d_a_rel(&mainboard.traj, -150, 30);
1573                         err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
1574                         trajectory_d_a_rel(&mainboard.traj, -140, -30);
1575                         err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
1576                         first_try = 0;
1577                         goto retry;
1578                 }
1579                 else if (!right_ok && left_ok) {
1580                         i2c_mechboard_mode_prepare_get_lintel();
1581                         time_wait_ms(300);
1582                         strat_set_speed(500, 500);
1583                         trajectory_d_a_rel(&mainboard.traj, -150, -30);
1584                         err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
1585                         trajectory_d_a_rel(&mainboard.traj, -140, 30);
1586                         err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
1587                         first_try = 0;
1588                         goto retry;
1589                 }
1590                 /* else, lintel is ok */
1591                 else {
1592                         i2c_mechboard_mode_put_lintel();
1593                 }
1594         }
1595         else {
1596                 if (right_ok && left_ok) {
1597                         /* lintel is ok */
1598                         i2c_mechboard_mode_put_lintel();
1599                 }
1600                 else {
1601                         i2c_mechboard_mode_prepare_get_lintel();
1602                         time_wait_ms(300);
1603                 }
1604         }
1605
1606         strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
1607         trajectory_d_rel(&mainboard.traj, -250);
1608         err = wait_traj_end(TRAJ_FLAGS_STD);
1609         return;
1610         
1611 fail:
1612         printf_P(PSTR("fail\r\n"));
1613         return;
1614 }
1615
1616 prog_char str_lintel_test_arg0[] = "lintel_test";
1617 parse_pgm_token_string_t cmd_lintel_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_lintel_test_result, arg0, str_lintel_test_arg0);
1618
1619 prog_char help_lintel_test[] = "Lintel_Test function";
1620 parse_pgm_inst_t cmd_lintel_test = {
1621         .f = cmd_lintel_test_parsed,  /* function to call */
1622         .data = NULL,      /* 2nd arg of func */
1623         .help_str = help_lintel_test,
1624         .tokens = {        /* token list, NULL terminated */
1625                 (prog_void *)&cmd_lintel_test_arg0,
1626                 NULL,
1627         },
1628 };
1629
1630 /**********************************************************/
1631 /* Scan_Test */
1632
1633 /* this structure is filled when cmd_scan_test is parsed successfully */
1634 struct cmd_scan_test_result {
1635         fixed_string_t arg0;
1636         fixed_string_t arg1;
1637         int16_t start_dist;
1638         int16_t scan_dist;
1639         int16_t scan_speed;
1640         int16_t center_x;
1641         int16_t center_y;
1642         uint8_t level;
1643 };
1644
1645 #define SCAN_MODE_CHECK_TEMPLE 0
1646 #define SCAN_MODE_SCAN_COL     1
1647 #define SCAN_MODE_SCAN_TEMPLE  2
1648 #define SCAN_MODE_TRAJ_ONLY    3
1649
1650 /* function called when cmd_scan_test is parsed successfully */
1651 static void cmd_scan_test_parsed(void *parsed_result, void *data)
1652 {
1653         uint8_t err, mode=0, c;
1654         int16_t pos1x, pos1y, dist;
1655         struct cmd_scan_test_result *res = parsed_result;
1656         int16_t back_mm = 0;
1657
1658         int16_t ckpt_rel_x = 0, ckpt_rel_y = 0;
1659
1660         double center_abs_x, center_abs_y;
1661         double ckpt_rel_d, ckpt_rel_a;
1662         double ckpt_abs_x, ckpt_abs_y;
1663
1664         if (!strcmp_P(res->arg1, PSTR("traj_only")))
1665                 mode = SCAN_MODE_TRAJ_ONLY;
1666         else if (!strcmp_P(res->arg1, PSTR("check_temple")))
1667                 mode = SCAN_MODE_CHECK_TEMPLE;
1668         else if (!strcmp_P(res->arg1, PSTR("scan_col")))
1669                 mode = SCAN_MODE_SCAN_COL;
1670         else if (!strcmp_P(res->arg1, PSTR("scan_temple")))
1671                 mode = SCAN_MODE_SCAN_TEMPLE;
1672
1673         /* go to disc */
1674         strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_VERY_SLOW);
1675         trajectory_d_rel(&mainboard.traj, 400);
1676         err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
1677         if (err != END_BLOCKING) 
1678                 return;
1679
1680         /* save absolute position of disc */
1681         rel_da_to_abs_xy(265, 0, &center_abs_x, &center_abs_y);
1682
1683         /* go back and prepare to scan */
1684         strat_set_speed(1000, 1000);
1685         trajectory_d_a_rel(&mainboard.traj, -140, 130);
1686         err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
1687         if (!TRAJ_SUCCESS(err))
1688                 return;
1689
1690         /* prepare scanner arm */
1691         if (mode != SCAN_MODE_TRAJ_ONLY)
1692                 i2c_sensorboard_scanner_prepare();
1693         time_wait_ms(250);
1694
1695         strat_set_speed(res->scan_speed, 1000);
1696
1697         pos1x = position_get_x_s16(&mainboard.pos);
1698         pos1y = position_get_y_s16(&mainboard.pos);
1699         trajectory_d_rel(&mainboard.traj, -res->scan_dist);
1700         
1701         while (1) {
1702                 err = test_traj_end(TRAJ_FLAGS_SMALL_DIST);
1703                 if (err != 0)
1704                         break;
1705                 
1706                 dist = distance_from_robot(pos1x, pos1y);
1707
1708                 if (dist > res->start_dist)
1709                         break;
1710
1711                 if (get_scanner_status() & I2C_SCAN_MAX_COLUMN) {
1712                         err = END_ERROR;
1713                         break;
1714                 }
1715         }
1716         
1717         if (err) {
1718                 if (TRAJ_SUCCESS(err))
1719                         err = END_ERROR; /* should not reach end */
1720                 strat_hardstop();
1721                 trajectory_goto_xy_abs(&mainboard.traj, pos1x, pos1y);
1722                 wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
1723                 if (mode != SCAN_MODE_TRAJ_ONLY)
1724                         i2c_sensorboard_scanner_stop();
1725                 return;
1726         }
1727
1728         /* start the scanner */
1729
1730         if (mode != SCAN_MODE_TRAJ_ONLY)
1731                 i2c_sensorboard_scanner_start();
1732
1733         err = WAIT_COND_OR_TRAJ_END(get_scanner_status() & I2C_SCAN_MAX_COLUMN,
1734                                     TRAJ_FLAGS_NO_NEAR);
1735         if (err == 0)
1736                 err = END_ERROR;
1737         if (!TRAJ_SUCCESS(err)) {
1738                 strat_hardstop();
1739                 trajectory_goto_xy_abs(&mainboard.traj, pos1x, pos1y);
1740                 wait_traj_end(TRAJ_FLAGS_NO_NEAR);
1741                 if (mode != SCAN_MODE_TRAJ_ONLY)
1742                         i2c_sensorboard_scanner_stop();
1743                 return;
1744         }
1745
1746         if (mode == SCAN_MODE_TRAJ_ONLY)
1747                 return;
1748
1749         wait_scan_done(10000);
1750
1751         i2c_sensorboard_scanner_stop();
1752
1753         if (mode == SCAN_MODE_CHECK_TEMPLE) {
1754                 i2c_sensorboard_scanner_algo_check(res->level,
1755                                                    res->center_x, res->center_y);
1756                 i2cproto_wait_update();
1757                 wait_scan_done(10000);
1758                 scanner_dump_state();
1759
1760                 if (sensorboard.dropzone_h == -1) {
1761                         printf_P(PSTR("-- try to build a temple\r\n"));
1762                         res->center_x = 15;
1763                         res->center_y = 13;
1764                         mode = SCAN_MODE_SCAN_TEMPLE;
1765                 }
1766         }
1767
1768         if (mode == SCAN_MODE_SCAN_TEMPLE) {
1769                 i2c_sensorboard_scanner_algo_temple(I2C_SCANNER_ZONE_DISC,
1770                                                     res->center_x,
1771                                                     res->center_y);
1772                 i2cproto_wait_update();
1773                 wait_scan_done(10000);
1774                 scanner_dump_state();
1775                 
1776                 if (sensorboard.dropzone_h == -1 ||
1777                     strat_scan_get_checkpoint(mode, &ckpt_rel_x,
1778                                               &ckpt_rel_y, &back_mm)) {
1779                         printf_P(PSTR("-- try to build a column\r\n"));
1780                         mode = SCAN_MODE_SCAN_COL;
1781                 }
1782         }
1783
1784         if (mode == SCAN_MODE_SCAN_COL) {
1785                 i2c_sensorboard_scanner_algo_column(I2C_SCANNER_ZONE_DISC,
1786                                                     res->center_x, res->center_y);
1787                 i2cproto_wait_update();
1788                 wait_scan_done(10000);
1789                 scanner_dump_state();
1790
1791                 if (sensorboard.dropzone_h == -1 ||
1792                     strat_scan_get_checkpoint(mode, &ckpt_rel_x,
1793                                               &ckpt_rel_y, &back_mm)) {
1794                         return;
1795                 }
1796         }
1797
1798         if (sensorboard.dropzone_h == -1)
1799                 return;
1800
1801         if (mode == SCAN_MODE_CHECK_TEMPLE) {
1802                 ckpt_rel_x = 220;
1803                 ckpt_rel_y = 100;
1804         }
1805
1806
1807         printf_P(PSTR("rel xy for ckpt is %d,%d\r\n"), ckpt_rel_x, ckpt_rel_y);
1808
1809         rel_xy_to_abs_xy(ckpt_rel_x, ckpt_rel_y, &ckpt_abs_x, &ckpt_abs_y);
1810         abs_xy_to_rel_da(ckpt_abs_x, ckpt_abs_y, &ckpt_rel_d, &ckpt_rel_a);
1811
1812         printf_P(PSTR("abs ckpt is %2.2f,%2.2f\r\n"), ckpt_abs_x, ckpt_abs_y);
1813
1814         printf_P(PSTR("ok ? (y/n)\r\n"));
1815
1816         c = cmdline_getchar_wait();
1817
1818         if (c != 'y')
1819                 return;
1820
1821         strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
1822
1823         /* intermediate checkpoint for some positions */
1824         if ( (DEG(ckpt_rel_a) < 0 && DEG(ckpt_rel_a) > -90) ) {
1825                 trajectory_goto_xy_rel(&mainboard.traj, 200, 100);
1826                 err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
1827                 if (!TRAJ_SUCCESS(err))
1828                         return;
1829         }
1830
1831         trajectory_goto_xy_abs(&mainboard.traj, ckpt_abs_x, ckpt_abs_y);
1832         err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
1833         if (!TRAJ_SUCCESS(err))
1834                 return;
1835
1836         trajectory_turnto_xy(&mainboard.traj, center_abs_x, center_abs_y);
1837         err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
1838         if (!TRAJ_SUCCESS(err))
1839                 return;
1840
1841         c = cmdline_getchar_wait();
1842
1843         pos1x = position_get_x_s16(&mainboard.pos);
1844         pos1y = position_get_y_s16(&mainboard.pos);
1845
1846         strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW);
1847         trajectory_d_rel(&mainboard.traj, 200);
1848         err = WAIT_COND_OR_TRAJ_END(distance_from_robot(pos1x, pos1y) > 200,
1849                                     TRAJ_FLAGS_SMALL_DIST);
1850         if (err == 0) {
1851                 strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_VERY_SLOW);
1852                 trajectory_d_rel(&mainboard.traj, 400);
1853                 err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
1854         }
1855         if (err != END_BLOCKING) 
1856                 return;
1857
1858         if (back_mm) {
1859                 trajectory_d_rel(&mainboard.traj, -back_mm);
1860                 wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
1861         }
1862 }
1863
1864 prog_char str_scan_test_arg0[] = "scan_test";
1865 parse_pgm_token_string_t cmd_scan_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_scan_test_result, arg0, str_scan_test_arg0);
1866 prog_char str_scan_test_arg1[] = "traj_only#scan_col#scan_temple";
1867 parse_pgm_token_string_t cmd_scan_test_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_scan_test_result, arg1, str_scan_test_arg1);
1868 parse_pgm_token_num_t cmd_scan_test_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_scan_test_result, start_dist, INT16);
1869 parse_pgm_token_num_t cmd_scan_test_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_scan_test_result, scan_dist, INT16);
1870 parse_pgm_token_num_t cmd_scan_test_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_scan_test_result, scan_speed, INT16);
1871 parse_pgm_token_num_t cmd_scan_test_arg5 = TOKEN_NUM_INITIALIZER(struct cmd_scan_test_result, center_x, INT16);
1872 parse_pgm_token_num_t cmd_scan_test_arg6 = TOKEN_NUM_INITIALIZER(struct cmd_scan_test_result, center_y, INT16);
1873
1874 prog_char help_scan_test[] = "Scan_Test function (start_dist, scan_dist, speed_dist, centerx, centery)";
1875 parse_pgm_inst_t cmd_scan_test = {
1876         .f = cmd_scan_test_parsed,  /* function to call */
1877         .data = NULL,      /* 2nd arg of func */
1878         .help_str = help_scan_test,
1879         .tokens = {        /* token list, NULL terminated */
1880                 (prog_void *)&cmd_scan_test_arg0,
1881                 (prog_void *)&cmd_scan_test_arg1,
1882                 (prog_void *)&cmd_scan_test_arg2,
1883                 (prog_void *)&cmd_scan_test_arg3,
1884                 (prog_void *)&cmd_scan_test_arg4,
1885                 (prog_void *)&cmd_scan_test_arg5,
1886                 (prog_void *)&cmd_scan_test_arg6,
1887                 NULL,
1888         },
1889 };
1890
1891 prog_char str_scan_test_arg1b[] = "check_temple";
1892 parse_pgm_token_string_t cmd_scan_test_arg1b = TOKEN_STRING_INITIALIZER(struct cmd_scan_test_result, arg1, str_scan_test_arg1b);
1893 parse_pgm_token_num_t cmd_scan_test_arg7 = TOKEN_NUM_INITIALIZER(struct cmd_scan_test_result, level, UINT8);
1894
1895 prog_char help_scan_test2[] = "Scan_Test function (start_dist, scan_dist, speed_dist, templex, templey, level)";
1896 parse_pgm_inst_t cmd_scan_test2 = {
1897         .f = cmd_scan_test_parsed,  /* function to call */
1898         .data = NULL,      /* 2nd arg of func */
1899         .help_str = help_scan_test,
1900         .tokens = {        /* token list, NULL terminated */
1901                 (prog_void *)&cmd_scan_test_arg0,
1902                 (prog_void *)&cmd_scan_test_arg1b,
1903                 (prog_void *)&cmd_scan_test_arg2,
1904                 (prog_void *)&cmd_scan_test_arg3,
1905                 (prog_void *)&cmd_scan_test_arg4,
1906                 (prog_void *)&cmd_scan_test_arg5,
1907                 (prog_void *)&cmd_scan_test_arg6,
1908                 (prog_void *)&cmd_scan_test_arg7,
1909                 NULL,
1910         },
1911 };
1912
1913 /**********************************************************/
1914 /* Time_Monitor */
1915
1916 /* this structure is filled when cmd_time_monitor is parsed successfully */
1917 struct cmd_time_monitor_result {
1918         fixed_string_t arg0;
1919         fixed_string_t arg1;
1920 };
1921
1922 /* function called when cmd_time_monitor is parsed successfully */
1923 static void cmd_time_monitor_parsed(void *parsed_result, void *data)
1924 {
1925         struct cmd_time_monitor_result *res = parsed_result;
1926         uint16_t seconds;
1927
1928         if (!strcmp_P(res->arg1, PSTR("reset"))) {
1929                 eeprom_write_word(EEPROM_TIME_ADDRESS, 0);
1930         }
1931         seconds = eeprom_read_word(EEPROM_TIME_ADDRESS);
1932         printf_P(PSTR("Running since %d mn %d\r\n"), seconds/60, seconds%60);
1933 }
1934
1935 prog_char str_time_monitor_arg0[] = "time_monitor";
1936 parse_pgm_token_string_t cmd_time_monitor_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_time_monitor_result, arg0, str_time_monitor_arg0);
1937 prog_char str_time_monitor_arg1[] = "show#reset";
1938 parse_pgm_token_string_t cmd_time_monitor_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_time_monitor_result, arg1, str_time_monitor_arg1);
1939
1940 prog_char help_time_monitor[] = "Show since how long we are running";
1941 parse_pgm_inst_t cmd_time_monitor = {
1942         .f = cmd_time_monitor_parsed,  /* function to call */
1943         .data = NULL,      /* 2nd arg of func */
1944         .help_str = help_time_monitor,
1945         .tokens = {        /* token list, NULL terminated */
1946                 (prog_void *)&cmd_time_monitor_arg0, 
1947                 (prog_void *)&cmd_time_monitor_arg1, 
1948                 NULL,
1949         },
1950 };
1951
1952
1953 /**********************************************************/
1954 /* Scanner */
1955
1956 /* this structure is filled when cmd_scanner is parsed successfully */
1957 struct cmd_scanner_result {
1958         fixed_string_t arg0;
1959         fixed_string_t arg1;
1960 };
1961
1962 /* function called when cmd_scanner is parsed successfully */
1963 static void cmd_scanner_parsed(void *parsed_result, void *data)
1964 {
1965         struct cmd_scanner_result *res = parsed_result;
1966
1967         if (!strcmp_P(res->arg1, PSTR("prepare"))) {
1968                 i2c_sensorboard_scanner_prepare();
1969         }
1970         else if (!strcmp_P(res->arg1, PSTR("stop"))) {
1971                 i2c_sensorboard_scanner_stop();
1972         }
1973         else if (!strcmp_P(res->arg1, PSTR("start"))) {
1974                 i2c_sensorboard_scanner_start();
1975         }
1976         else if (!strcmp_P(res->arg1, PSTR("algo_col"))) {
1977                 i2c_sensorboard_scanner_algo_column(I2C_SCANNER_ZONE_DISC,
1978                                                      15, 15);
1979         }
1980         else if (!strcmp_P(res->arg1, PSTR("algo_check"))) {
1981                 i2c_sensorboard_scanner_algo_check(2, 15, 15); // XXX
1982         }
1983         else if (!strcmp_P(res->arg1, PSTR("calib"))) {
1984                 i2c_sensorboard_scanner_calib();
1985         }
1986         else if (!strcmp_P(res->arg1, PSTR("show"))) {
1987                 scanner_dump_state();
1988         }
1989 }
1990
1991 prog_char str_scanner_arg0[] = "scanner";
1992 parse_pgm_token_string_t cmd_scanner_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_scanner_result, arg0, str_scanner_arg0);
1993 prog_char str_scanner_arg1[] = "prepare#start#algo_col#algo_check#stop#show#calib";
1994 parse_pgm_token_string_t cmd_scanner_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_scanner_result, arg1, str_scanner_arg1);
1995
1996 prog_char help_scanner[] = "send commands to scanner";
1997 parse_pgm_inst_t cmd_scanner = {
1998         .f = cmd_scanner_parsed,  /* function to call */
1999         .data = NULL,      /* 2nd arg of func */
2000         .help_str = help_scanner,
2001         .tokens = {        /* token list, NULL terminated */
2002                 (prog_void *)&cmd_scanner_arg0, 
2003                 (prog_void *)&cmd_scanner_arg1, 
2004                 NULL,
2005         },
2006 };
2007
2008 /**********************************************************/
2009 /* Build_Z1 */
2010
2011 /* this structure is filled when cmd_build_z1 is parsed successfully */
2012 struct cmd_build_z1_result {
2013         fixed_string_t arg0;
2014         uint8_t level;
2015         int16_t d1;
2016         int16_t d2;
2017         int16_t d3;
2018 };
2019
2020 /* function called when cmd_build_z1 is parsed successfully */
2021 static void cmd_build_z1_parsed(void *parsed_result, void *data)
2022 {
2023         struct cmd_build_z1_result *res = parsed_result;
2024
2025         strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_SLOW);
2026         trajectory_d_rel(&mainboard.traj, 400);
2027         wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
2028
2029         trajectory_d_rel(&mainboard.traj, -200);
2030         wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
2031
2032         i2c_mechboard_mode_prepare_pickup_next(I2C_LEFT_SIDE,
2033                                                I2C_MECHBOARD_MODE_HARVEST);
2034
2035         while (get_column_count() != 4);
2036
2037         i2c_mechboard_mode_prepare_build_both(res->level);
2038         time_wait_ms(500);
2039
2040         trajectory_d_rel(&mainboard.traj, 400);
2041         wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
2042
2043         strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW);
2044         trajectory_d_rel(&mainboard.traj, -res->d1);
2045         wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
2046         i2c_mechboard_mode_autobuild(res->level, 2, I2C_AUTOBUILD_DEFAULT_DIST,
2047                                      res->level, 2, I2C_AUTOBUILD_DEFAULT_DIST,
2048                                      1);
2049         WAIT_COND_OR_TIMEOUT(get_mechboard_mode() == 
2050                              I2C_MECHBOARD_MODE_AUTOBUILD, 100);
2051         WAIT_COND_OR_TIMEOUT(get_mechboard_mode() != 
2052                              I2C_MECHBOARD_MODE_AUTOBUILD, 10000);
2053
2054         strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW);
2055         trajectory_d_rel(&mainboard.traj, -res->d2);
2056         wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
2057         i2c_mechboard_mode_push_temple(1);
2058         time_wait_ms(400);
2059         strat_set_speed(200, SPEED_ANGLE_SLOW);
2060         trajectory_d_rel(&mainboard.traj, res->d3);
2061         wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
2062 }
2063
2064 prog_char str_build_z1_arg0[] = "build_z1";
2065 parse_pgm_token_string_t cmd_build_z1_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_build_z1_result, arg0, str_build_z1_arg0);
2066 parse_pgm_token_num_t cmd_build_z1_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_build_z1_result, level, UINT8);
2067 parse_pgm_token_num_t cmd_build_z1_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_build_z1_result, d1, INT16);
2068 parse_pgm_token_num_t cmd_build_z1_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_build_z1_result, d2, INT16);
2069 parse_pgm_token_num_t cmd_build_z1_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_build_z1_result, d3, INT16);
2070
2071 prog_char help_build_z1[] = "Build_Z1 function (level, d1, d2, d3)";
2072 parse_pgm_inst_t cmd_build_z1 = {
2073         .f = cmd_build_z1_parsed,  /* function to call */
2074         .data = NULL,      /* 2nd arg of func */
2075         .help_str = help_build_z1,
2076         .tokens = {        /* token list, NULL terminated */
2077                 (prog_void *)&cmd_build_z1_arg0, 
2078                 (prog_void *)&cmd_build_z1_arg1, 
2079                 (prog_void *)&cmd_build_z1_arg2, 
2080                 (prog_void *)&cmd_build_z1_arg3, 
2081                 (prog_void *)&cmd_build_z1_arg4, 
2082                 NULL,
2083         },
2084 };
2085
2086 #ifdef TEST_BEACON
2087 /**********************************************************/
2088 /* Beacon_Opp_Dump */
2089
2090 /* this structure is filled when cmd_beacon_opp_dump is parsed successfully */
2091 struct cmd_beacon_opp_dump_result {
2092         fixed_string_t arg0;
2093 };
2094
2095 void beacon_dump_samples(void);
2096
2097 /* function called when cmd_beacon_opp_dump is parsed successfully */
2098 static void cmd_beacon_opp_dump_parsed(void *parsed_result, void *data)
2099 {
2100         beacon_dump_samples();
2101 }
2102
2103 prog_char str_beacon_opp_dump_arg0[] = "beacon_opp_dump";
2104 parse_pgm_token_string_t cmd_beacon_opp_dump_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_beacon_opp_dump_result, arg0, str_beacon_opp_dump_arg0);
2105
2106 prog_char help_beacon_opp_dump[] = "Dump beacon samples";
2107 parse_pgm_inst_t cmd_beacon_opp_dump = {
2108         .f = cmd_beacon_opp_dump_parsed,  /* function to call */
2109         .data = NULL,      /* 2nd arg of func */
2110         .help_str = help_beacon_opp_dump,
2111         .tokens = {        /* token list, NULL terminated */
2112                 (prog_void *)&cmd_beacon_opp_dump_arg0, 
2113                 NULL,
2114         },
2115 };
2116 #endif
2117
2118 /**********************************************************/
2119 /* Test */
2120
2121 /* this structure is filled when cmd_test is parsed successfully */
2122 struct cmd_test_result {
2123         fixed_string_t arg0;
2124         int32_t radius;
2125 };
2126 void circle_get_da_speed_from_radius(struct trajectory *traj,
2127                                 double radius_mm,
2128                                 double *speed_d,
2129                                 double *speed_a);
2130 /* function called when cmd_test is parsed successfully */
2131 static void cmd_test_parsed(void *parsed_result, void *data)
2132 {
2133         struct cmd_test_result *res = parsed_result;
2134         double d,a;
2135         strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW);
2136         circle_get_da_speed_from_radius(&mainboard.traj, res->radius, &d, &a);
2137         printf_P(PSTR("d=%2.2f a=%2.2f\r\n"), d, a);
2138 }
2139
2140 prog_char str_test_arg0[] = "test";
2141 parse_pgm_token_string_t cmd_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_test_result, arg0, str_test_arg0);
2142 parse_pgm_token_num_t cmd_test_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_test_result, radius, INT32);
2143
2144 prog_char help_test[] = "Test function";
2145 parse_pgm_inst_t cmd_test = {
2146         .f = cmd_test_parsed,  /* function to call */
2147         .data = NULL,      /* 2nd arg of func */
2148         .help_str = help_test,
2149         .tokens = {        /* token list, NULL terminated */
2150                 (prog_void *)&cmd_test_arg0,
2151                 (prog_void *)&cmd_test_arg1,
2152                 NULL,
2153         },
2154 };