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>
26 #include <aversive/pgmspace.h>
27 #include <aversive/wait.h>
28 #include <aversive/error.h>
29 #include <avr/eeprom.h>
40 #include <control_system_manager.h>
41 #include <trajectory_manager.h>
42 #include <vect_base.h>
45 #include <obstacle_avoidance.h>
46 #include <blocking_detection_manager.h>
47 #include <robot_system.h>
48 #include <position_manager.h>
52 #include <parse_string.h>
53 #include <parse_num.h>
55 #include "../common/i2c_commands.h"
56 #include "../common/eeprom_mapping.h"
62 #include "strat_utils.h"
63 #include "strat_base.h"
64 #include "i2c_protocol.h"
67 struct cmd_event_result {
74 /* function called when cmd_event is parsed successfully */
75 static void cmd_event_parsed(void *parsed_result, void *data)
79 struct cmd_event_result * res = parsed_result;
81 if (!strcmp_P(res->arg1, PSTR("all"))) {
82 bit = DO_ENCODERS | DO_CS | DO_RS | DO_POS |
83 DO_BD | DO_TIMER | DO_POWER;
84 if (!strcmp_P(res->arg2, PSTR("on")))
85 mainboard.flags |= bit;
86 else if (!strcmp_P(res->arg2, PSTR("off")))
87 mainboard.flags &= bit;
89 printf_P(PSTR("encoders is %s\r\n"),
90 (DO_ENCODERS & mainboard.flags) ? "on":"off");
91 printf_P(PSTR("cs is %s\r\n"),
92 (DO_CS & mainboard.flags) ? "on":"off");
93 printf_P(PSTR("rs is %s\r\n"),
94 (DO_RS & mainboard.flags) ? "on":"off");
95 printf_P(PSTR("pos is %s\r\n"),
96 (DO_POS & mainboard.flags) ? "on":"off");
97 printf_P(PSTR("bd is %s\r\n"),
98 (DO_BD & mainboard.flags) ? "on":"off");
99 printf_P(PSTR("timer is %s\r\n"),
100 (DO_TIMER & mainboard.flags) ? "on":"off");
101 printf_P(PSTR("power is %s\r\n"),
102 (DO_POWER & mainboard.flags) ? "on":"off");
107 if (!strcmp_P(res->arg1, PSTR("encoders")))
109 else if (!strcmp_P(res->arg1, PSTR("cs"))) {
113 else if (!strcmp_P(res->arg1, PSTR("rs")))
115 else if (!strcmp_P(res->arg1, PSTR("pos")))
117 else if (!strcmp_P(res->arg1, PSTR("bd")))
119 else if (!strcmp_P(res->arg1, PSTR("timer"))) {
123 else if (!strcmp_P(res->arg1, PSTR("power")))
126 if (!strcmp_P(res->arg2, PSTR("on")))
127 mainboard.flags |= bit;
128 else if (!strcmp_P(res->arg2, PSTR("off"))) {
129 if (!strcmp_P(res->arg1, PSTR("cs"))) {
130 pwm_ng_set(LEFT_PWM, 0);
131 pwm_ng_set(RIGHT_PWM, 0);
133 mainboard.flags &= (~bit);
135 printf_P(PSTR("%s is %s\r\n"), res->arg1,
136 (bit & mainboard.flags) ? "on":"off");
139 prog_char str_event_arg0[] = "event";
140 parse_pgm_token_string_t cmd_event_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg0, str_event_arg0);
141 prog_char str_event_arg1[] = "all#encoders#cs#rs#pos#bd#timer#power";
142 parse_pgm_token_string_t cmd_event_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg1, str_event_arg1);
143 prog_char str_event_arg2[] = "on#off#show";
144 parse_pgm_token_string_t cmd_event_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg2, str_event_arg2);
146 prog_char help_event[] = "Enable/disable events";
147 parse_pgm_inst_t cmd_event = {
148 .f = cmd_event_parsed, /* function to call */
149 .data = NULL, /* 2nd arg of func */
150 .help_str = help_event,
151 .tokens = { /* token list, NULL terminated */
152 (prog_void *)&cmd_event_arg0,
153 (prog_void *)&cmd_event_arg1,
154 (prog_void *)&cmd_event_arg2,
160 /**********************************************************/
163 /* this structure is filled when cmd_spi_test is parsed successfully */
164 struct cmd_spi_test_result {
168 /* function called when cmd_spi_test is parsed successfully */
169 static void cmd_spi_test_parsed(void * parsed_result, void * data)
171 uint16_t i = 0, ret = 0, ret2 = 0;
173 if (mainboard.flags & DO_ENCODERS) {
174 printf_P(PSTR("Disable encoder event first\r\n"));
180 ret = spi_send_and_receive_byte(i);
181 ret2 = spi_send_and_receive_byte(i);
182 spi_slave_deselect(0);
184 if ((i & 0x7ff) == 0)
185 printf_P(PSTR("Sent %.4x twice, received %x %x\r\n"),
189 } while(!cmdline_keypressed());
192 prog_char str_spi_test_arg0[] = "spi_test";
193 parse_pgm_token_string_t cmd_spi_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_spi_test_result, arg0, str_spi_test_arg0);
195 prog_char help_spi_test[] = "Test the SPI";
196 parse_pgm_inst_t cmd_spi_test = {
197 .f = cmd_spi_test_parsed, /* function to call */
198 .data = NULL, /* 2nd arg of func */
199 .help_str = help_spi_test,
200 .tokens = { /* token list, NULL terminated */
201 (prog_void *)&cmd_spi_test_arg0,
208 /**********************************************************/
211 /* this structure is filled when cmd_opponent is parsed successfully */
212 struct cmd_opponent_result {
219 /* function called when cmd_opponent is parsed successfully */
220 static void cmd_opponent_parsed(void *parsed_result, void *data)
224 if (get_opponent_xyda(&x, &y, &d, &a))
225 printf_P(PSTR("No opponent\r\n"));
227 printf_P(PSTR("x=%d y=%d, d=%d a=%d\r\n"), x, y, d, a);
230 prog_char str_opponent_arg0[] = "opponent";
231 parse_pgm_token_string_t cmd_opponent_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_opponent_result, arg0, str_opponent_arg0);
232 prog_char str_opponent_arg1[] = "show";
233 parse_pgm_token_string_t cmd_opponent_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_opponent_result, arg1, str_opponent_arg1);
235 prog_char help_opponent[] = "Show (x,y) opponent";
236 parse_pgm_inst_t cmd_opponent = {
237 .f = cmd_opponent_parsed, /* function to call */
238 .data = NULL, /* 2nd arg of func */
239 .help_str = help_opponent,
240 .tokens = { /* token list, NULL terminated */
241 (prog_void *)&cmd_opponent_arg0,
242 (prog_void *)&cmd_opponent_arg1,
248 prog_char str_opponent_arg1_set[] = "set";
249 parse_pgm_token_string_t cmd_opponent_arg1_set = TOKEN_STRING_INITIALIZER(struct cmd_opponent_result, arg1, str_opponent_arg1_set);
250 parse_pgm_token_num_t cmd_opponent_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_opponent_result, arg2, INT32);
251 parse_pgm_token_num_t cmd_opponent_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_opponent_result, arg3, INT32);
253 prog_char help_opponent_set[] = "Set (x,y) opponent";
254 parse_pgm_inst_t cmd_opponent_set = {
255 .f = cmd_opponent_parsed, /* function to call */
256 .data = NULL, /* 2nd arg of func */
257 .help_str = help_opponent_set,
258 .tokens = { /* token list, NULL terminated */
259 (prog_void *)&cmd_opponent_arg0,
260 (prog_void *)&cmd_opponent_arg1_set,
261 (prog_void *)&cmd_opponent_arg2,
262 (prog_void *)&cmd_opponent_arg3,
268 /**********************************************************/
271 /* this structure is filled when cmd_start is parsed successfully */
272 struct cmd_start_result {
274 fixed_string_t color;
275 fixed_string_t debug;
278 /* function called when cmd_start is parsed successfully */
279 static void cmd_start_parsed(void *parsed_result, void *data)
281 struct cmd_start_result *res = parsed_result;
282 uint8_t old_level = gen.log_level;
284 gen.logs[NB_LOGS] = E_USER_STRAT;
285 if (!strcmp_P(res->debug, PSTR("debug"))) {
286 strat_infos.dump_enabled = 1;
290 strat_infos.dump_enabled = 0;
294 if (!strcmp_P(res->color, PSTR("red"))) {
295 mainboard.our_color = I2C_COLOR_RED;
296 i2c_set_color(I2C_MECHBOARD_ADDR, I2C_COLOR_RED);
297 i2c_set_color(I2C_SENSORBOARD_ADDR, I2C_COLOR_RED);
299 else if (!strcmp_P(res->color, PSTR("green"))) {
300 mainboard.our_color = I2C_COLOR_GREEN;
301 i2c_set_color(I2C_MECHBOARD_ADDR, I2C_COLOR_GREEN);
302 i2c_set_color(I2C_SENSORBOARD_ADDR, I2C_COLOR_GREEN);
305 printf_P(PSTR("Check that lintel is loaded\r\n"));
306 while(!cmdline_keypressed());
308 printf_P(PSTR("Press a key when beacon ready\r\n"));
309 i2c_sensorboard_set_beacon(0);
310 while(!cmdline_keypressed());
311 i2c_sensorboard_set_beacon(1);
315 gen.logs[NB_LOGS] = 0;
316 gen.log_level = old_level;
319 prog_char str_start_arg0[] = "start";
320 parse_pgm_token_string_t cmd_start_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_start_result, arg0, str_start_arg0);
321 prog_char str_start_color[] = "green#red";
322 parse_pgm_token_string_t cmd_start_color = TOKEN_STRING_INITIALIZER(struct cmd_start_result, color, str_start_color);
323 prog_char str_start_debug[] = "debug#match";
324 parse_pgm_token_string_t cmd_start_debug = TOKEN_STRING_INITIALIZER(struct cmd_start_result, debug, str_start_debug);
326 prog_char help_start[] = "Start the robot";
327 parse_pgm_inst_t cmd_start = {
328 .f = cmd_start_parsed, /* function to call */
329 .data = NULL, /* 2nd arg of func */
330 .help_str = help_start,
331 .tokens = { /* token list, NULL terminated */
332 (prog_void *)&cmd_start_arg0,
333 (prog_void *)&cmd_start_color,
334 (prog_void *)&cmd_start_debug,
341 /**********************************************************/
344 /* this structure is filled when cmd_interact is parsed successfully */
345 struct cmd_interact_result {
349 static void print_cs(void)
351 printf_P(PSTR("cons_d=% .8ld cons_a=% .8ld fil_d=% .8ld fil_a=% .8ld "
352 "err_d=% .8ld err_a=% .8ld out_d=% .8ld out_a=% .8ld\r\n"),
353 cs_get_consign(&mainboard.distance.cs),
354 cs_get_consign(&mainboard.angle.cs),
355 cs_get_filtered_consign(&mainboard.distance.cs),
356 cs_get_filtered_consign(&mainboard.angle.cs),
357 cs_get_error(&mainboard.distance.cs),
358 cs_get_error(&mainboard.angle.cs),
359 cs_get_out(&mainboard.distance.cs),
360 cs_get_out(&mainboard.angle.cs));
363 static void print_pos(void)
365 printf_P(PSTR("x=% .8d y=% .8d a=% .8d\r\n"),
366 position_get_x_s16(&mainboard.pos),
367 position_get_y_s16(&mainboard.pos),
368 position_get_a_deg_s16(&mainboard.pos));
371 static void print_time(void)
373 printf_P(PSTR("time %d\r\n"),time_get_s());
377 static void print_sensors(void)
380 if (sensor_start_switch())
381 printf_P(PSTR("Start switch | "));
383 printf_P(PSTR(" | "));
385 if (IR_DISP_SENSOR())
386 printf_P(PSTR("IR disp | "));
388 printf_P(PSTR(" | "));
390 printf_P(PSTR("\r\n"));
394 static void print_pid(void)
396 printf_P(PSTR("P=% .8ld I=% .8ld D=% .8ld out=% .8ld | "
397 "P=% .8ld I=% .8ld D=% .8ld out=% .8ld\r\n"),
398 pid_get_value_in(&mainboard.distance.pid) * pid_get_gain_P(&mainboard.distance.pid),
399 pid_get_value_I(&mainboard.distance.pid) * pid_get_gain_I(&mainboard.distance.pid),
400 pid_get_value_D(&mainboard.distance.pid) * pid_get_gain_D(&mainboard.distance.pid),
401 pid_get_value_out(&mainboard.distance.pid),
402 pid_get_value_in(&mainboard.angle.pid) * pid_get_gain_P(&mainboard.angle.pid),
403 pid_get_value_I(&mainboard.angle.pid) * pid_get_gain_I(&mainboard.angle.pid),
404 pid_get_value_D(&mainboard.angle.pid) * pid_get_gain_D(&mainboard.angle.pid),
405 pid_get_value_out(&mainboard.angle.pid));
408 #define PRINT_POS (1<<0)
409 #define PRINT_PID (1<<1)
410 #define PRINT_CS (1<<2)
411 #define PRINT_SENSORS (1<<3)
412 #define PRINT_TIME (1<<4)
413 #define PRINT_BLOCKING (1<<5)
415 static void cmd_interact_parsed(void * parsed_result, void * data)
424 printf_P(PSTR("Display debugs:\r\n"
430 /* " 6:blocking\r\n" */
437 mainboard.flags &= (~DO_CS);
438 pwm_set_and_save(LEFT_PWM, 0);
439 pwm_set_and_save(RIGHT_PWM, 0);
442 if (print & PRINT_POS) {
446 if (print & PRINT_PID) {
450 if (print & PRINT_CS) {
454 if (print & PRINT_SENSORS) {
458 if (print & PRINT_TIME) {
461 /* if (print & PRINT_BLOCKING) { */
462 /* printf_P(PSTR("%s %s blocking=%d\r\n"), */
463 /* mainboard.blocking ? "BLOCK1":" ", */
464 /* rs_is_blocked(&mainboard.rs) ? "BLOCK2":" ", */
465 /* rs_get_blocking(&mainboard.rs)); */
468 c = cmdline_getchar();
473 cmd = vt100_parser(&vt100, c);
481 case '1': print ^= PRINT_POS; break;
482 case '2': print ^= PRINT_PID; break;
483 case '3': print ^= PRINT_CS; break;
484 case '4': print ^= PRINT_SENSORS; break;
485 case '5': print ^= PRINT_TIME; break;
486 case '6': print ^= PRINT_BLOCKING; break;
489 if (mainboard.flags & DO_CS)
491 pwm_set_and_save(LEFT_PWM, 0);
492 pwm_set_and_save(RIGHT_PWM, 0);
495 pwm_set_and_save(LEFT_PWM, 0);
496 pwm_set_and_save(RIGHT_PWM, 0);
505 pwm_set_and_save(LEFT_PWM, 1200);
506 pwm_set_and_save(RIGHT_PWM, 1200);
509 pwm_set_and_save(LEFT_PWM, -1200);
510 pwm_set_and_save(RIGHT_PWM, 1200);
513 pwm_set_and_save(LEFT_PWM, -1200);
514 pwm_set_and_save(RIGHT_PWM, -1200);
517 pwm_set_and_save(LEFT_PWM, 1200);
518 pwm_set_and_save(RIGHT_PWM, -1200);
526 prog_char str_interact_arg0[] = "interact";
527 parse_pgm_token_string_t cmd_interact_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_interact_result, arg0, str_interact_arg0);
529 prog_char help_interact[] = "Interactive mode";
530 parse_pgm_inst_t cmd_interact = {
531 .f = cmd_interact_parsed, /* function to call */
532 .data = NULL, /* 2nd arg of func */
533 .help_str = help_interact,
534 .tokens = { /* token list, NULL terminated */
535 (prog_void *)&cmd_interact_arg0,
541 /**********************************************************/
544 /* this structure is filled when cmd_color is parsed successfully */
545 struct cmd_color_result {
547 fixed_string_t color;
550 /* function called when cmd_color is parsed successfully */
551 static void cmd_color_parsed(void *parsed_result, void *data)
553 struct cmd_color_result *res = (struct cmd_color_result *) parsed_result;
554 if (!strcmp_P(res->color, PSTR("red"))) {
555 mainboard.our_color = I2C_COLOR_RED;
556 i2c_set_color(I2C_MECHBOARD_ADDR, I2C_COLOR_RED);
557 i2c_set_color(I2C_SENSORBOARD_ADDR, I2C_COLOR_RED);
559 else if (!strcmp_P(res->color, PSTR("green"))) {
560 mainboard.our_color = I2C_COLOR_GREEN;
561 i2c_set_color(I2C_MECHBOARD_ADDR, I2C_COLOR_GREEN);
562 i2c_set_color(I2C_SENSORBOARD_ADDR, I2C_COLOR_GREEN);
564 printf_P(PSTR("Done\r\n"));
567 prog_char str_color_arg0[] = "color";
568 parse_pgm_token_string_t cmd_color_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_color_result, arg0, str_color_arg0);
569 prog_char str_color_color[] = "green#red";
570 parse_pgm_token_string_t cmd_color_color = TOKEN_STRING_INITIALIZER(struct cmd_color_result, color, str_color_color);
572 prog_char help_color[] = "Set our color";
573 parse_pgm_inst_t cmd_color = {
574 .f = cmd_color_parsed, /* function to call */
575 .data = NULL, /* 2nd arg of func */
576 .help_str = help_color,
577 .tokens = { /* token list, NULL terminated */
578 (prog_void *)&cmd_color_arg0,
579 (prog_void *)&cmd_color_color,
585 /**********************************************************/
588 /* this structure is filled when cmd_rs is parsed successfully */
589 struct cmd_rs_result {
594 /* function called when cmd_rs is parsed successfully */
595 static void cmd_rs_parsed(void *parsed_result, void *data)
597 // struct cmd_rs_result *res = parsed_result;
599 printf_P(PSTR("angle cons=% .6ld in=% .6ld out=% .6ld / "),
600 cs_get_consign(&mainboard.angle.cs),
601 cs_get_filtered_feedback(&mainboard.angle.cs),
602 cs_get_out(&mainboard.angle.cs));
603 printf_P(PSTR("distance cons=% .6ld in=% .6ld out=% .6ld / "),
604 cs_get_consign(&mainboard.distance.cs),
605 cs_get_filtered_feedback(&mainboard.distance.cs),
606 cs_get_out(&mainboard.distance.cs));
607 printf_P(PSTR("l=% .4ld r=% .4ld\r\n"), mainboard.pwm_l,
610 } while(!cmdline_keypressed());
613 prog_char str_rs_arg0[] = "rs";
614 parse_pgm_token_string_t cmd_rs_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_rs_result, arg0, str_rs_arg0);
615 prog_char str_rs_arg1[] = "show";
616 parse_pgm_token_string_t cmd_rs_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_rs_result, arg1, str_rs_arg1);
618 prog_char help_rs[] = "Show rs (robot system) values";
619 parse_pgm_inst_t cmd_rs = {
620 .f = cmd_rs_parsed, /* function to call */
621 .data = NULL, /* 2nd arg of func */
623 .tokens = { /* token list, NULL terminated */
624 (prog_void *)&cmd_rs_arg0,
625 (prog_void *)&cmd_rs_arg1,
630 /**********************************************************/
633 /* this structure is filled when cmd_i2cdebug is parsed successfully */
634 struct cmd_i2cdebug_result {
638 /* function called when cmd_i2cdebug is parsed successfully */
639 static void cmd_i2cdebug_parsed(void * parsed_result, void * data)
642 i2c_protocol_debug();
645 prog_char str_i2cdebug_arg0[] = "i2cdebug";
646 parse_pgm_token_string_t cmd_i2cdebug_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_i2cdebug_result, arg0, str_i2cdebug_arg0);
648 prog_char help_i2cdebug[] = "I2c debug infos";
649 parse_pgm_inst_t cmd_i2cdebug = {
650 .f = cmd_i2cdebug_parsed, /* function to call */
651 .data = NULL, /* 2nd arg of func */
652 .help_str = help_i2cdebug,
653 .tokens = { /* token list, NULL terminated */
654 (prog_void *)&cmd_i2cdebug_arg0,
659 /**********************************************************/
662 /* this structure is filled when cmd_mechboard_show is parsed successfully */
663 struct cmd_mechboard_show_result {
668 /* function called when cmd_mechboard_show is parsed successfully */
669 static void cmd_mechboard_show_parsed(void * parsed_result, void * data)
671 printf_P(PSTR("mode = %x\r\n"), mechboard.mode);
672 printf_P(PSTR("status = %x\r\n"), mechboard.status);
673 printf_P(PSTR("lintel_count = %d\r\n"), mechboard.lintel_count);
675 printf_P(PSTR("column_count = %d\r\n"), get_column_count());
676 printf_P(PSTR("left1=%d left2=%d right1=%d right2=%d\r\n"),
677 pump_left1_is_full(), pump_left2_is_full(),
678 pump_right1_is_full(), pump_right2_is_full());
680 printf_P(PSTR("pump_left1 = %d\r\n"), mechboard.pump_left1);
681 printf_P(PSTR("pump_left2 = %d\r\n"), mechboard.pump_left2);
682 printf_P(PSTR("pump_right1 = %d\r\n"), mechboard.pump_right1);
683 printf_P(PSTR("pump_right2 = %d\r\n"), mechboard.pump_right2);
685 printf_P(PSTR("servo_lintel_left = %d\r\n"), mechboard.servo_lintel_left);
686 printf_P(PSTR("servo_lintel_right = %d\r\n"), mechboard.servo_lintel_right);
690 prog_char str_mechboard_show_arg0[] = "mechboard";
691 parse_pgm_token_string_t cmd_mechboard_show_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_show_result, arg0, str_mechboard_show_arg0);
692 prog_char str_mechboard_show_arg1[] = "show";
693 parse_pgm_token_string_t cmd_mechboard_show_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_show_result, arg1, str_mechboard_show_arg1);
695 prog_char help_mechboard_show[] = "show mechboard status";
696 parse_pgm_inst_t cmd_mechboard_show = {
697 .f = cmd_mechboard_show_parsed, /* function to call */
698 .data = NULL, /* 2nd arg of func */
699 .help_str = help_mechboard_show,
700 .tokens = { /* token list, NULL terminated */
701 (prog_void *)&cmd_mechboard_show_arg0,
702 (prog_void *)&cmd_mechboard_show_arg1,
707 /**********************************************************/
708 /* Mechboard_Setmode1 */
710 /* this structure is filled when cmd_mechboard_setmode1 is parsed successfully */
711 struct cmd_mechboard_setmode1_result {
716 /* function called when cmd_mechboard_setmode1 is parsed successfully */
717 static void cmd_mechboard_setmode1_parsed(void *parsed_result, void *data)
719 struct cmd_mechboard_setmode1_result *res = parsed_result;
721 if (!strcmp_P(res->arg1, PSTR("init")))
722 i2c_mechboard_mode_init();
723 else if (!strcmp_P(res->arg1, PSTR("manual")))
724 i2c_mechboard_mode_manual();
725 else if (!strcmp_P(res->arg1, PSTR("pickup")))
726 i2c_mechboard_mode_pickup();
727 else if (!strcmp_P(res->arg1, PSTR("lazy_harvest")))
728 i2c_mechboard_mode_lazy_harvest();
729 else if (!strcmp_P(res->arg1, PSTR("harvest")))
730 i2c_mechboard_mode_harvest();
731 else if (!strcmp_P(res->arg1, PSTR("prepare_get_lintel")))
732 i2c_mechboard_mode_prepare_get_lintel();
733 else if (!strcmp_P(res->arg1, PSTR("get_lintel")))
734 i2c_mechboard_mode_get_lintel();
735 else if (!strcmp_P(res->arg1, PSTR("put_lintel")))
736 i2c_mechboard_mode_put_lintel();
737 else if (!strcmp_P(res->arg1, PSTR("init")))
738 i2c_mechboard_mode_init();
739 else if (!strcmp_P(res->arg1, PSTR("eject")))
740 i2c_mechboard_mode_init();
741 else if (!strcmp_P(res->arg1, PSTR("clear")))
742 i2c_mechboard_mode_clear();
743 else if (!strcmp_P(res->arg1, PSTR("loaded")))
744 i2c_mechboard_mode_loaded();
745 else if (!strcmp_P(res->arg1, PSTR("store")))
746 i2c_mechboard_mode_store();
747 else if (!strcmp_P(res->arg1, PSTR("manivelle")))
748 i2c_mechboard_mode_manivelle();
749 else if (!strcmp_P(res->arg1, PSTR("lazy_pickup")))
750 i2c_mechboard_mode_lazy_pickup();
753 prog_char str_mechboard_setmode1_arg0[] = "mechboard";
754 parse_pgm_token_string_t cmd_mechboard_setmode1_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode1_result, arg0, str_mechboard_setmode1_arg0);
755 prog_char str_mechboard_setmode1_arg1[] = "manivelle#init#manual#pickup#prepare_get_lintel#get_lintel#put_lintel1#eject#clear#harvest#lazy_harvest#store#lazy_pickup";
756 parse_pgm_token_string_t cmd_mechboard_setmode1_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode1_result, arg1, str_mechboard_setmode1_arg1);
758 prog_char help_mechboard_setmode1[] = "set mechboard mode (mode)";
759 parse_pgm_inst_t cmd_mechboard_setmode1 = {
760 .f = cmd_mechboard_setmode1_parsed, /* function to call */
761 .data = NULL, /* 2nd arg of func */
762 .help_str = help_mechboard_setmode1,
763 .tokens = { /* token list, NULL terminated */
764 (prog_void *)&cmd_mechboard_setmode1_arg0,
765 (prog_void *)&cmd_mechboard_setmode1_arg1,
770 /**********************************************************/
771 /* Mechboard_Setmode2 */
773 /* this structure is filled when cmd_mechboard_setmode2 is parsed successfully */
774 struct cmd_mechboard_setmode2_result {
780 /* function called when cmd_mechboard_setmode2 is parsed successfully */
781 static void cmd_mechboard_setmode2_parsed(void * parsed_result, void * data)
783 struct cmd_mechboard_setmode2_result *res = parsed_result;
784 uint8_t side = I2C_LEFT_SIDE;
786 if (!strcmp_P(res->arg2, PSTR("left")))
787 side = I2C_LEFT_SIDE;
788 else if (!strcmp_P(res->arg2, PSTR("right")))
789 side = I2C_RIGHT_SIDE;
790 else if (!strcmp_P(res->arg2, PSTR("center")))
791 side = I2C_CENTER_SIDE;
792 else if (!strcmp_P(res->arg2, PSTR("auto")))
793 side = I2C_AUTO_SIDE;
795 if (!strcmp_P(res->arg1, PSTR("prepare_pickup")))
796 i2c_mechboard_mode_prepare_pickup(side);
797 else if (!strcmp_P(res->arg1, PSTR("push_temple_disc")))
798 i2c_mechboard_mode_push_temple_disc(side);
801 prog_char str_mechboard_setmode2_arg0[] = "mechboard";
802 parse_pgm_token_string_t cmd_mechboard_setmode2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode2_result, arg0, str_mechboard_setmode2_arg0);
803 prog_char str_mechboard_setmode2_arg1[] = "prepare_pickup#push_temple_disc";
804 parse_pgm_token_string_t cmd_mechboard_setmode2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode2_result, arg1, str_mechboard_setmode2_arg1);
805 prog_char str_mechboard_setmode2_arg2[] = "left#right#auto#center";
806 parse_pgm_token_string_t cmd_mechboard_setmode2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode2_result, arg2, str_mechboard_setmode2_arg2);
808 prog_char help_mechboard_setmode2[] = "set mechboard mode (more, side)";
809 parse_pgm_inst_t cmd_mechboard_setmode2 = {
810 .f = cmd_mechboard_setmode2_parsed, /* function to call */
811 .data = NULL, /* 2nd arg of func */
812 .help_str = help_mechboard_setmode2,
813 .tokens = { /* token list, NULL terminated */
814 (prog_void *)&cmd_mechboard_setmode2_arg0,
815 (prog_void *)&cmd_mechboard_setmode2_arg1,
816 (prog_void *)&cmd_mechboard_setmode2_arg2,
821 /**********************************************************/
822 /* Mechboard_Setmode3 */
824 /* this structure is filled when cmd_mechboard_setmode3 is parsed successfully */
825 struct cmd_mechboard_setmode3_result {
831 /* function called when cmd_mechboard_setmode3 is parsed successfully */
832 static void cmd_mechboard_setmode3_parsed(void *parsed_result, void *data)
834 struct cmd_mechboard_setmode3_result *res = parsed_result;
835 if (!strcmp_P(res->arg1, PSTR("autobuild")))
836 i2c_mechboard_mode_simple_autobuild(res->level);
837 else if (!strcmp_P(res->arg1, PSTR("prepare_build")))
838 i2c_mechboard_mode_prepare_build_both(res->level);
839 else if (!strcmp_P(res->arg1, PSTR("prepare_inside")))
840 i2c_mechboard_mode_prepare_inside_both(res->level);
841 else if (!strcmp_P(res->arg1, PSTR("push_temple")))
842 i2c_mechboard_mode_push_temple(res->level);
845 prog_char str_mechboard_setmode3_arg0[] = "mechboard";
846 parse_pgm_token_string_t cmd_mechboard_setmode3_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode3_result, arg0, str_mechboard_setmode3_arg0);
847 prog_char str_mechboard_setmode3_arg1[] = "autobuild#prepare_build#prepare_inside#push_temple";
848 parse_pgm_token_string_t cmd_mechboard_setmode3_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode3_result, arg1, str_mechboard_setmode3_arg1);
849 parse_pgm_token_num_t cmd_mechboard_setmode3_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_mechboard_setmode3_result, level, UINT8);
851 prog_char help_mechboard_setmode3[] = "set mechboard mode (mode, level)";
852 parse_pgm_inst_t cmd_mechboard_setmode3 = {
853 .f = cmd_mechboard_setmode3_parsed, /* function to call */
854 .data = NULL, /* 2nd arg of func */
855 .help_str = help_mechboard_setmode3,
856 .tokens = { /* token list, NULL terminated */
857 (prog_void *)&cmd_mechboard_setmode3_arg0,
858 (prog_void *)&cmd_mechboard_setmode3_arg1,
859 (prog_void *)&cmd_mechboard_setmode3_arg2,
864 /**********************************************************/
865 /* Mechboard_Setmode4 */
867 /* this structure is filled when cmd_mechboard_setmode4 is parsed successfully */
868 struct cmd_mechboard_setmode4_result {
880 /* function called when cmd_mechboard_setmode4 is parsed successfully */
881 static void cmd_mechboard_setmode4_parsed(void *parsed_result, void *data)
883 struct cmd_mechboard_setmode4_result *res = parsed_result;
884 i2c_mechboard_mode_autobuild(res->level_l, res->count_l, res->dist_l,
885 res->level_r, res->count_r, res->dist_r,
889 prog_char str_mechboard_setmode4_arg0[] = "mechboard";
890 parse_pgm_token_string_t cmd_mechboard_setmode4_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode4_result, arg0, str_mechboard_setmode4_arg0);
891 prog_char str_mechboard_setmode4_arg1[] = "autobuild";
892 parse_pgm_token_string_t cmd_mechboard_setmode4_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode4_result, arg1, str_mechboard_setmode4_arg1);
893 parse_pgm_token_num_t cmd_mechboard_setmode4_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_mechboard_setmode4_result, level_l, UINT8);
894 parse_pgm_token_num_t cmd_mechboard_setmode4_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_mechboard_setmode4_result, count_l, UINT8);
895 parse_pgm_token_num_t cmd_mechboard_setmode4_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_mechboard_setmode4_result, dist_l, UINT8);
896 parse_pgm_token_num_t cmd_mechboard_setmode4_arg5 = TOKEN_NUM_INITIALIZER(struct cmd_mechboard_setmode4_result, level_r, UINT8);
897 parse_pgm_token_num_t cmd_mechboard_setmode4_arg6 = TOKEN_NUM_INITIALIZER(struct cmd_mechboard_setmode4_result, count_r, UINT8);
898 parse_pgm_token_num_t cmd_mechboard_setmode4_arg7 = TOKEN_NUM_INITIALIZER(struct cmd_mechboard_setmode4_result, dist_r, UINT8);
899 parse_pgm_token_num_t cmd_mechboard_setmode4_arg8 = TOKEN_NUM_INITIALIZER(struct cmd_mechboard_setmode4_result, do_lintel, UINT8);
901 prog_char help_mechboard_setmode4[] = "set mechboard mode (autobuild level_l count_l dist_l level_r count_r dist_r lintel)";
902 parse_pgm_inst_t cmd_mechboard_setmode4 = {
903 .f = cmd_mechboard_setmode4_parsed, /* function to call */
904 .data = NULL, /* 2nd arg of func */
905 .help_str = help_mechboard_setmode4,
906 .tokens = { /* token list, NULL terminated */
907 (prog_void *)&cmd_mechboard_setmode4_arg0,
908 (prog_void *)&cmd_mechboard_setmode4_arg1,
909 (prog_void *)&cmd_mechboard_setmode4_arg2,
910 (prog_void *)&cmd_mechboard_setmode4_arg3,
911 (prog_void *)&cmd_mechboard_setmode4_arg4,
912 (prog_void *)&cmd_mechboard_setmode4_arg5,
913 (prog_void *)&cmd_mechboard_setmode4_arg6,
914 (prog_void *)&cmd_mechboard_setmode4_arg7,
915 (prog_void *)&cmd_mechboard_setmode4_arg8,
920 /**********************************************************/
921 /* Mechboard_Setmode5 */
923 /* this structure is filled when cmd_mechboard_setmode5 is parsed successfully */
924 struct cmd_mechboard_setmode5_result {
931 /* function called when cmd_mechboard_setmode5 is parsed successfully */
932 static void cmd_mechboard_setmode5_parsed(void *parsed_result, void * data)
934 struct cmd_mechboard_setmode5_result *res = parsed_result;
935 uint8_t side = I2C_LEFT_SIDE, next_mode = I2C_MECHBOARD_MODE_PREPARE_PICKUP;
937 if (!strcmp_P(res->arg2, PSTR("left")))
938 side = I2C_LEFT_SIDE;
939 else if (!strcmp_P(res->arg2, PSTR("right")))
940 side = I2C_RIGHT_SIDE;
941 else if (!strcmp_P(res->arg2, PSTR("center")))
942 side = I2C_CENTER_SIDE;
943 else if (!strcmp_P(res->arg2, PSTR("auto")))
944 side = I2C_AUTO_SIDE;
946 if (!strcmp_P(res->arg3, PSTR("harvest")))
947 next_mode = I2C_MECHBOARD_MODE_HARVEST;
948 else if (!strcmp_P(res->arg3, PSTR("lazy_harvest")))
949 next_mode = I2C_MECHBOARD_MODE_LAZY_HARVEST;
950 else if (!strcmp_P(res->arg3, PSTR("pickup")))
951 next_mode = I2C_MECHBOARD_MODE_PICKUP;
952 else if (!strcmp_P(res->arg3, PSTR("clear")))
953 next_mode = I2C_MECHBOARD_MODE_CLEAR;
954 else if (!strcmp_P(res->arg3, PSTR("store")))
955 next_mode = I2C_MECHBOARD_MODE_STORE;
956 else if (!strcmp_P(res->arg3, PSTR("lazy_pickup")))
957 next_mode = I2C_MECHBOARD_MODE_LAZY_PICKUP;
959 if (!strcmp_P(res->arg1, PSTR("prepare_pickup")))
960 i2c_mechboard_mode_prepare_pickup_next(side, next_mode);
963 prog_char str_mechboard_setmode5_arg0[] = "mechboard";
964 parse_pgm_token_string_t cmd_mechboard_setmode5_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode5_result, arg0, str_mechboard_setmode5_arg0);
965 prog_char str_mechboard_setmode5_arg1[] = "prepare_pickup";
966 parse_pgm_token_string_t cmd_mechboard_setmode5_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode5_result, arg1, str_mechboard_setmode5_arg1);
967 prog_char str_mechboard_setmode5_arg2[] = "left#right#auto#center";
968 parse_pgm_token_string_t cmd_mechboard_setmode5_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode5_result, arg2, str_mechboard_setmode5_arg2);
969 prog_char str_mechboard_setmode5_arg3[] = "harvest#pickup#store#lazy_harvest#lazy_pickup#clear";
970 parse_pgm_token_string_t cmd_mechboard_setmode5_arg3 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode5_result, arg3, str_mechboard_setmode5_arg3);
972 prog_char help_mechboard_setmode5[] = "set mechboard mode (more, side)";
973 parse_pgm_inst_t cmd_mechboard_setmode5 = {
974 .f = cmd_mechboard_setmode5_parsed, /* function to call */
975 .data = NULL, /* 2nd arg of func */
976 .help_str = help_mechboard_setmode5,
977 .tokens = { /* token list, NULL terminated */
978 (prog_void *)&cmd_mechboard_setmode5_arg0,
979 (prog_void *)&cmd_mechboard_setmode5_arg1,
980 (prog_void *)&cmd_mechboard_setmode5_arg2,
981 (prog_void *)&cmd_mechboard_setmode5_arg3,
986 /**********************************************************/
989 /* this structure is filled when cmd_pickup_wheels is parsed successfully */
990 struct cmd_pickup_wheels_result {
995 /* function called when cmd_pickup_wheels is parsed successfully */
996 static void cmd_pickup_wheels_parsed(void *parsed_result, void *data)
998 struct cmd_pickup_wheels_result *res = parsed_result;
1000 if (!strcmp_P(res->arg1, PSTR("on")))
1003 pickup_wheels_off();
1006 prog_char str_pickup_wheels_arg0[] = "pickup_wheels";
1007 parse_pgm_token_string_t cmd_pickup_wheels_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_pickup_wheels_result, arg0, str_pickup_wheels_arg0);
1008 prog_char str_pickup_wheels_arg1[] = "on#off";
1009 parse_pgm_token_string_t cmd_pickup_wheels_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_pickup_wheels_result, arg1, str_pickup_wheels_arg1);
1011 prog_char help_pickup_wheels[] = "Enable/disable pickup wheels";
1012 parse_pgm_inst_t cmd_pickup_wheels = {
1013 .f = cmd_pickup_wheels_parsed, /* function to call */
1014 .data = NULL, /* 2nd arg of func */
1015 .help_str = help_pickup_wheels,
1016 .tokens = { /* token list, NULL terminated */
1017 (prog_void *)&cmd_pickup_wheels_arg0,
1018 (prog_void *)&cmd_pickup_wheels_arg1,
1023 /**********************************************************/
1026 /* this structure is filled when cmd_beacon_start is parsed successfully */
1027 struct cmd_beacon_start_result {
1028 fixed_string_t arg0;
1029 fixed_string_t arg1;
1032 /* function called when cmd_beacon_start is parsed successfully */
1033 static void cmd_beacon_start_parsed(void *parsed_result, void *data)
1035 struct cmd_beacon_start_result *res = parsed_result;
1037 if (!strcmp_P(res->arg1, PSTR("start")))
1038 i2c_sensorboard_set_beacon(1);
1040 i2c_sensorboard_set_beacon(0);
1043 prog_char str_beacon_start_arg0[] = "beacon";
1044 parse_pgm_token_string_t cmd_beacon_start_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_beacon_start_result, arg0, str_beacon_start_arg0);
1045 prog_char str_beacon_start_arg1[] = "start#stop";
1046 parse_pgm_token_string_t cmd_beacon_start_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_beacon_start_result, arg1, str_beacon_start_arg1);
1048 prog_char help_beacon_start[] = "Beacon enabled/disable";
1049 parse_pgm_inst_t cmd_beacon_start = {
1050 .f = cmd_beacon_start_parsed, /* function to call */
1051 .data = NULL, /* 2nd arg of func */
1052 .help_str = help_beacon_start,
1053 .tokens = { /* token list, NULL terminated */
1054 (prog_void *)&cmd_beacon_start_arg0,
1055 (prog_void *)&cmd_beacon_start_arg1,
1060 /**********************************************************/
1063 /* this structure is filled when cmd_pump_current is parsed successfully */
1064 struct cmd_pump_current_result {
1065 fixed_string_t arg0;
1066 fixed_string_t arg1;
1069 /* function called when cmd_pump_current is parsed successfully */
1070 static void cmd_pump_current_parsed(__attribute__((unused)) void *parsed_result,
1071 __attribute__((unused)) void *data)
1073 printf_P(PSTR("l1=%d l2=%d r1=%d r2=%d\r\n"),
1074 sensor_get_adc(ADC_CSENSE3), sensor_get_adc(ADC_CSENSE4),
1075 mechboard.pump_right1_current, mechboard.pump_right2_current);
1078 prog_char str_pump_current_arg0[] = "pump_current";
1079 parse_pgm_token_string_t cmd_pump_current_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_pump_current_result, arg0, str_pump_current_arg0);
1080 prog_char str_pump_current_arg1[] = "show";
1081 parse_pgm_token_string_t cmd_pump_current_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_pump_current_result, arg1, str_pump_current_arg1);
1083 prog_char help_pump_current[] = "dump pump current";
1084 parse_pgm_inst_t cmd_pump_current = {
1085 .f = cmd_pump_current_parsed, /* function to call */
1086 .data = NULL, /* 2nd arg of func */
1087 .help_str = help_pump_current,
1088 .tokens = { /* token list, NULL terminated */
1089 (prog_void *)&cmd_pump_current_arg0,
1090 (prog_void *)&cmd_pump_current_arg1,
1095 /**********************************************************/
1098 /* this structure is filled when cmd_build_test is parsed successfully */
1099 struct cmd_build_test_result {
1100 fixed_string_t arg0;
1103 /* function called when cmd_build_test is parsed successfully */
1104 static void cmd_build_test_parsed(void *parsed_result, void *data)
1106 //struct cmd_build_test_result *res = parsed_result;
1108 printf_P(PSTR("lintel must be there\r\n"));
1109 i2c_mechboard_mode_prepare_pickup_next(I2C_LEFT_SIDE,
1110 I2C_MECHBOARD_MODE_HARVEST);
1113 printf_P(PSTR("Insert 4 colums\r\n"));
1114 while (get_column_count() != 4);
1116 i2c_mechboard_mode_prepare_build_both(0);
1117 trajectory_d_rel(&mainboard.traj, 200);
1118 wait_traj_end(TRAJ_FLAGS_NO_NEAR);
1121 i2c_mechboard_mode_simple_autobuild(0);
1123 while (get_mechboard_mode() == I2C_MECHBOARD_MODE_AUTOBUILD);
1125 trajectory_d_rel(&mainboard.traj, -200);
1126 wait_traj_end(TRAJ_FLAGS_NO_NEAR);
1127 i2c_mechboard_mode_prepare_pickup_next(I2C_LEFT_SIDE,
1128 I2C_MECHBOARD_MODE_HARVEST);
1130 while (get_column_count() != 3);
1132 i2c_mechboard_mode_prepare_build_both(3);
1133 trajectory_d_rel(&mainboard.traj, 200);
1134 wait_traj_end(TRAJ_FLAGS_NO_NEAR);
1137 i2c_mechboard_mode_autobuild(3, 1, I2C_AUTOBUILD_DEFAULT_DIST,
1138 3, 2,I2C_AUTOBUILD_DEFAULT_DIST, 0);
1139 i2cproto_wait_update();
1140 while (get_mechboard_mode() == I2C_MECHBOARD_MODE_AUTOBUILD);
1142 trajectory_d_rel(&mainboard.traj, -200);
1143 wait_traj_end(TRAJ_FLAGS_NO_NEAR);
1144 i2c_mechboard_mode_prepare_pickup(I2C_RIGHT_SIDE);
1147 i2c_mechboard_mode_harvest();
1148 while (get_column_count() != 3);
1150 i2c_mechboard_mode_prepare_build_both(5);
1151 trajectory_d_rel(&mainboard.traj, 200);
1152 wait_traj_end(TRAJ_FLAGS_NO_NEAR);
1155 i2c_mechboard_mode_autobuild(4, 2, I2C_AUTOBUILD_DEFAULT_DIST,
1156 5, 1, I2C_AUTOBUILD_DEFAULT_DIST, 0);
1157 i2cproto_wait_update();
1158 while (get_mechboard_mode() == I2C_MECHBOARD_MODE_AUTOBUILD);
1160 trajectory_d_rel(&mainboard.traj, -200);
1163 prog_char str_build_test_arg0[] = "build_test";
1164 parse_pgm_token_string_t cmd_build_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_build_test_result, arg0, str_build_test_arg0);
1166 prog_char help_build_test[] = "Build_Test function";
1167 parse_pgm_inst_t cmd_build_test = {
1168 .f = cmd_build_test_parsed, /* function to call */
1169 .data = NULL, /* 2nd arg of func */
1170 .help_str = help_build_test,
1171 .tokens = { /* token list, NULL terminated */
1172 (prog_void *)&cmd_build_test_arg0,
1178 /**********************************************************/
1181 /* this structure is filled when cmd_column_test is parsed successfully */
1182 struct cmd_column_test_result {
1183 fixed_string_t arg0;
1193 /* function called when cmd_column_test is parsed successfully */
1194 static void cmd_column_test_parsed(void *parsed_result, void *data)
1196 struct cmd_column_test_result *res = parsed_result;
1197 uint8_t level = res->level, debug = 0;
1198 uint8_t c, push = 0;
1206 res->arm_dist = 220;
1210 if (!strcmp_P(res->arg0, PSTR("column_test_debug")))
1212 if (!strcmp_P(res->arg0, PSTR("column_test_push")))
1215 strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_VERY_SLOW);
1219 trajectory_d_rel(&mainboard.traj, 200);
1220 wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
1222 /* go back, insert colums */
1224 trajectory_d_rel(&mainboard.traj, -200);
1225 wait_traj_end(TRAJ_FLAGS_NO_NEAR);
1227 i2c_mechboard_mode_prepare_pickup_next(I2C_LEFT_SIDE,
1228 I2C_MECHBOARD_MODE_HARVEST);
1229 printf_P(PSTR("Insert 4 colums\r\n"));
1230 while (get_column_count() != 4);
1232 /* build with left arm */
1234 i2c_mechboard_mode_prepare_inside_both(level);
1235 trajectory_d_rel(&mainboard.traj, 200-(res->dist));
1236 wait_traj_end(TRAJ_FLAGS_NO_NEAR);
1239 c = cmdline_getchar_wait();
1241 trajectory_a_rel(&mainboard.traj, res->a1);
1242 wait_traj_end(TRAJ_FLAGS_NO_NEAR);
1245 c = cmdline_getchar_wait();
1247 i2c_mechboard_mode_prepare_build_select(level, -1);
1250 c = cmdline_getchar_wait();
1251 i2c_mechboard_mode_autobuild(level, res->nb_col, res->arm_dist,
1252 0, 0, res->arm_dist, 0);
1253 while (get_mechboard_mode() != I2C_MECHBOARD_MODE_AUTOBUILD);
1254 while (get_mechboard_mode() == I2C_MECHBOARD_MODE_AUTOBUILD);
1257 c = cmdline_getchar_wait();
1258 i2c_mechboard_mode_prepare_inside_select(level+res->nb_col, -1);
1261 c = cmdline_getchar_wait();
1262 /* build with right arm */
1264 trajectory_a_rel(&mainboard.traj, res->a2);
1265 wait_traj_end(TRAJ_FLAGS_NO_NEAR);
1268 c = cmdline_getchar_wait();
1269 /* only ok for nb_col == 2 */
1270 if ((level + res->nb_col) >= 7)
1271 i2c_mechboard_mode_prepare_build_select(-1, level + res->nb_col + 1);
1273 i2c_mechboard_mode_prepare_build_select(-1, level + res->nb_col);
1276 c = cmdline_getchar_wait();
1277 i2c_mechboard_mode_autobuild(0, 0, res->arm_dist,
1278 level + res->nb_col, res->nb_col,
1280 while (get_mechboard_mode() != I2C_MECHBOARD_MODE_AUTOBUILD);
1281 while (get_mechboard_mode() == I2C_MECHBOARD_MODE_AUTOBUILD);
1285 strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST);
1286 trajectory_d_rel(&mainboard.traj, -100);
1287 wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
1288 i2c_mechboard_mode_push_temple_disc(I2C_RIGHT_SIDE);
1290 trajectory_d_rel(&mainboard.traj, 100);
1291 wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
1293 else if (level == 1 || level == 0) {
1294 trajectory_d_rel(&mainboard.traj, -100);
1295 wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
1296 i2c_mechboard_mode_push_temple(level);
1298 strat_set_speed(200, SPEED_ANGLE_SLOW);
1299 trajectory_d_rel(&mainboard.traj, 120);
1300 wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
1303 strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
1306 c = cmdline_getchar_wait();
1307 i2c_mechboard_mode_prepare_inside_select(-1, level+res->nb_col*2);
1310 c = cmdline_getchar_wait();
1312 trajectory_a_rel(&mainboard.traj, res->a3);
1313 wait_traj_end(TRAJ_FLAGS_NO_NEAR);
1316 c = cmdline_getchar_wait();
1317 /* go back, insert colums */
1319 trajectory_d_rel(&mainboard.traj, -100);
1324 prog_char str_column_test_arg0[] = "column_test#column_test_debug#column_test_push";
1325 parse_pgm_token_string_t cmd_column_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_column_test_result, arg0, str_column_test_arg0);
1326 parse_pgm_token_num_t cmd_column_test_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_column_test_result, level, UINT8);
1328 prog_char help_column_test[] = "Column_Test function (level)";
1329 parse_pgm_inst_t cmd_column_test = {
1330 .f = cmd_column_test_parsed, /* function to call */
1331 .data = (void *)1, /* 2nd arg of func */
1332 .help_str = help_column_test,
1333 .tokens = { /* token list, NULL terminated */
1334 (prog_void *)&cmd_column_test_arg0,
1335 (prog_void *)&cmd_column_test_arg1,
1340 parse_pgm_token_num_t cmd_column_test_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_column_test_result, dist, INT16);
1341 parse_pgm_token_num_t cmd_column_test_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_column_test_result, a1, INT8);
1342 parse_pgm_token_num_t cmd_column_test_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_column_test_result, a2, INT8);
1343 parse_pgm_token_num_t cmd_column_test_arg5 = TOKEN_NUM_INITIALIZER(struct cmd_column_test_result, a3, INT8);
1344 parse_pgm_token_num_t cmd_column_test_arg6 = TOKEN_NUM_INITIALIZER(struct cmd_column_test_result, arm_dist, INT16);
1345 parse_pgm_token_num_t cmd_column_test_arg7 = TOKEN_NUM_INITIALIZER(struct cmd_column_test_result, nb_col, INT8);
1347 prog_char help_column_test2[] = "Column_Test function (level, dist, a1, a2, a3, arm_dist, nb_col)";
1348 parse_pgm_inst_t cmd_column_test2 = {
1349 .f = cmd_column_test_parsed, /* function to call */
1350 .data = NULL, /* 2nd arg of func */
1351 .help_str = help_column_test2,
1352 .tokens = { /* token list, NULL terminated */
1353 (prog_void *)&cmd_column_test_arg0,
1354 (prog_void *)&cmd_column_test_arg1,
1355 (prog_void *)&cmd_column_test_arg2,
1356 (prog_void *)&cmd_column_test_arg3,
1357 (prog_void *)&cmd_column_test_arg4,
1358 (prog_void *)&cmd_column_test_arg5,
1359 (prog_void *)&cmd_column_test_arg6,
1360 (prog_void *)&cmd_column_test_arg7,
1366 /**********************************************************/
1369 /* this structure is filled when cmd_pickup_test is parsed successfully */
1370 struct cmd_pickup_test_result {
1371 fixed_string_t arg0;
1372 fixed_string_t arg1;
1376 /* return red or green sensor */
1377 #define COLOR_IR_SENSOR() \
1379 uint8_t __ret = 0; \
1380 if (side == I2C_RIGHT_SIDE) \
1381 __ret = sensor_get(S_DISP_RIGHT); \
1383 __ret = sensor_get(S_DISP_LEFT); \
1386 /* column dispensers */
1387 #define COL_SCAN_MARGIN 200
1388 /* distance between the wheel axis and the IR sensor */
1390 /* function called when cmd_pickup_test is parsed successfully */
1391 static void cmd_pickup_test_parsed(void *parsed_result, void *data)
1393 uint8_t err, side, first_try = 1;
1394 int8_t cols_count_before, cols_count_after, cols;
1395 struct cmd_pickup_test_result *res = parsed_result;
1396 int16_t pos1, pos2, pos;
1398 int16_t dist = res->dist;
1399 uint8_t timeout = 0;
1401 if (!strcmp_P(res->arg1, PSTR("left")))
1402 side = I2C_LEFT_SIDE;
1404 side = I2C_RIGHT_SIDE;
1406 i2c_mechboard_mode_prepare_pickup(I2C_AUTO_SIDE);
1407 cols_count_before = get_column_count();
1408 position_set(&mainboard.pos, 0, 0, 0);
1410 strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_FAST);
1411 trajectory_d_rel(&mainboard.traj, -1000);
1412 err = WAIT_COND_OR_TRAJ_END(!COLOR_IR_SENSOR(), TRAJ_FLAGS_NO_NEAR);
1413 if (err) /* we should not reach end */
1415 pos1 = position_get_x_s16(&mainboard.pos);
1416 printf_P(PSTR("pos1 = %d\r\n"), pos1);
1418 err = WAIT_COND_OR_TRAJ_END(COLOR_IR_SENSOR(), TRAJ_FLAGS_NO_NEAR);
1421 pos2 = position_get_x_s16(&mainboard.pos);
1422 printf_P(PSTR("pos2 = %d\r\n"), pos2);
1424 pos = ABS(pos1 - pos2);
1425 printf_P(PSTR("pos = %d\r\n"), pos);
1427 trajectory_d_rel(&mainboard.traj, -dist + pos/2);
1428 err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
1430 if (side == I2C_LEFT_SIDE)
1431 trajectory_a_rel(&mainboard.traj, 90);
1433 trajectory_a_rel(&mainboard.traj, -90);
1434 err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
1439 i2c_mechboard_mode_lazy_harvest();
1441 i2c_mechboard_mode_prepare_pickup(I2C_AUTO_SIDE);
1444 strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_FAST);
1445 trajectory_d_rel(&mainboard.traj, 300);
1446 err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST | END_NEAR);
1447 strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST);
1448 err = strat_calib(600, TRAJ_FLAGS_SMALL_DIST);
1450 trajectory_d_rel(&mainboard.traj, -DIST_BACK_DISPENSER);
1451 err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
1452 if (!TRAJ_SUCCESS(err))
1455 position_set(&mainboard.pos, 0, 0, 0);
1456 if (get_mechboard_mode() == I2C_MECHBOARD_MODE_PREPARE_EJECT) {
1457 strat_eject_col(90, 0);
1461 /* start to pickup with finger / arms */
1463 printf_P(PSTR("%s pickup now\r\n"), __FUNCTION__);
1464 i2c_mechboard_mode_pickup();
1465 WAIT_COND_OR_TIMEOUT(get_mechboard_mode() ==
1466 I2C_MECHBOARD_MODE_PICKUP, 100);
1467 us = time_get_us2();
1468 cols = get_column_count();
1469 while (get_mechboard_mode() == I2C_MECHBOARD_MODE_PICKUP) {
1470 if (get_column_count() != cols) {
1471 cols = get_column_count();
1472 us = time_get_us2();
1474 if ((get_column_count() - cols_count_before) >= 4) {
1475 printf_P(PSTR("%s no more cols in disp\r\n"), __FUNCTION__);
1478 /* 1 second timeout */
1479 if (time_get_us2() - us > 1500000L) {
1480 printf_P(PSTR("%s timeout\r\n"), __FUNCTION__);
1486 /* eject if we found a bad color column */
1488 if (get_mechboard_mode() == I2C_MECHBOARD_MODE_PREPARE_EJECT) {
1489 strat_eject_col(90, 0);
1493 strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
1494 trajectory_d_rel(&mainboard.traj, -250);
1495 wait_traj_end(TRAJ_FLAGS_SMALL_DIST | END_NEAR);
1497 cols_count_after = get_column_count();
1498 cols = cols_count_after - cols_count_before;
1499 DEBUG(E_USER_STRAT, "%s we got %d cols", __FUNCTION__, cols);
1501 pickup_wheels_off();
1502 i2c_mechboard_mode_clear();
1507 printf_P(PSTR("failed\r\n"));
1511 prog_char str_pickup_test_arg0[] = "pickup_test";
1512 parse_pgm_token_string_t cmd_pickup_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_pickup_test_result, arg0, str_pickup_test_arg0);
1513 prog_char str_pickup_test_arg1[] = "left#right";
1514 parse_pgm_token_string_t cmd_pickup_test_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_pickup_test_result, arg1, str_pickup_test_arg1);
1515 parse_pgm_token_num_t cmd_pickup_test_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_pickup_test_result, dist, INT16);
1517 prog_char help_pickup_test[] = "Pickup_Test function";
1518 parse_pgm_inst_t cmd_pickup_test = {
1519 .f = cmd_pickup_test_parsed, /* function to call */
1520 .data = NULL, /* 2nd arg of func */
1521 .help_str = help_pickup_test,
1522 .tokens = { /* token list, NULL terminated */
1523 (prog_void *)&cmd_pickup_test_arg0,
1524 (prog_void *)&cmd_pickup_test_arg1,
1525 (prog_void *)&cmd_pickup_test_arg2,
1530 /**********************************************************/
1533 /* this structure is filled when cmd_lintel_test is parsed successfully */
1534 struct cmd_lintel_test_result {
1535 fixed_string_t arg0;
1538 /* function called when cmd_lintel_test is parsed successfully */
1539 static void cmd_lintel_test_parsed(void *parsed_result, void *data)
1541 uint8_t err, first_try = 1, right_ok, left_ok;
1542 int16_t left_cur, right_cur;
1544 i2c_mechboard_mode_prepare_get_lintel();
1547 strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST);
1548 trajectory_d_rel(&mainboard.traj, 500);
1549 err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
1550 if (!TRAJ_SUCCESS(err) && err != END_BLOCKING)
1553 i2c_mechboard_mode_get_lintel();
1556 left_cur = sensor_get_adc(ADC_CSENSE3);
1557 left_ok = (left_cur > I2C_MECHBOARD_CURRENT_COLUMN);
1558 right_cur = mechboard.pump_right1_current;
1559 right_ok = (right_cur > I2C_MECHBOARD_CURRENT_COLUMN);
1561 printf_P(PSTR("left_ok=%d (%d), right_ok=%d (%d)\r\n"),
1562 left_ok, left_cur, right_ok, right_cur);
1564 if (!right_ok && !left_ok) {
1565 i2c_mechboard_mode_prepare_get_lintel();
1568 else if (right_ok && !left_ok) {
1569 i2c_mechboard_mode_prepare_get_lintel();
1571 strat_set_speed(500, 500);
1572 trajectory_d_a_rel(&mainboard.traj, -150, 30);
1573 err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
1574 trajectory_d_a_rel(&mainboard.traj, -140, -30);
1575 err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
1579 else if (!right_ok && left_ok) {
1580 i2c_mechboard_mode_prepare_get_lintel();
1582 strat_set_speed(500, 500);
1583 trajectory_d_a_rel(&mainboard.traj, -150, -30);
1584 err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
1585 trajectory_d_a_rel(&mainboard.traj, -140, 30);
1586 err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
1590 /* else, lintel is ok */
1592 i2c_mechboard_mode_put_lintel();
1596 if (right_ok && left_ok) {
1598 i2c_mechboard_mode_put_lintel();
1601 i2c_mechboard_mode_prepare_get_lintel();
1606 strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
1607 trajectory_d_rel(&mainboard.traj, -250);
1608 err = wait_traj_end(TRAJ_FLAGS_STD);
1612 printf_P(PSTR("fail\r\n"));
1616 prog_char str_lintel_test_arg0[] = "lintel_test";
1617 parse_pgm_token_string_t cmd_lintel_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_lintel_test_result, arg0, str_lintel_test_arg0);
1619 prog_char help_lintel_test[] = "Lintel_Test function";
1620 parse_pgm_inst_t cmd_lintel_test = {
1621 .f = cmd_lintel_test_parsed, /* function to call */
1622 .data = NULL, /* 2nd arg of func */
1623 .help_str = help_lintel_test,
1624 .tokens = { /* token list, NULL terminated */
1625 (prog_void *)&cmd_lintel_test_arg0,
1630 /**********************************************************/
1633 /* this structure is filled when cmd_scan_test is parsed successfully */
1634 struct cmd_scan_test_result {
1635 fixed_string_t arg0;
1636 fixed_string_t arg1;
1645 #define SCAN_MODE_CHECK_TEMPLE 0
1646 #define SCAN_MODE_SCAN_COL 1
1647 #define SCAN_MODE_SCAN_TEMPLE 2
1648 #define SCAN_MODE_TRAJ_ONLY 3
1650 /* function called when cmd_scan_test is parsed successfully */
1651 static void cmd_scan_test_parsed(void *parsed_result, void *data)
1653 uint8_t err, mode=0, c;
1654 int16_t pos1x, pos1y, dist;
1655 struct cmd_scan_test_result *res = parsed_result;
1656 int16_t back_mm = 0;
1658 int16_t ckpt_rel_x = 0, ckpt_rel_y = 0;
1660 double center_abs_x, center_abs_y;
1661 double ckpt_rel_d, ckpt_rel_a;
1662 double ckpt_abs_x, ckpt_abs_y;
1664 if (!strcmp_P(res->arg1, PSTR("traj_only")))
1665 mode = SCAN_MODE_TRAJ_ONLY;
1666 else if (!strcmp_P(res->arg1, PSTR("check_temple")))
1667 mode = SCAN_MODE_CHECK_TEMPLE;
1668 else if (!strcmp_P(res->arg1, PSTR("scan_col")))
1669 mode = SCAN_MODE_SCAN_COL;
1670 else if (!strcmp_P(res->arg1, PSTR("scan_temple")))
1671 mode = SCAN_MODE_SCAN_TEMPLE;
1674 strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_VERY_SLOW);
1675 trajectory_d_rel(&mainboard.traj, 400);
1676 err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
1677 if (err != END_BLOCKING)
1680 /* save absolute position of disc */
1681 rel_da_to_abs_xy(265, 0, ¢er_abs_x, ¢er_abs_y);
1683 /* go back and prepare to scan */
1684 strat_set_speed(1000, 1000);
1685 trajectory_d_a_rel(&mainboard.traj, -140, 130);
1686 err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
1687 if (!TRAJ_SUCCESS(err))
1690 /* prepare scanner arm */
1691 if (mode != SCAN_MODE_TRAJ_ONLY)
1692 i2c_sensorboard_scanner_prepare();
1695 strat_set_speed(res->scan_speed, 1000);
1697 pos1x = position_get_x_s16(&mainboard.pos);
1698 pos1y = position_get_y_s16(&mainboard.pos);
1699 trajectory_d_rel(&mainboard.traj, -res->scan_dist);
1702 err = test_traj_end(TRAJ_FLAGS_SMALL_DIST);
1706 dist = distance_from_robot(pos1x, pos1y);
1708 if (dist > res->start_dist)
1711 if (get_scanner_status() & I2C_SCAN_MAX_COLUMN) {
1718 if (TRAJ_SUCCESS(err))
1719 err = END_ERROR; /* should not reach end */
1721 trajectory_goto_xy_abs(&mainboard.traj, pos1x, pos1y);
1722 wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
1723 if (mode != SCAN_MODE_TRAJ_ONLY)
1724 i2c_sensorboard_scanner_stop();
1728 /* start the scanner */
1730 if (mode != SCAN_MODE_TRAJ_ONLY)
1731 i2c_sensorboard_scanner_start();
1733 err = WAIT_COND_OR_TRAJ_END(get_scanner_status() & I2C_SCAN_MAX_COLUMN,
1734 TRAJ_FLAGS_NO_NEAR);
1737 if (!TRAJ_SUCCESS(err)) {
1739 trajectory_goto_xy_abs(&mainboard.traj, pos1x, pos1y);
1740 wait_traj_end(TRAJ_FLAGS_NO_NEAR);
1741 if (mode != SCAN_MODE_TRAJ_ONLY)
1742 i2c_sensorboard_scanner_stop();
1746 if (mode == SCAN_MODE_TRAJ_ONLY)
1749 wait_scan_done(10000);
1751 i2c_sensorboard_scanner_stop();
1753 if (mode == SCAN_MODE_CHECK_TEMPLE) {
1754 i2c_sensorboard_scanner_algo_check(res->level,
1755 res->center_x, res->center_y);
1756 i2cproto_wait_update();
1757 wait_scan_done(10000);
1758 scanner_dump_state();
1760 if (sensorboard.dropzone_h == -1) {
1761 printf_P(PSTR("-- try to build a temple\r\n"));
1764 mode = SCAN_MODE_SCAN_TEMPLE;
1768 if (mode == SCAN_MODE_SCAN_TEMPLE) {
1769 i2c_sensorboard_scanner_algo_temple(I2C_SCANNER_ZONE_DISC,
1772 i2cproto_wait_update();
1773 wait_scan_done(10000);
1774 scanner_dump_state();
1776 if (sensorboard.dropzone_h == -1 ||
1777 strat_scan_get_checkpoint(mode, &ckpt_rel_x,
1778 &ckpt_rel_y, &back_mm)) {
1779 printf_P(PSTR("-- try to build a column\r\n"));
1780 mode = SCAN_MODE_SCAN_COL;
1784 if (mode == SCAN_MODE_SCAN_COL) {
1785 i2c_sensorboard_scanner_algo_column(I2C_SCANNER_ZONE_DISC,
1786 res->center_x, res->center_y);
1787 i2cproto_wait_update();
1788 wait_scan_done(10000);
1789 scanner_dump_state();
1791 if (sensorboard.dropzone_h == -1 ||
1792 strat_scan_get_checkpoint(mode, &ckpt_rel_x,
1793 &ckpt_rel_y, &back_mm)) {
1798 if (sensorboard.dropzone_h == -1)
1801 if (mode == SCAN_MODE_CHECK_TEMPLE) {
1807 printf_P(PSTR("rel xy for ckpt is %d,%d\r\n"), ckpt_rel_x, ckpt_rel_y);
1809 rel_xy_to_abs_xy(ckpt_rel_x, ckpt_rel_y, &ckpt_abs_x, &ckpt_abs_y);
1810 abs_xy_to_rel_da(ckpt_abs_x, ckpt_abs_y, &ckpt_rel_d, &ckpt_rel_a);
1812 printf_P(PSTR("abs ckpt is %2.2f,%2.2f\r\n"), ckpt_abs_x, ckpt_abs_y);
1814 printf_P(PSTR("ok ? (y/n)\r\n"));
1816 c = cmdline_getchar_wait();
1821 strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
1823 /* intermediate checkpoint for some positions */
1824 if ( (DEG(ckpt_rel_a) < 0 && DEG(ckpt_rel_a) > -90) ) {
1825 trajectory_goto_xy_rel(&mainboard.traj, 200, 100);
1826 err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
1827 if (!TRAJ_SUCCESS(err))
1831 trajectory_goto_xy_abs(&mainboard.traj, ckpt_abs_x, ckpt_abs_y);
1832 err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
1833 if (!TRAJ_SUCCESS(err))
1836 trajectory_turnto_xy(&mainboard.traj, center_abs_x, center_abs_y);
1837 err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
1838 if (!TRAJ_SUCCESS(err))
1841 c = cmdline_getchar_wait();
1843 pos1x = position_get_x_s16(&mainboard.pos);
1844 pos1y = position_get_y_s16(&mainboard.pos);
1846 strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW);
1847 trajectory_d_rel(&mainboard.traj, 200);
1848 err = WAIT_COND_OR_TRAJ_END(distance_from_robot(pos1x, pos1y) > 200,
1849 TRAJ_FLAGS_SMALL_DIST);
1851 strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_VERY_SLOW);
1852 trajectory_d_rel(&mainboard.traj, 400);
1853 err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
1855 if (err != END_BLOCKING)
1859 trajectory_d_rel(&mainboard.traj, -back_mm);
1860 wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
1864 prog_char str_scan_test_arg0[] = "scan_test";
1865 parse_pgm_token_string_t cmd_scan_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_scan_test_result, arg0, str_scan_test_arg0);
1866 prog_char str_scan_test_arg1[] = "traj_only#scan_col#scan_temple";
1867 parse_pgm_token_string_t cmd_scan_test_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_scan_test_result, arg1, str_scan_test_arg1);
1868 parse_pgm_token_num_t cmd_scan_test_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_scan_test_result, start_dist, INT16);
1869 parse_pgm_token_num_t cmd_scan_test_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_scan_test_result, scan_dist, INT16);
1870 parse_pgm_token_num_t cmd_scan_test_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_scan_test_result, scan_speed, INT16);
1871 parse_pgm_token_num_t cmd_scan_test_arg5 = TOKEN_NUM_INITIALIZER(struct cmd_scan_test_result, center_x, INT16);
1872 parse_pgm_token_num_t cmd_scan_test_arg6 = TOKEN_NUM_INITIALIZER(struct cmd_scan_test_result, center_y, INT16);
1874 prog_char help_scan_test[] = "Scan_Test function (start_dist, scan_dist, speed_dist, centerx, centery)";
1875 parse_pgm_inst_t cmd_scan_test = {
1876 .f = cmd_scan_test_parsed, /* function to call */
1877 .data = NULL, /* 2nd arg of func */
1878 .help_str = help_scan_test,
1879 .tokens = { /* token list, NULL terminated */
1880 (prog_void *)&cmd_scan_test_arg0,
1881 (prog_void *)&cmd_scan_test_arg1,
1882 (prog_void *)&cmd_scan_test_arg2,
1883 (prog_void *)&cmd_scan_test_arg3,
1884 (prog_void *)&cmd_scan_test_arg4,
1885 (prog_void *)&cmd_scan_test_arg5,
1886 (prog_void *)&cmd_scan_test_arg6,
1891 prog_char str_scan_test_arg1b[] = "check_temple";
1892 parse_pgm_token_string_t cmd_scan_test_arg1b = TOKEN_STRING_INITIALIZER(struct cmd_scan_test_result, arg1, str_scan_test_arg1b);
1893 parse_pgm_token_num_t cmd_scan_test_arg7 = TOKEN_NUM_INITIALIZER(struct cmd_scan_test_result, level, UINT8);
1895 prog_char help_scan_test2[] = "Scan_Test function (start_dist, scan_dist, speed_dist, templex, templey, level)";
1896 parse_pgm_inst_t cmd_scan_test2 = {
1897 .f = cmd_scan_test_parsed, /* function to call */
1898 .data = NULL, /* 2nd arg of func */
1899 .help_str = help_scan_test,
1900 .tokens = { /* token list, NULL terminated */
1901 (prog_void *)&cmd_scan_test_arg0,
1902 (prog_void *)&cmd_scan_test_arg1b,
1903 (prog_void *)&cmd_scan_test_arg2,
1904 (prog_void *)&cmd_scan_test_arg3,
1905 (prog_void *)&cmd_scan_test_arg4,
1906 (prog_void *)&cmd_scan_test_arg5,
1907 (prog_void *)&cmd_scan_test_arg6,
1908 (prog_void *)&cmd_scan_test_arg7,
1913 /**********************************************************/
1916 /* this structure is filled when cmd_time_monitor is parsed successfully */
1917 struct cmd_time_monitor_result {
1918 fixed_string_t arg0;
1919 fixed_string_t arg1;
1922 /* function called when cmd_time_monitor is parsed successfully */
1923 static void cmd_time_monitor_parsed(void *parsed_result, void *data)
1925 struct cmd_time_monitor_result *res = parsed_result;
1928 if (!strcmp_P(res->arg1, PSTR("reset"))) {
1929 eeprom_write_word(EEPROM_TIME_ADDRESS, 0);
1931 seconds = eeprom_read_word(EEPROM_TIME_ADDRESS);
1932 printf_P(PSTR("Running since %d mn %d\r\n"), seconds/60, seconds%60);
1935 prog_char str_time_monitor_arg0[] = "time_monitor";
1936 parse_pgm_token_string_t cmd_time_monitor_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_time_monitor_result, arg0, str_time_monitor_arg0);
1937 prog_char str_time_monitor_arg1[] = "show#reset";
1938 parse_pgm_token_string_t cmd_time_monitor_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_time_monitor_result, arg1, str_time_monitor_arg1);
1940 prog_char help_time_monitor[] = "Show since how long we are running";
1941 parse_pgm_inst_t cmd_time_monitor = {
1942 .f = cmd_time_monitor_parsed, /* function to call */
1943 .data = NULL, /* 2nd arg of func */
1944 .help_str = help_time_monitor,
1945 .tokens = { /* token list, NULL terminated */
1946 (prog_void *)&cmd_time_monitor_arg0,
1947 (prog_void *)&cmd_time_monitor_arg1,
1953 /**********************************************************/
1956 /* this structure is filled when cmd_scanner is parsed successfully */
1957 struct cmd_scanner_result {
1958 fixed_string_t arg0;
1959 fixed_string_t arg1;
1962 /* function called when cmd_scanner is parsed successfully */
1963 static void cmd_scanner_parsed(void *parsed_result, void *data)
1965 struct cmd_scanner_result *res = parsed_result;
1967 if (!strcmp_P(res->arg1, PSTR("prepare"))) {
1968 i2c_sensorboard_scanner_prepare();
1970 else if (!strcmp_P(res->arg1, PSTR("stop"))) {
1971 i2c_sensorboard_scanner_stop();
1973 else if (!strcmp_P(res->arg1, PSTR("start"))) {
1974 i2c_sensorboard_scanner_start();
1976 else if (!strcmp_P(res->arg1, PSTR("algo_col"))) {
1977 i2c_sensorboard_scanner_algo_column(I2C_SCANNER_ZONE_DISC,
1980 else if (!strcmp_P(res->arg1, PSTR("algo_check"))) {
1981 i2c_sensorboard_scanner_algo_check(2, 15, 15); // XXX
1983 else if (!strcmp_P(res->arg1, PSTR("calib"))) {
1984 i2c_sensorboard_scanner_calib();
1986 else if (!strcmp_P(res->arg1, PSTR("show"))) {
1987 scanner_dump_state();
1991 prog_char str_scanner_arg0[] = "scanner";
1992 parse_pgm_token_string_t cmd_scanner_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_scanner_result, arg0, str_scanner_arg0);
1993 prog_char str_scanner_arg1[] = "prepare#start#algo_col#algo_check#stop#show#calib";
1994 parse_pgm_token_string_t cmd_scanner_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_scanner_result, arg1, str_scanner_arg1);
1996 prog_char help_scanner[] = "send commands to scanner";
1997 parse_pgm_inst_t cmd_scanner = {
1998 .f = cmd_scanner_parsed, /* function to call */
1999 .data = NULL, /* 2nd arg of func */
2000 .help_str = help_scanner,
2001 .tokens = { /* token list, NULL terminated */
2002 (prog_void *)&cmd_scanner_arg0,
2003 (prog_void *)&cmd_scanner_arg1,
2008 /**********************************************************/
2011 /* this structure is filled when cmd_build_z1 is parsed successfully */
2012 struct cmd_build_z1_result {
2013 fixed_string_t arg0;
2020 /* function called when cmd_build_z1 is parsed successfully */
2021 static void cmd_build_z1_parsed(void *parsed_result, void *data)
2023 struct cmd_build_z1_result *res = parsed_result;
2025 strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_SLOW);
2026 trajectory_d_rel(&mainboard.traj, 400);
2027 wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
2029 trajectory_d_rel(&mainboard.traj, -200);
2030 wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
2032 i2c_mechboard_mode_prepare_pickup_next(I2C_LEFT_SIDE,
2033 I2C_MECHBOARD_MODE_HARVEST);
2035 while (get_column_count() != 4);
2037 i2c_mechboard_mode_prepare_build_both(res->level);
2040 trajectory_d_rel(&mainboard.traj, 400);
2041 wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
2043 strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW);
2044 trajectory_d_rel(&mainboard.traj, -res->d1);
2045 wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
2046 i2c_mechboard_mode_autobuild(res->level, 2, I2C_AUTOBUILD_DEFAULT_DIST,
2047 res->level, 2, I2C_AUTOBUILD_DEFAULT_DIST,
2049 WAIT_COND_OR_TIMEOUT(get_mechboard_mode() ==
2050 I2C_MECHBOARD_MODE_AUTOBUILD, 100);
2051 WAIT_COND_OR_TIMEOUT(get_mechboard_mode() !=
2052 I2C_MECHBOARD_MODE_AUTOBUILD, 10000);
2054 strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW);
2055 trajectory_d_rel(&mainboard.traj, -res->d2);
2056 wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
2057 i2c_mechboard_mode_push_temple(1);
2059 strat_set_speed(200, SPEED_ANGLE_SLOW);
2060 trajectory_d_rel(&mainboard.traj, res->d3);
2061 wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
2064 prog_char str_build_z1_arg0[] = "build_z1";
2065 parse_pgm_token_string_t cmd_build_z1_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_build_z1_result, arg0, str_build_z1_arg0);
2066 parse_pgm_token_num_t cmd_build_z1_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_build_z1_result, level, UINT8);
2067 parse_pgm_token_num_t cmd_build_z1_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_build_z1_result, d1, INT16);
2068 parse_pgm_token_num_t cmd_build_z1_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_build_z1_result, d2, INT16);
2069 parse_pgm_token_num_t cmd_build_z1_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_build_z1_result, d3, INT16);
2071 prog_char help_build_z1[] = "Build_Z1 function (level, d1, d2, d3)";
2072 parse_pgm_inst_t cmd_build_z1 = {
2073 .f = cmd_build_z1_parsed, /* function to call */
2074 .data = NULL, /* 2nd arg of func */
2075 .help_str = help_build_z1,
2076 .tokens = { /* token list, NULL terminated */
2077 (prog_void *)&cmd_build_z1_arg0,
2078 (prog_void *)&cmd_build_z1_arg1,
2079 (prog_void *)&cmd_build_z1_arg2,
2080 (prog_void *)&cmd_build_z1_arg3,
2081 (prog_void *)&cmd_build_z1_arg4,
2087 /**********************************************************/
2088 /* Beacon_Opp_Dump */
2090 /* this structure is filled when cmd_beacon_opp_dump is parsed successfully */
2091 struct cmd_beacon_opp_dump_result {
2092 fixed_string_t arg0;
2095 void beacon_dump_samples(void);
2097 /* function called when cmd_beacon_opp_dump is parsed successfully */
2098 static void cmd_beacon_opp_dump_parsed(void *parsed_result, void *data)
2100 beacon_dump_samples();
2103 prog_char str_beacon_opp_dump_arg0[] = "beacon_opp_dump";
2104 parse_pgm_token_string_t cmd_beacon_opp_dump_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_beacon_opp_dump_result, arg0, str_beacon_opp_dump_arg0);
2106 prog_char help_beacon_opp_dump[] = "Dump beacon samples";
2107 parse_pgm_inst_t cmd_beacon_opp_dump = {
2108 .f = cmd_beacon_opp_dump_parsed, /* function to call */
2109 .data = NULL, /* 2nd arg of func */
2110 .help_str = help_beacon_opp_dump,
2111 .tokens = { /* token list, NULL terminated */
2112 (prog_void *)&cmd_beacon_opp_dump_arg0,
2118 /**********************************************************/
2121 /* this structure is filled when cmd_test is parsed successfully */
2122 struct cmd_test_result {
2123 fixed_string_t arg0;
2126 void circle_get_da_speed_from_radius(struct trajectory *traj,
2130 /* function called when cmd_test is parsed successfully */
2131 static void cmd_test_parsed(void *parsed_result, void *data)
2133 struct cmd_test_result *res = parsed_result;
2135 strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW);
2136 circle_get_da_speed_from_radius(&mainboard.traj, res->radius, &d, &a);
2137 printf_P(PSTR("d=%2.2f a=%2.2f\r\n"), d, a);
2140 prog_char str_test_arg0[] = "test";
2141 parse_pgm_token_string_t cmd_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_test_result, arg0, str_test_arg0);
2142 parse_pgm_token_num_t cmd_test_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_test_result, radius, INT32);
2144 prog_char help_test[] = "Test function";
2145 parse_pgm_inst_t cmd_test = {
2146 .f = cmd_test_parsed, /* function to call */
2147 .data = NULL, /* 2nd arg of func */
2148 .help_str = help_test,
2149 .tokens = { /* token list, NULL terminated */
2150 (prog_void *)&cmd_test_arg0,
2151 (prog_void *)&cmd_test_arg1,