13a327ae95ba9309d38daccf9be04e31ae81e82d
[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_autoharvest(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         else if (!strcmp_P(res->arg1, PSTR("deploy_nomove"))) {
789                 i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST);
790                 i2c_cobboard_deploy_nomove(side);
791         }
792         else if (!strcmp_P(res->arg1, PSTR("harvest_nomove"))) {
793                 i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST);
794                 i2c_cobboard_autoharvest_nomove(side);
795         }
796 }
797
798 prog_char str_cobboard_setmode2_arg0[] = "cobboard";
799 parse_pgm_token_string_t cmd_cobboard_setmode2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode2_result, arg0, str_cobboard_setmode2_arg0);
800 prog_char str_cobboard_setmode2_arg1[] = "harvest#deploy#pack#harvest_nomove#deploy_nomove";
801 parse_pgm_token_string_t cmd_cobboard_setmode2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode2_result, arg1, str_cobboard_setmode2_arg1);
802 prog_char str_cobboard_setmode2_arg2[] = "left#right";
803 parse_pgm_token_string_t cmd_cobboard_setmode2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode2_result, arg2, str_cobboard_setmode2_arg2);
804
805 prog_char help_cobboard_setmode2[] = "set cobboard mode (mode, side)";
806 parse_pgm_inst_t cmd_cobboard_setmode2 = {
807         .f = cmd_cobboard_setmode2_parsed,  /* function to call */
808         .data = NULL,      /* 2nd arg of func */
809         .help_str = help_cobboard_setmode2,
810         .tokens = {        /* token list, NULL terminated */
811                 (prog_void *)&cmd_cobboard_setmode2_arg0,
812                 (prog_void *)&cmd_cobboard_setmode2_arg1,
813                 (prog_void *)&cmd_cobboard_setmode2_arg2,
814                 NULL,
815         },
816 };
817
818 /**********************************************************/
819 /* Cobboard_Setmode3 */
820
821 /* this structure is filled when cmd_cobboard_setmode3 is parsed successfully */
822 struct cmd_cobboard_setmode3_result {
823         fixed_string_t arg0;
824         fixed_string_t arg1;
825         uint8_t level;
826 };
827
828 /* function called when cmd_cobboard_setmode3 is parsed successfully */
829 static void cmd_cobboard_setmode3_parsed(void *parsed_result, void *data)
830 {
831         struct cmd_cobboard_setmode3_result *res = parsed_result;
832         if (!strcmp_P(res->arg1, PSTR("xxx")))
833                 printf("faux\r\n");
834 }
835
836 prog_char str_cobboard_setmode3_arg0[] = "cobboard";
837 parse_pgm_token_string_t cmd_cobboard_setmode3_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode3_result, arg0, str_cobboard_setmode3_arg0);
838 prog_char str_cobboard_setmode3_arg1[] = "xxx";
839 parse_pgm_token_string_t cmd_cobboard_setmode3_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode3_result, arg1, str_cobboard_setmode3_arg1);
840 parse_pgm_token_num_t cmd_cobboard_setmode3_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_cobboard_setmode3_result, level, UINT8);
841
842 prog_char help_cobboard_setmode3[] = "set cobboard mode (mode, level)";
843 parse_pgm_inst_t cmd_cobboard_setmode3 = {
844         .f = cmd_cobboard_setmode3_parsed,  /* function to call */
845         .data = NULL,      /* 2nd arg of func */
846         .help_str = help_cobboard_setmode3,
847         .tokens = {        /* token list, NULL terminated */
848                 (prog_void *)&cmd_cobboard_setmode3_arg0,
849                 (prog_void *)&cmd_cobboard_setmode3_arg1,
850                 (prog_void *)&cmd_cobboard_setmode3_arg2,
851                 NULL,
852         },
853 };
854
855 /**********************************************************/
856 /* Ballboard_Show */
857
858 /* this structure is filled when cmd_ballboard_show is parsed successfully */
859 struct cmd_ballboard_show_result {
860         fixed_string_t arg0;
861         fixed_string_t arg1;
862 };
863
864 /* function called when cmd_ballboard_show is parsed successfully */
865 static void cmd_ballboard_show_parsed(void * parsed_result, void * data)
866 {
867 #ifdef HOST_VERSION
868         printf("not implemented\n");
869 #else
870         printf_P(PSTR("mode = %x\r\n"), ballboard.mode);
871         printf_P(PSTR("status = %x\r\n"), ballboard.status);
872         printf_P(PSTR("ball_count = %d\r\n"), ballboard.ball_count);
873 #endif
874 }
875
876 prog_char str_ballboard_show_arg0[] = "ballboard";
877 parse_pgm_token_string_t cmd_ballboard_show_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_show_result, arg0, str_ballboard_show_arg0);
878 prog_char str_ballboard_show_arg1[] = "show";
879 parse_pgm_token_string_t cmd_ballboard_show_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_show_result, arg1, str_ballboard_show_arg1);
880
881 prog_char help_ballboard_show[] = "show ballboard status";
882 parse_pgm_inst_t cmd_ballboard_show = {
883         .f = cmd_ballboard_show_parsed,  /* function to call */
884         .data = NULL,      /* 2nd arg of func */
885         .help_str = help_ballboard_show,
886         .tokens = {        /* token list, NULL terminated */
887                 (prog_void *)&cmd_ballboard_show_arg0,
888                 (prog_void *)&cmd_ballboard_show_arg1,
889                 NULL,
890         },
891 };
892
893 /**********************************************************/
894 /* Ballboard_Setmode1 */
895
896 /* this structure is filled when cmd_ballboard_setmode1 is parsed successfully */
897 struct cmd_ballboard_setmode1_result {
898         fixed_string_t arg0;
899         fixed_string_t arg1;
900 };
901
902 /* function called when cmd_ballboard_setmode1 is parsed successfully */
903 static void cmd_ballboard_setmode1_parsed(void *parsed_result, void *data)
904 {
905         struct cmd_ballboard_setmode1_result *res = parsed_result;
906
907         if (!strcmp_P(res->arg1, PSTR("init")))
908                 i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_INIT);
909         else if (!strcmp_P(res->arg1, PSTR("off")))
910                 i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_OFF);
911         else if (!strcmp_P(res->arg1, PSTR("eject")))
912                 i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_EJECT);
913         else if (!strcmp_P(res->arg1, PSTR("harvest")))
914                 i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_HARVEST);
915
916         /* other commands */
917 }
918
919 prog_char str_ballboard_setmode1_arg0[] = "ballboard";
920 parse_pgm_token_string_t cmd_ballboard_setmode1_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode1_result, arg0, str_ballboard_setmode1_arg0);
921 prog_char str_ballboard_setmode1_arg1[] = "init#eject#harvest#off";
922 parse_pgm_token_string_t cmd_ballboard_setmode1_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode1_result, arg1, str_ballboard_setmode1_arg1);
923
924 prog_char help_ballboard_setmode1[] = "set ballboard mode (mode)";
925 parse_pgm_inst_t cmd_ballboard_setmode1 = {
926         .f = cmd_ballboard_setmode1_parsed,  /* function to call */
927         .data = NULL,      /* 2nd arg of func */
928         .help_str = help_ballboard_setmode1,
929         .tokens = {        /* token list, NULL terminated */
930                 (prog_void *)&cmd_ballboard_setmode1_arg0,
931                 (prog_void *)&cmd_ballboard_setmode1_arg1,
932                 NULL,
933         },
934 };
935
936 /**********************************************************/
937 /* Ballboard_Setmode2 */
938
939 /* this structure is filled when cmd_ballboard_setmode2 is parsed successfully */
940 struct cmd_ballboard_setmode2_result {
941         fixed_string_t arg0;
942         fixed_string_t arg1;
943         fixed_string_t arg2;
944 };
945
946 /* function called when cmd_ballboard_setmode2 is parsed successfully */
947 static void cmd_ballboard_setmode2_parsed(void * parsed_result, void * data)
948 {
949         struct cmd_ballboard_setmode2_result *res = parsed_result;
950         uint8_t mode = I2C_BALLBOARD_MODE_INIT;
951
952         if (!strcmp_P(res->arg2, PSTR("left"))) {
953                 if (!strcmp_P(res->arg1, PSTR("prepare")))
954                         mode = I2C_BALLBOARD_MODE_PREP_L_FORK;
955                 else if (!strcmp_P(res->arg1, PSTR("take")))
956                         mode = I2C_BALLBOARD_MODE_TAKE_L_FORK;
957         }
958         else {
959                 if (!strcmp_P(res->arg1, PSTR("prepare")))
960                         mode = I2C_BALLBOARD_MODE_PREP_R_FORK;
961                 else if (!strcmp_P(res->arg1, PSTR("take")))
962                         mode = I2C_BALLBOARD_MODE_TAKE_R_FORK;
963         }
964         i2c_ballboard_set_mode(mode);
965 }
966
967 prog_char str_ballboard_setmode2_arg0[] = "ballboard";
968 parse_pgm_token_string_t cmd_ballboard_setmode2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode2_result, arg0, str_ballboard_setmode2_arg0);
969 prog_char str_ballboard_setmode2_arg1[] = "prepare#take";
970 parse_pgm_token_string_t cmd_ballboard_setmode2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode2_result, arg1, str_ballboard_setmode2_arg1);
971 prog_char str_ballboard_setmode2_arg2[] = "left#right";
972 parse_pgm_token_string_t cmd_ballboard_setmode2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode2_result, arg2, str_ballboard_setmode2_arg2);
973
974 prog_char help_ballboard_setmode2[] = "set ballboard mode (mode, side)";
975 parse_pgm_inst_t cmd_ballboard_setmode2 = {
976         .f = cmd_ballboard_setmode2_parsed,  /* function to call */
977         .data = NULL,      /* 2nd arg of func */
978         .help_str = help_ballboard_setmode2,
979         .tokens = {        /* token list, NULL terminated */
980                 (prog_void *)&cmd_ballboard_setmode2_arg0,
981                 (prog_void *)&cmd_ballboard_setmode2_arg1,
982                 (prog_void *)&cmd_ballboard_setmode2_arg2,
983                 NULL,
984         },
985 };
986
987 /**********************************************************/
988 /* Ballboard_Setmode3 */
989
990 /* this structure is filled when cmd_ballboard_setmode3 is parsed successfully */
991 struct cmd_ballboard_setmode3_result {
992         fixed_string_t arg0;
993         fixed_string_t arg1;
994         uint8_t level;
995 };
996
997 /* function called when cmd_ballboard_setmode3 is parsed successfully */
998 static void cmd_ballboard_setmode3_parsed(void *parsed_result, void *data)
999 {
1000         struct cmd_ballboard_setmode3_result *res = parsed_result;
1001         if (!strcmp_P(res->arg1, PSTR("xxx")))
1002                 printf("faux\r\n");
1003 }
1004
1005 prog_char str_ballboard_setmode3_arg0[] = "ballboard";
1006 parse_pgm_token_string_t cmd_ballboard_setmode3_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode3_result, arg0, str_ballboard_setmode3_arg0);
1007 prog_char str_ballboard_setmode3_arg1[] = "xxx";
1008 parse_pgm_token_string_t cmd_ballboard_setmode3_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode3_result, arg1, str_ballboard_setmode3_arg1);
1009 parse_pgm_token_num_t cmd_ballboard_setmode3_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_ballboard_setmode3_result, level, UINT8);
1010
1011 prog_char help_ballboard_setmode3[] = "set ballboard mode (mode, level)";
1012 parse_pgm_inst_t cmd_ballboard_setmode3 = {
1013         .f = cmd_ballboard_setmode3_parsed,  /* function to call */
1014         .data = NULL,      /* 2nd arg of func */
1015         .help_str = help_ballboard_setmode3,
1016         .tokens = {        /* token list, NULL terminated */
1017                 (prog_void *)&cmd_ballboard_setmode3_arg0,
1018                 (prog_void *)&cmd_ballboard_setmode3_arg1,
1019                 (prog_void *)&cmd_ballboard_setmode3_arg2,
1020                 NULL,
1021         },
1022 };
1023
1024 /**********************************************************/
1025 /* Servo_Balls */
1026
1027 /* this structure is filled when cmd_servo_balls is parsed successfully */
1028 struct cmd_servo_balls_result {
1029         fixed_string_t arg0;
1030         fixed_string_t arg1;
1031 };
1032
1033 /* function called when cmd_servo_balls is parsed successfully */
1034 static void cmd_servo_balls_parsed(void *parsed_result,
1035                                    __attribute__((unused)) void *data)
1036 {
1037         struct cmd_servo_balls_result *res = parsed_result;
1038
1039         if (!strcmp_P(res->arg1, PSTR("deploy")))
1040                 support_balls_deploy();
1041         else if (!strcmp_P(res->arg1, PSTR("pack")))
1042                 support_balls_pack();
1043 }
1044
1045 prog_char str_servo_balls_arg0[] = "support_balls";
1046 parse_pgm_token_string_t cmd_servo_balls_arg0 =
1047         TOKEN_STRING_INITIALIZER(struct cmd_servo_balls_result, arg0, str_servo_balls_arg0);
1048 prog_char str_servo_balls_arg1[] = "deploy#pack";
1049 parse_pgm_token_string_t cmd_servo_balls_arg1 =
1050         TOKEN_STRING_INITIALIZER(struct cmd_servo_balls_result, arg1, str_servo_balls_arg1);
1051
1052 prog_char help_servo_balls[] = "control support balls";
1053 parse_pgm_inst_t cmd_servo_balls = {
1054         .f = cmd_servo_balls_parsed,  /* function to call */
1055         .data = NULL,      /* 2nd arg of func */
1056         .help_str = help_servo_balls,
1057         .tokens = {        /* token list, NULL terminated */
1058                 (prog_void *)&cmd_servo_balls_arg0,
1059                 (prog_void *)&cmd_servo_balls_arg1,
1060                 NULL,
1061         },
1062 };
1063
1064 /**********************************************************/
1065 /* Clitoid */
1066
1067 /* this structure is filled when cmd_clitoid is parsed successfully */
1068 struct cmd_clitoid_result {
1069         fixed_string_t arg0;
1070         float alpha_deg;
1071         float beta_deg;
1072         float R_mm;
1073         float Vd;
1074         float Amax;
1075         float d_inter_mm;
1076 };
1077
1078 /* function called when cmd_test is parsed successfully */
1079 static void cmd_clitoid_parsed(void *parsed_result, void *data)
1080 {
1081         struct cmd_clitoid_result *res = parsed_result;
1082 /*      clitoid(res->alpha_deg, res->beta_deg, res->R_mm, */
1083 /*              res->Vd, res->Amax, res->d_inter_mm); */
1084         double x = position_get_x_double(&mainboard.pos);
1085         double y = position_get_y_double(&mainboard.pos);
1086         double a = position_get_a_rad_double(&mainboard.pos);
1087
1088         strat_set_speed(res->Vd, SPEED_ANGLE_FAST);
1089         trajectory_clitoid(&mainboard.traj, x, y, a, 150.,
1090                            res->alpha_deg, res->beta_deg, res->R_mm,
1091                            res->d_inter_mm);
1092 }
1093
1094 prog_char str_clitoid_arg0[] = "clitoid";
1095 parse_pgm_token_string_t cmd_clitoid_arg0 =
1096         TOKEN_STRING_INITIALIZER(struct cmd_clitoid_result,
1097                                  arg0, str_clitoid_arg0);
1098 parse_pgm_token_num_t cmd_clitoid_alpha_deg =
1099         TOKEN_NUM_INITIALIZER(struct cmd_clitoid_result,
1100                               alpha_deg, FLOAT);
1101 parse_pgm_token_num_t cmd_clitoid_beta_deg =
1102         TOKEN_NUM_INITIALIZER(struct cmd_clitoid_result,
1103                               beta_deg, FLOAT);
1104 parse_pgm_token_num_t cmd_clitoid_R_mm =
1105         TOKEN_NUM_INITIALIZER(struct cmd_clitoid_result,
1106                               R_mm, FLOAT);
1107 parse_pgm_token_num_t cmd_clitoid_Vd =
1108         TOKEN_NUM_INITIALIZER(struct cmd_clitoid_result,
1109                               Vd, FLOAT);
1110 parse_pgm_token_num_t cmd_clitoid_Amax =
1111         TOKEN_NUM_INITIALIZER(struct cmd_clitoid_result,
1112                               Amax, FLOAT);
1113 parse_pgm_token_num_t cmd_clitoid_d_inter_mm =
1114         TOKEN_NUM_INITIALIZER(struct cmd_clitoid_result,
1115                               d_inter_mm, FLOAT);
1116
1117 prog_char help_clitoid[] = "do a clitoid (alpha, beta, R, Vd, Amax, d_inter)";
1118 parse_pgm_inst_t cmd_clitoid = {
1119         .f = cmd_clitoid_parsed,  /* function to call */
1120         .data = NULL,      /* 2nd arg of func */
1121         .help_str = help_clitoid,
1122         .tokens = {        /* token list, NULL terminated */
1123                 (prog_void *)&cmd_clitoid_arg0,
1124                 (prog_void *)&cmd_clitoid_alpha_deg,
1125                 (prog_void *)&cmd_clitoid_beta_deg,
1126                 (prog_void *)&cmd_clitoid_R_mm,
1127                 (prog_void *)&cmd_clitoid_Vd,
1128                 (prog_void *)&cmd_clitoid_Amax,
1129                 (prog_void *)&cmd_clitoid_d_inter_mm,
1130                 NULL,
1131         },
1132 };
1133
1134 /**********************************************************/
1135 /* Time_Monitor */
1136
1137 /* this structure is filled when cmd_time_monitor is parsed successfully */
1138 struct cmd_time_monitor_result {
1139         fixed_string_t arg0;
1140         fixed_string_t arg1;
1141 };
1142
1143 /* function called when cmd_time_monitor is parsed successfully */
1144 static void cmd_time_monitor_parsed(void *parsed_result, void *data)
1145 {
1146 #ifndef HOST_VERSION
1147         struct cmd_time_monitor_result *res = parsed_result;
1148         uint16_t seconds;
1149
1150         if (!strcmp_P(res->arg1, PSTR("reset"))) {
1151                 eeprom_write_word(EEPROM_TIME_ADDRESS, 0);
1152         }
1153         seconds = eeprom_read_word(EEPROM_TIME_ADDRESS);
1154         printf_P(PSTR("Running since %d mn %d\r\n"), seconds/60, seconds%60);
1155 #endif
1156 }
1157
1158 prog_char str_time_monitor_arg0[] = "time_monitor";
1159 parse_pgm_token_string_t cmd_time_monitor_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_time_monitor_result, arg0, str_time_monitor_arg0);
1160 prog_char str_time_monitor_arg1[] = "show#reset";
1161 parse_pgm_token_string_t cmd_time_monitor_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_time_monitor_result, arg1, str_time_monitor_arg1);
1162
1163 prog_char help_time_monitor[] = "Show since how long we are running";
1164 parse_pgm_inst_t cmd_time_monitor = {
1165         .f = cmd_time_monitor_parsed,  /* function to call */
1166         .data = NULL,      /* 2nd arg of func */
1167         .help_str = help_time_monitor,
1168         .tokens = {        /* token list, NULL terminated */
1169                 (prog_void *)&cmd_time_monitor_arg0,
1170                 (prog_void *)&cmd_time_monitor_arg1,
1171                 NULL,
1172         },
1173 };
1174
1175
1176 /**********************************************************/
1177 /* Strat_Event */
1178
1179 /* this structure is filled when cmd_strat_event is parsed successfully */
1180 struct cmd_strat_event_result {
1181         fixed_string_t arg0;
1182         fixed_string_t arg1;
1183 };
1184
1185 /* function called when cmd_strat_event is parsed successfully */
1186 static void cmd_strat_event_parsed(void *parsed_result, void *data)
1187 {
1188         struct cmd_strat_event_result *res = parsed_result;
1189
1190         if (!strcmp_P(res->arg1, PSTR("on")))
1191                 strat_event_enable();
1192         else
1193                 strat_event_disable();
1194 }
1195
1196 prog_char str_strat_event_arg0[] = "strat_event";
1197 parse_pgm_token_string_t cmd_strat_event_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_event_result, arg0, str_strat_event_arg0);
1198 prog_char str_strat_event_arg1[] = "on#off";
1199 parse_pgm_token_string_t cmd_strat_event_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_event_result, arg1, str_strat_event_arg1);
1200
1201 prog_char help_strat_event[] = "Enable/disable strat_event callback";
1202 parse_pgm_inst_t cmd_strat_event = {
1203         .f = cmd_strat_event_parsed,  /* function to call */
1204         .data = NULL,      /* 2nd arg of func */
1205         .help_str = help_strat_event,
1206         .tokens = {        /* token list, NULL terminated */
1207                 (prog_void *)&cmd_strat_event_arg0,
1208                 (prog_void *)&cmd_strat_event_arg1,
1209                 NULL,
1210         },
1211 };
1212
1213 /**********************************************************/
1214 /* Test */
1215
1216 /* this structure is filled when cmd_test is parsed successfully */
1217 struct cmd_test_result {
1218         fixed_string_t arg0;
1219         int32_t radius;
1220         int32_t dist;
1221 };
1222
1223 /* function called when cmd_test is parsed successfully */
1224 static void cmd_test_parsed(void *parsed_result, void *data)
1225 {
1226         strat_db.dump_enabled = 1;
1227         strat_db_dump(__FUNCTION__);
1228
1229         corn_set_color(strat_db.corn_table[0], I2C_COB_BLACK);
1230         strat_db_dump(__FUNCTION__);
1231
1232         corn_set_color(strat_db.corn_table[3], I2C_COB_WHITE);
1233         strat_db_dump(__FUNCTION__);
1234         corn_set_color(strat_db.corn_table[4], I2C_COB_WHITE);
1235         strat_db_dump(__FUNCTION__);
1236         corn_set_color(strat_db.corn_table[5], I2C_COB_WHITE);
1237         strat_db_dump(__FUNCTION__);
1238 }
1239
1240 prog_char str_test_arg0[] = "test";
1241 parse_pgm_token_string_t cmd_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_test_result, arg0, str_test_arg0);
1242 parse_pgm_token_num_t cmd_test_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_test_result, radius, INT32);
1243 parse_pgm_token_num_t cmd_test_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_test_result, dist, INT32);
1244
1245 prog_char help_test[] = "Test function";
1246 parse_pgm_inst_t cmd_test = {
1247         .f = cmd_test_parsed,  /* function to call */
1248         .data = NULL,      /* 2nd arg of func */
1249         .help_str = help_test,
1250         .tokens = {        /* token list, NULL terminated */
1251                 (prog_void *)&cmd_test_arg0,
1252                 NULL,
1253         },
1254 };