2 * Copyright Droids Corporation (2009)
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.
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.
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
18 * Revision : $Id: commands_mainboard.c,v 1.9 2009-11-08 17:24:33 zer0 Exp $
20 * Olivier MATZ <zer0@droids-corp.org>
27 #include <aversive/pgmspace.h>
28 #include <aversive/wait.h>
29 #include <aversive/error.h>
30 #include <aversive/eeprom.h>
33 #include <clock_time.h>
37 #include <control_system_manager.h>
38 #include <trajectory_manager.h>
39 #include <vect_base.h>
42 #include <obstacle_avoidance.h>
43 #include <blocking_detection_manager.h>
44 #include <robot_system.h>
45 #include <position_manager.h>
49 #include <parse_string.h>
50 #include <parse_num.h>
57 #include "strat_base.h"
58 #include "strat_utils.h"
60 struct cmd_event_result {
67 /* function called when cmd_event is parsed successfully */
68 static void cmd_event_parsed(void *parsed_result, void *data)
72 struct cmd_event_result * res = parsed_result;
74 if (!strcmp_P(res->arg1, PSTR("all"))) {
75 bit = DO_ENCODERS | DO_CS | DO_RS | DO_POS |
76 DO_BD | DO_TIMER | DO_POWER;
77 if (!strcmp_P(res->arg2, PSTR("on")))
78 mainboard.flags |= bit;
79 else if (!strcmp_P(res->arg2, PSTR("off")))
80 mainboard.flags &= bit;
82 printf_P(PSTR("encoders is %s\r\n"),
83 (DO_ENCODERS & mainboard.flags) ? "on":"off");
84 printf_P(PSTR("cs is %s\r\n"),
85 (DO_CS & mainboard.flags) ? "on":"off");
86 printf_P(PSTR("rs is %s\r\n"),
87 (DO_RS & mainboard.flags) ? "on":"off");
88 printf_P(PSTR("pos is %s\r\n"),
89 (DO_POS & mainboard.flags) ? "on":"off");
90 printf_P(PSTR("bd is %s\r\n"),
91 (DO_BD & mainboard.flags) ? "on":"off");
92 printf_P(PSTR("timer is %s\r\n"),
93 (DO_TIMER & mainboard.flags) ? "on":"off");
94 printf_P(PSTR("power is %s\r\n"),
95 (DO_POWER & mainboard.flags) ? "on":"off");
100 if (!strcmp_P(res->arg1, PSTR("encoders")))
102 else if (!strcmp_P(res->arg1, PSTR("cs"))) {
106 else if (!strcmp_P(res->arg1, PSTR("rs")))
108 else if (!strcmp_P(res->arg1, PSTR("pos")))
110 else if (!strcmp_P(res->arg1, PSTR("bd")))
112 else if (!strcmp_P(res->arg1, PSTR("timer"))) {
116 else if (!strcmp_P(res->arg1, PSTR("power")))
119 if (!strcmp_P(res->arg2, PSTR("on")))
120 mainboard.flags |= bit;
121 else if (!strcmp_P(res->arg2, PSTR("off"))) {
122 if (!strcmp_P(res->arg1, PSTR("cs"))) {
124 robotsim_pwm(LEFT_PWM, 0);
125 robotsim_pwm(RIGHT_PWM, 0);
127 pwm_ng_set(LEFT_PWM, 0);
128 pwm_ng_set(RIGHT_PWM, 0);
131 mainboard.flags &= (~bit);
133 printf_P(PSTR("%s is %s\r\n"), res->arg1,
134 (bit & mainboard.flags) ? "on":"off");
137 prog_char str_event_arg0[] = "event";
138 parse_pgm_token_string_t cmd_event_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg0, str_event_arg0);
139 prog_char str_event_arg1[] = "all#encoders#cs#rs#pos#bd#timer#power";
140 parse_pgm_token_string_t cmd_event_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg1, str_event_arg1);
141 prog_char str_event_arg2[] = "on#off#show";
142 parse_pgm_token_string_t cmd_event_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg2, str_event_arg2);
144 prog_char help_event[] = "Enable/disable events";
145 parse_pgm_inst_t cmd_event = {
146 .f = cmd_event_parsed, /* function to call */
147 .data = NULL, /* 2nd arg of func */
148 .help_str = help_event,
149 .tokens = { /* token list, NULL terminated */
150 (prog_void *)&cmd_event_arg0,
151 (prog_void *)&cmd_event_arg1,
152 (prog_void *)&cmd_event_arg2,
158 /**********************************************************/
161 /* this structure is filled when cmd_spi_test is parsed successfully */
162 struct cmd_spi_test_result {
166 /* function called when cmd_spi_test is parsed successfully */
167 static void cmd_spi_test_parsed(void * parsed_result, void * data)
170 printf("not implemented\n");
172 uint16_t i = 0, ret = 0, ret2 = 0;
174 if (mainboard.flags & DO_ENCODERS) {
175 printf_P(PSTR("Disable encoder event first\r\n"));
181 ret = spi_send_and_receive_byte(i);
182 ret2 = spi_send_and_receive_byte(i);
183 spi_slave_deselect(0);
185 if ((i & 0x7ff) == 0)
186 printf_P(PSTR("Sent %.4x twice, received %x %x\r\n"),
190 } while(!cmdline_keypressed());
194 prog_char str_spi_test_arg0[] = "spi_test";
195 parse_pgm_token_string_t cmd_spi_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_spi_test_result, arg0, str_spi_test_arg0);
197 prog_char help_spi_test[] = "Test the SPI";
198 parse_pgm_inst_t cmd_spi_test = {
199 .f = cmd_spi_test_parsed, /* function to call */
200 .data = NULL, /* 2nd arg of func */
201 .help_str = help_spi_test,
202 .tokens = { /* token list, NULL terminated */
203 (prog_void *)&cmd_spi_test_arg0,
210 /**********************************************************/
213 /* this structure is filled when cmd_opponent is parsed successfully */
214 struct cmd_opponent_result {
221 /* function called when cmd_opponent is parsed successfully */
222 static void cmd_opponent_parsed(void *parsed_result, void *data)
226 if (get_opponent_xyda(&x, &y, &d, &a))
227 printf_P(PSTR("No opponent\r\n"));
229 printf_P(PSTR("x=%d y=%d, d=%d a=%d\r\n"), x, y, d, a);
232 prog_char str_opponent_arg0[] = "opponent";
233 parse_pgm_token_string_t cmd_opponent_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_opponent_result, arg0, str_opponent_arg0);
234 prog_char str_opponent_arg1[] = "show";
235 parse_pgm_token_string_t cmd_opponent_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_opponent_result, arg1, str_opponent_arg1);
237 prog_char help_opponent[] = "Show (x,y) opponent";
238 parse_pgm_inst_t cmd_opponent = {
239 .f = cmd_opponent_parsed, /* function to call */
240 .data = NULL, /* 2nd arg of func */
241 .help_str = help_opponent,
242 .tokens = { /* token list, NULL terminated */
243 (prog_void *)&cmd_opponent_arg0,
244 (prog_void *)&cmd_opponent_arg1,
250 prog_char str_opponent_arg1_set[] = "set";
251 parse_pgm_token_string_t cmd_opponent_arg1_set = TOKEN_STRING_INITIALIZER(struct cmd_opponent_result, arg1, str_opponent_arg1_set);
252 parse_pgm_token_num_t cmd_opponent_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_opponent_result, arg2, INT32);
253 parse_pgm_token_num_t cmd_opponent_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_opponent_result, arg3, INT32);
255 prog_char help_opponent_set[] = "Set (x,y) opponent";
256 parse_pgm_inst_t cmd_opponent_set = {
257 .f = cmd_opponent_parsed, /* function to call */
258 .data = NULL, /* 2nd arg of func */
259 .help_str = help_opponent_set,
260 .tokens = { /* token list, NULL terminated */
261 (prog_void *)&cmd_opponent_arg0,
262 (prog_void *)&cmd_opponent_arg1_set,
263 (prog_void *)&cmd_opponent_arg2,
264 (prog_void *)&cmd_opponent_arg3,
270 /**********************************************************/
273 /* this structure is filled when cmd_start is parsed successfully */
274 struct cmd_start_result {
276 fixed_string_t color;
277 fixed_string_t debug;
280 /* function called when cmd_start is parsed successfully */
281 static void cmd_start_parsed(void *parsed_result, void *data)
284 printf("not implemented\n");
286 struct cmd_start_result *res = parsed_result;
287 uint8_t old_level = gen.log_level;
289 gen.logs[NB_LOGS] = E_USER_STRAT;
290 if (!strcmp_P(res->debug, PSTR("debug"))) {
291 strat_infos.dump_enabled = 1;
295 strat_infos.dump_enabled = 0;
299 if (!strcmp_P(res->color, PSTR("red"))) {
300 mainboard.our_color = I2C_COLOR_RED;
301 i2c_set_color(I2C_MECHBOARD_ADDR, I2C_COLOR_RED);
302 i2c_set_color(I2C_SENSORBOARD_ADDR, I2C_COLOR_RED);
304 else if (!strcmp_P(res->color, PSTR("green"))) {
305 mainboard.our_color = I2C_COLOR_GREEN;
306 i2c_set_color(I2C_MECHBOARD_ADDR, I2C_COLOR_GREEN);
307 i2c_set_color(I2C_SENSORBOARD_ADDR, I2C_COLOR_GREEN);
310 printf_P(PSTR("Check that lintel is loaded\r\n"));
311 while(!cmdline_keypressed());
313 printf_P(PSTR("Press a key when beacon ready\r\n"));
314 i2c_sensorboard_set_beacon(0);
315 while(!cmdline_keypressed());
316 i2c_sensorboard_set_beacon(1);
320 gen.logs[NB_LOGS] = 0;
321 gen.log_level = old_level;
325 prog_char str_start_arg0[] = "start";
326 parse_pgm_token_string_t cmd_start_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_start_result, arg0, str_start_arg0);
327 prog_char str_start_color[] = "green#red";
328 parse_pgm_token_string_t cmd_start_color = TOKEN_STRING_INITIALIZER(struct cmd_start_result, color, str_start_color);
329 prog_char str_start_debug[] = "debug#match";
330 parse_pgm_token_string_t cmd_start_debug = TOKEN_STRING_INITIALIZER(struct cmd_start_result, debug, str_start_debug);
332 prog_char help_start[] = "Start the robot";
333 parse_pgm_inst_t cmd_start = {
334 .f = cmd_start_parsed, /* function to call */
335 .data = NULL, /* 2nd arg of func */
336 .help_str = help_start,
337 .tokens = { /* token list, NULL terminated */
338 (prog_void *)&cmd_start_arg0,
339 (prog_void *)&cmd_start_color,
340 (prog_void *)&cmd_start_debug,
347 /**********************************************************/
350 /* this structure is filled when cmd_interact is parsed successfully */
351 struct cmd_interact_result {
355 static void print_cs(void)
357 printf_P(PSTR("cons_d=% .8"PRIi32" cons_a=% .8"PRIi32" fil_d=% .8"PRIi32" fil_a=% .8"PRIi32" "
358 "err_d=% .8"PRIi32" err_a=% .8"PRIi32" out_d=% .8"PRIi32" out_a=% .8"PRIi32"\r\n"),
359 cs_get_consign(&mainboard.distance.cs),
360 cs_get_consign(&mainboard.angle.cs),
361 cs_get_filtered_consign(&mainboard.distance.cs),
362 cs_get_filtered_consign(&mainboard.angle.cs),
363 cs_get_error(&mainboard.distance.cs),
364 cs_get_error(&mainboard.angle.cs),
365 cs_get_out(&mainboard.distance.cs),
366 cs_get_out(&mainboard.angle.cs));
369 static void print_pos(void)
371 printf_P(PSTR("x=% .8d y=% .8d a=% .8d\r\n"),
372 position_get_x_s16(&mainboard.pos),
373 position_get_y_s16(&mainboard.pos),
374 position_get_a_deg_s16(&mainboard.pos));
377 static void print_time(void)
379 printf_P(PSTR("time %d\r\n"),time_get_s());
383 static void print_sensors(void)
386 if (sensor_start_switch())
387 printf_P(PSTR("Start switch | "));
389 printf_P(PSTR(" | "));
391 if (IR_DISP_SENSOR())
392 printf_P(PSTR("IR disp | "));
394 printf_P(PSTR(" | "));
396 printf_P(PSTR("\r\n"));
400 static void print_pid(void)
402 printf_P(PSTR("P=% .8"PRIi32" I=% .8"PRIi32" D=% .8"PRIi32" out=% .8"PRIi32" | "
403 "P=% .8"PRIi32" I=% .8"PRIi32" D=% .8"PRIi32" out=% .8"PRIi32"\r\n"),
404 pid_get_value_in(&mainboard.distance.pid) * pid_get_gain_P(&mainboard.distance.pid),
405 pid_get_value_I(&mainboard.distance.pid) * pid_get_gain_I(&mainboard.distance.pid),
406 pid_get_value_D(&mainboard.distance.pid) * pid_get_gain_D(&mainboard.distance.pid),
407 pid_get_value_out(&mainboard.distance.pid),
408 pid_get_value_in(&mainboard.angle.pid) * pid_get_gain_P(&mainboard.angle.pid),
409 pid_get_value_I(&mainboard.angle.pid) * pid_get_gain_I(&mainboard.angle.pid),
410 pid_get_value_D(&mainboard.angle.pid) * pid_get_gain_D(&mainboard.angle.pid),
411 pid_get_value_out(&mainboard.angle.pid));
414 #define PRINT_POS (1<<0)
415 #define PRINT_PID (1<<1)
416 #define PRINT_CS (1<<2)
417 #define PRINT_SENSORS (1<<3)
418 #define PRINT_TIME (1<<4)
419 #define PRINT_BLOCKING (1<<5)
421 static void cmd_interact_parsed(void * parsed_result, void * data)
430 printf_P(PSTR("Display debugs:\r\n"
436 /* " 6:blocking\r\n" */
443 mainboard.flags &= (~DO_CS);
444 pwm_set_and_save(LEFT_PWM, 0);
445 pwm_set_and_save(RIGHT_PWM, 0);
448 if (print & PRINT_POS) {
452 if (print & PRINT_PID) {
456 if (print & PRINT_CS) {
460 if (print & PRINT_SENSORS) {
464 if (print & PRINT_TIME) {
467 /* if (print & PRINT_BLOCKING) { */
468 /* printf_P(PSTR("%s %s blocking=%d\r\n"), */
469 /* mainboard.blocking ? "BLOCK1":" ", */
470 /* rs_is_blocked(&mainboard.rs) ? "BLOCK2":" ", */
471 /* rs_get_blocking(&mainboard.rs)); */
474 c = cmdline_getchar();
479 cmd = vt100_parser(&vt100, c);
487 case '1': print ^= PRINT_POS; break;
488 case '2': print ^= PRINT_PID; break;
489 case '3': print ^= PRINT_CS; break;
490 case '4': print ^= PRINT_SENSORS; break;
491 case '5': print ^= PRINT_TIME; break;
492 case '6': print ^= PRINT_BLOCKING; break;
495 if (mainboard.flags & DO_CS)
497 pwm_set_and_save(LEFT_PWM, 0);
498 pwm_set_and_save(RIGHT_PWM, 0);
501 pwm_set_and_save(LEFT_PWM, 0);
502 pwm_set_and_save(RIGHT_PWM, 0);
511 pwm_set_and_save(LEFT_PWM, 1200);
512 pwm_set_and_save(RIGHT_PWM, 1200);
515 pwm_set_and_save(LEFT_PWM, -1200);
516 pwm_set_and_save(RIGHT_PWM, 1200);
519 pwm_set_and_save(LEFT_PWM, -1200);
520 pwm_set_and_save(RIGHT_PWM, -1200);
523 pwm_set_and_save(LEFT_PWM, 1200);
524 pwm_set_and_save(RIGHT_PWM, -1200);
532 prog_char str_interact_arg0[] = "interact";
533 parse_pgm_token_string_t cmd_interact_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_interact_result, arg0, str_interact_arg0);
535 prog_char help_interact[] = "Interactive mode";
536 parse_pgm_inst_t cmd_interact = {
537 .f = cmd_interact_parsed, /* function to call */
538 .data = NULL, /* 2nd arg of func */
539 .help_str = help_interact,
540 .tokens = { /* token list, NULL terminated */
541 (prog_void *)&cmd_interact_arg0,
547 /**********************************************************/
550 /* this structure is filled when cmd_color is parsed successfully */
551 struct cmd_color_result {
553 fixed_string_t color;
556 /* function called when cmd_color is parsed successfully */
557 static void cmd_color_parsed(void *parsed_result, void *data)
560 printf("not implemented\n");
562 struct cmd_color_result *res = (struct cmd_color_result *) parsed_result;
563 if (!strcmp_P(res->color, PSTR("red"))) {
564 mainboard.our_color = I2C_COLOR_RED;
565 i2c_set_color(I2C_MECHBOARD_ADDR, I2C_COLOR_RED);
566 i2c_set_color(I2C_SENSORBOARD_ADDR, I2C_COLOR_RED);
568 else if (!strcmp_P(res->color, PSTR("green"))) {
569 mainboard.our_color = I2C_COLOR_GREEN;
570 i2c_set_color(I2C_MECHBOARD_ADDR, I2C_COLOR_GREEN);
571 i2c_set_color(I2C_SENSORBOARD_ADDR, I2C_COLOR_GREEN);
573 printf_P(PSTR("Done\r\n"));
577 prog_char str_color_arg0[] = "color";
578 parse_pgm_token_string_t cmd_color_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_color_result, arg0, str_color_arg0);
579 prog_char str_color_color[] = "green#red";
580 parse_pgm_token_string_t cmd_color_color = TOKEN_STRING_INITIALIZER(struct cmd_color_result, color, str_color_color);
582 prog_char help_color[] = "Set our color";
583 parse_pgm_inst_t cmd_color = {
584 .f = cmd_color_parsed, /* function to call */
585 .data = NULL, /* 2nd arg of func */
586 .help_str = help_color,
587 .tokens = { /* token list, NULL terminated */
588 (prog_void *)&cmd_color_arg0,
589 (prog_void *)&cmd_color_color,
595 /**********************************************************/
598 /* this structure is filled when cmd_rs is parsed successfully */
599 struct cmd_rs_result {
604 /* function called when cmd_rs is parsed successfully */
605 static void cmd_rs_parsed(void *parsed_result, void *data)
607 // struct cmd_rs_result *res = parsed_result;
609 printf_P(PSTR("angle cons=% .6"PRIi32" in=% .6"PRIi32" out=% .6"PRIi32" / "),
610 cs_get_consign(&mainboard.angle.cs),
611 cs_get_filtered_feedback(&mainboard.angle.cs),
612 cs_get_out(&mainboard.angle.cs));
613 printf_P(PSTR("distance cons=% .6"PRIi32" in=% .6"PRIi32" out=% .6"PRIi32" / "),
614 cs_get_consign(&mainboard.distance.cs),
615 cs_get_filtered_feedback(&mainboard.distance.cs),
616 cs_get_out(&mainboard.distance.cs));
617 printf_P(PSTR("l=% .4"PRIi32" r=% .4"PRIi32"\r\n"), mainboard.pwm_l,
620 } while(!cmdline_keypressed());
623 prog_char str_rs_arg0[] = "rs";
624 parse_pgm_token_string_t cmd_rs_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_rs_result, arg0, str_rs_arg0);
625 prog_char str_rs_arg1[] = "show";
626 parse_pgm_token_string_t cmd_rs_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_rs_result, arg1, str_rs_arg1);
628 prog_char help_rs[] = "Show rs (robot system) values";
629 parse_pgm_inst_t cmd_rs = {
630 .f = cmd_rs_parsed, /* function to call */
631 .data = NULL, /* 2nd arg of func */
633 .tokens = { /* token list, NULL terminated */
634 (prog_void *)&cmd_rs_arg0,
635 (prog_void *)&cmd_rs_arg1,
640 /**********************************************************/
643 /* this structure is filled when cmd_i2cdebug is parsed successfully */
644 struct cmd_i2cdebug_result {
648 /* function called when cmd_i2cdebug is parsed successfully */
649 static void cmd_i2cdebug_parsed(void * parsed_result, void * data)
652 printf("not implemented\n");
655 i2c_protocol_debug();
659 prog_char str_i2cdebug_arg0[] = "i2cdebug";
660 parse_pgm_token_string_t cmd_i2cdebug_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_i2cdebug_result, arg0, str_i2cdebug_arg0);
662 prog_char help_i2cdebug[] = "I2c debug infos";
663 parse_pgm_inst_t cmd_i2cdebug = {
664 .f = cmd_i2cdebug_parsed, /* function to call */
665 .data = NULL, /* 2nd arg of func */
666 .help_str = help_i2cdebug,
667 .tokens = { /* token list, NULL terminated */
668 (prog_void *)&cmd_i2cdebug_arg0,
673 /**********************************************************/
676 /* this structure is filled when cmd_mechboard_show is parsed successfully */
677 struct cmd_mechboard_show_result {
682 /* function called when cmd_mechboard_show is parsed successfully */
683 static void cmd_mechboard_show_parsed(void * parsed_result, void * data)
686 printf("not implemented\n");
688 printf_P(PSTR("mode = %x\r\n"), mechboard.mode);
689 printf_P(PSTR("status = %x\r\n"), mechboard.status);
690 printf_P(PSTR("lintel_count = %d\r\n"), mechboard.lintel_count);
692 printf_P(PSTR("column_count = %d\r\n"), get_column_count());
693 printf_P(PSTR("left1=%d left2=%d right1=%d right2=%d\r\n"),
694 pump_left1_is_full(), pump_left2_is_full(),
695 pump_right1_is_full(), pump_right2_is_full());
697 printf_P(PSTR("pump_left1 = %d\r\n"), mechboard.pump_left1);
698 printf_P(PSTR("pump_left2 = %d\r\n"), mechboard.pump_left2);
699 printf_P(PSTR("pump_right1 = %d\r\n"), mechboard.pump_right1);
700 printf_P(PSTR("pump_right2 = %d\r\n"), mechboard.pump_right2);
702 printf_P(PSTR("servo_lintel_left = %d\r\n"), mechboard.servo_lintel_left);
703 printf_P(PSTR("servo_lintel_right = %d\r\n"), mechboard.servo_lintel_right);
708 prog_char str_mechboard_show_arg0[] = "mechboard";
709 parse_pgm_token_string_t cmd_mechboard_show_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_show_result, arg0, str_mechboard_show_arg0);
710 prog_char str_mechboard_show_arg1[] = "show";
711 parse_pgm_token_string_t cmd_mechboard_show_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_show_result, arg1, str_mechboard_show_arg1);
713 prog_char help_mechboard_show[] = "show mechboard status";
714 parse_pgm_inst_t cmd_mechboard_show = {
715 .f = cmd_mechboard_show_parsed, /* function to call */
716 .data = NULL, /* 2nd arg of func */
717 .help_str = help_mechboard_show,
718 .tokens = { /* token list, NULL terminated */
719 (prog_void *)&cmd_mechboard_show_arg0,
720 (prog_void *)&cmd_mechboard_show_arg1,
725 /**********************************************************/
726 /* Mechboard_Setmode1 */
728 /* this structure is filled when cmd_mechboard_setmode1 is parsed successfully */
729 struct cmd_mechboard_setmode1_result {
734 /* function called when cmd_mechboard_setmode1 is parsed successfully */
735 static void cmd_mechboard_setmode1_parsed(void *parsed_result, void *data)
738 printf("not implemented\n");
740 struct cmd_mechboard_setmode1_result *res = parsed_result;
742 if (!strcmp_P(res->arg1, PSTR("init")))
743 i2c_mechboard_mode_init();
744 else if (!strcmp_P(res->arg1, PSTR("manual")))
745 i2c_mechboard_mode_manual();
746 else if (!strcmp_P(res->arg1, PSTR("pickup")))
747 i2c_mechboard_mode_pickup();
748 else if (!strcmp_P(res->arg1, PSTR("lazy_harvest")))
749 i2c_mechboard_mode_lazy_harvest();
750 else if (!strcmp_P(res->arg1, PSTR("harvest")))
751 i2c_mechboard_mode_harvest();
752 else if (!strcmp_P(res->arg1, PSTR("prepare_get_lintel")))
753 i2c_mechboard_mode_prepare_get_lintel();
754 else if (!strcmp_P(res->arg1, PSTR("get_lintel")))
755 i2c_mechboard_mode_get_lintel();
756 else if (!strcmp_P(res->arg1, PSTR("put_lintel")))
757 i2c_mechboard_mode_put_lintel();
758 else if (!strcmp_P(res->arg1, PSTR("init")))
759 i2c_mechboard_mode_init();
760 else if (!strcmp_P(res->arg1, PSTR("eject")))
761 i2c_mechboard_mode_init();
762 else if (!strcmp_P(res->arg1, PSTR("clear")))
763 i2c_mechboard_mode_clear();
764 else if (!strcmp_P(res->arg1, PSTR("loaded")))
765 i2c_mechboard_mode_loaded();
766 else if (!strcmp_P(res->arg1, PSTR("store")))
767 i2c_mechboard_mode_store();
768 else if (!strcmp_P(res->arg1, PSTR("manivelle")))
769 i2c_mechboard_mode_manivelle();
770 else if (!strcmp_P(res->arg1, PSTR("lazy_pickup")))
771 i2c_mechboard_mode_lazy_pickup();
775 prog_char str_mechboard_setmode1_arg0[] = "mechboard";
776 parse_pgm_token_string_t cmd_mechboard_setmode1_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode1_result, arg0, str_mechboard_setmode1_arg0);
777 prog_char str_mechboard_setmode1_arg1[] = "manivelle#init#manual#pickup#prepare_get_lintel#get_lintel#put_lintel1#eject#clear#harvest#lazy_harvest#store#lazy_pickup";
778 parse_pgm_token_string_t cmd_mechboard_setmode1_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode1_result, arg1, str_mechboard_setmode1_arg1);
780 prog_char help_mechboard_setmode1[] = "set mechboard mode (mode)";
781 parse_pgm_inst_t cmd_mechboard_setmode1 = {
782 .f = cmd_mechboard_setmode1_parsed, /* function to call */
783 .data = NULL, /* 2nd arg of func */
784 .help_str = help_mechboard_setmode1,
785 .tokens = { /* token list, NULL terminated */
786 (prog_void *)&cmd_mechboard_setmode1_arg0,
787 (prog_void *)&cmd_mechboard_setmode1_arg1,
792 /**********************************************************/
793 /* Mechboard_Setmode2 */
795 /* this structure is filled when cmd_mechboard_setmode2 is parsed successfully */
796 struct cmd_mechboard_setmode2_result {
802 /* function called when cmd_mechboard_setmode2 is parsed successfully */
803 static void cmd_mechboard_setmode2_parsed(void * parsed_result, void * data)
806 printf("not implemented\n");
808 struct cmd_mechboard_setmode2_result *res = parsed_result;
809 uint8_t side = I2C_LEFT_SIDE;
811 if (!strcmp_P(res->arg2, PSTR("left")))
812 side = I2C_LEFT_SIDE;
813 else if (!strcmp_P(res->arg2, PSTR("right")))
814 side = I2C_RIGHT_SIDE;
815 else if (!strcmp_P(res->arg2, PSTR("center")))
816 side = I2C_CENTER_SIDE;
817 else if (!strcmp_P(res->arg2, PSTR("auto")))
818 side = I2C_AUTO_SIDE;
820 if (!strcmp_P(res->arg1, PSTR("prepare_pickup")))
821 i2c_mechboard_mode_prepare_pickup(side);
822 else if (!strcmp_P(res->arg1, PSTR("push_temple_disc")))
823 i2c_mechboard_mode_push_temple_disc(side);
827 prog_char str_mechboard_setmode2_arg0[] = "mechboard";
828 parse_pgm_token_string_t cmd_mechboard_setmode2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode2_result, arg0, str_mechboard_setmode2_arg0);
829 prog_char str_mechboard_setmode2_arg1[] = "prepare_pickup#push_temple_disc";
830 parse_pgm_token_string_t cmd_mechboard_setmode2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode2_result, arg1, str_mechboard_setmode2_arg1);
831 prog_char str_mechboard_setmode2_arg2[] = "left#right#auto#center";
832 parse_pgm_token_string_t cmd_mechboard_setmode2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode2_result, arg2, str_mechboard_setmode2_arg2);
834 prog_char help_mechboard_setmode2[] = "set mechboard mode (more, side)";
835 parse_pgm_inst_t cmd_mechboard_setmode2 = {
836 .f = cmd_mechboard_setmode2_parsed, /* function to call */
837 .data = NULL, /* 2nd arg of func */
838 .help_str = help_mechboard_setmode2,
839 .tokens = { /* token list, NULL terminated */
840 (prog_void *)&cmd_mechboard_setmode2_arg0,
841 (prog_void *)&cmd_mechboard_setmode2_arg1,
842 (prog_void *)&cmd_mechboard_setmode2_arg2,
847 /**********************************************************/
848 /* Mechboard_Setmode3 */
850 /* this structure is filled when cmd_mechboard_setmode3 is parsed successfully */
851 struct cmd_mechboard_setmode3_result {
857 /* function called when cmd_mechboard_setmode3 is parsed successfully */
858 static void cmd_mechboard_setmode3_parsed(void *parsed_result, void *data)
861 printf("not implemented\n");
863 struct cmd_mechboard_setmode3_result *res = parsed_result;
864 if (!strcmp_P(res->arg1, PSTR("autobuild")))
865 i2c_mechboard_mode_simple_autobuild(res->level);
866 else if (!strcmp_P(res->arg1, PSTR("prepare_build")))
867 i2c_mechboard_mode_prepare_build_both(res->level);
868 else if (!strcmp_P(res->arg1, PSTR("prepare_inside")))
869 i2c_mechboard_mode_prepare_inside_both(res->level);
870 else if (!strcmp_P(res->arg1, PSTR("push_temple")))
871 i2c_mechboard_mode_push_temple(res->level);
875 prog_char str_mechboard_setmode3_arg0[] = "mechboard";
876 parse_pgm_token_string_t cmd_mechboard_setmode3_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode3_result, arg0, str_mechboard_setmode3_arg0);
877 prog_char str_mechboard_setmode3_arg1[] = "autobuild#prepare_build#prepare_inside#push_temple";
878 parse_pgm_token_string_t cmd_mechboard_setmode3_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode3_result, arg1, str_mechboard_setmode3_arg1);
879 parse_pgm_token_num_t cmd_mechboard_setmode3_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_mechboard_setmode3_result, level, UINT8);
881 prog_char help_mechboard_setmode3[] = "set mechboard mode (mode, level)";
882 parse_pgm_inst_t cmd_mechboard_setmode3 = {
883 .f = cmd_mechboard_setmode3_parsed, /* function to call */
884 .data = NULL, /* 2nd arg of func */
885 .help_str = help_mechboard_setmode3,
886 .tokens = { /* token list, NULL terminated */
887 (prog_void *)&cmd_mechboard_setmode3_arg0,
888 (prog_void *)&cmd_mechboard_setmode3_arg1,
889 (prog_void *)&cmd_mechboard_setmode3_arg2,
894 /**********************************************************/
895 /* Mechboard_Setmode4 */
897 /* this structure is filled when cmd_mechboard_setmode4 is parsed successfully */
898 struct cmd_mechboard_setmode4_result {
910 /* function called when cmd_mechboard_setmode4 is parsed successfully */
911 static void cmd_mechboard_setmode4_parsed(void *parsed_result, void *data)
914 printf("not implemented\n");
916 struct cmd_mechboard_setmode4_result *res = parsed_result;
917 i2c_mechboard_mode_autobuild(res->level_l, res->count_l, res->dist_l,
918 res->level_r, res->count_r, res->dist_r,
923 prog_char str_mechboard_setmode4_arg0[] = "mechboard";
924 parse_pgm_token_string_t cmd_mechboard_setmode4_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode4_result, arg0, str_mechboard_setmode4_arg0);
925 prog_char str_mechboard_setmode4_arg1[] = "autobuild";
926 parse_pgm_token_string_t cmd_mechboard_setmode4_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode4_result, arg1, str_mechboard_setmode4_arg1);
927 parse_pgm_token_num_t cmd_mechboard_setmode4_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_mechboard_setmode4_result, level_l, UINT8);
928 parse_pgm_token_num_t cmd_mechboard_setmode4_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_mechboard_setmode4_result, count_l, UINT8);
929 parse_pgm_token_num_t cmd_mechboard_setmode4_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_mechboard_setmode4_result, dist_l, UINT8);
930 parse_pgm_token_num_t cmd_mechboard_setmode4_arg5 = TOKEN_NUM_INITIALIZER(struct cmd_mechboard_setmode4_result, level_r, UINT8);
931 parse_pgm_token_num_t cmd_mechboard_setmode4_arg6 = TOKEN_NUM_INITIALIZER(struct cmd_mechboard_setmode4_result, count_r, UINT8);
932 parse_pgm_token_num_t cmd_mechboard_setmode4_arg7 = TOKEN_NUM_INITIALIZER(struct cmd_mechboard_setmode4_result, dist_r, UINT8);
933 parse_pgm_token_num_t cmd_mechboard_setmode4_arg8 = TOKEN_NUM_INITIALIZER(struct cmd_mechboard_setmode4_result, do_lintel, UINT8);
935 prog_char help_mechboard_setmode4[] = "set mechboard mode (autobuild level_l count_l dist_l level_r count_r dist_r lintel)";
936 parse_pgm_inst_t cmd_mechboard_setmode4 = {
937 .f = cmd_mechboard_setmode4_parsed, /* function to call */
938 .data = NULL, /* 2nd arg of func */
939 .help_str = help_mechboard_setmode4,
940 .tokens = { /* token list, NULL terminated */
941 (prog_void *)&cmd_mechboard_setmode4_arg0,
942 (prog_void *)&cmd_mechboard_setmode4_arg1,
943 (prog_void *)&cmd_mechboard_setmode4_arg2,
944 (prog_void *)&cmd_mechboard_setmode4_arg3,
945 (prog_void *)&cmd_mechboard_setmode4_arg4,
946 (prog_void *)&cmd_mechboard_setmode4_arg5,
947 (prog_void *)&cmd_mechboard_setmode4_arg6,
948 (prog_void *)&cmd_mechboard_setmode4_arg7,
949 (prog_void *)&cmd_mechboard_setmode4_arg8,
954 /**********************************************************/
955 /* Mechboard_Setmode5 */
957 /* this structure is filled when cmd_mechboard_setmode5 is parsed successfully */
958 struct cmd_mechboard_setmode5_result {
965 /* function called when cmd_mechboard_setmode5 is parsed successfully */
966 static void cmd_mechboard_setmode5_parsed(void *parsed_result, void * data)
969 printf("not implemented\n");
971 struct cmd_mechboard_setmode5_result *res = parsed_result;
972 uint8_t side = I2C_LEFT_SIDE, next_mode = I2C_MECHBOARD_MODE_PREPARE_PICKUP;
974 if (!strcmp_P(res->arg2, PSTR("left")))
975 side = I2C_LEFT_SIDE;
976 else if (!strcmp_P(res->arg2, PSTR("right")))
977 side = I2C_RIGHT_SIDE;
978 else if (!strcmp_P(res->arg2, PSTR("center")))
979 side = I2C_CENTER_SIDE;
980 else if (!strcmp_P(res->arg2, PSTR("auto")))
981 side = I2C_AUTO_SIDE;
983 if (!strcmp_P(res->arg3, PSTR("harvest")))
984 next_mode = I2C_MECHBOARD_MODE_HARVEST;
985 else if (!strcmp_P(res->arg3, PSTR("lazy_harvest")))
986 next_mode = I2C_MECHBOARD_MODE_LAZY_HARVEST;
987 else if (!strcmp_P(res->arg3, PSTR("pickup")))
988 next_mode = I2C_MECHBOARD_MODE_PICKUP;
989 else if (!strcmp_P(res->arg3, PSTR("clear")))
990 next_mode = I2C_MECHBOARD_MODE_CLEAR;
991 else if (!strcmp_P(res->arg3, PSTR("store")))
992 next_mode = I2C_MECHBOARD_MODE_STORE;
993 else if (!strcmp_P(res->arg3, PSTR("lazy_pickup")))
994 next_mode = I2C_MECHBOARD_MODE_LAZY_PICKUP;
996 if (!strcmp_P(res->arg1, PSTR("prepare_pickup")))
997 i2c_mechboard_mode_prepare_pickup_next(side, next_mode);
1001 prog_char str_mechboard_setmode5_arg0[] = "mechboard";
1002 parse_pgm_token_string_t cmd_mechboard_setmode5_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode5_result, arg0, str_mechboard_setmode5_arg0);
1003 prog_char str_mechboard_setmode5_arg1[] = "prepare_pickup";
1004 parse_pgm_token_string_t cmd_mechboard_setmode5_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode5_result, arg1, str_mechboard_setmode5_arg1);
1005 prog_char str_mechboard_setmode5_arg2[] = "left#right#auto#center";
1006 parse_pgm_token_string_t cmd_mechboard_setmode5_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode5_result, arg2, str_mechboard_setmode5_arg2);
1007 prog_char str_mechboard_setmode5_arg3[] = "harvest#pickup#store#lazy_harvest#lazy_pickup#clear";
1008 parse_pgm_token_string_t cmd_mechboard_setmode5_arg3 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode5_result, arg3, str_mechboard_setmode5_arg3);
1010 prog_char help_mechboard_setmode5[] = "set mechboard mode (more, side)";
1011 parse_pgm_inst_t cmd_mechboard_setmode5 = {
1012 .f = cmd_mechboard_setmode5_parsed, /* function to call */
1013 .data = NULL, /* 2nd arg of func */
1014 .help_str = help_mechboard_setmode5,
1015 .tokens = { /* token list, NULL terminated */
1016 (prog_void *)&cmd_mechboard_setmode5_arg0,
1017 (prog_void *)&cmd_mechboard_setmode5_arg1,
1018 (prog_void *)&cmd_mechboard_setmode5_arg2,
1019 (prog_void *)&cmd_mechboard_setmode5_arg3,
1024 /**********************************************************/
1027 /* this structure is filled when cmd_pickup_wheels is parsed successfully */
1028 struct cmd_pickup_wheels_result {
1029 fixed_string_t arg0;
1030 fixed_string_t arg1;
1033 /* function called when cmd_pickup_wheels is parsed successfully */
1034 static void cmd_pickup_wheels_parsed(void *parsed_result, void *data)
1036 struct cmd_pickup_wheels_result *res = parsed_result;
1038 if (!strcmp_P(res->arg1, PSTR("on")))
1041 pickup_wheels_off();
1044 prog_char str_pickup_wheels_arg0[] = "pickup_wheels";
1045 parse_pgm_token_string_t cmd_pickup_wheels_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_pickup_wheels_result, arg0, str_pickup_wheels_arg0);
1046 prog_char str_pickup_wheels_arg1[] = "on#off";
1047 parse_pgm_token_string_t cmd_pickup_wheels_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_pickup_wheels_result, arg1, str_pickup_wheels_arg1);
1049 prog_char help_pickup_wheels[] = "Enable/disable pickup wheels";
1050 parse_pgm_inst_t cmd_pickup_wheels = {
1051 .f = cmd_pickup_wheels_parsed, /* function to call */
1052 .data = NULL, /* 2nd arg of func */
1053 .help_str = help_pickup_wheels,
1054 .tokens = { /* token list, NULL terminated */
1055 (prog_void *)&cmd_pickup_wheels_arg0,
1056 (prog_void *)&cmd_pickup_wheels_arg1,
1061 /**********************************************************/
1064 /* this structure is filled when cmd_beacon_start is parsed successfully */
1065 struct cmd_beacon_start_result {
1066 fixed_string_t arg0;
1067 fixed_string_t arg1;
1070 /* function called when cmd_beacon_start is parsed successfully */
1071 static void cmd_beacon_start_parsed(void *parsed_result, void *data)
1074 printf("not implemented\n");
1076 struct cmd_beacon_start_result *res = parsed_result;
1078 if (!strcmp_P(res->arg1, PSTR("start")))
1079 i2c_sensorboard_set_beacon(1);
1081 i2c_sensorboard_set_beacon(0);
1085 prog_char str_beacon_start_arg0[] = "beacon";
1086 parse_pgm_token_string_t cmd_beacon_start_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_beacon_start_result, arg0, str_beacon_start_arg0);
1087 prog_char str_beacon_start_arg1[] = "start#stop";
1088 parse_pgm_token_string_t cmd_beacon_start_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_beacon_start_result, arg1, str_beacon_start_arg1);
1090 prog_char help_beacon_start[] = "Beacon enabled/disable";
1091 parse_pgm_inst_t cmd_beacon_start = {
1092 .f = cmd_beacon_start_parsed, /* function to call */
1093 .data = NULL, /* 2nd arg of func */
1094 .help_str = help_beacon_start,
1095 .tokens = { /* token list, NULL terminated */
1096 (prog_void *)&cmd_beacon_start_arg0,
1097 (prog_void *)&cmd_beacon_start_arg1,
1102 /**********************************************************/
1105 /* this structure is filled when cmd_pump_current is parsed successfully */
1106 struct cmd_pump_current_result {
1107 fixed_string_t arg0;
1108 fixed_string_t arg1;
1111 /* function called when cmd_pump_current is parsed successfully */
1112 static void cmd_pump_current_parsed(__attribute__((unused)) void *parsed_result,
1113 __attribute__((unused)) void *data)
1116 printf("not implemented\n");
1118 printf_P(PSTR("l1=%d l2=%d r1=%d r2=%d\r\n"),
1119 sensor_get_adc(ADC_CSENSE3), sensor_get_adc(ADC_CSENSE4),
1120 mechboard.pump_right1_current, mechboard.pump_right2_current);
1124 prog_char str_pump_current_arg0[] = "pump_current";
1125 parse_pgm_token_string_t cmd_pump_current_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_pump_current_result, arg0, str_pump_current_arg0);
1126 prog_char str_pump_current_arg1[] = "show";
1127 parse_pgm_token_string_t cmd_pump_current_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_pump_current_result, arg1, str_pump_current_arg1);
1129 prog_char help_pump_current[] = "dump pump current";
1130 parse_pgm_inst_t cmd_pump_current = {
1131 .f = cmd_pump_current_parsed, /* function to call */
1132 .data = NULL, /* 2nd arg of func */
1133 .help_str = help_pump_current,
1134 .tokens = { /* token list, NULL terminated */
1135 (prog_void *)&cmd_pump_current_arg0,
1136 (prog_void *)&cmd_pump_current_arg1,
1141 /**********************************************************/
1144 /* this structure is filled when cmd_build_test is parsed successfully */
1145 struct cmd_build_test_result {
1146 fixed_string_t arg0;
1149 /* function called when cmd_build_test is parsed successfully */
1150 static void cmd_build_test_parsed(void *parsed_result, void *data)
1153 printf("not implemented\n");
1155 //struct cmd_build_test_result *res = parsed_result;
1157 printf_P(PSTR("lintel must be there\r\n"));
1158 i2c_mechboard_mode_prepare_pickup_next(I2C_LEFT_SIDE,
1159 I2C_MECHBOARD_MODE_HARVEST);
1162 printf_P(PSTR("Insert 4 colums\r\n"));
1163 while (get_column_count() != 4);
1165 i2c_mechboard_mode_prepare_build_both(0);
1166 trajectory_d_rel(&mainboard.traj, 200);
1167 wait_traj_end(TRAJ_FLAGS_NO_NEAR);
1170 i2c_mechboard_mode_simple_autobuild(0);
1172 while (get_mechboard_mode() == I2C_MECHBOARD_MODE_AUTOBUILD);
1174 trajectory_d_rel(&mainboard.traj, -200);
1175 wait_traj_end(TRAJ_FLAGS_NO_NEAR);
1176 i2c_mechboard_mode_prepare_pickup_next(I2C_LEFT_SIDE,
1177 I2C_MECHBOARD_MODE_HARVEST);
1179 while (get_column_count() != 3);
1181 i2c_mechboard_mode_prepare_build_both(3);
1182 trajectory_d_rel(&mainboard.traj, 200);
1183 wait_traj_end(TRAJ_FLAGS_NO_NEAR);
1186 i2c_mechboard_mode_autobuild(3, 1, I2C_AUTOBUILD_DEFAULT_DIST,
1187 3, 2,I2C_AUTOBUILD_DEFAULT_DIST, 0);
1188 i2cproto_wait_update();
1189 while (get_mechboard_mode() == I2C_MECHBOARD_MODE_AUTOBUILD);
1191 trajectory_d_rel(&mainboard.traj, -200);
1192 wait_traj_end(TRAJ_FLAGS_NO_NEAR);
1193 i2c_mechboard_mode_prepare_pickup(I2C_RIGHT_SIDE);
1196 i2c_mechboard_mode_harvest();
1197 while (get_column_count() != 3);
1199 i2c_mechboard_mode_prepare_build_both(5);
1200 trajectory_d_rel(&mainboard.traj, 200);
1201 wait_traj_end(TRAJ_FLAGS_NO_NEAR);
1204 i2c_mechboard_mode_autobuild(4, 2, I2C_AUTOBUILD_DEFAULT_DIST,
1205 5, 1, I2C_AUTOBUILD_DEFAULT_DIST, 0);
1206 i2cproto_wait_update();
1207 while (get_mechboard_mode() == I2C_MECHBOARD_MODE_AUTOBUILD);
1209 trajectory_d_rel(&mainboard.traj, -200);
1213 prog_char str_build_test_arg0[] = "build_test";
1214 parse_pgm_token_string_t cmd_build_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_build_test_result, arg0, str_build_test_arg0);
1216 prog_char help_build_test[] = "Build_Test function";
1217 parse_pgm_inst_t cmd_build_test = {
1218 .f = cmd_build_test_parsed, /* function to call */
1219 .data = NULL, /* 2nd arg of func */
1220 .help_str = help_build_test,
1221 .tokens = { /* token list, NULL terminated */
1222 (prog_void *)&cmd_build_test_arg0,
1228 /**********************************************************/
1231 /* this structure is filled when cmd_column_test is parsed successfully */
1232 struct cmd_column_test_result {
1233 fixed_string_t arg0;
1243 /* function called when cmd_column_test is parsed successfully */
1244 static void cmd_column_test_parsed(void *parsed_result, void *data)
1247 printf("not implemented\n");
1249 struct cmd_column_test_result *res = parsed_result;
1250 uint8_t level = res->level, debug = 0;
1251 uint8_t c, push = 0;
1259 res->arm_dist = 220;
1263 if (!strcmp_P(res->arg0, PSTR("column_test_debug")))
1265 if (!strcmp_P(res->arg0, PSTR("column_test_push")))
1268 strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_VERY_SLOW);
1272 trajectory_d_rel(&mainboard.traj, 200);
1273 wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
1275 /* go back, insert colums */
1277 trajectory_d_rel(&mainboard.traj, -200);
1278 wait_traj_end(TRAJ_FLAGS_NO_NEAR);
1280 i2c_mechboard_mode_prepare_pickup_next(I2C_LEFT_SIDE,
1281 I2C_MECHBOARD_MODE_HARVEST);
1282 printf_P(PSTR("Insert 4 colums\r\n"));
1283 while (get_column_count() != 4);
1285 /* build with left arm */
1287 i2c_mechboard_mode_prepare_inside_both(level);
1288 trajectory_d_rel(&mainboard.traj, 200-(res->dist));
1289 wait_traj_end(TRAJ_FLAGS_NO_NEAR);
1292 c = cmdline_getchar_wait();
1294 trajectory_a_rel(&mainboard.traj, res->a1);
1295 wait_traj_end(TRAJ_FLAGS_NO_NEAR);
1298 c = cmdline_getchar_wait();
1300 i2c_mechboard_mode_prepare_build_select(level, -1);
1303 c = cmdline_getchar_wait();
1304 i2c_mechboard_mode_autobuild(level, res->nb_col, res->arm_dist,
1305 0, 0, res->arm_dist, 0);
1306 while (get_mechboard_mode() != I2C_MECHBOARD_MODE_AUTOBUILD);
1307 while (get_mechboard_mode() == I2C_MECHBOARD_MODE_AUTOBUILD);
1310 c = cmdline_getchar_wait();
1311 i2c_mechboard_mode_prepare_inside_select(level+res->nb_col, -1);
1314 c = cmdline_getchar_wait();
1315 /* build with right arm */
1317 trajectory_a_rel(&mainboard.traj, res->a2);
1318 wait_traj_end(TRAJ_FLAGS_NO_NEAR);
1321 c = cmdline_getchar_wait();
1322 /* only ok for nb_col == 2 */
1323 if ((level + res->nb_col) >= 7)
1324 i2c_mechboard_mode_prepare_build_select(-1, level + res->nb_col + 1);
1326 i2c_mechboard_mode_prepare_build_select(-1, level + res->nb_col);
1329 c = cmdline_getchar_wait();
1330 i2c_mechboard_mode_autobuild(0, 0, res->arm_dist,
1331 level + res->nb_col, res->nb_col,
1333 while (get_mechboard_mode() != I2C_MECHBOARD_MODE_AUTOBUILD);
1334 while (get_mechboard_mode() == I2C_MECHBOARD_MODE_AUTOBUILD);
1338 strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST);
1339 trajectory_d_rel(&mainboard.traj, -100);
1340 wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
1341 i2c_mechboard_mode_push_temple_disc(I2C_RIGHT_SIDE);
1343 trajectory_d_rel(&mainboard.traj, 100);
1344 wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
1346 else if (level == 1 || level == 0) {
1347 trajectory_d_rel(&mainboard.traj, -100);
1348 wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
1349 i2c_mechboard_mode_push_temple(level);
1351 strat_set_speed(200, SPEED_ANGLE_SLOW);
1352 trajectory_d_rel(&mainboard.traj, 120);
1353 wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
1356 strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
1359 c = cmdline_getchar_wait();
1360 i2c_mechboard_mode_prepare_inside_select(-1, level+res->nb_col*2);
1363 c = cmdline_getchar_wait();
1365 trajectory_a_rel(&mainboard.traj, res->a3);
1366 wait_traj_end(TRAJ_FLAGS_NO_NEAR);
1369 c = cmdline_getchar_wait();
1370 /* go back, insert colums */
1372 trajectory_d_rel(&mainboard.traj, -100);
1378 prog_char str_column_test_arg0[] = "column_test#column_test_debug#column_test_push";
1379 parse_pgm_token_string_t cmd_column_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_column_test_result, arg0, str_column_test_arg0);
1380 parse_pgm_token_num_t cmd_column_test_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_column_test_result, level, UINT8);
1382 prog_char help_column_test[] = "Column_Test function (level)";
1383 parse_pgm_inst_t cmd_column_test = {
1384 .f = cmd_column_test_parsed, /* function to call */
1385 .data = (void *)1, /* 2nd arg of func */
1386 .help_str = help_column_test,
1387 .tokens = { /* token list, NULL terminated */
1388 (prog_void *)&cmd_column_test_arg0,
1389 (prog_void *)&cmd_column_test_arg1,
1394 parse_pgm_token_num_t cmd_column_test_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_column_test_result, dist, INT16);
1395 parse_pgm_token_num_t cmd_column_test_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_column_test_result, a1, INT8);
1396 parse_pgm_token_num_t cmd_column_test_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_column_test_result, a2, INT8);
1397 parse_pgm_token_num_t cmd_column_test_arg5 = TOKEN_NUM_INITIALIZER(struct cmd_column_test_result, a3, INT8);
1398 parse_pgm_token_num_t cmd_column_test_arg6 = TOKEN_NUM_INITIALIZER(struct cmd_column_test_result, arm_dist, INT16);
1399 parse_pgm_token_num_t cmd_column_test_arg7 = TOKEN_NUM_INITIALIZER(struct cmd_column_test_result, nb_col, INT8);
1401 prog_char help_column_test2[] = "Column_Test function (level, dist, a1, a2, a3, arm_dist, nb_col)";
1402 parse_pgm_inst_t cmd_column_test2 = {
1403 .f = cmd_column_test_parsed, /* function to call */
1404 .data = NULL, /* 2nd arg of func */
1405 .help_str = help_column_test2,
1406 .tokens = { /* token list, NULL terminated */
1407 (prog_void *)&cmd_column_test_arg0,
1408 (prog_void *)&cmd_column_test_arg1,
1409 (prog_void *)&cmd_column_test_arg2,
1410 (prog_void *)&cmd_column_test_arg3,
1411 (prog_void *)&cmd_column_test_arg4,
1412 (prog_void *)&cmd_column_test_arg5,
1413 (prog_void *)&cmd_column_test_arg6,
1414 (prog_void *)&cmd_column_test_arg7,
1420 /**********************************************************/
1423 /* this structure is filled when cmd_pickup_test is parsed successfully */
1424 struct cmd_pickup_test_result {
1425 fixed_string_t arg0;
1426 fixed_string_t arg1;
1430 /* return red or green sensor */
1431 #define COLOR_IR_SENSOR() \
1433 uint8_t __ret = 0; \
1434 if (side == I2C_RIGHT_SIDE) \
1435 __ret = sensor_get(S_DISP_RIGHT); \
1437 __ret = sensor_get(S_DISP_LEFT); \
1440 /* column dispensers */
1441 #define COL_SCAN_MARGIN 200
1442 /* distance between the wheel axis and the IR sensor */
1444 /* function called when cmd_pickup_test is parsed successfully */
1445 static void cmd_pickup_test_parsed(void *parsed_result, void *data)
1448 printf("not implemented\n");
1450 uint8_t err, side, first_try = 1;
1451 int8_t cols_count_before, cols_count_after, cols;
1452 struct cmd_pickup_test_result *res = parsed_result;
1453 int16_t pos1, pos2, pos;
1455 int16_t dist = res->dist;
1456 uint8_t timeout = 0;
1458 if (!strcmp_P(res->arg1, PSTR("left")))
1459 side = I2C_LEFT_SIDE;
1461 side = I2C_RIGHT_SIDE;
1463 i2c_mechboard_mode_prepare_pickup(I2C_AUTO_SIDE);
1464 cols_count_before = get_column_count();
1465 position_set(&mainboard.pos, 0, 0, 0);
1467 strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_FAST);
1468 trajectory_d_rel(&mainboard.traj, -1000);
1469 err = WAIT_COND_OR_TRAJ_END(!COLOR_IR_SENSOR(), TRAJ_FLAGS_NO_NEAR);
1470 if (err) /* we should not reach end */
1472 pos1 = position_get_x_s16(&mainboard.pos);
1473 printf_P(PSTR("pos1 = %d\r\n"), pos1);
1475 err = WAIT_COND_OR_TRAJ_END(COLOR_IR_SENSOR(), TRAJ_FLAGS_NO_NEAR);
1478 pos2 = position_get_x_s16(&mainboard.pos);
1479 printf_P(PSTR("pos2 = %d\r\n"), pos2);
1481 pos = ABS(pos1 - pos2);
1482 printf_P(PSTR("pos = %d\r\n"), pos);
1484 trajectory_d_rel(&mainboard.traj, -dist + pos/2);
1485 err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
1487 if (side == I2C_LEFT_SIDE)
1488 trajectory_a_rel(&mainboard.traj, 90);
1490 trajectory_a_rel(&mainboard.traj, -90);
1491 err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
1496 i2c_mechboard_mode_lazy_harvest();
1498 i2c_mechboard_mode_prepare_pickup(I2C_AUTO_SIDE);
1501 strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_FAST);
1502 trajectory_d_rel(&mainboard.traj, 300);
1503 err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST | END_NEAR);
1504 strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST);
1505 err = strat_calib(600, TRAJ_FLAGS_SMALL_DIST);
1507 trajectory_d_rel(&mainboard.traj, -DIST_BACK_DISPENSER);
1508 err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
1509 if (!TRAJ_SUCCESS(err))
1512 position_set(&mainboard.pos, 0, 0, 0);
1513 if (get_mechboard_mode() == I2C_MECHBOARD_MODE_PREPARE_EJECT) {
1514 strat_eject_col(90, 0);
1518 /* start to pickup with finger / arms */
1520 printf_P(PSTR("%s pickup now\r\n"), __FUNCTION__);
1521 i2c_mechboard_mode_pickup();
1522 WAIT_COND_OR_TIMEOUT(get_mechboard_mode() ==
1523 I2C_MECHBOARD_MODE_PICKUP, 100);
1524 us = time_get_us2();
1525 cols = get_column_count();
1526 while (get_mechboard_mode() == I2C_MECHBOARD_MODE_PICKUP) {
1527 if (get_column_count() != cols) {
1528 cols = get_column_count();
1529 us = time_get_us2();
1531 if ((get_column_count() - cols_count_before) >= 4) {
1532 printf_P(PSTR("%s no more cols in disp\r\n"), __FUNCTION__);
1535 /* 1 second timeout */
1536 if (time_get_us2() - us > 1500000L) {
1537 printf_P(PSTR("%s timeout\r\n"), __FUNCTION__);
1543 /* eject if we found a bad color column */
1545 if (get_mechboard_mode() == I2C_MECHBOARD_MODE_PREPARE_EJECT) {
1546 strat_eject_col(90, 0);
1550 strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
1551 trajectory_d_rel(&mainboard.traj, -250);
1552 wait_traj_end(TRAJ_FLAGS_SMALL_DIST | END_NEAR);
1554 cols_count_after = get_column_count();
1555 cols = cols_count_after - cols_count_before;
1556 DEBUG(E_USER_STRAT, "%s we got %d cols", __FUNCTION__, cols);
1558 pickup_wheels_off();
1559 i2c_mechboard_mode_clear();
1564 printf_P(PSTR("failed\r\n"));
1569 prog_char str_pickup_test_arg0[] = "pickup_test";
1570 parse_pgm_token_string_t cmd_pickup_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_pickup_test_result, arg0, str_pickup_test_arg0);
1571 prog_char str_pickup_test_arg1[] = "left#right";
1572 parse_pgm_token_string_t cmd_pickup_test_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_pickup_test_result, arg1, str_pickup_test_arg1);
1573 parse_pgm_token_num_t cmd_pickup_test_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_pickup_test_result, dist, INT16);
1575 prog_char help_pickup_test[] = "Pickup_Test function";
1576 parse_pgm_inst_t cmd_pickup_test = {
1577 .f = cmd_pickup_test_parsed, /* function to call */
1578 .data = NULL, /* 2nd arg of func */
1579 .help_str = help_pickup_test,
1580 .tokens = { /* token list, NULL terminated */
1581 (prog_void *)&cmd_pickup_test_arg0,
1582 (prog_void *)&cmd_pickup_test_arg1,
1583 (prog_void *)&cmd_pickup_test_arg2,
1588 /**********************************************************/
1591 /* this structure is filled when cmd_lintel_test is parsed successfully */
1592 struct cmd_lintel_test_result {
1593 fixed_string_t arg0;
1596 /* function called when cmd_lintel_test is parsed successfully */
1597 static void cmd_lintel_test_parsed(void *parsed_result, void *data)
1600 printf("not implemented\n");
1602 uint8_t err, first_try = 1, right_ok, left_ok;
1603 int16_t left_cur, right_cur;
1605 i2c_mechboard_mode_prepare_get_lintel();
1608 strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST);
1609 trajectory_d_rel(&mainboard.traj, 500);
1610 err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
1611 if (!TRAJ_SUCCESS(err) && err != END_BLOCKING)
1614 i2c_mechboard_mode_get_lintel();
1617 left_cur = sensor_get_adc(ADC_CSENSE3);
1618 left_ok = (left_cur > I2C_MECHBOARD_CURRENT_COLUMN);
1619 right_cur = mechboard.pump_right1_current;
1620 right_ok = (right_cur > I2C_MECHBOARD_CURRENT_COLUMN);
1622 printf_P(PSTR("left_ok=%d (%d), right_ok=%d (%d)\r\n"),
1623 left_ok, left_cur, right_ok, right_cur);
1625 if (!right_ok && !left_ok) {
1626 i2c_mechboard_mode_prepare_get_lintel();
1629 else if (right_ok && !left_ok) {
1630 i2c_mechboard_mode_prepare_get_lintel();
1632 strat_set_speed(500, 500);
1633 trajectory_d_a_rel(&mainboard.traj, -150, 30);
1634 err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
1635 trajectory_d_a_rel(&mainboard.traj, -140, -30);
1636 err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
1640 else if (!right_ok && left_ok) {
1641 i2c_mechboard_mode_prepare_get_lintel();
1643 strat_set_speed(500, 500);
1644 trajectory_d_a_rel(&mainboard.traj, -150, -30);
1645 err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
1646 trajectory_d_a_rel(&mainboard.traj, -140, 30);
1647 err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
1651 /* else, lintel is ok */
1653 i2c_mechboard_mode_put_lintel();
1657 if (right_ok && left_ok) {
1659 i2c_mechboard_mode_put_lintel();
1662 i2c_mechboard_mode_prepare_get_lintel();
1667 strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
1668 trajectory_d_rel(&mainboard.traj, -250);
1669 err = wait_traj_end(TRAJ_FLAGS_STD);
1673 printf_P(PSTR("fail\r\n"));
1678 prog_char str_lintel_test_arg0[] = "lintel_test";
1679 parse_pgm_token_string_t cmd_lintel_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_lintel_test_result, arg0, str_lintel_test_arg0);
1681 prog_char help_lintel_test[] = "Lintel_Test function";
1682 parse_pgm_inst_t cmd_lintel_test = {
1683 .f = cmd_lintel_test_parsed, /* function to call */
1684 .data = NULL, /* 2nd arg of func */
1685 .help_str = help_lintel_test,
1686 .tokens = { /* token list, NULL terminated */
1687 (prog_void *)&cmd_lintel_test_arg0,
1692 /**********************************************************/
1695 /* this structure is filled when cmd_scan_test is parsed successfully */
1696 struct cmd_scan_test_result {
1697 fixed_string_t arg0;
1698 fixed_string_t arg1;
1707 #define SCAN_MODE_CHECK_TEMPLE 0
1708 #define SCAN_MODE_SCAN_COL 1
1709 #define SCAN_MODE_SCAN_TEMPLE 2
1710 #define SCAN_MODE_TRAJ_ONLY 3
1712 /* function called when cmd_scan_test is parsed successfully */
1713 static void cmd_scan_test_parsed(void *parsed_result, void *data)
1716 printf("not implemented\n");
1718 uint8_t err, mode=0, c;
1719 int16_t pos1x, pos1y, dist;
1720 struct cmd_scan_test_result *res = parsed_result;
1721 int16_t back_mm = 0;
1723 int16_t ckpt_rel_x = 0, ckpt_rel_y = 0;
1725 double center_abs_x, center_abs_y;
1726 double ckpt_rel_d, ckpt_rel_a;
1727 double ckpt_abs_x, ckpt_abs_y;
1729 if (!strcmp_P(res->arg1, PSTR("traj_only")))
1730 mode = SCAN_MODE_TRAJ_ONLY;
1731 else if (!strcmp_P(res->arg1, PSTR("check_temple")))
1732 mode = SCAN_MODE_CHECK_TEMPLE;
1733 else if (!strcmp_P(res->arg1, PSTR("scan_col")))
1734 mode = SCAN_MODE_SCAN_COL;
1735 else if (!strcmp_P(res->arg1, PSTR("scan_temple")))
1736 mode = SCAN_MODE_SCAN_TEMPLE;
1739 strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_VERY_SLOW);
1740 trajectory_d_rel(&mainboard.traj, 400);
1741 err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
1742 if (err != END_BLOCKING)
1745 /* save absolute position of disc */
1746 rel_da_to_abs_xy(265, 0, ¢er_abs_x, ¢er_abs_y);
1748 /* go back and prepare to scan */
1749 strat_set_speed(1000, 1000);
1750 trajectory_d_a_rel(&mainboard.traj, -140, 130);
1751 err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
1752 if (!TRAJ_SUCCESS(err))
1755 /* prepare scanner arm */
1756 if (mode != SCAN_MODE_TRAJ_ONLY)
1757 i2c_sensorboard_scanner_prepare();
1760 strat_set_speed(res->scan_speed, 1000);
1762 pos1x = position_get_x_s16(&mainboard.pos);
1763 pos1y = position_get_y_s16(&mainboard.pos);
1764 trajectory_d_rel(&mainboard.traj, -res->scan_dist);
1767 err = test_traj_end(TRAJ_FLAGS_SMALL_DIST);
1771 dist = distance_from_robot(pos1x, pos1y);
1773 if (dist > res->start_dist)
1776 if (get_scanner_status() & I2C_SCAN_MAX_COLUMN) {
1783 if (TRAJ_SUCCESS(err))
1784 err = END_ERROR; /* should not reach end */
1786 trajectory_goto_xy_abs(&mainboard.traj, pos1x, pos1y);
1787 wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
1788 if (mode != SCAN_MODE_TRAJ_ONLY)
1789 i2c_sensorboard_scanner_stop();
1793 /* start the scanner */
1795 if (mode != SCAN_MODE_TRAJ_ONLY)
1796 i2c_sensorboard_scanner_start();
1798 err = WAIT_COND_OR_TRAJ_END(get_scanner_status() & I2C_SCAN_MAX_COLUMN,
1799 TRAJ_FLAGS_NO_NEAR);
1802 if (!TRAJ_SUCCESS(err)) {
1804 trajectory_goto_xy_abs(&mainboard.traj, pos1x, pos1y);
1805 wait_traj_end(TRAJ_FLAGS_NO_NEAR);
1806 if (mode != SCAN_MODE_TRAJ_ONLY)
1807 i2c_sensorboard_scanner_stop();
1811 if (mode == SCAN_MODE_TRAJ_ONLY)
1814 wait_scan_done(10000);
1816 i2c_sensorboard_scanner_stop();
1818 if (mode == SCAN_MODE_CHECK_TEMPLE) {
1819 i2c_sensorboard_scanner_algo_check(res->level,
1820 res->center_x, res->center_y);
1821 i2cproto_wait_update();
1822 wait_scan_done(10000);
1823 scanner_dump_state();
1825 if (sensorboard.dropzone_h == -1) {
1826 printf_P(PSTR("-- try to build a temple\r\n"));
1829 mode = SCAN_MODE_SCAN_TEMPLE;
1833 if (mode == SCAN_MODE_SCAN_TEMPLE) {
1834 i2c_sensorboard_scanner_algo_temple(I2C_SCANNER_ZONE_DISC,
1837 i2cproto_wait_update();
1838 wait_scan_done(10000);
1839 scanner_dump_state();
1841 if (sensorboard.dropzone_h == -1 ||
1842 strat_scan_get_checkpoint(mode, &ckpt_rel_x,
1843 &ckpt_rel_y, &back_mm)) {
1844 printf_P(PSTR("-- try to build a column\r\n"));
1845 mode = SCAN_MODE_SCAN_COL;
1849 if (mode == SCAN_MODE_SCAN_COL) {
1850 i2c_sensorboard_scanner_algo_column(I2C_SCANNER_ZONE_DISC,
1851 res->center_x, res->center_y);
1852 i2cproto_wait_update();
1853 wait_scan_done(10000);
1854 scanner_dump_state();
1856 if (sensorboard.dropzone_h == -1 ||
1857 strat_scan_get_checkpoint(mode, &ckpt_rel_x,
1858 &ckpt_rel_y, &back_mm)) {
1863 if (sensorboard.dropzone_h == -1)
1866 if (mode == SCAN_MODE_CHECK_TEMPLE) {
1872 printf_P(PSTR("rel xy for ckpt is %d,%d\r\n"), ckpt_rel_x, ckpt_rel_y);
1874 rel_xy_to_abs_xy(ckpt_rel_x, ckpt_rel_y, &ckpt_abs_x, &ckpt_abs_y);
1875 abs_xy_to_rel_da(ckpt_abs_x, ckpt_abs_y, &ckpt_rel_d, &ckpt_rel_a);
1877 printf_P(PSTR("abs ckpt is %2.2f,%2.2f\r\n"), ckpt_abs_x, ckpt_abs_y);
1879 printf_P(PSTR("ok ? (y/n)\r\n"));
1881 c = cmdline_getchar_wait();
1886 strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
1888 /* intermediate checkpoint for some positions */
1889 if ( (DEG(ckpt_rel_a) < 0 && DEG(ckpt_rel_a) > -90) ) {
1890 trajectory_goto_xy_rel(&mainboard.traj, 200, 100);
1891 err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
1892 if (!TRAJ_SUCCESS(err))
1896 trajectory_goto_xy_abs(&mainboard.traj, ckpt_abs_x, ckpt_abs_y);
1897 err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
1898 if (!TRAJ_SUCCESS(err))
1901 trajectory_turnto_xy(&mainboard.traj, center_abs_x, center_abs_y);
1902 err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
1903 if (!TRAJ_SUCCESS(err))
1906 c = cmdline_getchar_wait();
1908 pos1x = position_get_x_s16(&mainboard.pos);
1909 pos1y = position_get_y_s16(&mainboard.pos);
1911 strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW);
1912 trajectory_d_rel(&mainboard.traj, 200);
1913 err = WAIT_COND_OR_TRAJ_END(distance_from_robot(pos1x, pos1y) > 200,
1914 TRAJ_FLAGS_SMALL_DIST);
1916 strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_VERY_SLOW);
1917 trajectory_d_rel(&mainboard.traj, 400);
1918 err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
1920 if (err != END_BLOCKING)
1924 trajectory_d_rel(&mainboard.traj, -back_mm);
1925 wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
1930 prog_char str_scan_test_arg0[] = "scan_test";
1931 parse_pgm_token_string_t cmd_scan_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_scan_test_result, arg0, str_scan_test_arg0);
1932 prog_char str_scan_test_arg1[] = "traj_only#scan_col#scan_temple";
1933 parse_pgm_token_string_t cmd_scan_test_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_scan_test_result, arg1, str_scan_test_arg1);
1934 parse_pgm_token_num_t cmd_scan_test_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_scan_test_result, start_dist, INT16);
1935 parse_pgm_token_num_t cmd_scan_test_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_scan_test_result, scan_dist, INT16);
1936 parse_pgm_token_num_t cmd_scan_test_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_scan_test_result, scan_speed, INT16);
1937 parse_pgm_token_num_t cmd_scan_test_arg5 = TOKEN_NUM_INITIALIZER(struct cmd_scan_test_result, center_x, INT16);
1938 parse_pgm_token_num_t cmd_scan_test_arg6 = TOKEN_NUM_INITIALIZER(struct cmd_scan_test_result, center_y, INT16);
1940 prog_char help_scan_test[] = "Scan_Test function (start_dist, scan_dist, speed_dist, centerx, centery)";
1941 parse_pgm_inst_t cmd_scan_test = {
1942 .f = cmd_scan_test_parsed, /* function to call */
1943 .data = NULL, /* 2nd arg of func */
1944 .help_str = help_scan_test,
1945 .tokens = { /* token list, NULL terminated */
1946 (prog_void *)&cmd_scan_test_arg0,
1947 (prog_void *)&cmd_scan_test_arg1,
1948 (prog_void *)&cmd_scan_test_arg2,
1949 (prog_void *)&cmd_scan_test_arg3,
1950 (prog_void *)&cmd_scan_test_arg4,
1951 (prog_void *)&cmd_scan_test_arg5,
1952 (prog_void *)&cmd_scan_test_arg6,
1957 prog_char str_scan_test_arg1b[] = "check_temple";
1958 parse_pgm_token_string_t cmd_scan_test_arg1b = TOKEN_STRING_INITIALIZER(struct cmd_scan_test_result, arg1, str_scan_test_arg1b);
1959 parse_pgm_token_num_t cmd_scan_test_arg7 = TOKEN_NUM_INITIALIZER(struct cmd_scan_test_result, level, UINT8);
1961 prog_char help_scan_test2[] = "Scan_Test function (start_dist, scan_dist, speed_dist, templex, templey, level)";
1962 parse_pgm_inst_t cmd_scan_test2 = {
1963 .f = cmd_scan_test_parsed, /* function to call */
1964 .data = NULL, /* 2nd arg of func */
1965 .help_str = help_scan_test,
1966 .tokens = { /* token list, NULL terminated */
1967 (prog_void *)&cmd_scan_test_arg0,
1968 (prog_void *)&cmd_scan_test_arg1b,
1969 (prog_void *)&cmd_scan_test_arg2,
1970 (prog_void *)&cmd_scan_test_arg3,
1971 (prog_void *)&cmd_scan_test_arg4,
1972 (prog_void *)&cmd_scan_test_arg5,
1973 (prog_void *)&cmd_scan_test_arg6,
1974 (prog_void *)&cmd_scan_test_arg7,
1979 /**********************************************************/
1982 /* this structure is filled when cmd_time_monitor is parsed successfully */
1983 struct cmd_time_monitor_result {
1984 fixed_string_t arg0;
1985 fixed_string_t arg1;
1988 /* function called when cmd_time_monitor is parsed successfully */
1989 static void cmd_time_monitor_parsed(void *parsed_result, void *data)
1992 printf("not implemented\n");
1994 struct cmd_time_monitor_result *res = parsed_result;
1997 if (!strcmp_P(res->arg1, PSTR("reset"))) {
1998 eeprom_write_word(EEPROM_TIME_ADDRESS, 0);
2000 seconds = eeprom_read_word(EEPROM_TIME_ADDRESS);
2001 printf_P(PSTR("Running since %d mn %d\r\n"), seconds/60, seconds%60);
2005 prog_char str_time_monitor_arg0[] = "time_monitor";
2006 parse_pgm_token_string_t cmd_time_monitor_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_time_monitor_result, arg0, str_time_monitor_arg0);
2007 prog_char str_time_monitor_arg1[] = "show#reset";
2008 parse_pgm_token_string_t cmd_time_monitor_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_time_monitor_result, arg1, str_time_monitor_arg1);
2010 prog_char help_time_monitor[] = "Show since how long we are running";
2011 parse_pgm_inst_t cmd_time_monitor = {
2012 .f = cmd_time_monitor_parsed, /* function to call */
2013 .data = NULL, /* 2nd arg of func */
2014 .help_str = help_time_monitor,
2015 .tokens = { /* token list, NULL terminated */
2016 (prog_void *)&cmd_time_monitor_arg0,
2017 (prog_void *)&cmd_time_monitor_arg1,
2023 /**********************************************************/
2026 /* this structure is filled when cmd_scanner is parsed successfully */
2027 struct cmd_scanner_result {
2028 fixed_string_t arg0;
2029 fixed_string_t arg1;
2032 /* function called when cmd_scanner is parsed successfully */
2033 static void cmd_scanner_parsed(void *parsed_result, void *data)
2036 printf("not implemented\n");
2038 struct cmd_scanner_result *res = parsed_result;
2040 if (!strcmp_P(res->arg1, PSTR("prepare"))) {
2041 i2c_sensorboard_scanner_prepare();
2043 else if (!strcmp_P(res->arg1, PSTR("stop"))) {
2044 i2c_sensorboard_scanner_stop();
2046 else if (!strcmp_P(res->arg1, PSTR("start"))) {
2047 i2c_sensorboard_scanner_start();
2049 else if (!strcmp_P(res->arg1, PSTR("algo_col"))) {
2050 i2c_sensorboard_scanner_algo_column(I2C_SCANNER_ZONE_DISC,
2053 else if (!strcmp_P(res->arg1, PSTR("algo_check"))) {
2054 i2c_sensorboard_scanner_algo_check(2, 15, 15); // XXX
2056 else if (!strcmp_P(res->arg1, PSTR("calib"))) {
2057 i2c_sensorboard_scanner_calib();
2059 else if (!strcmp_P(res->arg1, PSTR("show"))) {
2060 scanner_dump_state();
2065 prog_char str_scanner_arg0[] = "scanner";
2066 parse_pgm_token_string_t cmd_scanner_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_scanner_result, arg0, str_scanner_arg0);
2067 prog_char str_scanner_arg1[] = "prepare#start#algo_col#algo_check#stop#show#calib";
2068 parse_pgm_token_string_t cmd_scanner_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_scanner_result, arg1, str_scanner_arg1);
2070 prog_char help_scanner[] = "send commands to scanner";
2071 parse_pgm_inst_t cmd_scanner = {
2072 .f = cmd_scanner_parsed, /* function to call */
2073 .data = NULL, /* 2nd arg of func */
2074 .help_str = help_scanner,
2075 .tokens = { /* token list, NULL terminated */
2076 (prog_void *)&cmd_scanner_arg0,
2077 (prog_void *)&cmd_scanner_arg1,
2082 /**********************************************************/
2085 /* this structure is filled when cmd_build_z1 is parsed successfully */
2086 struct cmd_build_z1_result {
2087 fixed_string_t arg0;
2094 /* function called when cmd_build_z1 is parsed successfully */
2095 static void cmd_build_z1_parsed(void *parsed_result, void *data)
2098 printf("not implemented\n");
2100 struct cmd_build_z1_result *res = parsed_result;
2102 strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_SLOW);
2103 trajectory_d_rel(&mainboard.traj, 400);
2104 wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
2106 trajectory_d_rel(&mainboard.traj, -200);
2107 wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
2109 i2c_mechboard_mode_prepare_pickup_next(I2C_LEFT_SIDE,
2110 I2C_MECHBOARD_MODE_HARVEST);
2112 while (get_column_count() != 4);
2114 i2c_mechboard_mode_prepare_build_both(res->level);
2117 trajectory_d_rel(&mainboard.traj, 400);
2118 wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
2120 strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW);
2121 trajectory_d_rel(&mainboard.traj, -res->d1);
2122 wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
2123 i2c_mechboard_mode_autobuild(res->level, 2, I2C_AUTOBUILD_DEFAULT_DIST,
2124 res->level, 2, I2C_AUTOBUILD_DEFAULT_DIST,
2126 WAIT_COND_OR_TIMEOUT(get_mechboard_mode() ==
2127 I2C_MECHBOARD_MODE_AUTOBUILD, 100);
2128 WAIT_COND_OR_TIMEOUT(get_mechboard_mode() !=
2129 I2C_MECHBOARD_MODE_AUTOBUILD, 10000);
2131 strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW);
2132 trajectory_d_rel(&mainboard.traj, -res->d2);
2133 wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
2134 i2c_mechboard_mode_push_temple(1);
2136 strat_set_speed(200, SPEED_ANGLE_SLOW);
2137 trajectory_d_rel(&mainboard.traj, res->d3);
2138 wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
2142 prog_char str_build_z1_arg0[] = "build_z1";
2143 parse_pgm_token_string_t cmd_build_z1_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_build_z1_result, arg0, str_build_z1_arg0);
2144 parse_pgm_token_num_t cmd_build_z1_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_build_z1_result, level, UINT8);
2145 parse_pgm_token_num_t cmd_build_z1_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_build_z1_result, d1, INT16);
2146 parse_pgm_token_num_t cmd_build_z1_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_build_z1_result, d2, INT16);
2147 parse_pgm_token_num_t cmd_build_z1_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_build_z1_result, d3, INT16);
2149 prog_char help_build_z1[] = "Build_Z1 function (level, d1, d2, d3)";
2150 parse_pgm_inst_t cmd_build_z1 = {
2151 .f = cmd_build_z1_parsed, /* function to call */
2152 .data = NULL, /* 2nd arg of func */
2153 .help_str = help_build_z1,
2154 .tokens = { /* token list, NULL terminated */
2155 (prog_void *)&cmd_build_z1_arg0,
2156 (prog_void *)&cmd_build_z1_arg1,
2157 (prog_void *)&cmd_build_z1_arg2,
2158 (prog_void *)&cmd_build_z1_arg3,
2159 (prog_void *)&cmd_build_z1_arg4,
2165 /**********************************************************/
2166 /* Beacon_Opp_Dump */
2168 /* this structure is filled when cmd_beacon_opp_dump is parsed successfully */
2169 struct cmd_beacon_opp_dump_result {
2170 fixed_string_t arg0;
2173 void beacon_dump_samples(void);
2175 /* function called when cmd_beacon_opp_dump is parsed successfully */
2176 static void cmd_beacon_opp_dump_parsed(void *parsed_result, void *data)
2179 printf("not implemented\n");
2181 beacon_dump_samples();
2185 prog_char str_beacon_opp_dump_arg0[] = "beacon_opp_dump";
2186 parse_pgm_token_string_t cmd_beacon_opp_dump_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_beacon_opp_dump_result, arg0, str_beacon_opp_dump_arg0);
2188 prog_char help_beacon_opp_dump[] = "Dump beacon samples";
2189 parse_pgm_inst_t cmd_beacon_opp_dump = {
2190 .f = cmd_beacon_opp_dump_parsed, /* function to call */
2191 .data = NULL, /* 2nd arg of func */
2192 .help_str = help_beacon_opp_dump,
2193 .tokens = { /* token list, NULL terminated */
2194 (prog_void *)&cmd_beacon_opp_dump_arg0,
2200 /**********************************************************/
2203 /* this structure is filled when cmd_test is parsed successfully */
2204 struct cmd_test_result {
2205 fixed_string_t arg0;
2208 void circle_get_da_speed_from_radius(struct trajectory *traj,
2212 /* function called when cmd_test is parsed successfully */
2213 static void cmd_test_parsed(void *parsed_result, void *data)
2215 struct cmd_test_result *res = parsed_result;
2217 strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW);
2218 circle_get_da_speed_from_radius(&mainboard.traj, res->radius, &d, &a);
2219 printf_P(PSTR("d=%2.2f a=%2.2f\r\n"), d, a);
2222 prog_char str_test_arg0[] = "test";
2223 parse_pgm_token_string_t cmd_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_test_result, arg0, str_test_arg0);
2224 parse_pgm_token_num_t cmd_test_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_test_result, radius, INT32);
2226 prog_char help_test[] = "Test function";
2227 parse_pgm_inst_t cmd_test = {
2228 .f = cmd_test_parsed, /* function to call */
2229 .data = NULL, /* 2nd arg of func */
2230 .help_str = help_test,
2231 .tokens = { /* token list, NULL terminated */
2232 (prog_void *)&cmd_test_arg0,
2233 (prog_void *)&cmd_test_arg1,