cob detection
[aversive.git] / projects / microb2010 / mainboard / commands_mainboard.c
1 /*
2  *  Copyright Droids Corporation (2009)
3  *
4  *  This program is free software; you can redistribute it and/or modify
5  *  it under the terms of the GNU General Public License as published by
6  *  the Free Software Foundation; either version 2 of the License, or
7  *  (at your option) any later version.
8  *
9  *  This program is distributed in the hope that it will be useful,
10  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
11  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
12  *  GNU General Public License for more details.
13  *
14  *  You should have received a copy of the GNU General Public License
15  *  along with this program; if not, write to the Free Software
16  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
17  *
18  *  Revision : $Id: commands_mainboard.c,v 1.9 2009-11-08 17:24:33 zer0 Exp $
19  *
20  *  Olivier MATZ <zer0@droids-corp.org>
21  */
22
23 #include <stdio.h>
24 #include <string.h>
25 #include <math.h>
26
27 #include <hostsim.h>
28 #include <aversive/pgmspace.h>
29 #include <aversive/wait.h>
30 #include <aversive/error.h>
31 #include <aversive/eeprom.h>
32
33 #include <ax12.h>
34 #include <uart.h>
35 #include <pwm_ng.h>
36 #include <clock_time.h>
37 #include <spi.h>
38 #include <i2c.h>
39
40 #include <pid.h>
41 #include <quadramp.h>
42 #include <control_system_manager.h>
43 #include <trajectory_manager.h>
44 #include <trajectory_manager_core.h>
45 #include <trajectory_manager_utils.h>
46 #include <vect_base.h>
47 #include <lines.h>
48 #include <polygon.h>
49 #include <obstacle_avoidance.h>
50 #include <blocking_detection_manager.h>
51 #include <robot_system.h>
52 #include <position_manager.h>
53
54 #include <rdline.h>
55 #include <parse.h>
56 #include <parse_string.h>
57 #include <parse_num.h>
58
59 #include "../common/i2c_commands.h"
60 #include "../common/eeprom_mapping.h"
61
62 #include "main.h"
63 #include "robotsim.h"
64 #include "sensor.h"
65 #include "cmdline.h"
66 #include "strat.h"
67 #include "strat_utils.h"
68 #include "strat_base.h"
69 #include "strat_db.h"
70 #include "strat_corn.h"
71 #include "i2c_protocol.h"
72 #include "actuator.h"
73
74 struct cmd_event_result {
75         fixed_string_t arg0;
76         fixed_string_t arg1;
77         fixed_string_t arg2;
78 };
79
80
81 /* function called when cmd_event is parsed successfully */
82 static void cmd_event_parsed(void *parsed_result, void *data)
83 {
84         u08 bit=0;
85
86         struct cmd_event_result * res = parsed_result;
87
88         if (!strcmp_P(res->arg1, PSTR("all"))) {
89                 bit = 0xFF;
90                 if (!strcmp_P(res->arg2, PSTR("on")))
91                         mainboard.flags |= bit;
92                 else if (!strcmp_P(res->arg2, PSTR("off")))
93                         mainboard.flags &= bit;
94                 else { /* show */
95                         printf_P(PSTR("encoders is %s\r\n"),
96                                  (DO_ENCODERS & mainboard.flags) ? "on":"off");
97                         printf_P(PSTR("cs is %s\r\n"),
98                                  (DO_CS & mainboard.flags) ? "on":"off");
99                         printf_P(PSTR("rs is %s\r\n"),
100                                  (DO_RS & mainboard.flags) ? "on":"off");
101                         printf_P(PSTR("pos is %s\r\n"),
102                                  (DO_POS & mainboard.flags) ? "on":"off");
103                         printf_P(PSTR("bd is %s\r\n"),
104                                  (DO_BD & mainboard.flags) ? "on":"off");
105                         printf_P(PSTR("timer is %s\r\n"),
106                                  (DO_TIMER & mainboard.flags) ? "on":"off");
107                         printf_P(PSTR("power is %s\r\n"),
108                                  (DO_POWER & mainboard.flags) ? "on":"off");
109                         printf_P(PSTR("errblock is %s\r\n"),
110                                  (DO_ERRBLOCKING & mainboard.flags) ? "on":"off");
111                 }
112                 return;
113         }
114
115         if (!strcmp_P(res->arg1, PSTR("encoders")))
116                 bit = DO_ENCODERS;
117         else if (!strcmp_P(res->arg1, PSTR("cs"))) {
118                 strat_hardstop();
119                 bit = DO_CS;
120         }
121         else if (!strcmp_P(res->arg1, PSTR("rs")))
122                 bit = DO_RS;
123         else if (!strcmp_P(res->arg1, PSTR("pos")))
124                 bit = DO_POS;
125         else if (!strcmp_P(res->arg1, PSTR("bd")))
126                 bit = DO_BD;
127         else if (!strcmp_P(res->arg1, PSTR("timer"))) {
128                 time_reset();
129                 bit = DO_TIMER;
130         }
131         else if (!strcmp_P(res->arg1, PSTR("power")))
132                 bit = DO_POWER;
133         else if (!strcmp_P(res->arg1, PSTR("errblock")))
134                 bit = DO_ERRBLOCKING;
135
136         if (!strcmp_P(res->arg2, PSTR("on")))
137                 mainboard.flags |= bit;
138         else if (!strcmp_P(res->arg2, PSTR("off"))) {
139                 if (!strcmp_P(res->arg1, PSTR("cs"))) {
140 #ifdef HOST_VERSION
141                         robotsim_pwm(LEFT_PWM, 0);
142                         robotsim_pwm(RIGHT_PWM, 0);
143 #else
144                         pwm_ng_set(LEFT_PWM, 0);
145                         pwm_ng_set(RIGHT_PWM, 0);
146 #endif
147                 }
148                 mainboard.flags &= (~bit);
149         }
150         printf_P(PSTR("%s is %s\r\n"), res->arg1,
151                       (bit & mainboard.flags) ? "on":"off");
152 }
153
154 prog_char str_event_arg0[] = "event";
155 parse_pgm_token_string_t cmd_event_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg0, str_event_arg0);
156 prog_char str_event_arg1[] = "all#encoders#cs#rs#pos#bd#timer#power#errblock";
157 parse_pgm_token_string_t cmd_event_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg1, str_event_arg1);
158 prog_char str_event_arg2[] = "on#off#show";
159 parse_pgm_token_string_t cmd_event_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg2, str_event_arg2);
160
161 prog_char help_event[] = "Enable/disable events";
162 parse_pgm_inst_t cmd_event = {
163         .f = cmd_event_parsed,  /* function to call */
164         .data = NULL,      /* 2nd arg of func */
165         .help_str = help_event,
166         .tokens = {        /* token list, NULL terminated */
167                 (prog_void *)&cmd_event_arg0,
168                 (prog_void *)&cmd_event_arg1,
169                 (prog_void *)&cmd_event_arg2,
170                 NULL,
171         },
172 };
173
174
175 /**********************************************************/
176 /* Spi_Test */
177
178 /* this structure is filled when cmd_spi_test is parsed successfully */
179 struct cmd_spi_test_result {
180         fixed_string_t arg0;
181 };
182
183 /* function called when cmd_spi_test is parsed successfully */
184 static void cmd_spi_test_parsed(void * parsed_result, void * data)
185 {
186 #ifdef HOST_VERSION
187         printf("not implemented\n");
188 #else
189         uint16_t i = 0, ret = 0, ret2 = 0;
190
191         if (mainboard.flags & DO_ENCODERS) {
192                 printf_P(PSTR("Disable encoder event first\r\n"));
193                 return;
194         }
195
196         do {
197                 spi_slave_select(0);
198                 ret = spi_send_and_receive_byte(i);
199                 ret2 = spi_send_and_receive_byte(i);
200                 spi_slave_deselect(0);
201
202                 if ((i & 0x7ff) == 0)
203                         printf_P(PSTR("Sent %.4x twice, received %x %x\r\n"),
204                                  i, ret, ret2);
205
206                 i++;
207         } while(!cmdline_keypressed());
208 #endif
209 }
210
211 prog_char str_spi_test_arg0[] = "spi_test";
212 parse_pgm_token_string_t cmd_spi_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_spi_test_result, arg0, str_spi_test_arg0);
213
214 prog_char help_spi_test[] = "Test the SPI";
215 parse_pgm_inst_t cmd_spi_test = {
216         .f = cmd_spi_test_parsed,  /* function to call */
217         .data = NULL,      /* 2nd arg of func */
218         .help_str = help_spi_test,
219         .tokens = {        /* token list, NULL terminated */
220                 (prog_void *)&cmd_spi_test_arg0,
221                 NULL,
222         },
223 };
224
225
226
227 /**********************************************************/
228 /* Opponent tests */
229
230 /* this structure is filled when cmd_opponent is parsed successfully */
231 struct cmd_opponent_result {
232         fixed_string_t arg0;
233         fixed_string_t arg1;
234         int32_t arg2;
235         int32_t arg3;
236 };
237
238 /* function called when cmd_opponent is parsed successfully */
239 static void cmd_opponent_parsed(void *parsed_result, void *data)
240 {
241         int16_t x,y,d,a;
242
243         if (get_opponent_xyda(&x, &y, &d, &a))
244                 printf_P(PSTR("No opponent\r\n"));
245         else
246                 printf_P(PSTR("x=%d y=%d, d=%d a=%d\r\n"), x, y, d, a);
247 }
248
249 prog_char str_opponent_arg0[] = "opponent";
250 parse_pgm_token_string_t cmd_opponent_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_opponent_result, arg0, str_opponent_arg0);
251 prog_char str_opponent_arg1[] = "show";
252 parse_pgm_token_string_t cmd_opponent_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_opponent_result, arg1, str_opponent_arg1);
253
254 prog_char help_opponent[] = "Show (x,y) opponent";
255 parse_pgm_inst_t cmd_opponent = {
256         .f = cmd_opponent_parsed,  /* function to call */
257         .data = NULL,      /* 2nd arg of func */
258         .help_str = help_opponent,
259         .tokens = {        /* token list, NULL terminated */
260                 (prog_void *)&cmd_opponent_arg0,
261                 (prog_void *)&cmd_opponent_arg1,
262                 NULL,
263         },
264 };
265
266
267 prog_char str_opponent_arg1_set[] = "set";
268 parse_pgm_token_string_t cmd_opponent_arg1_set = TOKEN_STRING_INITIALIZER(struct cmd_opponent_result, arg1, str_opponent_arg1_set);
269 parse_pgm_token_num_t cmd_opponent_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_opponent_result, arg2, INT32);
270 parse_pgm_token_num_t cmd_opponent_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_opponent_result, arg3, INT32);
271
272 prog_char help_opponent_set[] = "Set (x,y) opponent";
273 parse_pgm_inst_t cmd_opponent_set = {
274         .f = cmd_opponent_parsed,  /* function to call */
275         .data = NULL,      /* 2nd arg of func */
276         .help_str = help_opponent_set,
277         .tokens = {        /* token list, NULL terminated */
278                 (prog_void *)&cmd_opponent_arg0,
279                 (prog_void *)&cmd_opponent_arg1_set,
280                 (prog_void *)&cmd_opponent_arg2,
281                 (prog_void *)&cmd_opponent_arg3,
282                 NULL,
283         },
284 };
285
286
287 /**********************************************************/
288 /* Start */
289
290 /* this structure is filled when cmd_start is parsed successfully */
291 struct cmd_start_result {
292         fixed_string_t arg0;
293         fixed_string_t color;
294         fixed_string_t debug;
295 };
296
297 /* function called when cmd_start is parsed successfully */
298 static void cmd_start_parsed(void *parsed_result, void *data)
299 {
300         struct cmd_start_result *res = parsed_result;
301         uint8_t old_level = gen.log_level;
302
303         gen.logs[NB_LOGS] = E_USER_STRAT;
304         if (!strcmp_P(res->debug, PSTR("debug"))) {
305                 strat_db.dump_enabled = 1;
306                 gen.log_level = 5;
307         }
308         else {
309                 strat_db.dump_enabled = 0;
310                 gen.log_level = 0;
311         }
312
313         if (!strcmp_P(res->color, PSTR("yellow"))) {
314                 mainboard.our_color = I2C_COLOR_YELLOW;
315                 i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_YELLOW);
316                 i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_YELLOW);
317         }
318         else if (!strcmp_P(res->color, PSTR("blue"))) {
319                 mainboard.our_color = I2C_COLOR_BLUE;
320                 i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_BLUE);
321                 i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_BLUE);
322         }
323
324         strat_start();
325
326         gen.logs[NB_LOGS] = 0;
327         gen.log_level = old_level;
328 }
329
330 prog_char str_start_arg0[] = "start";
331 parse_pgm_token_string_t cmd_start_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_start_result, arg0, str_start_arg0);
332 prog_char str_start_color[] = "blue#yellow";
333 parse_pgm_token_string_t cmd_start_color = TOKEN_STRING_INITIALIZER(struct cmd_start_result, color, str_start_color);
334 prog_char str_start_debug[] = "debug#match";
335 parse_pgm_token_string_t cmd_start_debug = TOKEN_STRING_INITIALIZER(struct cmd_start_result, debug, str_start_debug);
336
337 prog_char help_start[] = "Start the robot";
338 parse_pgm_inst_t cmd_start = {
339         .f = cmd_start_parsed,  /* function to call */
340         .data = NULL,      /* 2nd arg of func */
341         .help_str = help_start,
342         .tokens = {        /* token list, NULL terminated */
343                 (prog_void *)&cmd_start_arg0,
344                 (prog_void *)&cmd_start_color,
345                 (prog_void *)&cmd_start_debug,
346                 NULL,
347         },
348 };
349
350
351
352 /**********************************************************/
353 /* Interact */
354
355 /* this structure is filled when cmd_interact is parsed successfully */
356 struct cmd_interact_result {
357         fixed_string_t arg0;
358 };
359
360 static void print_cs(void)
361 {
362         printf_P(PSTR("cons_d=% .8"PRIi32" cons_a=% .8"PRIi32" fil_d=% .8"PRIi32" fil_a=% .8"PRIi32" "
363                       "err_d=% .8"PRIi32" err_a=% .8"PRIi32" out_d=% .8"PRIi32" out_a=% .8"PRIi32"\r\n"),
364                  cs_get_consign(&mainboard.distance.cs),
365                  cs_get_consign(&mainboard.angle.cs),
366                  cs_get_filtered_consign(&mainboard.distance.cs),
367                  cs_get_filtered_consign(&mainboard.angle.cs),
368                  cs_get_error(&mainboard.distance.cs),
369                  cs_get_error(&mainboard.angle.cs),
370                  cs_get_out(&mainboard.distance.cs),
371                  cs_get_out(&mainboard.angle.cs));
372 }
373
374 static void print_pos(void)
375 {
376         printf_P(PSTR("x=% .8d y=% .8d a=% .8d\r\n"),
377                  position_get_x_s16(&mainboard.pos),
378                  position_get_y_s16(&mainboard.pos),
379                  position_get_a_deg_s16(&mainboard.pos));
380 }
381
382 static void print_time(void)
383 {
384         printf_P(PSTR("time %d\r\n"),time_get_s());
385 }
386
387
388 static void print_sensors(void)
389 {
390 #ifdef notyet
391         if (sensor_start_switch())
392                 printf_P(PSTR("Start switch | "));
393         else
394                 printf_P(PSTR("             | "));
395
396         if (IR_DISP_SENSOR())
397                 printf_P(PSTR("IR disp | "));
398         else
399                 printf_P(PSTR("        | "));
400
401         printf_P(PSTR("\r\n"));
402 #endif
403 }
404
405 static void print_pid(void)
406 {
407         printf_P(PSTR("P=% .8"PRIi32" I=% .8"PRIi32" D=% .8"PRIi32" out=% .8"PRIi32" | "
408                       "P=% .8"PRIi32" I=% .8"PRIi32" D=% .8"PRIi32" out=% .8"PRIi32"\r\n"),
409                  pid_get_value_in(&mainboard.distance.pid) * pid_get_gain_P(&mainboard.distance.pid),
410                  pid_get_value_I(&mainboard.distance.pid) * pid_get_gain_I(&mainboard.distance.pid),
411                  pid_get_value_D(&mainboard.distance.pid) * pid_get_gain_D(&mainboard.distance.pid),
412                  pid_get_value_out(&mainboard.distance.pid),
413                  pid_get_value_in(&mainboard.angle.pid) * pid_get_gain_P(&mainboard.angle.pid),
414                  pid_get_value_I(&mainboard.angle.pid) * pid_get_gain_I(&mainboard.angle.pid),
415                  pid_get_value_D(&mainboard.angle.pid) * pid_get_gain_D(&mainboard.angle.pid),
416                  pid_get_value_out(&mainboard.angle.pid));
417 }
418
419 #define PRINT_POS       (1<<0)
420 #define PRINT_PID       (1<<1)
421 #define PRINT_CS        (1<<2)
422 #define PRINT_SENSORS   (1<<3)
423 #define PRINT_TIME      (1<<4)
424 #define PRINT_BLOCKING  (1<<5)
425
426 static void cmd_interact_parsed(void * parsed_result, void * data)
427 {
428         int c;
429         int8_t cmd;
430         uint8_t print = 0;
431         struct vt100 vt100;
432
433         vt100_init(&vt100);
434
435         printf_P(PSTR("Display debugs:\r\n"
436                       "  1:pos\r\n"
437                       "  2:pid\r\n"
438                       "  3:cs\r\n"
439                       "  4:sensors\r\n"
440                       "  5:time\r\n"
441                       /* "  6:blocking\r\n" */
442                       "Commands:\r\n"
443                       "  arrows:move\r\n"
444                       "  space:stop\r\n"
445                       "  q:quit\r\n"));
446
447         /* stop motors */
448         mainboard.flags &= (~DO_CS);
449         pwm_set_and_save(LEFT_PWM, 0);
450         pwm_set_and_save(RIGHT_PWM, 0);
451
452         while(1) {
453                 if (print & PRINT_POS) {
454                         print_pos();
455                 }
456
457                 if (print & PRINT_PID) {
458                         print_pid();
459                 }
460
461                 if (print & PRINT_CS) {
462                         print_cs();
463                 }
464
465                 if (print & PRINT_SENSORS) {
466                         print_sensors();
467                 }
468
469                 if (print & PRINT_TIME) {
470                         print_time();
471                 }
472 /*              if (print & PRINT_BLOCKING) { */
473 /*                      printf_P(PSTR("%s %s blocking=%d\r\n"),  */
474 /*                               mainboard.blocking ? "BLOCK1":"      ", */
475 /*                               rs_is_blocked(&mainboard.rs) ? "BLOCK2":"      ", */
476 /*                               rs_get_blocking(&mainboard.rs)); */
477 /*              } */
478
479                 c = cmdline_getchar();
480                 if (c == -1) {
481                         wait_ms(10);
482                         continue;
483                 }
484                 cmd = vt100_parser(&vt100, c);
485                 if (cmd == -2) {
486                         wait_ms(10);
487                         continue;
488                 }
489
490                 if (cmd == -1) {
491                         switch(c) {
492                         case '1': print ^= PRINT_POS; break;
493                         case '2': print ^= PRINT_PID; break;
494                         case '3': print ^= PRINT_CS; break;
495                         case '4': print ^= PRINT_SENSORS; break;
496                         case '5': print ^= PRINT_TIME; break;
497                         case '6': print ^= PRINT_BLOCKING; break;
498
499                         case 'q':
500                                 if (mainboard.flags & DO_CS)
501                                         strat_hardstop();
502                                 pwm_set_and_save(LEFT_PWM, 0);
503                                 pwm_set_and_save(RIGHT_PWM, 0);
504                                 return;
505                         case ' ':
506                                 pwm_set_and_save(LEFT_PWM, 0);
507                                 pwm_set_and_save(RIGHT_PWM, 0);
508                                 break;
509                         default:
510                                 break;
511                         }
512                 }
513                 else {
514                         switch(cmd) {
515                         case KEY_UP_ARR:
516                                 pwm_set_and_save(LEFT_PWM, 1200);
517                                 pwm_set_and_save(RIGHT_PWM, 1200);
518                                 break;
519                         case KEY_LEFT_ARR:
520                                 pwm_set_and_save(LEFT_PWM, -1200);
521                                 pwm_set_and_save(RIGHT_PWM, 1200);
522                                 break;
523                         case KEY_DOWN_ARR:
524                                 pwm_set_and_save(LEFT_PWM, -1200);
525                                 pwm_set_and_save(RIGHT_PWM, -1200);
526                                 break;
527                         case KEY_RIGHT_ARR:
528                                 pwm_set_and_save(LEFT_PWM, 1200);
529                                 pwm_set_and_save(RIGHT_PWM, -1200);
530                                 break;
531                         }
532                 }
533                 wait_ms(10);
534         }
535 }
536
537 prog_char str_interact_arg0[] = "interact";
538 parse_pgm_token_string_t cmd_interact_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_interact_result, arg0, str_interact_arg0);
539
540 prog_char help_interact[] = "Interactive mode";
541 parse_pgm_inst_t cmd_interact = {
542         .f = cmd_interact_parsed,  /* function to call */
543         .data = NULL,      /* 2nd arg of func */
544         .help_str = help_interact,
545         .tokens = {        /* token list, NULL terminated */
546                 (prog_void *)&cmd_interact_arg0,
547                 NULL,
548         },
549 };
550
551
552 /**********************************************************/
553 /* Color */
554
555 /* this structure is filled when cmd_color is parsed successfully */
556 struct cmd_color_result {
557         fixed_string_t arg0;
558         fixed_string_t color;
559 };
560
561 /* function called when cmd_color is parsed successfully */
562 static void cmd_color_parsed(void *parsed_result, void *data)
563 {
564 #ifdef HOST_VERSION
565         printf("not implemented\n");
566 #else
567         struct cmd_color_result *res = (struct cmd_color_result *) parsed_result;
568         if (!strcmp_P(res->color, PSTR("yellow"))) {
569                 mainboard.our_color = I2C_COLOR_YELLOW;
570                 i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_YELLOW);
571                 i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_YELLOW);
572         }
573         else if (!strcmp_P(res->color, PSTR("blue"))) {
574                 mainboard.our_color = I2C_COLOR_BLUE;
575                 i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_BLUE);
576                 i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_BLUE);
577         }
578         printf_P(PSTR("Done\r\n"));
579 #endif
580 }
581
582 prog_char str_color_arg0[] = "color";
583 parse_pgm_token_string_t cmd_color_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_color_result, arg0, str_color_arg0);
584 prog_char str_color_color[] = "blue#yellow";
585 parse_pgm_token_string_t cmd_color_color = TOKEN_STRING_INITIALIZER(struct cmd_color_result, color, str_color_color);
586
587 prog_char help_color[] = "Set our color";
588 parse_pgm_inst_t cmd_color = {
589         .f = cmd_color_parsed,  /* function to call */
590         .data = NULL,      /* 2nd arg of func */
591         .help_str = help_color,
592         .tokens = {        /* token list, NULL terminated */
593                 (prog_void *)&cmd_color_arg0,
594                 (prog_void *)&cmd_color_color,
595                 NULL,
596         },
597 };
598
599
600 /**********************************************************/
601 /* Rs tests */
602
603 /* this structure is filled when cmd_rs is parsed successfully */
604 struct cmd_rs_result {
605         fixed_string_t arg0;
606         fixed_string_t arg1;
607 };
608
609 /* function called when cmd_rs is parsed successfully */
610 static void cmd_rs_parsed(void *parsed_result, void *data)
611 {
612         //      struct cmd_rs_result *res = parsed_result;
613         do {
614                 printf_P(PSTR("angle cons=% .6"PRIi32" in=% .6"PRIi32" out=% .6"PRIi32" / "),
615                          cs_get_consign(&mainboard.angle.cs),
616                          cs_get_filtered_feedback(&mainboard.angle.cs),
617                          cs_get_out(&mainboard.angle.cs));
618                 printf_P(PSTR("distance cons=% .6"PRIi32" in=% .6"PRIi32" out=% .6"PRIi32" / "),
619                          cs_get_consign(&mainboard.distance.cs),
620                          cs_get_filtered_feedback(&mainboard.distance.cs),
621                          cs_get_out(&mainboard.distance.cs));
622                 printf_P(PSTR("l=% .4"PRIi32" r=% .4"PRIi32"\r\n"), mainboard.pwm_l,
623                          mainboard.pwm_r);
624                 wait_ms(100);
625         } while(!cmdline_keypressed());
626 }
627
628 prog_char str_rs_arg0[] = "rs";
629 parse_pgm_token_string_t cmd_rs_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_rs_result, arg0, str_rs_arg0);
630 prog_char str_rs_arg1[] = "show";
631 parse_pgm_token_string_t cmd_rs_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_rs_result, arg1, str_rs_arg1);
632
633 prog_char help_rs[] = "Show rs (robot system) values";
634 parse_pgm_inst_t cmd_rs = {
635         .f = cmd_rs_parsed,  /* function to call */
636         .data = NULL,      /* 2nd arg of func */
637         .help_str = help_rs,
638         .tokens = {        /* token list, NULL terminated */
639                 (prog_void *)&cmd_rs_arg0,
640                 (prog_void *)&cmd_rs_arg1,
641                 NULL,
642         },
643 };
644
645 /**********************************************************/
646 /* I2cdebug */
647
648 /* this structure is filled when cmd_i2cdebug is parsed successfully */
649 struct cmd_i2cdebug_result {
650         fixed_string_t arg0;
651 };
652
653 /* function called when cmd_i2cdebug is parsed successfully */
654 static void cmd_i2cdebug_parsed(void * parsed_result, void * data)
655 {
656 #ifdef HOST_VERSION
657         printf("not implemented\n");
658 #else
659         i2c_debug();
660         i2c_protocol_debug();
661 #endif
662 }
663
664 prog_char str_i2cdebug_arg0[] = "i2cdebug";
665 parse_pgm_token_string_t cmd_i2cdebug_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_i2cdebug_result, arg0, str_i2cdebug_arg0);
666
667 prog_char help_i2cdebug[] = "I2c debug infos";
668 parse_pgm_inst_t cmd_i2cdebug = {
669         .f = cmd_i2cdebug_parsed,  /* function to call */
670         .data = NULL,      /* 2nd arg of func */
671         .help_str = help_i2cdebug,
672         .tokens = {        /* token list, NULL terminated */
673                 (prog_void *)&cmd_i2cdebug_arg0,
674                 NULL,
675         },
676 };
677
678 /**********************************************************/
679 /* Cobboard_Show */
680
681 /* this structure is filled when cmd_cobboard_show is parsed successfully */
682 struct cmd_cobboard_show_result {
683         fixed_string_t arg0;
684         fixed_string_t arg1;
685 };
686
687 /* function called when cmd_cobboard_show is parsed successfully */
688 static void cmd_cobboard_show_parsed(void * parsed_result, void * data)
689 {
690 #ifdef HOST_VERSION
691         printf("not implemented\n");
692 #else
693         printf_P(PSTR("mode = %x\r\n"), cobboard.mode);
694         printf_P(PSTR("status = %x\r\n"), cobboard.status);
695         printf_P(PSTR("cob_count = %x\r\n"), cobboard.cob_count);
696         printf_P(PSTR("left_cobroller_speed = %d\r\n"), cobboard.left_cobroller_speed);
697         printf_P(PSTR("right_cobroller_speed = %d\r\n"), cobboard.right_cobroller_speed);
698 #endif
699 }
700
701 prog_char str_cobboard_show_arg0[] = "cobboard";
702 parse_pgm_token_string_t cmd_cobboard_show_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_show_result, arg0, str_cobboard_show_arg0);
703 prog_char str_cobboard_show_arg1[] = "show";
704 parse_pgm_token_string_t cmd_cobboard_show_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_show_result, arg1, str_cobboard_show_arg1);
705
706 prog_char help_cobboard_show[] = "show cobboard status";
707 parse_pgm_inst_t cmd_cobboard_show = {
708         .f = cmd_cobboard_show_parsed,  /* function to call */
709         .data = NULL,      /* 2nd arg of func */
710         .help_str = help_cobboard_show,
711         .tokens = {        /* token list, NULL terminated */
712                 (prog_void *)&cmd_cobboard_show_arg0,
713                 (prog_void *)&cmd_cobboard_show_arg1,
714                 NULL,
715         },
716 };
717
718 /**********************************************************/
719 /* Cobboard_Setmode1 */
720
721 /* this structure is filled when cmd_cobboard_setmode1 is parsed successfully */
722 struct cmd_cobboard_setmode1_result {
723         fixed_string_t arg0;
724         fixed_string_t arg1;
725 };
726
727 /* function called when cmd_cobboard_setmode1 is parsed successfully */
728 static void cmd_cobboard_setmode1_parsed(void *parsed_result, void *data)
729 {
730         struct cmd_cobboard_setmode1_result *res = parsed_result;
731
732         if (!strcmp_P(res->arg1, PSTR("init")))
733                 i2c_cobboard_set_mode(I2C_COBBOARD_MODE_INIT);
734         else if (!strcmp_P(res->arg1, PSTR("eject")))
735                 i2c_cobboard_set_mode(I2C_COBBOARD_MODE_EJECT);
736 }
737
738 prog_char str_cobboard_setmode1_arg0[] = "cobboard";
739 parse_pgm_token_string_t cmd_cobboard_setmode1_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode1_result, arg0, str_cobboard_setmode1_arg0);
740 prog_char str_cobboard_setmode1_arg1[] = "init#eject";
741 parse_pgm_token_string_t cmd_cobboard_setmode1_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode1_result, arg1, str_cobboard_setmode1_arg1);
742
743 prog_char help_cobboard_setmode1[] = "set cobboard mode (mode)";
744 parse_pgm_inst_t cmd_cobboard_setmode1 = {
745         .f = cmd_cobboard_setmode1_parsed,  /* function to call */
746         .data = NULL,      /* 2nd arg of func */
747         .help_str = help_cobboard_setmode1,
748         .tokens = {        /* token list, NULL terminated */
749                 (prog_void *)&cmd_cobboard_setmode1_arg0,
750                 (prog_void *)&cmd_cobboard_setmode1_arg1,
751                 NULL,
752         },
753 };
754
755 /**********************************************************/
756 /* Cobboard_Setmode2 */
757
758 /* this structure is filled when cmd_cobboard_setmode2 is parsed successfully */
759 struct cmd_cobboard_setmode2_result {
760         fixed_string_t arg0;
761         fixed_string_t arg1;
762         fixed_string_t arg2;
763 };
764
765 /* function called when cmd_cobboard_setmode2 is parsed successfully */
766 static void cmd_cobboard_setmode2_parsed(void *parsed_result, void *data)
767 {
768         struct cmd_cobboard_setmode2_result *res = parsed_result;
769         uint8_t side = I2C_LEFT_SIDE;
770
771         if (!strcmp_P(res->arg2, PSTR("left")))
772                 side = I2C_LEFT_SIDE;
773         else if (!strcmp_P(res->arg2, PSTR("right")))
774                 side = I2C_RIGHT_SIDE;
775
776         if (!strcmp_P(res->arg1, PSTR("deploy"))) {
777                 i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST);
778                 i2c_cobboard_deploy(side);
779         }
780         else if (!strcmp_P(res->arg1, PSTR("harvest"))) {
781                 i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST);
782                 i2c_cobboard_harvest(side);
783         }
784         else if (!strcmp_P(res->arg1, PSTR("pack"))) {
785                 i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST);
786                 i2c_cobboard_pack(side);
787         }
788 }
789
790 prog_char str_cobboard_setmode2_arg0[] = "cobboard";
791 parse_pgm_token_string_t cmd_cobboard_setmode2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode2_result, arg0, str_cobboard_setmode2_arg0);
792 prog_char str_cobboard_setmode2_arg1[] = "harvest#deploy#pack";
793 parse_pgm_token_string_t cmd_cobboard_setmode2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode2_result, arg1, str_cobboard_setmode2_arg1);
794 prog_char str_cobboard_setmode2_arg2[] = "left#right";
795 parse_pgm_token_string_t cmd_cobboard_setmode2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode2_result, arg2, str_cobboard_setmode2_arg2);
796
797 prog_char help_cobboard_setmode2[] = "set cobboard mode (mode, side)";
798 parse_pgm_inst_t cmd_cobboard_setmode2 = {
799         .f = cmd_cobboard_setmode2_parsed,  /* function to call */
800         .data = NULL,      /* 2nd arg of func */
801         .help_str = help_cobboard_setmode2,
802         .tokens = {        /* token list, NULL terminated */
803                 (prog_void *)&cmd_cobboard_setmode2_arg0,
804                 (prog_void *)&cmd_cobboard_setmode2_arg1,
805                 (prog_void *)&cmd_cobboard_setmode2_arg2,
806                 NULL,
807         },
808 };
809
810 /**********************************************************/
811 /* Cobboard_Setmode3 */
812
813 /* this structure is filled when cmd_cobboard_setmode3 is parsed successfully */
814 struct cmd_cobboard_setmode3_result {
815         fixed_string_t arg0;
816         fixed_string_t arg1;
817         uint8_t level;
818 };
819
820 /* function called when cmd_cobboard_setmode3 is parsed successfully */
821 static void cmd_cobboard_setmode3_parsed(void *parsed_result, void *data)
822 {
823         struct cmd_cobboard_setmode3_result *res = parsed_result;
824         if (!strcmp_P(res->arg1, PSTR("xxx")))
825                 printf("faux\r\n");
826 }
827
828 prog_char str_cobboard_setmode3_arg0[] = "cobboard";
829 parse_pgm_token_string_t cmd_cobboard_setmode3_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode3_result, arg0, str_cobboard_setmode3_arg0);
830 prog_char str_cobboard_setmode3_arg1[] = "xxx";
831 parse_pgm_token_string_t cmd_cobboard_setmode3_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode3_result, arg1, str_cobboard_setmode3_arg1);
832 parse_pgm_token_num_t cmd_cobboard_setmode3_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_cobboard_setmode3_result, level, UINT8);
833
834 prog_char help_cobboard_setmode3[] = "set cobboard mode (mode, level)";
835 parse_pgm_inst_t cmd_cobboard_setmode3 = {
836         .f = cmd_cobboard_setmode3_parsed,  /* function to call */
837         .data = NULL,      /* 2nd arg of func */
838         .help_str = help_cobboard_setmode3,
839         .tokens = {        /* token list, NULL terminated */
840                 (prog_void *)&cmd_cobboard_setmode3_arg0,
841                 (prog_void *)&cmd_cobboard_setmode3_arg1,
842                 (prog_void *)&cmd_cobboard_setmode3_arg2,
843                 NULL,
844         },
845 };
846
847 /**********************************************************/
848 /* Ballboard_Show */
849
850 /* this structure is filled when cmd_ballboard_show is parsed successfully */
851 struct cmd_ballboard_show_result {
852         fixed_string_t arg0;
853         fixed_string_t arg1;
854 };
855
856 /* function called when cmd_ballboard_show is parsed successfully */
857 static void cmd_ballboard_show_parsed(void * parsed_result, void * data)
858 {
859 #ifdef HOST_VERSION
860         printf("not implemented\n");
861 #else
862         printf_P(PSTR("mode = %x\r\n"), ballboard.mode);
863         printf_P(PSTR("status = %x\r\n"), ballboard.status);
864         printf_P(PSTR("ball_count = %d\r\n"), ballboard.ball_count);
865 #endif
866 }
867
868 prog_char str_ballboard_show_arg0[] = "ballboard";
869 parse_pgm_token_string_t cmd_ballboard_show_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_show_result, arg0, str_ballboard_show_arg0);
870 prog_char str_ballboard_show_arg1[] = "show";
871 parse_pgm_token_string_t cmd_ballboard_show_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_show_result, arg1, str_ballboard_show_arg1);
872
873 prog_char help_ballboard_show[] = "show ballboard status";
874 parse_pgm_inst_t cmd_ballboard_show = {
875         .f = cmd_ballboard_show_parsed,  /* function to call */
876         .data = NULL,      /* 2nd arg of func */
877         .help_str = help_ballboard_show,
878         .tokens = {        /* token list, NULL terminated */
879                 (prog_void *)&cmd_ballboard_show_arg0,
880                 (prog_void *)&cmd_ballboard_show_arg1,
881                 NULL,
882         },
883 };
884
885 /**********************************************************/
886 /* Ballboard_Setmode1 */
887
888 /* this structure is filled when cmd_ballboard_setmode1 is parsed successfully */
889 struct cmd_ballboard_setmode1_result {
890         fixed_string_t arg0;
891         fixed_string_t arg1;
892 };
893
894 /* function called when cmd_ballboard_setmode1 is parsed successfully */
895 static void cmd_ballboard_setmode1_parsed(void *parsed_result, void *data)
896 {
897         struct cmd_ballboard_setmode1_result *res = parsed_result;
898
899         if (!strcmp_P(res->arg1, PSTR("init")))
900                 i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_INIT);
901         else if (!strcmp_P(res->arg1, PSTR("off")))
902                 i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_OFF);
903         else if (!strcmp_P(res->arg1, PSTR("eject")))
904                 i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_EJECT);
905         else if (!strcmp_P(res->arg1, PSTR("harvest")))
906                 i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_HARVEST);
907
908         /* other commands */
909 }
910
911 prog_char str_ballboard_setmode1_arg0[] = "ballboard";
912 parse_pgm_token_string_t cmd_ballboard_setmode1_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode1_result, arg0, str_ballboard_setmode1_arg0);
913 prog_char str_ballboard_setmode1_arg1[] = "init#eject#harvest#off";
914 parse_pgm_token_string_t cmd_ballboard_setmode1_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode1_result, arg1, str_ballboard_setmode1_arg1);
915
916 prog_char help_ballboard_setmode1[] = "set ballboard mode (mode)";
917 parse_pgm_inst_t cmd_ballboard_setmode1 = {
918         .f = cmd_ballboard_setmode1_parsed,  /* function to call */
919         .data = NULL,      /* 2nd arg of func */
920         .help_str = help_ballboard_setmode1,
921         .tokens = {        /* token list, NULL terminated */
922                 (prog_void *)&cmd_ballboard_setmode1_arg0,
923                 (prog_void *)&cmd_ballboard_setmode1_arg1,
924                 NULL,
925         },
926 };
927
928 /**********************************************************/
929 /* Ballboard_Setmode2 */
930
931 /* this structure is filled when cmd_ballboard_setmode2 is parsed successfully */
932 struct cmd_ballboard_setmode2_result {
933         fixed_string_t arg0;
934         fixed_string_t arg1;
935         fixed_string_t arg2;
936 };
937
938 /* function called when cmd_ballboard_setmode2 is parsed successfully */
939 static void cmd_ballboard_setmode2_parsed(void * parsed_result, void * data)
940 {
941         struct cmd_ballboard_setmode2_result *res = parsed_result;
942         uint8_t mode = I2C_BALLBOARD_MODE_INIT;
943
944         if (!strcmp_P(res->arg2, PSTR("left"))) {
945                 if (!strcmp_P(res->arg1, PSTR("prepare")))
946                         mode = I2C_BALLBOARD_MODE_PREP_L_FORK;
947                 else if (!strcmp_P(res->arg1, PSTR("take")))
948                         mode = I2C_BALLBOARD_MODE_TAKE_L_FORK;
949         }
950         else {
951                 if (!strcmp_P(res->arg1, PSTR("prepare")))
952                         mode = I2C_BALLBOARD_MODE_PREP_R_FORK;
953                 else if (!strcmp_P(res->arg1, PSTR("take")))
954                         mode = I2C_BALLBOARD_MODE_TAKE_R_FORK;
955         }
956         i2c_ballboard_set_mode(mode);
957 }
958
959 prog_char str_ballboard_setmode2_arg0[] = "ballboard";
960 parse_pgm_token_string_t cmd_ballboard_setmode2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode2_result, arg0, str_ballboard_setmode2_arg0);
961 prog_char str_ballboard_setmode2_arg1[] = "prepare#take";
962 parse_pgm_token_string_t cmd_ballboard_setmode2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode2_result, arg1, str_ballboard_setmode2_arg1);
963 prog_char str_ballboard_setmode2_arg2[] = "left#right";
964 parse_pgm_token_string_t cmd_ballboard_setmode2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode2_result, arg2, str_ballboard_setmode2_arg2);
965
966 prog_char help_ballboard_setmode2[] = "set ballboard mode (mode, side)";
967 parse_pgm_inst_t cmd_ballboard_setmode2 = {
968         .f = cmd_ballboard_setmode2_parsed,  /* function to call */
969         .data = NULL,      /* 2nd arg of func */
970         .help_str = help_ballboard_setmode2,
971         .tokens = {        /* token list, NULL terminated */
972                 (prog_void *)&cmd_ballboard_setmode2_arg0,
973                 (prog_void *)&cmd_ballboard_setmode2_arg1,
974                 (prog_void *)&cmd_ballboard_setmode2_arg2,
975                 NULL,
976         },
977 };
978
979 /**********************************************************/
980 /* Ballboard_Setmode3 */
981
982 /* this structure is filled when cmd_ballboard_setmode3 is parsed successfully */
983 struct cmd_ballboard_setmode3_result {
984         fixed_string_t arg0;
985         fixed_string_t arg1;
986         uint8_t level;
987 };
988
989 /* function called when cmd_ballboard_setmode3 is parsed successfully */
990 static void cmd_ballboard_setmode3_parsed(void *parsed_result, void *data)
991 {
992         struct cmd_ballboard_setmode3_result *res = parsed_result;
993         if (!strcmp_P(res->arg1, PSTR("xxx")))
994                 printf("faux\r\n");
995 }
996
997 prog_char str_ballboard_setmode3_arg0[] = "ballboard";
998 parse_pgm_token_string_t cmd_ballboard_setmode3_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode3_result, arg0, str_ballboard_setmode3_arg0);
999 prog_char str_ballboard_setmode3_arg1[] = "xxx";
1000 parse_pgm_token_string_t cmd_ballboard_setmode3_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode3_result, arg1, str_ballboard_setmode3_arg1);
1001 parse_pgm_token_num_t cmd_ballboard_setmode3_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_ballboard_setmode3_result, level, UINT8);
1002
1003 prog_char help_ballboard_setmode3[] = "set ballboard mode (mode, level)";
1004 parse_pgm_inst_t cmd_ballboard_setmode3 = {
1005         .f = cmd_ballboard_setmode3_parsed,  /* function to call */
1006         .data = NULL,      /* 2nd arg of func */
1007         .help_str = help_ballboard_setmode3,
1008         .tokens = {        /* token list, NULL terminated */
1009                 (prog_void *)&cmd_ballboard_setmode3_arg0,
1010                 (prog_void *)&cmd_ballboard_setmode3_arg1,
1011                 (prog_void *)&cmd_ballboard_setmode3_arg2,
1012                 NULL,
1013         },
1014 };
1015
1016 /**********************************************************/
1017 /* Servo_Balls */
1018
1019 /* this structure is filled when cmd_servo_balls is parsed successfully */
1020 struct cmd_servo_balls_result {
1021         fixed_string_t arg0;
1022         fixed_string_t arg1;
1023 };
1024
1025 /* function called when cmd_servo_balls is parsed successfully */
1026 static void cmd_servo_balls_parsed(void *parsed_result,
1027                                    __attribute__((unused)) void *data)
1028 {
1029         struct cmd_servo_balls_result *res = parsed_result;
1030
1031         if (!strcmp_P(res->arg1, PSTR("deploy")))
1032                 support_balls_deploy();
1033         else if (!strcmp_P(res->arg1, PSTR("pack")))
1034                 support_balls_pack();
1035 }
1036
1037 prog_char str_servo_balls_arg0[] = "support_balls";
1038 parse_pgm_token_string_t cmd_servo_balls_arg0 =
1039         TOKEN_STRING_INITIALIZER(struct cmd_servo_balls_result, arg0, str_servo_balls_arg0);
1040 prog_char str_servo_balls_arg1[] = "deploy#pack";
1041 parse_pgm_token_string_t cmd_servo_balls_arg1 =
1042         TOKEN_STRING_INITIALIZER(struct cmd_servo_balls_result, arg1, str_servo_balls_arg1);
1043
1044 prog_char help_servo_balls[] = "control support balls";
1045 parse_pgm_inst_t cmd_servo_balls = {
1046         .f = cmd_servo_balls_parsed,  /* function to call */
1047         .data = NULL,      /* 2nd arg of func */
1048         .help_str = help_servo_balls,
1049         .tokens = {        /* token list, NULL terminated */
1050                 (prog_void *)&cmd_servo_balls_arg0,
1051                 (prog_void *)&cmd_servo_balls_arg1,
1052                 NULL,
1053         },
1054 };
1055
1056 /**********************************************************/
1057 /* Clitoid */
1058
1059 /* this structure is filled when cmd_clitoid is parsed successfully */
1060 struct cmd_clitoid_result {
1061         fixed_string_t arg0;
1062         float alpha_deg;
1063         float beta_deg;
1064         float R_mm;
1065         float Vd;
1066         float Amax;
1067         float d_inter_mm;
1068 };
1069
1070 /* function called when cmd_test is parsed successfully */
1071 static void cmd_clitoid_parsed(void *parsed_result, void *data)
1072 {
1073         struct cmd_clitoid_result *res = parsed_result;
1074 /*      clitoid(res->alpha_deg, res->beta_deg, res->R_mm, */
1075 /*              res->Vd, res->Amax, res->d_inter_mm); */
1076         double x = position_get_x_double(&mainboard.pos);
1077         double y = position_get_y_double(&mainboard.pos);
1078         double a = position_get_a_rad_double(&mainboard.pos);
1079
1080         strat_set_speed(res->Vd, SPEED_ANGLE_FAST);
1081         trajectory_clitoid(&mainboard.traj, x, y, a, 150.,
1082                            res->alpha_deg, res->beta_deg, res->R_mm,
1083                            res->d_inter_mm);
1084 }
1085
1086 prog_char str_clitoid_arg0[] = "clitoid";
1087 parse_pgm_token_string_t cmd_clitoid_arg0 =
1088         TOKEN_STRING_INITIALIZER(struct cmd_clitoid_result,
1089                                  arg0, str_clitoid_arg0);
1090 parse_pgm_token_num_t cmd_clitoid_alpha_deg =
1091         TOKEN_NUM_INITIALIZER(struct cmd_clitoid_result,
1092                               alpha_deg, FLOAT);
1093 parse_pgm_token_num_t cmd_clitoid_beta_deg =
1094         TOKEN_NUM_INITIALIZER(struct cmd_clitoid_result,
1095                               beta_deg, FLOAT);
1096 parse_pgm_token_num_t cmd_clitoid_R_mm =
1097         TOKEN_NUM_INITIALIZER(struct cmd_clitoid_result,
1098                               R_mm, FLOAT);
1099 parse_pgm_token_num_t cmd_clitoid_Vd =
1100         TOKEN_NUM_INITIALIZER(struct cmd_clitoid_result,
1101                               Vd, FLOAT);
1102 parse_pgm_token_num_t cmd_clitoid_Amax =
1103         TOKEN_NUM_INITIALIZER(struct cmd_clitoid_result,
1104                               Amax, FLOAT);
1105 parse_pgm_token_num_t cmd_clitoid_d_inter_mm =
1106         TOKEN_NUM_INITIALIZER(struct cmd_clitoid_result,
1107                               d_inter_mm, FLOAT);
1108
1109 prog_char help_clitoid[] = "do a clitoid (alpha, beta, R, Vd, Amax, d_inter)";
1110 parse_pgm_inst_t cmd_clitoid = {
1111         .f = cmd_clitoid_parsed,  /* function to call */
1112         .data = NULL,      /* 2nd arg of func */
1113         .help_str = help_clitoid,
1114         .tokens = {        /* token list, NULL terminated */
1115                 (prog_void *)&cmd_clitoid_arg0,
1116                 (prog_void *)&cmd_clitoid_alpha_deg,
1117                 (prog_void *)&cmd_clitoid_beta_deg,
1118                 (prog_void *)&cmd_clitoid_R_mm,
1119                 (prog_void *)&cmd_clitoid_Vd,
1120                 (prog_void *)&cmd_clitoid_Amax,
1121                 (prog_void *)&cmd_clitoid_d_inter_mm,
1122                 NULL,
1123         },
1124 };
1125
1126 /**********************************************************/
1127 /* Time_Monitor */
1128
1129 /* this structure is filled when cmd_time_monitor is parsed successfully */
1130 struct cmd_time_monitor_result {
1131         fixed_string_t arg0;
1132         fixed_string_t arg1;
1133 };
1134
1135 /* function called when cmd_time_monitor is parsed successfully */
1136 static void cmd_time_monitor_parsed(void *parsed_result, void *data)
1137 {
1138 #ifndef HOST_VERSION
1139         struct cmd_time_monitor_result *res = parsed_result;
1140         uint16_t seconds;
1141
1142         if (!strcmp_P(res->arg1, PSTR("reset"))) {
1143                 eeprom_write_word(EEPROM_TIME_ADDRESS, 0);
1144         }
1145         seconds = eeprom_read_word(EEPROM_TIME_ADDRESS);
1146         printf_P(PSTR("Running since %d mn %d\r\n"), seconds/60, seconds%60);
1147 #endif
1148 }
1149
1150 prog_char str_time_monitor_arg0[] = "time_monitor";
1151 parse_pgm_token_string_t cmd_time_monitor_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_time_monitor_result, arg0, str_time_monitor_arg0);
1152 prog_char str_time_monitor_arg1[] = "show#reset";
1153 parse_pgm_token_string_t cmd_time_monitor_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_time_monitor_result, arg1, str_time_monitor_arg1);
1154
1155 prog_char help_time_monitor[] = "Show since how long we are running";
1156 parse_pgm_inst_t cmd_time_monitor = {
1157         .f = cmd_time_monitor_parsed,  /* function to call */
1158         .data = NULL,      /* 2nd arg of func */
1159         .help_str = help_time_monitor,
1160         .tokens = {        /* token list, NULL terminated */
1161                 (prog_void *)&cmd_time_monitor_arg0,
1162                 (prog_void *)&cmd_time_monitor_arg1,
1163                 NULL,
1164         },
1165 };
1166
1167 /**********************************************************/
1168 /* Test */
1169
1170 /* this structure is filled when cmd_test is parsed successfully */
1171 struct cmd_test_result {
1172         fixed_string_t arg0;
1173         int32_t radius;
1174         int32_t dist;
1175 };
1176
1177 /* function called when cmd_test is parsed successfully */
1178 static void cmd_test_parsed(void *parsed_result, void *data)
1179 {
1180         strat_db.dump_enabled = 1;
1181         strat_db_dump(__FUNCTION__);
1182
1183         corn_set_color(strat_db.corn_table[0], I2C_COB_BLACK);
1184         strat_db_dump(__FUNCTION__);
1185
1186         corn_set_color(strat_db.corn_table[3], I2C_COB_WHITE);
1187         strat_db_dump(__FUNCTION__);
1188         corn_set_color(strat_db.corn_table[4], I2C_COB_WHITE);
1189         strat_db_dump(__FUNCTION__);
1190         corn_set_color(strat_db.corn_table[5], I2C_COB_WHITE);
1191         strat_db_dump(__FUNCTION__);
1192 }
1193
1194 prog_char str_test_arg0[] = "test";
1195 parse_pgm_token_string_t cmd_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_test_result, arg0, str_test_arg0);
1196 parse_pgm_token_num_t cmd_test_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_test_result, radius, INT32);
1197 parse_pgm_token_num_t cmd_test_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_test_result, dist, INT32);
1198
1199 prog_char help_test[] = "Test function";
1200 parse_pgm_inst_t cmd_test = {
1201         .f = cmd_test_parsed,  /* function to call */
1202         .data = NULL,      /* 2nd arg of func */
1203         .help_str = help_test,
1204         .tokens = {        /* token list, NULL terminated */
1205                 (prog_void *)&cmd_test_arg0,
1206                 NULL,
1207         },
1208 };