X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fmainboard%2Fcommands_mainboard.c;h=52b6b92ca20c03f0e17855610885bd33e40b8122;hp=ced8e580001a90a2ecdd33dd1a76f946b6b9f214;hb=17aadc4c8c3e60c2b5e6bbba91c8542849addbd7;hpb=b14123cac428083a50e2d0871514018810a779e5 diff --git a/projects/microb2010/mainboard/commands_mainboard.c b/projects/microb2010/mainboard/commands_mainboard.c index ced8e58..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,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,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, }, }; @@ -289,19 +297,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; } @@ -320,7 +325,6 @@ static void cmd_start_parsed(void *parsed_result, void *data) gen.logs[NB_LOGS] = 0; gen.log_level = old_level; -#endif } prog_char str_start_arg0[] = "start"; @@ -336,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, }, }; @@ -356,7 +360,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 +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)); @@ -482,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; @@ -492,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); @@ -502,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; @@ -539,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, }, }; @@ -586,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, }, }; @@ -607,11 +611,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 +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, }, }; @@ -666,7 +670,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 +709,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,16 +727,12 @@ 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); } prog_char str_cobboard_setmode1_arg0[] = "cobboard"; @@ -746,8 +746,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 +763,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 +773,31 @@ 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("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#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 +808,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 +828,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 +845,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 +870,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 +886,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 +904,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"))) @@ -911,7 +916,6 @@ static void cmd_ballboard_setmode1_parsed(void *parsed_result, void *data) i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_HARVEST); /* other commands */ -#endif } prog_char str_ballboard_setmode1_arg0[] = "ballboard"; @@ -925,8 +929,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,9 +948,6 @@ 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; @@ -963,7 +964,6 @@ static void cmd_ballboard_setmode2_parsed(void * parsed_result, void * data) mode = I2C_BALLBOARD_MODE_TAKE_R_FORK; } i2c_ballboard_set_mode(mode); -#endif } prog_char str_ballboard_setmode2_arg0[] = "ballboard"; @@ -979,9 +979,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 +999,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 +1016,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,8 +1057,157 @@ 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, }, }; @@ -1077,83 +1222,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"; @@ -1171,3 +1254,75 @@ parse_pgm_inst_t cmd_test = { NULL, }, }; + + + + +/**********************************************************/ +/* Climb */ + +/* this structure is filled when cmd_climb is parsed successfully */ +struct cmd_climb_result { + fixed_string_t arg0; + int32_t radius; + int32_t dist; +}; + + +//#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) +{ + uint8_t err; + + printf_P(PSTR("starting Clifenger\r\n")); + strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW ); + + 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); + + + + 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)