X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fmainboard%2Fcommands_mainboard.c;h=187596522c3d92df3c58cc759b5a49b32af630dd;hp=ced8e580001a90a2ecdd33dd1a76f946b6b9f214;hb=ad466314d3aff8f661654d4701b9d12fdeb7811f;hpb=b14123cac428083a50e2d0871514018810a779e5 diff --git a/projects/microb2010/mainboard/commands_mainboard.c b/projects/microb2010/mainboard/commands_mainboard.c index ced8e58..1875965 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,11 +17,12 @@ * * 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 @@ -40,6 +41,8 @@ #include #include #include +#include +#include #include #include #include @@ -63,6 +66,8 @@ #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" @@ -79,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; } @@ -124,6 +130,8 @@ 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; @@ -139,13 +147,13 @@ static void cmd_event_parsed(void *parsed_result, void *data) } 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); @@ -156,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, }, }; @@ -179,7 +187,7 @@ static void cmd_spi_test_parsed(void * parsed_result, void * data) 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; @@ -209,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, }, }; @@ -249,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, }, }; @@ -267,14 +275,50 @@ 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, }, }; +/**********************************************************/ +/* 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("start"))) + i2c_ballboard_set_beacon(1); + else + i2c_ballboard_set_beacon(0); +} + +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 help_beacon_start[] = "Beacon enabled/disable"; +parse_pgm_inst_t cmd_beacon_start = { + .f = cmd_beacon_start_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_beacon_start, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_beacon_start_arg0, + (prog_void *)&cmd_beacon_start_arg1, + NULL, + }, +}; /**********************************************************/ /* Start */ @@ -289,19 +333,16 @@ struct cmd_start_result { /* function called when cmd_start is parsed successfully */ static void cmd_start_parsed(void *parsed_result, void *data) { -#ifdef HOST_VERSION - printf("not implemented\n"); -#else struct cmd_start_result *res = parsed_result; uint8_t old_level = gen.log_level; 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; } @@ -316,11 +357,15 @@ static void cmd_start_parsed(void *parsed_result, void *data) i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_BLUE); } + printf_P(PSTR("Press a key when beacon ready\r\n")); + i2c_ballboard_set_beacon(0); + while(!cmdline_keypressed()); + i2c_ballboard_set_beacon(1); + strat_start(); gen.logs[NB_LOGS] = 0; gen.log_level = old_level; -#endif } prog_char str_start_arg0[] = "start"; @@ -336,9 +381,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, }, }; @@ -356,7 +401,7 @@ struct cmd_interact_result { static void print_cs(void) { 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"), + "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), @@ -369,7 +414,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)); @@ -482,7 +527,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; @@ -492,7 +537,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); @@ -502,27 +547,32 @@ 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 { +#ifdef HOST_VERSION +#define PWM_INTERACT 1200 +#else +#define PWM_INTERACT 1200 +#endif switch(cmd) { - case KEY_UP_ARR: - pwm_set_and_save(LEFT_PWM, 1200); - pwm_set_and_save(RIGHT_PWM, 1200); + case KEY_UP_ARR: + pwm_set_and_save(LEFT_PWM, PWM_INTERACT); + pwm_set_and_save(RIGHT_PWM, PWM_INTERACT); break; - case KEY_LEFT_ARR: - pwm_set_and_save(LEFT_PWM, -1200); - pwm_set_and_save(RIGHT_PWM, 1200); + case KEY_LEFT_ARR: + pwm_set_and_save(LEFT_PWM, -PWM_INTERACT); + pwm_set_and_save(RIGHT_PWM, PWM_INTERACT); break; - case KEY_DOWN_ARR: - pwm_set_and_save(LEFT_PWM, -1200); - pwm_set_and_save(RIGHT_PWM, -1200); + case KEY_DOWN_ARR: + pwm_set_and_save(LEFT_PWM, -PWM_INTERACT); + pwm_set_and_save(RIGHT_PWM, -PWM_INTERACT); break; case KEY_RIGHT_ARR: - pwm_set_and_save(LEFT_PWM, 1200); - pwm_set_and_save(RIGHT_PWM, -1200); + pwm_set_and_save(LEFT_PWM, PWM_INTERACT); + pwm_set_and_save(RIGHT_PWM, -PWM_INTERACT); break; } } @@ -539,7 +589,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, }, }; @@ -586,8 +636,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, }, }; @@ -607,11 +657,11 @@ static void cmd_rs_parsed(void *parsed_result, void *data) { // struct cmd_rs_result *res = parsed_result; do { - printf_P(PSTR("angle cons=% .6"PRIi32" in=% .6"PRIi32" out=% .6"PRIi32" / "), + 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=% .6"PRIi32" in=% .6"PRIi32" out=% .6"PRIi32" / "), + 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)); @@ -632,8 +682,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, }, }; @@ -666,7 +716,7 @@ 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, }, }; @@ -705,8 +755,8 @@ parse_pgm_inst_t cmd_cobboard_show = { .data = NULL, /* 2nd arg of func */ .help_str = help_cobboard_show, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_cobboard_show_arg0, - (prog_void *)&cmd_cobboard_show_arg1, + (prog_void *)&cmd_cobboard_show_arg0, + (prog_void *)&cmd_cobboard_show_arg1, NULL, }, }; @@ -723,21 +773,21 @@ struct cmd_cobboard_setmode1_result { /* function called when cmd_cobboard_setmode1 is parsed successfully */ static void cmd_cobboard_setmode1_parsed(void *parsed_result, void *data) { -#ifdef HOST_VERSION - printf("not implemented\n"); -#else struct cmd_cobboard_setmode1_result *res = parsed_result; if (!strcmp_P(res->arg1, PSTR("init"))) - i2c_cobboard_mode_init(); + i2c_cobboard_set_mode(I2C_COBBOARD_MODE_INIT); else if (!strcmp_P(res->arg1, PSTR("eject"))) - i2c_cobboard_mode_eject(); -#endif + i2c_cobboard_set_mode(I2C_COBBOARD_MODE_EJECT); + else if (!strcmp_P(res->arg1, PSTR("kickstand_up"))) + i2c_cobboard_set_mode(I2C_COBBOARD_MODE_KICKSTAND_UP); + else if (!strcmp_P(res->arg1, PSTR("kickstand_down"))) + i2c_cobboard_set_mode(I2C_COBBOARD_MODE_KICKSTAND_DOWN); } 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"; +prog_char str_cobboard_setmode1_arg1[] = "init#eject#kickstand_up#kickstand_down"; 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_cobboard_setmode1[] = "set cobboard mode (mode)"; @@ -746,8 +796,8 @@ parse_pgm_inst_t cmd_cobboard_setmode1 = { .data = NULL, /* 2nd arg of func */ .help_str = help_cobboard_setmode1, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_cobboard_setmode1_arg0, - (prog_void *)&cmd_cobboard_setmode1_arg1, + (prog_void *)&cmd_cobboard_setmode1_arg0, + (prog_void *)&cmd_cobboard_setmode1_arg1, NULL, }, }; @@ -763,11 +813,8 @@ struct cmd_cobboard_setmode2_result { }; /* function called when cmd_cobboard_setmode2 is parsed successfully */ -static void cmd_cobboard_setmode2_parsed(void * parsed_result, void * data) +static void cmd_cobboard_setmode2_parsed(void *parsed_result, void *data) { -#ifdef HOST_VERSION - printf("not implemented\n"); -#else struct cmd_cobboard_setmode2_result *res = parsed_result; uint8_t side = I2C_LEFT_SIDE; @@ -776,18 +823,35 @@ static void cmd_cobboard_setmode2_parsed(void * parsed_result, void * data) else if (!strcmp_P(res->arg2, PSTR("right"))) side = I2C_RIGHT_SIDE; - if (!strcmp_P(res->arg1, PSTR("deploy"))) - i2c_cobboard_mode_deploy(side); - else if (!strcmp_P(res->arg1, PSTR("harvest"))) - i2c_cobboard_mode_harvest(side); - else if (!strcmp_P(res->arg1, PSTR("pack"))) - i2c_cobboard_mode_pack(side); -#endif + 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("weak_pack"))) { + i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST); + i2c_cobboard_pack_weak(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_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"; +prog_char str_cobboard_setmode2_arg1[] = "harvest#deploy#pack#weak_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); @@ -798,9 +862,9 @@ parse_pgm_inst_t cmd_cobboard_setmode2 = { .data = NULL, /* 2nd arg of func */ .help_str = help_cobboard_setmode2, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_cobboard_setmode2_arg0, - (prog_void *)&cmd_cobboard_setmode2_arg1, - (prog_void *)&cmd_cobboard_setmode2_arg2, + (prog_void *)&cmd_cobboard_setmode2_arg0, + (prog_void *)&cmd_cobboard_setmode2_arg1, + (prog_void *)&cmd_cobboard_setmode2_arg2, NULL, }, }; @@ -818,13 +882,9 @@ struct cmd_cobboard_setmode3_result { /* function called when cmd_cobboard_setmode3 is parsed successfully */ static void cmd_cobboard_setmode3_parsed(void *parsed_result, void *data) { -#ifdef HOST_VERSION - printf("not implemented\n"); -#else struct cmd_cobboard_setmode3_result *res = parsed_result; if (!strcmp_P(res->arg1, PSTR("xxx"))) printf("faux\r\n"); -#endif } prog_char str_cobboard_setmode3_arg0[] = "cobboard"; @@ -839,9 +899,9 @@ parse_pgm_inst_t cmd_cobboard_setmode3 = { .data = NULL, /* 2nd arg of func */ .help_str = help_cobboard_setmode3, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_cobboard_setmode3_arg0, - (prog_void *)&cmd_cobboard_setmode3_arg1, - (prog_void *)&cmd_cobboard_setmode3_arg2, + (prog_void *)&cmd_cobboard_setmode3_arg0, + (prog_void *)&cmd_cobboard_setmode3_arg1, + (prog_void *)&cmd_cobboard_setmode3_arg2, NULL, }, }; @@ -864,6 +924,8 @@ static void cmd_ballboard_show_parsed(void * parsed_result, void * data) 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 } @@ -878,8 +940,8 @@ parse_pgm_inst_t cmd_ballboard_show = { .data = NULL, /* 2nd arg of func */ .help_str = help_ballboard_show, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_ballboard_show_arg0, - (prog_void *)&cmd_ballboard_show_arg1, + (prog_void *)&cmd_ballboard_show_arg0, + (prog_void *)&cmd_ballboard_show_arg1, NULL, }, }; @@ -896,9 +958,6 @@ struct cmd_ballboard_setmode1_result { /* function called when cmd_ballboard_setmode1 is parsed successfully */ static void cmd_ballboard_setmode1_parsed(void *parsed_result, void *data) { -#ifdef HOST_VERSION - printf("not implemented\n"); -#else struct cmd_ballboard_setmode1_result *res = parsed_result; if (!strcmp_P(res->arg1, PSTR("init"))) @@ -909,14 +968,17 @@ static void cmd_ballboard_setmode1_parsed(void *parsed_result, void *data) i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_EJECT); else if (!strcmp_P(res->arg1, PSTR("harvest"))) i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_HARVEST); + else if (!strcmp_P(res->arg1, PSTR("prepare"))) + i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_PREP_FORK); + else if (!strcmp_P(res->arg1, PSTR("take"))) + i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_TAKE_FORK); /* other commands */ -#endif } 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"; +prog_char str_ballboard_setmode1_arg1[] = "init#eject#harvest#off#take#prepare"; 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_ballboard_setmode1[] = "set ballboard mode (mode)"; @@ -925,8 +987,8 @@ parse_pgm_inst_t cmd_ballboard_setmode1 = { .data = NULL, /* 2nd arg of func */ .help_str = help_ballboard_setmode1, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_ballboard_setmode1_arg0, - (prog_void *)&cmd_ballboard_setmode1_arg1, + (prog_void *)&cmd_ballboard_setmode1_arg0, + (prog_void *)&cmd_ballboard_setmode1_arg1, NULL, }, }; @@ -944,31 +1006,11 @@ struct cmd_ballboard_setmode2_result { /* function called when cmd_ballboard_setmode2 is parsed successfully */ static void cmd_ballboard_setmode2_parsed(void * parsed_result, void * data) { -#ifdef HOST_VERSION - printf("not implemented\n"); -#else - 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; - } - 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; - } - i2c_ballboard_set_mode(mode); -#endif } 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"; +prog_char str_ballboard_setmode2_arg1[] = "xxx"; 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); @@ -979,9 +1021,9 @@ parse_pgm_inst_t cmd_ballboard_setmode2 = { .data = NULL, /* 2nd arg of func */ .help_str = help_ballboard_setmode2, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_ballboard_setmode2_arg0, - (prog_void *)&cmd_ballboard_setmode2_arg1, - (prog_void *)&cmd_ballboard_setmode2_arg2, + (prog_void *)&cmd_ballboard_setmode2_arg0, + (prog_void *)&cmd_ballboard_setmode2_arg1, + (prog_void *)&cmd_ballboard_setmode2_arg2, NULL, }, }; @@ -999,13 +1041,9 @@ struct cmd_ballboard_setmode3_result { /* function called when cmd_ballboard_setmode3 is parsed successfully */ static void cmd_ballboard_setmode3_parsed(void *parsed_result, void *data) { -#ifdef HOST_VERSION - printf("not implemented\n"); -#else struct cmd_ballboard_setmode3_result *res = parsed_result; if (!strcmp_P(res->arg1, PSTR("xxx"))) printf("faux\r\n"); -#endif } prog_char str_ballboard_setmode3_arg0[] = "ballboard"; @@ -1020,9 +1058,9 @@ parse_pgm_inst_t cmd_ballboard_setmode3 = { .data = NULL, /* 2nd arg of func */ .help_str = help_ballboard_setmode3, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_ballboard_setmode3_arg0, - (prog_void *)&cmd_ballboard_setmode3_arg1, - (prog_void *)&cmd_ballboard_setmode3_arg2, + (prog_void *)&cmd_ballboard_setmode3_arg0, + (prog_void *)&cmd_ballboard_setmode3_arg1, + (prog_void *)&cmd_ballboard_setmode3_arg2, NULL, }, }; @@ -1061,12 +1099,194 @@ parse_pgm_inst_t cmd_servo_balls = { .data = NULL, /* 2nd arg of func */ .help_str = help_servo_balls, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_servo_balls_arg0, - (prog_void *)&cmd_servo_balls_arg1, + (prog_void *)&cmd_servo_balls_arg0, + (prog_void *)&cmd_servo_balls_arg1, + NULL, + }, +}; + +/**********************************************************/ +/* Clitoid */ + +/* this structure is filled when cmd_clitoid is parsed successfully */ +struct cmd_clitoid_result { + fixed_string_t arg0; + float alpha_deg; + float beta_deg; + float R_mm; + float Vd; + float Amax; + float d_inter_mm; +}; + +/* function called when cmd_test is parsed successfully */ +static void cmd_clitoid_parsed(void *parsed_result, void *data) +{ + 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_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_clitoid, + .tokens = { /* token list, NULL terminated */ + (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, + }, +}; + +/**********************************************************/ +/* Time_Monitor */ + +/* this structure is filled when cmd_time_monitor is parsed successfully */ +struct cmd_time_monitor_result { + fixed_string_t arg0; + fixed_string_t arg1; +}; + +/* 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; + + if (!strcmp_P(res->arg1, PSTR("reset"))) { + eeprom_write_word(EEPROM_TIME_ADDRESS, 0); + } + 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"; +parse_pgm_token_string_t cmd_time_monitor_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_time_monitor_result, arg0, str_time_monitor_arg0); +prog_char str_time_monitor_arg1[] = "show#reset"; +parse_pgm_token_string_t cmd_time_monitor_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_time_monitor_result, arg1, str_time_monitor_arg1); + +prog_char help_time_monitor[] = "Show since how long we are running"; +parse_pgm_inst_t cmd_time_monitor = { + .f = cmd_time_monitor_parsed, /* function to call */ + .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, + NULL, + }, +}; + + +/**********************************************************/ +/* Strat_Event */ + +/* 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_strat_event is parsed successfully */ +static void cmd_strat_event_parsed(void *parsed_result, void *data) +{ + struct cmd_strat_event_result *res = parsed_result; + + if (!strcmp_P(res->arg1, PSTR("on"))) + strat_event_enable(); + else + strat_event_disable(); +} + +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_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_strat_event, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_strat_event_arg0, + (prog_void *)&cmd_strat_event_arg1, NULL, }, }; +/**********************************************************/ +/* Sleep */ + +/* this structure is filled when cmd_sleep is parsed successfully */ +struct cmd_sleep_result { + fixed_string_t arg0; + uint32_t ms; +}; + +/* function called when cmd_sleep is parsed successfully */ +static void cmd_sleep_parsed(void *parsed_result, void *data) +{ + struct cmd_sleep_result *res = parsed_result; + time_wait_ms(res->ms); +} + +prog_char str_sleep_arg0[] = "sleep"; +parse_pgm_token_string_t cmd_sleep_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_sleep_result, arg0, str_sleep_arg0); +parse_pgm_token_num_t cmd_sleep_ms = TOKEN_NUM_INITIALIZER(struct cmd_sleep_result, ms, UINT32); + +prog_char help_sleep[] = "Sleep during some miliseconds"; +parse_pgm_inst_t cmd_sleep = { + .f = cmd_sleep_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_sleep, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_sleep_arg0, + (prog_void *)&cmd_sleep_ms, + NULL, + }, +}; + + /**********************************************************/ /* Test */ @@ -1077,83 +1297,21 @@ struct cmd_test_result { int32_t dist; }; -/* function called when cmd_test is parsed successfully */ -static void line2line(double line1x1, double line1y1, - double line1x2, double line1y2, - double line2x1, double line2y1, - double line2x2, double line2y2, - double radius, double dist) -{ - uint8_t err; - int32_t dist_imp_target; - double speed_d, speed_a; - double distance, angle; - double line1_angle = atan2(line1y2-line1y1, line1x2-line1x1); - double line2_angle = atan2(line2y2-line2y1, line2x2-line2x1); - - printf("%s()\n", __FUNCTION__); - strat_set_speed(500, 500); - circle_get_da_speed_from_radius(&mainboard.traj, radius, - &speed_d, &speed_a); - trajectory_line_abs(&mainboard.traj, - line1x1, line1y1, - line1x2, line1y2, 150.); - err = WAIT_COND_OR_TRAJ_END(distance_from_robot(line1x2, line1y2) < - dist, TRAJ_FLAGS_NO_NEAR); - /* circle */ - strat_set_speed(speed_d, speed_a); - angle = line2_angle - line1_angle; - distance = angle * radius; - if (distance < 0) - distance = -distance; - dist_imp_target = rs_get_distance(&mainboard.rs) + - distance * mainboard.pos.phys.distance_imp_per_mm; - angle = DEG(angle); - distance += 100; /* take some margin to avoid deceleration */ - trajectory_d_a_rel(&mainboard.traj, distance, angle); - - err = WAIT_COND_OR_TRAJ_END(rs_get_distance(&mainboard.rs) > dist_imp_target, - TRAJ_FLAGS_NO_NEAR); - - strat_set_speed(500, 500); - trajectory_line_abs(&mainboard.traj, - line2x1, line2y1, - line2x2, line2y2, 150.); -} - /* 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; -#ifdef HOST_VERSION - strat_reset_pos(400, 400, 90); - mainboard.angle.on = 1; - mainboard.distance.on = 1; -#endif - line2line(375, 347, 375, 1847, - 375, 1847, 1050, 1472, - 100, 200); - line2line(825, 1596, 1050, 1472, - 1050, 1472, 1500, 1722, - 180, 120); - line2line(1050, 1472, 1500, 1722, - 1500, 1722, 2175, 1347, - 180, 120); - line2line(1500, 1722, 2175, 1347, - 2175, 1347, 2175, 847, - 150, 120); - line2line(2175, 1347, 2175, 847, - 2175, 847, 2400, 722, - 150, 120); - line2line(2175, 847, 2400, 722, - 2400, 722, 2625, 847, - 150, 100); - line2line(2400, 722, 2625, 847, - 2625, 847, 2625, 1847, - 150, 100); - line2line(2625, 847, 2625, 1847, - 2625, 1847, 375, 597, - 100, 200); + 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_test_arg0[] = "test";