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