X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fmainboard%2Fcommands_mainboard.c;h=52b6b92ca20c03f0e17855610885bd33e40b8122;hp=7c222b30dc5a85e82c565d07bbb31ea3e8133b74;hb=17aadc4c8c3e60c2b5e6bbba91c8542849addbd7;hpb=27b0a59ab1cb5eab29c6fc07476f5465dbd1f786 diff --git a/projects/microb2010/mainboard/commands_mainboard.c b/projects/microb2010/mainboard/commands_mainboard.c index 7c222b3..52b6b92 100644 --- a/projects/microb2010/mainboard/commands_mainboard.c +++ b/projects/microb2010/mainboard/commands_mainboard.c @@ -1,6 +1,6 @@ /* * Copyright Droids Corporation (2009) - * + * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation; either version 2 of the License, or @@ -17,12 +17,14 @@ * * Revision : $Id: commands_mainboard.c,v 1.9 2009-11-08 17:24:33 zer0 Exp $ * - * Olivier MATZ + * Olivier MATZ */ #include #include +#include +#include #include #include #include @@ -31,7 +33,7 @@ #include #include #include -#include +#include #include #include @@ -39,6 +41,8 @@ #include #include #include +#include +#include #include #include #include @@ -56,11 +60,14 @@ #include "../common/eeprom_mapping.h" #include "main.h" +#include "robotsim.h" #include "sensor.h" #include "cmdline.h" #include "strat.h" #include "strat_utils.h" #include "strat_base.h" +#include "strat_db.h" +#include "strat_corn.h" #include "i2c_protocol.h" #include "actuator.h" @@ -77,29 +84,30 @@ static void cmd_event_parsed(void *parsed_result, void *data) u08 bit=0; struct cmd_event_result * res = parsed_result; - + if (!strcmp_P(res->arg1, PSTR("all"))) { - bit = DO_ENCODERS | DO_CS | DO_RS | DO_POS | - DO_BD | DO_TIMER | DO_POWER; + bit = 0xFF; if (!strcmp_P(res->arg2, PSTR("on"))) mainboard.flags |= bit; else if (!strcmp_P(res->arg2, PSTR("off"))) mainboard.flags &= bit; else { /* show */ - printf_P(PSTR("encoders is %s\r\n"), + printf_P(PSTR("encoders is %s\r\n"), (DO_ENCODERS & mainboard.flags) ? "on":"off"); - printf_P(PSTR("cs is %s\r\n"), + printf_P(PSTR("cs is %s\r\n"), (DO_CS & mainboard.flags) ? "on":"off"); - printf_P(PSTR("rs is %s\r\n"), + printf_P(PSTR("rs is %s\r\n"), (DO_RS & mainboard.flags) ? "on":"off"); - printf_P(PSTR("pos is %s\r\n"), + printf_P(PSTR("pos is %s\r\n"), (DO_POS & mainboard.flags) ? "on":"off"); - printf_P(PSTR("bd is %s\r\n"), + printf_P(PSTR("bd is %s\r\n"), (DO_BD & mainboard.flags) ? "on":"off"); - printf_P(PSTR("timer is %s\r\n"), + printf_P(PSTR("timer is %s\r\n"), (DO_TIMER & mainboard.flags) ? "on":"off"); - printf_P(PSTR("power is %s\r\n"), + printf_P(PSTR("power is %s\r\n"), (DO_POWER & mainboard.flags) ? "on":"off"); + printf_P(PSTR("errblock is %s\r\n"), + (DO_ERRBLOCKING & mainboard.flags) ? "on":"off"); } return; } @@ -122,23 +130,30 @@ static void cmd_event_parsed(void *parsed_result, void *data) } else if (!strcmp_P(res->arg1, PSTR("power"))) bit = DO_POWER; + else if (!strcmp_P(res->arg1, PSTR("errblock"))) + bit = DO_ERRBLOCKING; if (!strcmp_P(res->arg2, PSTR("on"))) mainboard.flags |= bit; else if (!strcmp_P(res->arg2, PSTR("off"))) { if (!strcmp_P(res->arg1, PSTR("cs"))) { +#ifdef HOST_VERSION + robotsim_pwm(LEFT_PWM, 0); + robotsim_pwm(RIGHT_PWM, 0); +#else pwm_ng_set(LEFT_PWM, 0); pwm_ng_set(RIGHT_PWM, 0); +#endif } mainboard.flags &= (~bit); } - printf_P(PSTR("%s is %s\r\n"), res->arg1, + printf_P(PSTR("%s is %s\r\n"), res->arg1, (bit & mainboard.flags) ? "on":"off"); } prog_char str_event_arg0[] = "event"; parse_pgm_token_string_t cmd_event_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg0, str_event_arg0); -prog_char str_event_arg1[] = "all#encoders#cs#rs#pos#bd#timer#power"; +prog_char str_event_arg1[] = "all#encoders#cs#rs#pos#bd#timer#power#errblock"; parse_pgm_token_string_t cmd_event_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg1, str_event_arg1); prog_char str_event_arg2[] = "on#off#show"; parse_pgm_token_string_t cmd_event_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg2, str_event_arg2); @@ -149,9 +164,9 @@ parse_pgm_inst_t cmd_event = { .data = NULL, /* 2nd arg of func */ .help_str = help_event, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_event_arg0, - (prog_void *)&cmd_event_arg1, - (prog_void *)&cmd_event_arg2, + (prog_void *)&cmd_event_arg0, + (prog_void *)&cmd_event_arg1, + (prog_void *)&cmd_event_arg2, NULL, }, }; @@ -168,8 +183,11 @@ struct cmd_spi_test_result { /* function called when cmd_spi_test is parsed successfully */ static void cmd_spi_test_parsed(void * parsed_result, void * data) { +#ifdef HOST_VERSION + printf("not implemented\n"); +#else uint16_t i = 0, ret = 0, ret2 = 0; - + if (mainboard.flags & DO_ENCODERS) { printf_P(PSTR("Disable encoder event first\r\n")); return; @@ -187,6 +205,7 @@ static void cmd_spi_test_parsed(void * parsed_result, void * data) i++; } while(!cmdline_keypressed()); +#endif } prog_char str_spi_test_arg0[] = "spi_test"; @@ -198,7 +217,7 @@ parse_pgm_inst_t cmd_spi_test = { .data = NULL, /* 2nd arg of func */ .help_str = help_spi_test, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_spi_test_arg0, + (prog_void *)&cmd_spi_test_arg0, NULL, }, }; @@ -238,8 +257,8 @@ parse_pgm_inst_t cmd_opponent = { .data = NULL, /* 2nd arg of func */ .help_str = help_opponent, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_opponent_arg0, - (prog_void *)&cmd_opponent_arg1, + (prog_void *)&cmd_opponent_arg0, + (prog_void *)&cmd_opponent_arg1, NULL, }, }; @@ -256,10 +275,10 @@ parse_pgm_inst_t cmd_opponent_set = { .data = NULL, /* 2nd arg of func */ .help_str = help_opponent_set, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_opponent_arg0, + (prog_void *)&cmd_opponent_arg0, (prog_void *)&cmd_opponent_arg1_set, - (prog_void *)&cmd_opponent_arg2, - (prog_void *)&cmd_opponent_arg3, + (prog_void *)&cmd_opponent_arg2, + (prog_void *)&cmd_opponent_arg3, NULL, }, }; @@ -283,33 +302,25 @@ static void cmd_start_parsed(void *parsed_result, void *data) gen.logs[NB_LOGS] = E_USER_STRAT; if (!strcmp_P(res->debug, PSTR("debug"))) { - strat_infos.dump_enabled = 1; + strat_db.dump_enabled = 1; gen.log_level = 5; } else { - strat_infos.dump_enabled = 0; + strat_db.dump_enabled = 0; gen.log_level = 0; } - if (!strcmp_P(res->color, PSTR("red"))) { - mainboard.our_color = I2C_COLOR_RED; - i2c_set_color(I2C_MECHBOARD_ADDR, I2C_COLOR_RED); - i2c_set_color(I2C_SENSORBOARD_ADDR, I2C_COLOR_RED); + if (!strcmp_P(res->color, PSTR("yellow"))) { + mainboard.our_color = I2C_COLOR_YELLOW; + i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_YELLOW); + i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_YELLOW); } - else if (!strcmp_P(res->color, PSTR("green"))) { - mainboard.our_color = I2C_COLOR_GREEN; - i2c_set_color(I2C_MECHBOARD_ADDR, I2C_COLOR_GREEN); - i2c_set_color(I2C_SENSORBOARD_ADDR, I2C_COLOR_GREEN); + else if (!strcmp_P(res->color, PSTR("blue"))) { + mainboard.our_color = I2C_COLOR_BLUE; + i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_BLUE); + i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_BLUE); } - printf_P(PSTR("Check that lintel is loaded\r\n")); - while(!cmdline_keypressed()); - - printf_P(PSTR("Press a key when beacon ready\r\n")); - i2c_sensorboard_set_beacon(0); - while(!cmdline_keypressed()); - i2c_sensorboard_set_beacon(1); - strat_start(); gen.logs[NB_LOGS] = 0; @@ -318,7 +329,7 @@ static void cmd_start_parsed(void *parsed_result, void *data) prog_char str_start_arg0[] = "start"; parse_pgm_token_string_t cmd_start_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_start_result, arg0, str_start_arg0); -prog_char str_start_color[] = "green#red"; +prog_char str_start_color[] = "blue#yellow"; parse_pgm_token_string_t cmd_start_color = TOKEN_STRING_INITIALIZER(struct cmd_start_result, color, str_start_color); prog_char str_start_debug[] = "debug#match"; parse_pgm_token_string_t cmd_start_debug = TOKEN_STRING_INITIALIZER(struct cmd_start_result, debug, str_start_debug); @@ -329,9 +340,9 @@ parse_pgm_inst_t cmd_start = { .data = NULL, /* 2nd arg of func */ .help_str = help_start, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_start_arg0, - (prog_void *)&cmd_start_color, - (prog_void *)&cmd_start_debug, + (prog_void *)&cmd_start_arg0, + (prog_void *)&cmd_start_color, + (prog_void *)&cmd_start_debug, NULL, }, }; @@ -348,8 +359,8 @@ struct cmd_interact_result { static void print_cs(void) { - printf_P(PSTR("cons_d=% .8ld cons_a=% .8ld fil_d=% .8ld fil_a=% .8ld " - "err_d=% .8ld err_a=% .8ld out_d=% .8ld out_a=% .8ld\r\n"), + printf_P(PSTR("cons_d=% .8"PRIi32" cons_a=% .8"PRIi32" fil_d=% .8"PRIi32" fil_a=% .8"PRIi32" " + "err_d=% .8"PRIi32" err_a=% .8"PRIi32" out_d=% .8"PRIi32" out_a=% .8"PRIi32"\r\n"), cs_get_consign(&mainboard.distance.cs), cs_get_consign(&mainboard.angle.cs), cs_get_filtered_consign(&mainboard.distance.cs), @@ -362,7 +373,7 @@ static void print_cs(void) static void print_pos(void) { - printf_P(PSTR("x=% .8d y=% .8d a=% .8d\r\n"), + printf_P(PSTR("x=% .8d y=% .8d a=% .8d\r\n"), position_get_x_s16(&mainboard.pos), position_get_y_s16(&mainboard.pos), position_get_a_deg_s16(&mainboard.pos)); @@ -393,8 +404,8 @@ static void print_sensors(void) static void print_pid(void) { - printf_P(PSTR("P=% .8ld I=% .8ld D=% .8ld out=% .8ld | " - "P=% .8ld I=% .8ld D=% .8ld out=% .8ld\r\n"), + printf_P(PSTR("P=% .8"PRIi32" I=% .8"PRIi32" D=% .8"PRIi32" out=% .8"PRIi32" | " + "P=% .8"PRIi32" I=% .8"PRIi32" D=% .8"PRIi32" out=% .8"PRIi32"\r\n"), pid_get_value_in(&mainboard.distance.pid) * pid_get_gain_P(&mainboard.distance.pid), pid_get_value_I(&mainboard.distance.pid) * pid_get_gain_I(&mainboard.distance.pid), pid_get_value_D(&mainboard.distance.pid) * pid_get_gain_D(&mainboard.distance.pid), @@ -475,7 +486,7 @@ static void cmd_interact_parsed(void * parsed_result, void * data) wait_ms(10); continue; } - + if (cmd == -1) { switch(c) { case '1': print ^= PRINT_POS; break; @@ -485,7 +496,7 @@ static void cmd_interact_parsed(void * parsed_result, void * data) case '5': print ^= PRINT_TIME; break; case '6': print ^= PRINT_BLOCKING; break; - case 'q': + case 'q': if (mainboard.flags & DO_CS) strat_hardstop(); pwm_set_and_save(LEFT_PWM, 0); @@ -495,21 +506,21 @@ static void cmd_interact_parsed(void * parsed_result, void * data) pwm_set_and_save(LEFT_PWM, 0); pwm_set_and_save(RIGHT_PWM, 0); break; - default: + default: break; } } else { switch(cmd) { - case KEY_UP_ARR: + case KEY_UP_ARR: pwm_set_and_save(LEFT_PWM, 1200); pwm_set_and_save(RIGHT_PWM, 1200); break; - case KEY_LEFT_ARR: + case KEY_LEFT_ARR: pwm_set_and_save(LEFT_PWM, -1200); pwm_set_and_save(RIGHT_PWM, 1200); break; - case KEY_DOWN_ARR: + case KEY_DOWN_ARR: pwm_set_and_save(LEFT_PWM, -1200); pwm_set_and_save(RIGHT_PWM, -1200); break; @@ -532,7 +543,7 @@ parse_pgm_inst_t cmd_interact = { .data = NULL, /* 2nd arg of func */ .help_str = help_interact, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_interact_arg0, + (prog_void *)&cmd_interact_arg0, NULL, }, }; @@ -550,23 +561,27 @@ struct cmd_color_result { /* function called when cmd_color is parsed successfully */ static void cmd_color_parsed(void *parsed_result, void *data) { +#ifdef HOST_VERSION + printf("not implemented\n"); +#else struct cmd_color_result *res = (struct cmd_color_result *) parsed_result; - if (!strcmp_P(res->color, PSTR("red"))) { - mainboard.our_color = I2C_COLOR_RED; - i2c_set_color(I2C_MECHBOARD_ADDR, I2C_COLOR_RED); - i2c_set_color(I2C_SENSORBOARD_ADDR, I2C_COLOR_RED); + if (!strcmp_P(res->color, PSTR("yellow"))) { + mainboard.our_color = I2C_COLOR_YELLOW; + i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_YELLOW); + i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_YELLOW); } - else if (!strcmp_P(res->color, PSTR("green"))) { - mainboard.our_color = I2C_COLOR_GREEN; - i2c_set_color(I2C_MECHBOARD_ADDR, I2C_COLOR_GREEN); - i2c_set_color(I2C_SENSORBOARD_ADDR, I2C_COLOR_GREEN); + else if (!strcmp_P(res->color, PSTR("blue"))) { + mainboard.our_color = I2C_COLOR_BLUE; + i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_BLUE); + i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_BLUE); } printf_P(PSTR("Done\r\n")); +#endif } prog_char str_color_arg0[] = "color"; parse_pgm_token_string_t cmd_color_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_color_result, arg0, str_color_arg0); -prog_char str_color_color[] = "green#red"; +prog_char str_color_color[] = "blue#yellow"; parse_pgm_token_string_t cmd_color_color = TOKEN_STRING_INITIALIZER(struct cmd_color_result, color, str_color_color); prog_char help_color[] = "Set our color"; @@ -575,8 +590,8 @@ parse_pgm_inst_t cmd_color = { .data = NULL, /* 2nd arg of func */ .help_str = help_color, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_color_arg0, - (prog_void *)&cmd_color_color, + (prog_void *)&cmd_color_arg0, + (prog_void *)&cmd_color_color, NULL, }, }; @@ -596,15 +611,15 @@ static void cmd_rs_parsed(void *parsed_result, void *data) { // struct cmd_rs_result *res = parsed_result; do { - printf_P(PSTR("angle cons=% .6ld in=% .6ld out=% .6ld / "), + printf_P(PSTR("angle cons=% .6"PRIi32" in=% .6"PRIi32" out=% .6"PRIi32" / "), cs_get_consign(&mainboard.angle.cs), cs_get_filtered_feedback(&mainboard.angle.cs), cs_get_out(&mainboard.angle.cs)); - printf_P(PSTR("distance cons=% .6ld in=% .6ld out=% .6ld / "), + printf_P(PSTR("distance cons=% .6"PRIi32" in=% .6"PRIi32" out=% .6"PRIi32" / "), cs_get_consign(&mainboard.distance.cs), cs_get_filtered_feedback(&mainboard.distance.cs), cs_get_out(&mainboard.distance.cs)); - printf_P(PSTR("l=% .4ld r=% .4ld\r\n"), mainboard.pwm_l, + printf_P(PSTR("l=% .4"PRIi32" r=% .4"PRIi32"\r\n"), mainboard.pwm_l, mainboard.pwm_r); wait_ms(100); } while(!cmdline_keypressed()); @@ -621,8 +636,8 @@ parse_pgm_inst_t cmd_rs = { .data = NULL, /* 2nd arg of func */ .help_str = help_rs, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_rs_arg0, - (prog_void *)&cmd_rs_arg1, + (prog_void *)&cmd_rs_arg0, + (prog_void *)&cmd_rs_arg1, NULL, }, }; @@ -638,8 +653,12 @@ struct cmd_i2cdebug_result { /* function called when cmd_i2cdebug is parsed successfully */ static void cmd_i2cdebug_parsed(void * parsed_result, void * data) { +#ifdef HOST_VERSION + printf("not implemented\n"); +#else i2c_debug(); i2c_protocol_debug(); +#endif } prog_char str_i2cdebug_arg0[] = "i2cdebug"; @@ -651,1261 +670,465 @@ parse_pgm_inst_t cmd_i2cdebug = { .data = NULL, /* 2nd arg of func */ .help_str = help_i2cdebug, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_i2cdebug_arg0, + (prog_void *)&cmd_i2cdebug_arg0, NULL, }, }; /**********************************************************/ -/* Mechboard_Show */ +/* Cobboard_Show */ -/* this structure is filled when cmd_mechboard_show is parsed successfully */ -struct cmd_mechboard_show_result { +/* this structure is filled when cmd_cobboard_show is parsed successfully */ +struct cmd_cobboard_show_result { fixed_string_t arg0; fixed_string_t arg1; }; -/* function called when cmd_mechboard_show is parsed successfully */ -static void cmd_mechboard_show_parsed(void * parsed_result, void * data) +/* function called when cmd_cobboard_show is parsed successfully */ +static void cmd_cobboard_show_parsed(void * parsed_result, void * data) { - printf_P(PSTR("mode = %x\r\n"), mechboard.mode); - printf_P(PSTR("status = %x\r\n"), mechboard.status); - printf_P(PSTR("lintel_count = %d\r\n"), mechboard.lintel_count); - - printf_P(PSTR("column_count = %d\r\n"), get_column_count()); - printf_P(PSTR("left1=%d left2=%d right1=%d right2=%d\r\n"), - pump_left1_is_full(), pump_left2_is_full(), - pump_right1_is_full(), pump_right2_is_full()); - - printf_P(PSTR("pump_left1 = %d\r\n"), mechboard.pump_left1); - printf_P(PSTR("pump_left2 = %d\r\n"), mechboard.pump_left2); - printf_P(PSTR("pump_right1 = %d\r\n"), mechboard.pump_right1); - printf_P(PSTR("pump_right2 = %d\r\n"), mechboard.pump_right2); - - printf_P(PSTR("servo_lintel_left = %d\r\n"), mechboard.servo_lintel_left); - printf_P(PSTR("servo_lintel_right = %d\r\n"), mechboard.servo_lintel_right); - +#ifdef HOST_VERSION + printf("not implemented\n"); +#else + printf_P(PSTR("mode = %x\r\n"), cobboard.mode); + printf_P(PSTR("status = %x\r\n"), cobboard.status); + printf_P(PSTR("cob_count = %x\r\n"), cobboard.cob_count); + printf_P(PSTR("left_cobroller_speed = %d\r\n"), cobboard.left_cobroller_speed); + printf_P(PSTR("right_cobroller_speed = %d\r\n"), cobboard.right_cobroller_speed); +#endif } -prog_char str_mechboard_show_arg0[] = "mechboard"; -parse_pgm_token_string_t cmd_mechboard_show_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_show_result, arg0, str_mechboard_show_arg0); -prog_char str_mechboard_show_arg1[] = "show"; -parse_pgm_token_string_t cmd_mechboard_show_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_show_result, arg1, str_mechboard_show_arg1); +prog_char str_cobboard_show_arg0[] = "cobboard"; +parse_pgm_token_string_t cmd_cobboard_show_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_show_result, arg0, str_cobboard_show_arg0); +prog_char str_cobboard_show_arg1[] = "show"; +parse_pgm_token_string_t cmd_cobboard_show_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_show_result, arg1, str_cobboard_show_arg1); -prog_char help_mechboard_show[] = "show mechboard status"; -parse_pgm_inst_t cmd_mechboard_show = { - .f = cmd_mechboard_show_parsed, /* function to call */ +prog_char help_cobboard_show[] = "show cobboard status"; +parse_pgm_inst_t cmd_cobboard_show = { + .f = cmd_cobboard_show_parsed, /* function to call */ .data = NULL, /* 2nd arg of func */ - .help_str = help_mechboard_show, + .help_str = help_cobboard_show, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_mechboard_show_arg0, - (prog_void *)&cmd_mechboard_show_arg1, + (prog_void *)&cmd_cobboard_show_arg0, + (prog_void *)&cmd_cobboard_show_arg1, NULL, }, }; /**********************************************************/ -/* Mechboard_Setmode1 */ +/* Cobboard_Setmode1 */ -/* this structure is filled when cmd_mechboard_setmode1 is parsed successfully */ -struct cmd_mechboard_setmode1_result { +/* this structure is filled when cmd_cobboard_setmode1 is parsed successfully */ +struct cmd_cobboard_setmode1_result { fixed_string_t arg0; fixed_string_t arg1; }; -/* function called when cmd_mechboard_setmode1 is parsed successfully */ -static void cmd_mechboard_setmode1_parsed(void *parsed_result, void *data) +/* function called when cmd_cobboard_setmode1 is parsed successfully */ +static void cmd_cobboard_setmode1_parsed(void *parsed_result, void *data) { - struct cmd_mechboard_setmode1_result *res = parsed_result; + struct cmd_cobboard_setmode1_result *res = parsed_result; if (!strcmp_P(res->arg1, PSTR("init"))) - i2c_mechboard_mode_init(); - else if (!strcmp_P(res->arg1, PSTR("manual"))) - i2c_mechboard_mode_manual(); - else if (!strcmp_P(res->arg1, PSTR("pickup"))) - i2c_mechboard_mode_pickup(); - else if (!strcmp_P(res->arg1, PSTR("lazy_harvest"))) - i2c_mechboard_mode_lazy_harvest(); - else if (!strcmp_P(res->arg1, PSTR("harvest"))) - i2c_mechboard_mode_harvest(); - else if (!strcmp_P(res->arg1, PSTR("prepare_get_lintel"))) - i2c_mechboard_mode_prepare_get_lintel(); - else if (!strcmp_P(res->arg1, PSTR("get_lintel"))) - i2c_mechboard_mode_get_lintel(); - else if (!strcmp_P(res->arg1, PSTR("put_lintel"))) - i2c_mechboard_mode_put_lintel(); - else if (!strcmp_P(res->arg1, PSTR("init"))) - i2c_mechboard_mode_init(); + i2c_cobboard_set_mode(I2C_COBBOARD_MODE_INIT); else if (!strcmp_P(res->arg1, PSTR("eject"))) - i2c_mechboard_mode_init(); - else if (!strcmp_P(res->arg1, PSTR("clear"))) - i2c_mechboard_mode_clear(); - else if (!strcmp_P(res->arg1, PSTR("loaded"))) - i2c_mechboard_mode_loaded(); - else if (!strcmp_P(res->arg1, PSTR("store"))) - i2c_mechboard_mode_store(); - else if (!strcmp_P(res->arg1, PSTR("manivelle"))) - i2c_mechboard_mode_manivelle(); - else if (!strcmp_P(res->arg1, PSTR("lazy_pickup"))) - i2c_mechboard_mode_lazy_pickup(); + i2c_cobboard_set_mode(I2C_COBBOARD_MODE_EJECT); } -prog_char str_mechboard_setmode1_arg0[] = "mechboard"; -parse_pgm_token_string_t cmd_mechboard_setmode1_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode1_result, arg0, str_mechboard_setmode1_arg0); -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"; -parse_pgm_token_string_t cmd_mechboard_setmode1_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode1_result, arg1, str_mechboard_setmode1_arg1); +prog_char str_cobboard_setmode1_arg0[] = "cobboard"; +parse_pgm_token_string_t cmd_cobboard_setmode1_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode1_result, arg0, str_cobboard_setmode1_arg0); +prog_char str_cobboard_setmode1_arg1[] = "init#eject"; +parse_pgm_token_string_t cmd_cobboard_setmode1_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode1_result, arg1, str_cobboard_setmode1_arg1); -prog_char help_mechboard_setmode1[] = "set mechboard mode (mode)"; -parse_pgm_inst_t cmd_mechboard_setmode1 = { - .f = cmd_mechboard_setmode1_parsed, /* function to call */ +prog_char help_cobboard_setmode1[] = "set cobboard mode (mode)"; +parse_pgm_inst_t cmd_cobboard_setmode1 = { + .f = cmd_cobboard_setmode1_parsed, /* function to call */ .data = NULL, /* 2nd arg of func */ - .help_str = help_mechboard_setmode1, + .help_str = help_cobboard_setmode1, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_mechboard_setmode1_arg0, - (prog_void *)&cmd_mechboard_setmode1_arg1, + (prog_void *)&cmd_cobboard_setmode1_arg0, + (prog_void *)&cmd_cobboard_setmode1_arg1, NULL, }, }; /**********************************************************/ -/* Mechboard_Setmode2 */ +/* Cobboard_Setmode2 */ -/* this structure is filled when cmd_mechboard_setmode2 is parsed successfully */ -struct cmd_mechboard_setmode2_result { +/* this structure is filled when cmd_cobboard_setmode2 is parsed successfully */ +struct cmd_cobboard_setmode2_result { fixed_string_t arg0; fixed_string_t arg1; fixed_string_t arg2; }; -/* function called when cmd_mechboard_setmode2 is parsed successfully */ -static void cmd_mechboard_setmode2_parsed(void * parsed_result, void * data) +/* function called when cmd_cobboard_setmode2 is parsed successfully */ +static void cmd_cobboard_setmode2_parsed(void *parsed_result, void *data) { - struct cmd_mechboard_setmode2_result *res = parsed_result; + struct cmd_cobboard_setmode2_result *res = parsed_result; uint8_t side = I2C_LEFT_SIDE; if (!strcmp_P(res->arg2, PSTR("left"))) side = I2C_LEFT_SIDE; else if (!strcmp_P(res->arg2, PSTR("right"))) side = I2C_RIGHT_SIDE; - else if (!strcmp_P(res->arg2, PSTR("center"))) - side = I2C_CENTER_SIDE; - else if (!strcmp_P(res->arg2, PSTR("auto"))) - side = I2C_AUTO_SIDE; - - if (!strcmp_P(res->arg1, PSTR("prepare_pickup"))) - i2c_mechboard_mode_prepare_pickup(side); - else if (!strcmp_P(res->arg1, PSTR("push_temple_disc"))) - i2c_mechboard_mode_push_temple_disc(side); + + if (!strcmp_P(res->arg1, PSTR("deploy"))) { + i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST); + i2c_cobboard_deploy(side); + } + else if (!strcmp_P(res->arg1, PSTR("harvest"))) { + i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST); + i2c_cobboard_autoharvest(side); + } + else if (!strcmp_P(res->arg1, PSTR("pack"))) { + i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST); + i2c_cobboard_pack(side); + } + else if (!strcmp_P(res->arg1, PSTR("deploy_nomove"))) { + i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST); + i2c_cobboard_deploy_nomove(side); + } + else if (!strcmp_P(res->arg1, PSTR("harvest_nomove"))) { + i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST); + i2c_cobboard_autoharvest_nomove(side); + } } -prog_char str_mechboard_setmode2_arg0[] = "mechboard"; -parse_pgm_token_string_t cmd_mechboard_setmode2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode2_result, arg0, str_mechboard_setmode2_arg0); -prog_char str_mechboard_setmode2_arg1[] = "prepare_pickup#push_temple_disc"; -parse_pgm_token_string_t cmd_mechboard_setmode2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode2_result, arg1, str_mechboard_setmode2_arg1); -prog_char str_mechboard_setmode2_arg2[] = "left#right#auto#center"; -parse_pgm_token_string_t cmd_mechboard_setmode2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode2_result, arg2, str_mechboard_setmode2_arg2); +prog_char str_cobboard_setmode2_arg0[] = "cobboard"; +parse_pgm_token_string_t cmd_cobboard_setmode2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode2_result, arg0, str_cobboard_setmode2_arg0); +prog_char str_cobboard_setmode2_arg1[] = "harvest#deploy#pack#harvest_nomove#deploy_nomove"; +parse_pgm_token_string_t cmd_cobboard_setmode2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode2_result, arg1, str_cobboard_setmode2_arg1); +prog_char str_cobboard_setmode2_arg2[] = "left#right"; +parse_pgm_token_string_t cmd_cobboard_setmode2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode2_result, arg2, str_cobboard_setmode2_arg2); -prog_char help_mechboard_setmode2[] = "set mechboard mode (more, side)"; -parse_pgm_inst_t cmd_mechboard_setmode2 = { - .f = cmd_mechboard_setmode2_parsed, /* function to call */ +prog_char help_cobboard_setmode2[] = "set cobboard mode (mode, side)"; +parse_pgm_inst_t cmd_cobboard_setmode2 = { + .f = cmd_cobboard_setmode2_parsed, /* function to call */ .data = NULL, /* 2nd arg of func */ - .help_str = help_mechboard_setmode2, + .help_str = help_cobboard_setmode2, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_mechboard_setmode2_arg0, - (prog_void *)&cmd_mechboard_setmode2_arg1, - (prog_void *)&cmd_mechboard_setmode2_arg2, + (prog_void *)&cmd_cobboard_setmode2_arg0, + (prog_void *)&cmd_cobboard_setmode2_arg1, + (prog_void *)&cmd_cobboard_setmode2_arg2, NULL, }, }; /**********************************************************/ -/* Mechboard_Setmode3 */ +/* Cobboard_Setmode3 */ -/* this structure is filled when cmd_mechboard_setmode3 is parsed successfully */ -struct cmd_mechboard_setmode3_result { +/* this structure is filled when cmd_cobboard_setmode3 is parsed successfully */ +struct cmd_cobboard_setmode3_result { fixed_string_t arg0; fixed_string_t arg1; uint8_t level; }; -/* function called when cmd_mechboard_setmode3 is parsed successfully */ -static void cmd_mechboard_setmode3_parsed(void *parsed_result, void *data) +/* function called when cmd_cobboard_setmode3 is parsed successfully */ +static void cmd_cobboard_setmode3_parsed(void *parsed_result, void *data) { - struct cmd_mechboard_setmode3_result *res = parsed_result; - if (!strcmp_P(res->arg1, PSTR("autobuild"))) - i2c_mechboard_mode_simple_autobuild(res->level); - else if (!strcmp_P(res->arg1, PSTR("prepare_build"))) - i2c_mechboard_mode_prepare_build_both(res->level); - else if (!strcmp_P(res->arg1, PSTR("prepare_inside"))) - i2c_mechboard_mode_prepare_inside_both(res->level); - else if (!strcmp_P(res->arg1, PSTR("push_temple"))) - i2c_mechboard_mode_push_temple(res->level); + struct cmd_cobboard_setmode3_result *res = parsed_result; + if (!strcmp_P(res->arg1, PSTR("xxx"))) + printf("faux\r\n"); } -prog_char str_mechboard_setmode3_arg0[] = "mechboard"; -parse_pgm_token_string_t cmd_mechboard_setmode3_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode3_result, arg0, str_mechboard_setmode3_arg0); -prog_char str_mechboard_setmode3_arg1[] = "autobuild#prepare_build#prepare_inside#push_temple"; -parse_pgm_token_string_t cmd_mechboard_setmode3_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode3_result, arg1, str_mechboard_setmode3_arg1); -parse_pgm_token_num_t cmd_mechboard_setmode3_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_mechboard_setmode3_result, level, UINT8); +prog_char str_cobboard_setmode3_arg0[] = "cobboard"; +parse_pgm_token_string_t cmd_cobboard_setmode3_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode3_result, arg0, str_cobboard_setmode3_arg0); +prog_char str_cobboard_setmode3_arg1[] = "xxx"; +parse_pgm_token_string_t cmd_cobboard_setmode3_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode3_result, arg1, str_cobboard_setmode3_arg1); +parse_pgm_token_num_t cmd_cobboard_setmode3_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_cobboard_setmode3_result, level, UINT8); -prog_char help_mechboard_setmode3[] = "set mechboard mode (mode, level)"; -parse_pgm_inst_t cmd_mechboard_setmode3 = { - .f = cmd_mechboard_setmode3_parsed, /* function to call */ +prog_char help_cobboard_setmode3[] = "set cobboard mode (mode, level)"; +parse_pgm_inst_t cmd_cobboard_setmode3 = { + .f = cmd_cobboard_setmode3_parsed, /* function to call */ .data = NULL, /* 2nd arg of func */ - .help_str = help_mechboard_setmode3, + .help_str = help_cobboard_setmode3, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_mechboard_setmode3_arg0, - (prog_void *)&cmd_mechboard_setmode3_arg1, - (prog_void *)&cmd_mechboard_setmode3_arg2, + (prog_void *)&cmd_cobboard_setmode3_arg0, + (prog_void *)&cmd_cobboard_setmode3_arg1, + (prog_void *)&cmd_cobboard_setmode3_arg2, NULL, }, }; /**********************************************************/ -/* Mechboard_Setmode4 */ +/* Ballboard_Show */ -/* this structure is filled when cmd_mechboard_setmode4 is parsed successfully */ -struct cmd_mechboard_setmode4_result { +/* this structure is filled when cmd_ballboard_show is parsed successfully */ +struct cmd_ballboard_show_result { fixed_string_t arg0; fixed_string_t arg1; - uint8_t level_l; - uint8_t count_l; - uint8_t dist_l; - uint8_t level_r; - uint8_t count_r; - uint8_t dist_r; - uint8_t do_lintel; }; -/* function called when cmd_mechboard_setmode4 is parsed successfully */ -static void cmd_mechboard_setmode4_parsed(void *parsed_result, void *data) +/* function called when cmd_ballboard_show is parsed successfully */ +static void cmd_ballboard_show_parsed(void * parsed_result, void * data) { - struct cmd_mechboard_setmode4_result *res = parsed_result; - i2c_mechboard_mode_autobuild(res->level_l, res->count_l, res->dist_l, - res->level_r, res->count_r, res->dist_r, - res->do_lintel); +#ifdef HOST_VERSION + printf("not implemented\n"); +#else + printf_P(PSTR("mode = %x\r\n"), ballboard.mode); + printf_P(PSTR("status = %x\r\n"), ballboard.status); + printf_P(PSTR("ball_count = %d\r\n"), ballboard.ball_count); + printf_P(PSTR("lcob = %d\r\n"), ballboard.lcob); + printf_P(PSTR("rcob = %d\r\n"), ballboard.rcob); +#endif } -prog_char str_mechboard_setmode4_arg0[] = "mechboard"; -parse_pgm_token_string_t cmd_mechboard_setmode4_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode4_result, arg0, str_mechboard_setmode4_arg0); -prog_char str_mechboard_setmode4_arg1[] = "autobuild"; -parse_pgm_token_string_t cmd_mechboard_setmode4_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode4_result, arg1, str_mechboard_setmode4_arg1); -parse_pgm_token_num_t cmd_mechboard_setmode4_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_mechboard_setmode4_result, level_l, UINT8); -parse_pgm_token_num_t cmd_mechboard_setmode4_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_mechboard_setmode4_result, count_l, UINT8); -parse_pgm_token_num_t cmd_mechboard_setmode4_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_mechboard_setmode4_result, dist_l, UINT8); -parse_pgm_token_num_t cmd_mechboard_setmode4_arg5 = TOKEN_NUM_INITIALIZER(struct cmd_mechboard_setmode4_result, level_r, UINT8); -parse_pgm_token_num_t cmd_mechboard_setmode4_arg6 = TOKEN_NUM_INITIALIZER(struct cmd_mechboard_setmode4_result, count_r, UINT8); -parse_pgm_token_num_t cmd_mechboard_setmode4_arg7 = TOKEN_NUM_INITIALIZER(struct cmd_mechboard_setmode4_result, dist_r, UINT8); -parse_pgm_token_num_t cmd_mechboard_setmode4_arg8 = TOKEN_NUM_INITIALIZER(struct cmd_mechboard_setmode4_result, do_lintel, UINT8); - -prog_char help_mechboard_setmode4[] = "set mechboard mode (autobuild level_l count_l dist_l level_r count_r dist_r lintel)"; -parse_pgm_inst_t cmd_mechboard_setmode4 = { - .f = cmd_mechboard_setmode4_parsed, /* function to call */ - .data = NULL, /* 2nd arg of func */ - .help_str = help_mechboard_setmode4, - .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_mechboard_setmode4_arg0, - (prog_void *)&cmd_mechboard_setmode4_arg1, - (prog_void *)&cmd_mechboard_setmode4_arg2, - (prog_void *)&cmd_mechboard_setmode4_arg3, - (prog_void *)&cmd_mechboard_setmode4_arg4, - (prog_void *)&cmd_mechboard_setmode4_arg5, - (prog_void *)&cmd_mechboard_setmode4_arg6, - (prog_void *)&cmd_mechboard_setmode4_arg7, - (prog_void *)&cmd_mechboard_setmode4_arg8, - NULL, - }, -}; - -/**********************************************************/ -/* Mechboard_Setmode5 */ - -/* this structure is filled when cmd_mechboard_setmode5 is parsed successfully */ -struct cmd_mechboard_setmode5_result { - fixed_string_t arg0; - fixed_string_t arg1; - fixed_string_t arg2; - fixed_string_t arg3; -}; - -/* function called when cmd_mechboard_setmode5 is parsed successfully */ -static void cmd_mechboard_setmode5_parsed(void *parsed_result, void * data) -{ - struct cmd_mechboard_setmode5_result *res = parsed_result; - uint8_t side = I2C_LEFT_SIDE, next_mode = I2C_MECHBOARD_MODE_PREPARE_PICKUP; - - if (!strcmp_P(res->arg2, PSTR("left"))) - side = I2C_LEFT_SIDE; - else if (!strcmp_P(res->arg2, PSTR("right"))) - side = I2C_RIGHT_SIDE; - else if (!strcmp_P(res->arg2, PSTR("center"))) - side = I2C_CENTER_SIDE; - else if (!strcmp_P(res->arg2, PSTR("auto"))) - side = I2C_AUTO_SIDE; - - if (!strcmp_P(res->arg3, PSTR("harvest"))) - next_mode = I2C_MECHBOARD_MODE_HARVEST; - else if (!strcmp_P(res->arg3, PSTR("lazy_harvest"))) - next_mode = I2C_MECHBOARD_MODE_LAZY_HARVEST; - else if (!strcmp_P(res->arg3, PSTR("pickup"))) - next_mode = I2C_MECHBOARD_MODE_PICKUP; - else if (!strcmp_P(res->arg3, PSTR("clear"))) - next_mode = I2C_MECHBOARD_MODE_CLEAR; - else if (!strcmp_P(res->arg3, PSTR("store"))) - next_mode = I2C_MECHBOARD_MODE_STORE; - else if (!strcmp_P(res->arg3, PSTR("lazy_pickup"))) - next_mode = I2C_MECHBOARD_MODE_LAZY_PICKUP; - - if (!strcmp_P(res->arg1, PSTR("prepare_pickup"))) - i2c_mechboard_mode_prepare_pickup_next(side, next_mode); -} +prog_char str_ballboard_show_arg0[] = "ballboard"; +parse_pgm_token_string_t cmd_ballboard_show_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_show_result, arg0, str_ballboard_show_arg0); +prog_char str_ballboard_show_arg1[] = "show"; +parse_pgm_token_string_t cmd_ballboard_show_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_show_result, arg1, str_ballboard_show_arg1); -prog_char str_mechboard_setmode5_arg0[] = "mechboard"; -parse_pgm_token_string_t cmd_mechboard_setmode5_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode5_result, arg0, str_mechboard_setmode5_arg0); -prog_char str_mechboard_setmode5_arg1[] = "prepare_pickup"; -parse_pgm_token_string_t cmd_mechboard_setmode5_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode5_result, arg1, str_mechboard_setmode5_arg1); -prog_char str_mechboard_setmode5_arg2[] = "left#right#auto#center"; -parse_pgm_token_string_t cmd_mechboard_setmode5_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode5_result, arg2, str_mechboard_setmode5_arg2); -prog_char str_mechboard_setmode5_arg3[] = "harvest#pickup#store#lazy_harvest#lazy_pickup#clear"; -parse_pgm_token_string_t cmd_mechboard_setmode5_arg3 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode5_result, arg3, str_mechboard_setmode5_arg3); - -prog_char help_mechboard_setmode5[] = "set mechboard mode (more, side)"; -parse_pgm_inst_t cmd_mechboard_setmode5 = { - .f = cmd_mechboard_setmode5_parsed, /* function to call */ +prog_char help_ballboard_show[] = "show ballboard status"; +parse_pgm_inst_t cmd_ballboard_show = { + .f = cmd_ballboard_show_parsed, /* function to call */ .data = NULL, /* 2nd arg of func */ - .help_str = help_mechboard_setmode5, + .help_str = help_ballboard_show, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_mechboard_setmode5_arg0, - (prog_void *)&cmd_mechboard_setmode5_arg1, - (prog_void *)&cmd_mechboard_setmode5_arg2, - (prog_void *)&cmd_mechboard_setmode5_arg3, + (prog_void *)&cmd_ballboard_show_arg0, + (prog_void *)&cmd_ballboard_show_arg1, NULL, }, }; /**********************************************************/ -/* pickup wheels */ +/* Ballboard_Setmode1 */ -/* this structure is filled when cmd_pickup_wheels is parsed successfully */ -struct cmd_pickup_wheels_result { +/* this structure is filled when cmd_ballboard_setmode1 is parsed successfully */ +struct cmd_ballboard_setmode1_result { fixed_string_t arg0; fixed_string_t arg1; }; -/* function called when cmd_pickup_wheels is parsed successfully */ -static void cmd_pickup_wheels_parsed(void *parsed_result, void *data) +/* function called when cmd_ballboard_setmode1 is parsed successfully */ +static void cmd_ballboard_setmode1_parsed(void *parsed_result, void *data) { - struct cmd_pickup_wheels_result *res = parsed_result; - - if (!strcmp_P(res->arg1, PSTR("on"))) - pickup_wheels_on(); - else - pickup_wheels_off(); -} - -prog_char str_pickup_wheels_arg0[] = "pickup_wheels"; -parse_pgm_token_string_t cmd_pickup_wheels_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_pickup_wheels_result, arg0, str_pickup_wheels_arg0); -prog_char str_pickup_wheels_arg1[] = "on#off"; -parse_pgm_token_string_t cmd_pickup_wheels_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_pickup_wheels_result, arg1, str_pickup_wheels_arg1); - -prog_char help_pickup_wheels[] = "Enable/disable pickup wheels"; -parse_pgm_inst_t cmd_pickup_wheels = { - .f = cmd_pickup_wheels_parsed, /* function to call */ - .data = NULL, /* 2nd arg of func */ - .help_str = help_pickup_wheels, - .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_pickup_wheels_arg0, - (prog_void *)&cmd_pickup_wheels_arg1, - NULL, - }, -}; + struct cmd_ballboard_setmode1_result *res = parsed_result; -/**********************************************************/ -/* Beacon_Start */ - -/* this structure is filled when cmd_beacon_start is parsed successfully */ -struct cmd_beacon_start_result { - fixed_string_t arg0; - fixed_string_t arg1; -}; - -/* function called when cmd_beacon_start is parsed successfully */ -static void cmd_beacon_start_parsed(void *parsed_result, void *data) -{ - struct cmd_beacon_start_result *res = parsed_result; + if (!strcmp_P(res->arg1, PSTR("init"))) + i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_INIT); + else if (!strcmp_P(res->arg1, PSTR("off"))) + i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_OFF); + else if (!strcmp_P(res->arg1, PSTR("eject"))) + i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_EJECT); + else if (!strcmp_P(res->arg1, PSTR("harvest"))) + i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_HARVEST); - if (!strcmp_P(res->arg1, PSTR("start"))) - i2c_sensorboard_set_beacon(1); - else - i2c_sensorboard_set_beacon(0); + /* other commands */ } -prog_char str_beacon_start_arg0[] = "beacon"; -parse_pgm_token_string_t cmd_beacon_start_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_beacon_start_result, arg0, str_beacon_start_arg0); -prog_char str_beacon_start_arg1[] = "start#stop"; -parse_pgm_token_string_t cmd_beacon_start_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_beacon_start_result, arg1, str_beacon_start_arg1); +prog_char str_ballboard_setmode1_arg0[] = "ballboard"; +parse_pgm_token_string_t cmd_ballboard_setmode1_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode1_result, arg0, str_ballboard_setmode1_arg0); +prog_char str_ballboard_setmode1_arg1[] = "init#eject#harvest#off"; +parse_pgm_token_string_t cmd_ballboard_setmode1_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode1_result, arg1, str_ballboard_setmode1_arg1); -prog_char help_beacon_start[] = "Beacon enabled/disable"; -parse_pgm_inst_t cmd_beacon_start = { - .f = cmd_beacon_start_parsed, /* function to call */ +prog_char help_ballboard_setmode1[] = "set ballboard mode (mode)"; +parse_pgm_inst_t cmd_ballboard_setmode1 = { + .f = cmd_ballboard_setmode1_parsed, /* function to call */ .data = NULL, /* 2nd arg of func */ - .help_str = help_beacon_start, + .help_str = help_ballboard_setmode1, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_beacon_start_arg0, - (prog_void *)&cmd_beacon_start_arg1, + (prog_void *)&cmd_ballboard_setmode1_arg0, + (prog_void *)&cmd_ballboard_setmode1_arg1, NULL, }, }; /**********************************************************/ -/* Pump_Current */ +/* Ballboard_Setmode2 */ -/* this structure is filled when cmd_pump_current is parsed successfully */ -struct cmd_pump_current_result { +/* this structure is filled when cmd_ballboard_setmode2 is parsed successfully */ +struct cmd_ballboard_setmode2_result { fixed_string_t arg0; fixed_string_t arg1; + fixed_string_t arg2; }; -/* function called when cmd_pump_current is parsed successfully */ -static void cmd_pump_current_parsed(__attribute__((unused)) void *parsed_result, - __attribute__((unused)) void *data) -{ - printf_P(PSTR("l1=%d l2=%d r1=%d r2=%d\r\n"), - sensor_get_adc(ADC_CSENSE3), sensor_get_adc(ADC_CSENSE4), - mechboard.pump_right1_current, mechboard.pump_right2_current); -} - -prog_char str_pump_current_arg0[] = "pump_current"; -parse_pgm_token_string_t cmd_pump_current_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_pump_current_result, arg0, str_pump_current_arg0); -prog_char str_pump_current_arg1[] = "show"; -parse_pgm_token_string_t cmd_pump_current_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_pump_current_result, arg1, str_pump_current_arg1); - -prog_char help_pump_current[] = "dump pump current"; -parse_pgm_inst_t cmd_pump_current = { - .f = cmd_pump_current_parsed, /* function to call */ - .data = NULL, /* 2nd arg of func */ - .help_str = help_pump_current, - .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_pump_current_arg0, - (prog_void *)&cmd_pump_current_arg1, - NULL, - }, -}; - -/**********************************************************/ -/* Build_Test */ - -/* this structure is filled when cmd_build_test is parsed successfully */ -struct cmd_build_test_result { - fixed_string_t arg0; -}; - -/* function called when cmd_build_test is parsed successfully */ -static void cmd_build_test_parsed(void *parsed_result, void *data) -{ - //struct cmd_build_test_result *res = parsed_result; - - printf_P(PSTR("lintel must be there\r\n")); - i2c_mechboard_mode_prepare_pickup_next(I2C_LEFT_SIDE, - I2C_MECHBOARD_MODE_HARVEST); - wait_ms(500); - - printf_P(PSTR("Insert 4 colums\r\n")); - while (get_column_count() != 4); - - i2c_mechboard_mode_prepare_build_both(0); - trajectory_d_rel(&mainboard.traj, 200); - wait_traj_end(TRAJ_FLAGS_NO_NEAR); - wait_ms(500); - - i2c_mechboard_mode_simple_autobuild(0); - wait_ms(100); - while (get_mechboard_mode() == I2C_MECHBOARD_MODE_AUTOBUILD); - - trajectory_d_rel(&mainboard.traj, -200); - wait_traj_end(TRAJ_FLAGS_NO_NEAR); - i2c_mechboard_mode_prepare_pickup_next(I2C_LEFT_SIDE, - I2C_MECHBOARD_MODE_HARVEST); - - while (get_column_count() != 3); - - i2c_mechboard_mode_prepare_build_both(3); - trajectory_d_rel(&mainboard.traj, 200); - wait_traj_end(TRAJ_FLAGS_NO_NEAR); - wait_ms(500); - - i2c_mechboard_mode_autobuild(3, 1, I2C_AUTOBUILD_DEFAULT_DIST, - 3, 2,I2C_AUTOBUILD_DEFAULT_DIST, 0); - i2cproto_wait_update(); - while (get_mechboard_mode() == I2C_MECHBOARD_MODE_AUTOBUILD); - - trajectory_d_rel(&mainboard.traj, -200); - wait_traj_end(TRAJ_FLAGS_NO_NEAR); - i2c_mechboard_mode_prepare_pickup(I2C_RIGHT_SIDE); - wait_ms(500); - - i2c_mechboard_mode_harvest(); - while (get_column_count() != 3); - - i2c_mechboard_mode_prepare_build_both(5); - trajectory_d_rel(&mainboard.traj, 200); - wait_traj_end(TRAJ_FLAGS_NO_NEAR); - wait_ms(1000); - - i2c_mechboard_mode_autobuild(4, 2, I2C_AUTOBUILD_DEFAULT_DIST, - 5, 1, I2C_AUTOBUILD_DEFAULT_DIST, 0); - i2cproto_wait_update(); - while (get_mechboard_mode() == I2C_MECHBOARD_MODE_AUTOBUILD); - - trajectory_d_rel(&mainboard.traj, -200); -} - -prog_char str_build_test_arg0[] = "build_test"; -parse_pgm_token_string_t cmd_build_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_build_test_result, arg0, str_build_test_arg0); - -prog_char help_build_test[] = "Build_Test function"; -parse_pgm_inst_t cmd_build_test = { - .f = cmd_build_test_parsed, /* function to call */ - .data = NULL, /* 2nd arg of func */ - .help_str = help_build_test, - .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_build_test_arg0, - NULL, - }, -}; - - -/**********************************************************/ -/* Column_Test */ - -/* this structure is filled when cmd_column_test is parsed successfully */ -struct cmd_column_test_result { - fixed_string_t arg0; - uint8_t level; - int16_t dist; - int8_t a1; - int8_t a2; - int8_t a3; - int16_t arm_dist; - int8_t nb_col; -}; - -/* function called when cmd_column_test is parsed successfully */ -static void cmd_column_test_parsed(void *parsed_result, void *data) +/* function called when cmd_ballboard_setmode2 is parsed successfully */ +static void cmd_ballboard_setmode2_parsed(void * parsed_result, void * data) { - struct cmd_column_test_result *res = parsed_result; - uint8_t level = res->level, debug = 0; - uint8_t c, push = 0; - - /* default conf */ - if (data) { - res->dist = 70; - res->a1 = -20; - res->a2 = 40; - res->a3 = -20; - res->arm_dist = 220; - res->nb_col = 2; + struct cmd_ballboard_setmode2_result *res = parsed_result; + uint8_t mode = I2C_BALLBOARD_MODE_INIT; + + if (!strcmp_P(res->arg2, PSTR("left"))) { + if (!strcmp_P(res->arg1, PSTR("prepare"))) + mode = I2C_BALLBOARD_MODE_PREP_L_FORK; + else if (!strcmp_P(res->arg1, PSTR("take"))) + mode = I2C_BALLBOARD_MODE_TAKE_L_FORK; } - - if (!strcmp_P(res->arg0, PSTR("column_test_debug"))) - debug = 1; - if (!strcmp_P(res->arg0, PSTR("column_test_push"))) - push = 1; - - strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_VERY_SLOW); - - /* Go to disc */ - - trajectory_d_rel(&mainboard.traj, 200); - wait_traj_end(TRAJ_FLAGS_SMALL_DIST); - - /* go back, insert colums */ - - trajectory_d_rel(&mainboard.traj, -200); - wait_traj_end(TRAJ_FLAGS_NO_NEAR); - - i2c_mechboard_mode_prepare_pickup_next(I2C_LEFT_SIDE, - I2C_MECHBOARD_MODE_HARVEST); - printf_P(PSTR("Insert 4 colums\r\n")); - while (get_column_count() != 4); - - /* build with left arm */ - - i2c_mechboard_mode_prepare_inside_both(level); - trajectory_d_rel(&mainboard.traj, 200-(res->dist)); - wait_traj_end(TRAJ_FLAGS_NO_NEAR); - - if (debug) - c = cmdline_getchar_wait(); - - trajectory_a_rel(&mainboard.traj, res->a1); - wait_traj_end(TRAJ_FLAGS_NO_NEAR); - - if (debug) - c = cmdline_getchar_wait(); - - i2c_mechboard_mode_prepare_build_select(level, -1); - time_wait_ms(200); - if (debug) - c = cmdline_getchar_wait(); - i2c_mechboard_mode_autobuild(level, res->nb_col, res->arm_dist, - 0, 0, res->arm_dist, 0); - while (get_mechboard_mode() != I2C_MECHBOARD_MODE_AUTOBUILD); - while (get_mechboard_mode() == I2C_MECHBOARD_MODE_AUTOBUILD); - - if (debug) - c = cmdline_getchar_wait(); - i2c_mechboard_mode_prepare_inside_select(level+res->nb_col, -1); - - if (debug) - c = cmdline_getchar_wait(); - /* build with right arm */ - - trajectory_a_rel(&mainboard.traj, res->a2); - wait_traj_end(TRAJ_FLAGS_NO_NEAR); - - if (debug) - c = cmdline_getchar_wait(); - /* only ok for nb_col == 2 */ - if ((level + res->nb_col) >= 7) - i2c_mechboard_mode_prepare_build_select(-1, level + res->nb_col + 1); - else - i2c_mechboard_mode_prepare_build_select(-1, level + res->nb_col); - time_wait_ms(200); - if (debug) - c = cmdline_getchar_wait(); - i2c_mechboard_mode_autobuild(0, 0, res->arm_dist, - level + res->nb_col, res->nb_col, - res->arm_dist, 0); - while (get_mechboard_mode() != I2C_MECHBOARD_MODE_AUTOBUILD); - while (get_mechboard_mode() == I2C_MECHBOARD_MODE_AUTOBUILD); - - - if (push) { - strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST); - trajectory_d_rel(&mainboard.traj, -100); - wait_traj_end(TRAJ_FLAGS_SMALL_DIST); - i2c_mechboard_mode_push_temple_disc(I2C_RIGHT_SIDE); - time_wait_ms(500); - trajectory_d_rel(&mainboard.traj, 100); - wait_traj_end(TRAJ_FLAGS_SMALL_DIST); - } - else if (level == 1 || level == 0) { - trajectory_d_rel(&mainboard.traj, -100); - wait_traj_end(TRAJ_FLAGS_SMALL_DIST); - i2c_mechboard_mode_push_temple(level); - time_wait_ms(400); - strat_set_speed(200, SPEED_ANGLE_SLOW); - trajectory_d_rel(&mainboard.traj, 120); - wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + else { + if (!strcmp_P(res->arg1, PSTR("prepare"))) + mode = I2C_BALLBOARD_MODE_PREP_R_FORK; + else if (!strcmp_P(res->arg1, PSTR("take"))) + mode = I2C_BALLBOARD_MODE_TAKE_R_FORK; } - - strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST); - - if (debug) - c = cmdline_getchar_wait(); - i2c_mechboard_mode_prepare_inside_select(-1, level+res->nb_col*2); - - if (debug) - c = cmdline_getchar_wait(); - - trajectory_a_rel(&mainboard.traj, res->a3); - wait_traj_end(TRAJ_FLAGS_NO_NEAR); - - if (debug) - c = cmdline_getchar_wait(); - /* go back, insert colums */ - - trajectory_d_rel(&mainboard.traj, -100); - - return; + i2c_ballboard_set_mode(mode); } -prog_char str_column_test_arg0[] = "column_test#column_test_debug#column_test_push"; -parse_pgm_token_string_t cmd_column_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_column_test_result, arg0, str_column_test_arg0); -parse_pgm_token_num_t cmd_column_test_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_column_test_result, level, UINT8); - -prog_char help_column_test[] = "Column_Test function (level)"; -parse_pgm_inst_t cmd_column_test = { - .f = cmd_column_test_parsed, /* function to call */ - .data = (void *)1, /* 2nd arg of func */ - .help_str = help_column_test, - .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_column_test_arg0, - (prog_void *)&cmd_column_test_arg1, - NULL, - }, -}; - -parse_pgm_token_num_t cmd_column_test_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_column_test_result, dist, INT16); -parse_pgm_token_num_t cmd_column_test_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_column_test_result, a1, INT8); -parse_pgm_token_num_t cmd_column_test_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_column_test_result, a2, INT8); -parse_pgm_token_num_t cmd_column_test_arg5 = TOKEN_NUM_INITIALIZER(struct cmd_column_test_result, a3, INT8); -parse_pgm_token_num_t cmd_column_test_arg6 = TOKEN_NUM_INITIALIZER(struct cmd_column_test_result, arm_dist, INT16); -parse_pgm_token_num_t cmd_column_test_arg7 = TOKEN_NUM_INITIALIZER(struct cmd_column_test_result, nb_col, INT8); +prog_char str_ballboard_setmode2_arg0[] = "ballboard"; +parse_pgm_token_string_t cmd_ballboard_setmode2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode2_result, arg0, str_ballboard_setmode2_arg0); +prog_char str_ballboard_setmode2_arg1[] = "prepare#take"; +parse_pgm_token_string_t cmd_ballboard_setmode2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode2_result, arg1, str_ballboard_setmode2_arg1); +prog_char str_ballboard_setmode2_arg2[] = "left#right"; +parse_pgm_token_string_t cmd_ballboard_setmode2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode2_result, arg2, str_ballboard_setmode2_arg2); -prog_char help_column_test2[] = "Column_Test function (level, dist, a1, a2, a3, arm_dist, nb_col)"; -parse_pgm_inst_t cmd_column_test2 = { - .f = cmd_column_test_parsed, /* function to call */ +prog_char help_ballboard_setmode2[] = "set ballboard mode (mode, side)"; +parse_pgm_inst_t cmd_ballboard_setmode2 = { + .f = cmd_ballboard_setmode2_parsed, /* function to call */ .data = NULL, /* 2nd arg of func */ - .help_str = help_column_test2, + .help_str = help_ballboard_setmode2, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_column_test_arg0, - (prog_void *)&cmd_column_test_arg1, - (prog_void *)&cmd_column_test_arg2, - (prog_void *)&cmd_column_test_arg3, - (prog_void *)&cmd_column_test_arg4, - (prog_void *)&cmd_column_test_arg5, - (prog_void *)&cmd_column_test_arg6, - (prog_void *)&cmd_column_test_arg7, + (prog_void *)&cmd_ballboard_setmode2_arg0, + (prog_void *)&cmd_ballboard_setmode2_arg1, + (prog_void *)&cmd_ballboard_setmode2_arg2, NULL, }, }; - /**********************************************************/ -/* Pickup_Test */ +/* Ballboard_Setmode3 */ -/* this structure is filled when cmd_pickup_test is parsed successfully */ -struct cmd_pickup_test_result { +/* this structure is filled when cmd_ballboard_setmode3 is parsed successfully */ +struct cmd_ballboard_setmode3_result { fixed_string_t arg0; fixed_string_t arg1; - int16_t dist; + uint8_t level; }; -/* return red or green sensor */ -#define COLOR_IR_SENSOR() \ - ({ \ - uint8_t __ret = 0; \ - if (side == I2C_RIGHT_SIDE) \ - __ret = sensor_get(S_DISP_RIGHT); \ - else \ - __ret = sensor_get(S_DISP_LEFT); \ - __ret; \ - }) \ -/* column dispensers */ -#define COL_SCAN_MARGIN 200 -/* distance between the wheel axis and the IR sensor */ - -/* function called when cmd_pickup_test is parsed successfully */ -static void cmd_pickup_test_parsed(void *parsed_result, void *data) +/* function called when cmd_ballboard_setmode3 is parsed successfully */ +static void cmd_ballboard_setmode3_parsed(void *parsed_result, void *data) { - uint8_t err, side, first_try = 1; - int8_t cols_count_before, cols_count_after, cols; - struct cmd_pickup_test_result *res = parsed_result; - int16_t pos1, pos2, pos; - microseconds us; - int16_t dist = res->dist; - uint8_t timeout = 0; - - if (!strcmp_P(res->arg1, PSTR("left"))) - side = I2C_LEFT_SIDE; - else - side = I2C_RIGHT_SIDE; - - i2c_mechboard_mode_prepare_pickup(I2C_AUTO_SIDE); - cols_count_before = get_column_count(); - position_set(&mainboard.pos, 0, 0, 0); - - strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_FAST); - trajectory_d_rel(&mainboard.traj, -1000); - err = WAIT_COND_OR_TRAJ_END(!COLOR_IR_SENSOR(), TRAJ_FLAGS_NO_NEAR); - if (err) /* we should not reach end */ - goto fail; - pos1 = position_get_x_s16(&mainboard.pos); - printf_P(PSTR("pos1 = %d\r\n"), pos1); - - err = WAIT_COND_OR_TRAJ_END(COLOR_IR_SENSOR(), TRAJ_FLAGS_NO_NEAR); - if (err) - goto fail; - pos2 = position_get_x_s16(&mainboard.pos); - printf_P(PSTR("pos2 = %d\r\n"), pos2); - - pos = ABS(pos1 - pos2); - printf_P(PSTR("pos = %d\r\n"), pos); - - trajectory_d_rel(&mainboard.traj, -dist + pos/2); - err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); - - if (side == I2C_LEFT_SIDE) - trajectory_a_rel(&mainboard.traj, 90); - else - trajectory_a_rel(&mainboard.traj, -90); - err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); - - pickup_wheels_on(); - retry: - if (first_try) - i2c_mechboard_mode_lazy_harvest(); - else - i2c_mechboard_mode_prepare_pickup(I2C_AUTO_SIDE); - first_try = 0; - - strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_FAST); - trajectory_d_rel(&mainboard.traj, 300); - err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST | END_NEAR); - strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST); - err = strat_calib(600, TRAJ_FLAGS_SMALL_DIST); - - trajectory_d_rel(&mainboard.traj, -DIST_BACK_DISPENSER); - err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); - if (!TRAJ_SUCCESS(err)) - goto fail; - - position_set(&mainboard.pos, 0, 0, 0); - if (get_mechboard_mode() == I2C_MECHBOARD_MODE_PREPARE_EJECT) { - strat_eject_col(90, 0); - goto retry; - } - - /* start to pickup with finger / arms */ - - printf_P(PSTR("%s pickup now\r\n"), __FUNCTION__); - i2c_mechboard_mode_pickup(); - WAIT_COND_OR_TIMEOUT(get_mechboard_mode() == - I2C_MECHBOARD_MODE_PICKUP, 100); - us = time_get_us2(); - cols = get_column_count(); - while (get_mechboard_mode() == I2C_MECHBOARD_MODE_PICKUP) { - if (get_column_count() != cols) { - cols = get_column_count(); - us = time_get_us2(); - } - if ((get_column_count() - cols_count_before) >= 4) { - printf_P(PSTR("%s no more cols in disp\r\n"), __FUNCTION__); - break; - } - /* 1 second timeout */ - if (time_get_us2() - us > 1500000L) { - printf_P(PSTR("%s timeout\r\n"), __FUNCTION__); - timeout = 1; - break; - } - } - - /* eject if we found a bad color column */ - - if (get_mechboard_mode() == I2C_MECHBOARD_MODE_PREPARE_EJECT) { - strat_eject_col(90, 0); - goto retry; - } - - strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST); - trajectory_d_rel(&mainboard.traj, -250); - wait_traj_end(TRAJ_FLAGS_SMALL_DIST | END_NEAR); - - cols_count_after = get_column_count(); - cols = cols_count_after - cols_count_before; - DEBUG(E_USER_STRAT, "%s we got %d cols", __FUNCTION__, cols); - - pickup_wheels_off(); - i2c_mechboard_mode_clear(); - - wait_ms(1000); - return; - fail: - printf_P(PSTR("failed\r\n")); - strat_hardstop(); + struct cmd_ballboard_setmode3_result *res = parsed_result; + if (!strcmp_P(res->arg1, PSTR("xxx"))) + printf("faux\r\n"); } -prog_char str_pickup_test_arg0[] = "pickup_test"; -parse_pgm_token_string_t cmd_pickup_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_pickup_test_result, arg0, str_pickup_test_arg0); -prog_char str_pickup_test_arg1[] = "left#right"; -parse_pgm_token_string_t cmd_pickup_test_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_pickup_test_result, arg1, str_pickup_test_arg1); -parse_pgm_token_num_t cmd_pickup_test_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_pickup_test_result, dist, INT16); +prog_char str_ballboard_setmode3_arg0[] = "ballboard"; +parse_pgm_token_string_t cmd_ballboard_setmode3_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode3_result, arg0, str_ballboard_setmode3_arg0); +prog_char str_ballboard_setmode3_arg1[] = "xxx"; +parse_pgm_token_string_t cmd_ballboard_setmode3_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode3_result, arg1, str_ballboard_setmode3_arg1); +parse_pgm_token_num_t cmd_ballboard_setmode3_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_ballboard_setmode3_result, level, UINT8); -prog_char help_pickup_test[] = "Pickup_Test function"; -parse_pgm_inst_t cmd_pickup_test = { - .f = cmd_pickup_test_parsed, /* function to call */ +prog_char help_ballboard_setmode3[] = "set ballboard mode (mode, level)"; +parse_pgm_inst_t cmd_ballboard_setmode3 = { + .f = cmd_ballboard_setmode3_parsed, /* function to call */ .data = NULL, /* 2nd arg of func */ - .help_str = help_pickup_test, + .help_str = help_ballboard_setmode3, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_pickup_test_arg0, - (prog_void *)&cmd_pickup_test_arg1, - (prog_void *)&cmd_pickup_test_arg2, + (prog_void *)&cmd_ballboard_setmode3_arg0, + (prog_void *)&cmd_ballboard_setmode3_arg1, + (prog_void *)&cmd_ballboard_setmode3_arg2, NULL, }, }; /**********************************************************/ -/* Lintel_Test */ +/* Servo_Balls */ -/* this structure is filled when cmd_lintel_test is parsed successfully */ -struct cmd_lintel_test_result { +/* this structure is filled when cmd_servo_balls is parsed successfully */ +struct cmd_servo_balls_result { fixed_string_t arg0; + fixed_string_t arg1; }; -/* function called when cmd_lintel_test is parsed successfully */ -static void cmd_lintel_test_parsed(void *parsed_result, void *data) +/* function called when cmd_servo_balls is parsed successfully */ +static void cmd_servo_balls_parsed(void *parsed_result, + __attribute__((unused)) void *data) { - uint8_t err, first_try = 1, right_ok, left_ok; - int16_t left_cur, right_cur; - - i2c_mechboard_mode_prepare_get_lintel(); - time_wait_ms(500); - retry: - strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST); - trajectory_d_rel(&mainboard.traj, 500); - err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); - if (!TRAJ_SUCCESS(err) && err != END_BLOCKING) - goto fail; - - i2c_mechboard_mode_get_lintel(); - time_wait_ms(500); - - left_cur = sensor_get_adc(ADC_CSENSE3); - left_ok = (left_cur > I2C_MECHBOARD_CURRENT_COLUMN); - right_cur = mechboard.pump_right1_current; - right_ok = (right_cur > I2C_MECHBOARD_CURRENT_COLUMN); - - printf_P(PSTR("left_ok=%d (%d), right_ok=%d (%d)\r\n"), - left_ok, left_cur, right_ok, right_cur); - if (first_try) { - if (!right_ok && !left_ok) { - i2c_mechboard_mode_prepare_get_lintel(); - time_wait_ms(300); - } - else if (right_ok && !left_ok) { - i2c_mechboard_mode_prepare_get_lintel(); - time_wait_ms(300); - strat_set_speed(500, 500); - trajectory_d_a_rel(&mainboard.traj, -150, 30); - err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); - trajectory_d_a_rel(&mainboard.traj, -140, -30); - err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); - first_try = 0; - goto retry; - } - else if (!right_ok && left_ok) { - i2c_mechboard_mode_prepare_get_lintel(); - time_wait_ms(300); - strat_set_speed(500, 500); - trajectory_d_a_rel(&mainboard.traj, -150, -30); - err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); - trajectory_d_a_rel(&mainboard.traj, -140, 30); - err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); - first_try = 0; - goto retry; - } - /* else, lintel is ok */ - else { - i2c_mechboard_mode_put_lintel(); - } - } - else { - if (right_ok && left_ok) { - /* lintel is ok */ - i2c_mechboard_mode_put_lintel(); - } - else { - i2c_mechboard_mode_prepare_get_lintel(); - time_wait_ms(300); - } - } + struct cmd_servo_balls_result *res = parsed_result; - strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST); - trajectory_d_rel(&mainboard.traj, -250); - err = wait_traj_end(TRAJ_FLAGS_STD); - return; - -fail: - printf_P(PSTR("fail\r\n")); - return; + if (!strcmp_P(res->arg1, PSTR("deploy"))) + support_balls_deploy(); + else if (!strcmp_P(res->arg1, PSTR("pack"))) + support_balls_pack(); } -prog_char str_lintel_test_arg0[] = "lintel_test"; -parse_pgm_token_string_t cmd_lintel_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_lintel_test_result, arg0, str_lintel_test_arg0); +prog_char str_servo_balls_arg0[] = "support_balls"; +parse_pgm_token_string_t cmd_servo_balls_arg0 = + TOKEN_STRING_INITIALIZER(struct cmd_servo_balls_result, arg0, str_servo_balls_arg0); +prog_char str_servo_balls_arg1[] = "deploy#pack"; +parse_pgm_token_string_t cmd_servo_balls_arg1 = + TOKEN_STRING_INITIALIZER(struct cmd_servo_balls_result, arg1, str_servo_balls_arg1); -prog_char help_lintel_test[] = "Lintel_Test function"; -parse_pgm_inst_t cmd_lintel_test = { - .f = cmd_lintel_test_parsed, /* function to call */ +prog_char help_servo_balls[] = "control support balls"; +parse_pgm_inst_t cmd_servo_balls = { + .f = cmd_servo_balls_parsed, /* function to call */ .data = NULL, /* 2nd arg of func */ - .help_str = help_lintel_test, + .help_str = help_servo_balls, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_lintel_test_arg0, + (prog_void *)&cmd_servo_balls_arg0, + (prog_void *)&cmd_servo_balls_arg1, NULL, }, }; /**********************************************************/ -/* Scan_Test */ +/* Clitoid */ -/* this structure is filled when cmd_scan_test is parsed successfully */ -struct cmd_scan_test_result { +/* this structure is filled when cmd_clitoid is parsed successfully */ +struct cmd_clitoid_result { fixed_string_t arg0; - fixed_string_t arg1; - int16_t start_dist; - int16_t scan_dist; - int16_t scan_speed; - int16_t center_x; - int16_t center_y; - uint8_t level; + float alpha_deg; + float beta_deg; + float R_mm; + float Vd; + float Amax; + float d_inter_mm; }; -#define SCAN_MODE_CHECK_TEMPLE 0 -#define SCAN_MODE_SCAN_COL 1 -#define SCAN_MODE_SCAN_TEMPLE 2 -#define SCAN_MODE_TRAJ_ONLY 3 - -/* function called when cmd_scan_test is parsed successfully */ -static void cmd_scan_test_parsed(void *parsed_result, void *data) +/* function called when cmd_test is parsed successfully */ +static void cmd_clitoid_parsed(void *parsed_result, void *data) { - uint8_t err, mode=0, c; - int16_t pos1x, pos1y, dist; - struct cmd_scan_test_result *res = parsed_result; - int16_t back_mm = 0; - - int16_t ckpt_rel_x = 0, ckpt_rel_y = 0; - - double center_abs_x, center_abs_y; - double ckpt_rel_d, ckpt_rel_a; - double ckpt_abs_x, ckpt_abs_y; - - if (!strcmp_P(res->arg1, PSTR("traj_only"))) - mode = SCAN_MODE_TRAJ_ONLY; - else if (!strcmp_P(res->arg1, PSTR("check_temple"))) - mode = SCAN_MODE_CHECK_TEMPLE; - else if (!strcmp_P(res->arg1, PSTR("scan_col"))) - mode = SCAN_MODE_SCAN_COL; - else if (!strcmp_P(res->arg1, PSTR("scan_temple"))) - mode = SCAN_MODE_SCAN_TEMPLE; - - /* go to disc */ - strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_VERY_SLOW); - trajectory_d_rel(&mainboard.traj, 400); - err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); - if (err != END_BLOCKING) - return; - - /* save absolute position of disc */ - rel_da_to_abs_xy(265, 0, ¢er_abs_x, ¢er_abs_y); - - /* go back and prepare to scan */ - strat_set_speed(1000, 1000); - trajectory_d_a_rel(&mainboard.traj, -140, 130); - err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); - if (!TRAJ_SUCCESS(err)) - return; - - /* prepare scanner arm */ - if (mode != SCAN_MODE_TRAJ_ONLY) - i2c_sensorboard_scanner_prepare(); - time_wait_ms(250); - - strat_set_speed(res->scan_speed, 1000); - - pos1x = position_get_x_s16(&mainboard.pos); - pos1y = position_get_y_s16(&mainboard.pos); - trajectory_d_rel(&mainboard.traj, -res->scan_dist); - - while (1) { - err = test_traj_end(TRAJ_FLAGS_SMALL_DIST); - if (err != 0) - break; - - dist = distance_from_robot(pos1x, pos1y); - - if (dist > res->start_dist) - break; - - if (get_scanner_status() & I2C_SCAN_MAX_COLUMN) { - err = END_ERROR; - break; - } - } - - if (err) { - if (TRAJ_SUCCESS(err)) - err = END_ERROR; /* should not reach end */ - strat_hardstop(); - trajectory_goto_xy_abs(&mainboard.traj, pos1x, pos1y); - wait_traj_end(TRAJ_FLAGS_SMALL_DIST); - if (mode != SCAN_MODE_TRAJ_ONLY) - i2c_sensorboard_scanner_stop(); - return; - } - - /* start the scanner */ - - if (mode != SCAN_MODE_TRAJ_ONLY) - i2c_sensorboard_scanner_start(); - - err = WAIT_COND_OR_TRAJ_END(get_scanner_status() & I2C_SCAN_MAX_COLUMN, - TRAJ_FLAGS_NO_NEAR); - if (err == 0) - err = END_ERROR; - if (!TRAJ_SUCCESS(err)) { - strat_hardstop(); - trajectory_goto_xy_abs(&mainboard.traj, pos1x, pos1y); - wait_traj_end(TRAJ_FLAGS_NO_NEAR); - if (mode != SCAN_MODE_TRAJ_ONLY) - i2c_sensorboard_scanner_stop(); - return; - } - - if (mode == SCAN_MODE_TRAJ_ONLY) - return; - - wait_scan_done(10000); - - i2c_sensorboard_scanner_stop(); - - if (mode == SCAN_MODE_CHECK_TEMPLE) { - i2c_sensorboard_scanner_algo_check(res->level, - res->center_x, res->center_y); - i2cproto_wait_update(); - wait_scan_done(10000); - scanner_dump_state(); - - if (sensorboard.dropzone_h == -1) { - printf_P(PSTR("-- try to build a temple\r\n")); - res->center_x = 15; - res->center_y = 13; - mode = SCAN_MODE_SCAN_TEMPLE; - } - } - - if (mode == SCAN_MODE_SCAN_TEMPLE) { - i2c_sensorboard_scanner_algo_temple(I2C_SCANNER_ZONE_DISC, - res->center_x, - res->center_y); - i2cproto_wait_update(); - wait_scan_done(10000); - scanner_dump_state(); - - if (sensorboard.dropzone_h == -1 || - strat_scan_get_checkpoint(mode, &ckpt_rel_x, - &ckpt_rel_y, &back_mm)) { - printf_P(PSTR("-- try to build a column\r\n")); - mode = SCAN_MODE_SCAN_COL; - } - } - - if (mode == SCAN_MODE_SCAN_COL) { - i2c_sensorboard_scanner_algo_column(I2C_SCANNER_ZONE_DISC, - res->center_x, res->center_y); - i2cproto_wait_update(); - wait_scan_done(10000); - scanner_dump_state(); - - if (sensorboard.dropzone_h == -1 || - strat_scan_get_checkpoint(mode, &ckpt_rel_x, - &ckpt_rel_y, &back_mm)) { - return; - } - } - - if (sensorboard.dropzone_h == -1) - return; - - if (mode == SCAN_MODE_CHECK_TEMPLE) { - ckpt_rel_x = 220; - ckpt_rel_y = 100; - } - - - printf_P(PSTR("rel xy for ckpt is %d,%d\r\n"), ckpt_rel_x, ckpt_rel_y); - - rel_xy_to_abs_xy(ckpt_rel_x, ckpt_rel_y, &ckpt_abs_x, &ckpt_abs_y); - abs_xy_to_rel_da(ckpt_abs_x, ckpt_abs_y, &ckpt_rel_d, &ckpt_rel_a); - - printf_P(PSTR("abs ckpt is %2.2f,%2.2f\r\n"), ckpt_abs_x, ckpt_abs_y); - - printf_P(PSTR("ok ? (y/n)\r\n")); - - c = cmdline_getchar_wait(); - - if (c != 'y') - return; - - strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST); - - /* intermediate checkpoint for some positions */ - if ( (DEG(ckpt_rel_a) < 0 && DEG(ckpt_rel_a) > -90) ) { - trajectory_goto_xy_rel(&mainboard.traj, 200, 100); - err = wait_traj_end(TRAJ_FLAGS_NO_NEAR); - if (!TRAJ_SUCCESS(err)) - return; - } - - trajectory_goto_xy_abs(&mainboard.traj, ckpt_abs_x, ckpt_abs_y); - err = wait_traj_end(TRAJ_FLAGS_NO_NEAR); - if (!TRAJ_SUCCESS(err)) - return; - - trajectory_turnto_xy(&mainboard.traj, center_abs_x, center_abs_y); - err = wait_traj_end(TRAJ_FLAGS_NO_NEAR); - if (!TRAJ_SUCCESS(err)) - return; - - c = cmdline_getchar_wait(); - - pos1x = position_get_x_s16(&mainboard.pos); - pos1y = position_get_y_s16(&mainboard.pos); - - strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW); - trajectory_d_rel(&mainboard.traj, 200); - err = WAIT_COND_OR_TRAJ_END(distance_from_robot(pos1x, pos1y) > 200, - TRAJ_FLAGS_SMALL_DIST); - if (err == 0) { - strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_VERY_SLOW); - trajectory_d_rel(&mainboard.traj, 400); - err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); - } - if (err != END_BLOCKING) - return; - - if (back_mm) { - trajectory_d_rel(&mainboard.traj, -back_mm); - wait_traj_end(TRAJ_FLAGS_SMALL_DIST); - } + struct cmd_clitoid_result *res = parsed_result; +/* clitoid(res->alpha_deg, res->beta_deg, res->R_mm, */ +/* res->Vd, res->Amax, res->d_inter_mm); */ + double x = position_get_x_double(&mainboard.pos); + double y = position_get_y_double(&mainboard.pos); + double a = position_get_a_rad_double(&mainboard.pos); + + strat_set_speed(res->Vd, SPEED_ANGLE_FAST); + trajectory_clitoid(&mainboard.traj, x, y, a, 150., + res->alpha_deg, res->beta_deg, res->R_mm, + res->d_inter_mm); } -prog_char str_scan_test_arg0[] = "scan_test"; -parse_pgm_token_string_t cmd_scan_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_scan_test_result, arg0, str_scan_test_arg0); -prog_char str_scan_test_arg1[] = "traj_only#scan_col#scan_temple"; -parse_pgm_token_string_t cmd_scan_test_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_scan_test_result, arg1, str_scan_test_arg1); -parse_pgm_token_num_t cmd_scan_test_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_scan_test_result, start_dist, INT16); -parse_pgm_token_num_t cmd_scan_test_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_scan_test_result, scan_dist, INT16); -parse_pgm_token_num_t cmd_scan_test_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_scan_test_result, scan_speed, INT16); -parse_pgm_token_num_t cmd_scan_test_arg5 = TOKEN_NUM_INITIALIZER(struct cmd_scan_test_result, center_x, INT16); -parse_pgm_token_num_t cmd_scan_test_arg6 = TOKEN_NUM_INITIALIZER(struct cmd_scan_test_result, center_y, INT16); - -prog_char help_scan_test[] = "Scan_Test function (start_dist, scan_dist, speed_dist, centerx, centery)"; -parse_pgm_inst_t cmd_scan_test = { - .f = cmd_scan_test_parsed, /* function to call */ - .data = NULL, /* 2nd arg of func */ - .help_str = help_scan_test, - .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_scan_test_arg0, - (prog_void *)&cmd_scan_test_arg1, - (prog_void *)&cmd_scan_test_arg2, - (prog_void *)&cmd_scan_test_arg3, - (prog_void *)&cmd_scan_test_arg4, - (prog_void *)&cmd_scan_test_arg5, - (prog_void *)&cmd_scan_test_arg6, - NULL, - }, -}; - -prog_char str_scan_test_arg1b[] = "check_temple"; -parse_pgm_token_string_t cmd_scan_test_arg1b = TOKEN_STRING_INITIALIZER(struct cmd_scan_test_result, arg1, str_scan_test_arg1b); -parse_pgm_token_num_t cmd_scan_test_arg7 = TOKEN_NUM_INITIALIZER(struct cmd_scan_test_result, level, UINT8); - -prog_char help_scan_test2[] = "Scan_Test function (start_dist, scan_dist, speed_dist, templex, templey, level)"; -parse_pgm_inst_t cmd_scan_test2 = { - .f = cmd_scan_test_parsed, /* function to call */ +prog_char str_clitoid_arg0[] = "clitoid"; +parse_pgm_token_string_t cmd_clitoid_arg0 = + TOKEN_STRING_INITIALIZER(struct cmd_clitoid_result, + arg0, str_clitoid_arg0); +parse_pgm_token_num_t cmd_clitoid_alpha_deg = + TOKEN_NUM_INITIALIZER(struct cmd_clitoid_result, + alpha_deg, FLOAT); +parse_pgm_token_num_t cmd_clitoid_beta_deg = + TOKEN_NUM_INITIALIZER(struct cmd_clitoid_result, + beta_deg, FLOAT); +parse_pgm_token_num_t cmd_clitoid_R_mm = + TOKEN_NUM_INITIALIZER(struct cmd_clitoid_result, + R_mm, FLOAT); +parse_pgm_token_num_t cmd_clitoid_Vd = + TOKEN_NUM_INITIALIZER(struct cmd_clitoid_result, + Vd, FLOAT); +parse_pgm_token_num_t cmd_clitoid_Amax = + TOKEN_NUM_INITIALIZER(struct cmd_clitoid_result, + Amax, FLOAT); +parse_pgm_token_num_t cmd_clitoid_d_inter_mm = + TOKEN_NUM_INITIALIZER(struct cmd_clitoid_result, + d_inter_mm, FLOAT); + +prog_char help_clitoid[] = "do a clitoid (alpha, beta, R, Vd, Amax, d_inter)"; +parse_pgm_inst_t cmd_clitoid = { + .f = cmd_clitoid_parsed, /* function to call */ .data = NULL, /* 2nd arg of func */ - .help_str = help_scan_test, + .help_str = help_clitoid, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_scan_test_arg0, - (prog_void *)&cmd_scan_test_arg1b, - (prog_void *)&cmd_scan_test_arg2, - (prog_void *)&cmd_scan_test_arg3, - (prog_void *)&cmd_scan_test_arg4, - (prog_void *)&cmd_scan_test_arg5, - (prog_void *)&cmd_scan_test_arg6, - (prog_void *)&cmd_scan_test_arg7, + (prog_void *)&cmd_clitoid_arg0, + (prog_void *)&cmd_clitoid_alpha_deg, + (prog_void *)&cmd_clitoid_beta_deg, + (prog_void *)&cmd_clitoid_R_mm, + (prog_void *)&cmd_clitoid_Vd, + (prog_void *)&cmd_clitoid_Amax, + (prog_void *)&cmd_clitoid_d_inter_mm, NULL, }, }; @@ -1922,6 +1145,7 @@ struct cmd_time_monitor_result { /* function called when cmd_time_monitor is parsed successfully */ static void cmd_time_monitor_parsed(void *parsed_result, void *data) { +#ifndef HOST_VERSION struct cmd_time_monitor_result *res = parsed_result; uint16_t seconds; @@ -1930,6 +1154,7 @@ static void cmd_time_monitor_parsed(void *parsed_result, void *data) } seconds = eeprom_read_word(EEPROM_TIME_ADDRESS); printf_P(PSTR("Running since %d mn %d\r\n"), seconds/60, seconds%60); +#endif } prog_char str_time_monitor_arg0[] = "time_monitor"; @@ -1943,212 +1168,161 @@ parse_pgm_inst_t cmd_time_monitor = { .data = NULL, /* 2nd arg of func */ .help_str = help_time_monitor, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_time_monitor_arg0, - (prog_void *)&cmd_time_monitor_arg1, + (prog_void *)&cmd_time_monitor_arg0, + (prog_void *)&cmd_time_monitor_arg1, NULL, }, }; /**********************************************************/ -/* Scanner */ +/* Strat_Event */ -/* this structure is filled when cmd_scanner is parsed successfully */ -struct cmd_scanner_result { +/* this structure is filled when cmd_strat_event is parsed successfully */ +struct cmd_strat_event_result { fixed_string_t arg0; fixed_string_t arg1; }; -/* function called when cmd_scanner is parsed successfully */ -static void cmd_scanner_parsed(void *parsed_result, void *data) +/* function called when cmd_strat_event is parsed successfully */ +static void cmd_strat_event_parsed(void *parsed_result, void *data) { - struct cmd_scanner_result *res = parsed_result; + struct cmd_strat_event_result *res = parsed_result; - if (!strcmp_P(res->arg1, PSTR("prepare"))) { - i2c_sensorboard_scanner_prepare(); - } - else if (!strcmp_P(res->arg1, PSTR("stop"))) { - i2c_sensorboard_scanner_stop(); - } - else if (!strcmp_P(res->arg1, PSTR("start"))) { - i2c_sensorboard_scanner_start(); - } - else if (!strcmp_P(res->arg1, PSTR("algo_col"))) { - i2c_sensorboard_scanner_algo_column(I2C_SCANNER_ZONE_DISC, - 15, 15); - } - else if (!strcmp_P(res->arg1, PSTR("algo_check"))) { - i2c_sensorboard_scanner_algo_check(2, 15, 15); // XXX - } - else if (!strcmp_P(res->arg1, PSTR("calib"))) { - i2c_sensorboard_scanner_calib(); - } - else if (!strcmp_P(res->arg1, PSTR("show"))) { - scanner_dump_state(); - } + if (!strcmp_P(res->arg1, PSTR("on"))) + strat_event_enable(); + else + strat_event_disable(); } -prog_char str_scanner_arg0[] = "scanner"; -parse_pgm_token_string_t cmd_scanner_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_scanner_result, arg0, str_scanner_arg0); -prog_char str_scanner_arg1[] = "prepare#start#algo_col#algo_check#stop#show#calib"; -parse_pgm_token_string_t cmd_scanner_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_scanner_result, arg1, str_scanner_arg1); +prog_char str_strat_event_arg0[] = "strat_event"; +parse_pgm_token_string_t cmd_strat_event_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_event_result, arg0, str_strat_event_arg0); +prog_char str_strat_event_arg1[] = "on#off"; +parse_pgm_token_string_t cmd_strat_event_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_event_result, arg1, str_strat_event_arg1); -prog_char help_scanner[] = "send commands to scanner"; -parse_pgm_inst_t cmd_scanner = { - .f = cmd_scanner_parsed, /* function to call */ +prog_char help_strat_event[] = "Enable/disable strat_event callback"; +parse_pgm_inst_t cmd_strat_event = { + .f = cmd_strat_event_parsed, /* function to call */ .data = NULL, /* 2nd arg of func */ - .help_str = help_scanner, + .help_str = help_strat_event, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_scanner_arg0, - (prog_void *)&cmd_scanner_arg1, + (prog_void *)&cmd_strat_event_arg0, + (prog_void *)&cmd_strat_event_arg1, NULL, }, }; /**********************************************************/ -/* Build_Z1 */ +/* Test */ -/* this structure is filled when cmd_build_z1 is parsed successfully */ -struct cmd_build_z1_result { +/* this structure is filled when cmd_test is parsed successfully */ +struct cmd_test_result { fixed_string_t arg0; - uint8_t level; - int16_t d1; - int16_t d2; - int16_t d3; + int32_t radius; + int32_t dist; }; -/* function called when cmd_build_z1 is parsed successfully */ -static void cmd_build_z1_parsed(void *parsed_result, void *data) +/* function called when cmd_test is parsed successfully */ +static void cmd_test_parsed(void *parsed_result, void *data) { - struct cmd_build_z1_result *res = parsed_result; - - strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_SLOW); - trajectory_d_rel(&mainboard.traj, 400); - wait_traj_end(TRAJ_FLAGS_SMALL_DIST); - - trajectory_d_rel(&mainboard.traj, -200); - wait_traj_end(TRAJ_FLAGS_SMALL_DIST); - - i2c_mechboard_mode_prepare_pickup_next(I2C_LEFT_SIDE, - I2C_MECHBOARD_MODE_HARVEST); - - while (get_column_count() != 4); - - i2c_mechboard_mode_prepare_build_both(res->level); - time_wait_ms(500); - - trajectory_d_rel(&mainboard.traj, 400); - wait_traj_end(TRAJ_FLAGS_SMALL_DIST); - - strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW); - trajectory_d_rel(&mainboard.traj, -res->d1); - wait_traj_end(TRAJ_FLAGS_SMALL_DIST); - i2c_mechboard_mode_autobuild(res->level, 2, I2C_AUTOBUILD_DEFAULT_DIST, - res->level, 2, I2C_AUTOBUILD_DEFAULT_DIST, - 1); - WAIT_COND_OR_TIMEOUT(get_mechboard_mode() == - I2C_MECHBOARD_MODE_AUTOBUILD, 100); - WAIT_COND_OR_TIMEOUT(get_mechboard_mode() != - I2C_MECHBOARD_MODE_AUTOBUILD, 10000); - - strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW); - trajectory_d_rel(&mainboard.traj, -res->d2); - wait_traj_end(TRAJ_FLAGS_SMALL_DIST); - i2c_mechboard_mode_push_temple(1); - time_wait_ms(400); - strat_set_speed(200, SPEED_ANGLE_SLOW); - trajectory_d_rel(&mainboard.traj, res->d3); - wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + strat_db.dump_enabled = 1; + strat_db_dump(__FUNCTION__); + + corn_set_color(strat_db.corn_table[0], I2C_COB_BLACK); + strat_db_dump(__FUNCTION__); + + corn_set_color(strat_db.corn_table[3], I2C_COB_WHITE); + strat_db_dump(__FUNCTION__); + corn_set_color(strat_db.corn_table[4], I2C_COB_WHITE); + strat_db_dump(__FUNCTION__); + corn_set_color(strat_db.corn_table[5], I2C_COB_WHITE); + strat_db_dump(__FUNCTION__); } -prog_char str_build_z1_arg0[] = "build_z1"; -parse_pgm_token_string_t cmd_build_z1_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_build_z1_result, arg0, str_build_z1_arg0); -parse_pgm_token_num_t cmd_build_z1_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_build_z1_result, level, UINT8); -parse_pgm_token_num_t cmd_build_z1_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_build_z1_result, d1, INT16); -parse_pgm_token_num_t cmd_build_z1_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_build_z1_result, d2, INT16); -parse_pgm_token_num_t cmd_build_z1_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_build_z1_result, d3, INT16); +prog_char str_test_arg0[] = "test"; +parse_pgm_token_string_t cmd_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_test_result, arg0, str_test_arg0); +parse_pgm_token_num_t cmd_test_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_test_result, radius, INT32); +parse_pgm_token_num_t cmd_test_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_test_result, dist, INT32); -prog_char help_build_z1[] = "Build_Z1 function (level, d1, d2, d3)"; -parse_pgm_inst_t cmd_build_z1 = { - .f = cmd_build_z1_parsed, /* function to call */ +prog_char help_test[] = "Test function"; +parse_pgm_inst_t cmd_test = { + .f = cmd_test_parsed, /* function to call */ .data = NULL, /* 2nd arg of func */ - .help_str = help_build_z1, + .help_str = help_test, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_build_z1_arg0, - (prog_void *)&cmd_build_z1_arg1, - (prog_void *)&cmd_build_z1_arg2, - (prog_void *)&cmd_build_z1_arg3, - (prog_void *)&cmd_build_z1_arg4, + (prog_void *)&cmd_test_arg0, NULL, }, }; -#ifdef TEST_BEACON + + + /**********************************************************/ -/* Beacon_Opp_Dump */ +/* Climb */ -/* this structure is filled when cmd_beacon_opp_dump is parsed successfully */ -struct cmd_beacon_opp_dump_result { +/* this structure is filled when cmd_climb is parsed successfully */ +struct cmd_climb_result { fixed_string_t arg0; + int32_t radius; + int32_t dist; }; -void beacon_dump_samples(void); -/* function called when cmd_beacon_opp_dump is parsed successfully */ -static void cmd_beacon_opp_dump_parsed(void *parsed_result, void *data) +//#define DIM_BALLS_DOWN 620 +//#define DIM_BALLS_UP 700 + +#define DIM_BALLS_DOWN 530 +#define DIM_BALLS_UP 700 + +/* function called when cmd_climb is parsed successfully */ +static void cmd_climb_parsed(void *parsed_result, void *data) { - beacon_dump_samples(); -} + uint8_t err; -prog_char str_beacon_opp_dump_arg0[] = "beacon_opp_dump"; -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); + printf_P(PSTR("starting Clifenger\r\n")); + strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW ); -prog_char help_beacon_opp_dump[] = "Dump beacon samples"; -parse_pgm_inst_t cmd_beacon_opp_dump = { - .f = cmd_beacon_opp_dump_parsed, /* function to call */ - .data = NULL, /* 2nd arg of func */ - .help_str = help_beacon_opp_dump, - .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_beacon_opp_dump_arg0, - NULL, - }, -}; -#endif + trajectory_goto_xy_abs(&mainboard.traj, 250, 250); + err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + trajectory_a_abs(&mainboard.traj, 0); + err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); -/**********************************************************/ -/* Test */ -/* this structure is filled when cmd_test is parsed successfully */ -struct cmd_test_result { - fixed_string_t arg0; - int32_t radius; -}; -void circle_get_da_speed_from_radius(struct trajectory *traj, - double radius_mm, - double *speed_d, - double *speed_a); -/* function called when cmd_test is parsed successfully */ -static void cmd_test_parsed(void *parsed_result, void *data) -{ - struct cmd_test_result *res = parsed_result; - double d,a; - strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW); - circle_get_da_speed_from_radius(&mainboard.traj, res->radius, &d, &a); - printf_P(PSTR("d=%2.2f a=%2.2f\r\n"), d, a); + + trajectory_goto_xy_abs(&mainboard.traj, 1250, 250); + err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos)>DIM_BALLS_DOWN, 0xFF); + printf_P(PSTR("ball_pack\r\n")); + support_balls_pack(); + err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos)>DIM_BALLS_UP, 0xFF); + printf_P(PSTR("ball_deploy\r\n")); + support_balls_deploy(); + err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + + /* reach top, go down */ + trajectory_goto_xy_abs(&mainboard.traj, 250, 250); + err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos)