X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fmainboard%2Fcommands_traj.c;h=d08b4e5d47392693aaa44bc4cc593bdcc7113c99;hp=81e2e0447aef1395a7945f4e2d7c0cc777f60e42;hb=HEAD;hpb=ebfaaedd491e61696cc93b353471be15408d23e4 diff --git a/projects/microb2010/mainboard/commands_traj.c b/projects/microb2010/mainboard/commands_traj.c index 81e2e04..d08b4e5 100644 --- a/projects/microb2010/mainboard/commands_traj.c +++ b/projects/microb2010/mainboard/commands_traj.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,7 +17,7 @@ * * Revision : $Id: commands_traj.c,v 1.8 2009-11-08 17:24:33 zer0 Exp $ * - * Olivier MATZ + * Olivier MATZ */ #include @@ -39,6 +39,7 @@ #include #include #include +#include #include #include #include @@ -58,6 +59,7 @@ #include "strat_utils.h" #include "strat_base.h" #include "strat.h" +#include "strat_db.h" #include "../common/i2c_commands.h" #include "i2c_protocol.h" @@ -68,14 +70,14 @@ struct cmd_traj_speed_result { fixed_string_t arg0; fixed_string_t arg1; - uint16_t s; + float s; }; /* function called when cmd_traj_speed is parsed successfully */ static void cmd_traj_speed_parsed(void *parsed_result, void *data) { struct cmd_traj_speed_result * res = parsed_result; - + if (!strcmp_P(res->arg1, PSTR("angle"))) { trajectory_set_speed(&mainboard.traj, mainboard.traj.d_speed, res->s); } @@ -84,7 +86,7 @@ static void cmd_traj_speed_parsed(void *parsed_result, void *data) } /* else it is a "show" */ - printf_P(PSTR("angle %u, distance %u\r\n"), + printf_P(PSTR("angle %2.2f, distance %2.2f\r\n"), mainboard.traj.a_speed, mainboard.traj.d_speed); } @@ -93,7 +95,7 @@ prog_char str_traj_speed_arg0[] = "traj_speed"; parse_pgm_token_string_t cmd_traj_speed_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_traj_speed_result, arg0, str_traj_speed_arg0); prog_char str_traj_speed_arg1[] = "angle#distance"; parse_pgm_token_string_t cmd_traj_speed_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_traj_speed_result, arg1, str_traj_speed_arg1); -parse_pgm_token_num_t cmd_traj_speed_s = TOKEN_NUM_INITIALIZER(struct cmd_traj_speed_result, s, UINT16); +parse_pgm_token_num_t cmd_traj_speed_s = TOKEN_NUM_INITIALIZER(struct cmd_traj_speed_result, s, FLOAT); prog_char help_traj_speed[] = "Set traj_speed values for trajectory manager"; parse_pgm_inst_t cmd_traj_speed = { @@ -101,9 +103,9 @@ parse_pgm_inst_t cmd_traj_speed = { .data = NULL, /* 2nd arg of func */ .help_str = help_traj_speed, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_traj_speed_arg0, - (prog_void *)&cmd_traj_speed_arg1, - (prog_void *)&cmd_traj_speed_s, + (prog_void *)&cmd_traj_speed_arg0, + (prog_void *)&cmd_traj_speed_arg1, + (prog_void *)&cmd_traj_speed_s, NULL, }, }; @@ -119,12 +121,76 @@ parse_pgm_inst_t cmd_traj_speed_show = { .data = NULL, /* 2nd arg of func */ .help_str = help_traj_speed_show, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_traj_speed_arg0, + (prog_void *)&cmd_traj_speed_arg0, (prog_void *)&cmd_traj_speed_show_arg, NULL, }, }; +/**********************************************************/ +/* Traj_Accs for trajectory_manager */ + +/* this structure is filled when cmd_traj_acc is parsed successfully */ +struct cmd_traj_acc_result { + fixed_string_t arg0; + fixed_string_t arg1; + float s; +}; + +/* function called when cmd_traj_acc is parsed successfully */ +static void cmd_traj_acc_parsed(void *parsed_result, void *data) +{ + struct cmd_traj_acc_result * res = parsed_result; + + if (!strcmp_P(res->arg1, PSTR("angle"))) { + trajectory_set_acc(&mainboard.traj, mainboard.traj.d_acc, res->s); + } + else if (!strcmp_P(res->arg1, PSTR("distance"))) { + trajectory_set_acc(&mainboard.traj, res->s, mainboard.traj.a_acc); + } + /* else it is a "show" */ + + printf_P(PSTR("angle %2.2f, distance %2.2f\r\n"), + mainboard.traj.a_acc, + mainboard.traj.d_acc); +} + +prog_char str_traj_acc_arg0[] = "traj_acc"; +parse_pgm_token_string_t cmd_traj_acc_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_traj_acc_result, arg0, str_traj_acc_arg0); +prog_char str_traj_acc_arg1[] = "angle#distance"; +parse_pgm_token_string_t cmd_traj_acc_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_traj_acc_result, arg1, str_traj_acc_arg1); +parse_pgm_token_num_t cmd_traj_acc_s = TOKEN_NUM_INITIALIZER(struct cmd_traj_acc_result, s, FLOAT); + +prog_char help_traj_acc[] = "Set traj_acc values for trajectory manager"; +parse_pgm_inst_t cmd_traj_acc = { + .f = cmd_traj_acc_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_traj_acc, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_traj_acc_arg0, + (prog_void *)&cmd_traj_acc_arg1, + (prog_void *)&cmd_traj_acc_s, + NULL, + }, +}; + +/* show */ + +prog_char str_traj_acc_show_arg[] = "show"; +parse_pgm_token_string_t cmd_traj_acc_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_traj_acc_result, arg1, str_traj_acc_show_arg); + +prog_char help_traj_acc_show[] = "Show traj_acc values for trajectory manager"; +parse_pgm_inst_t cmd_traj_acc_show = { + .f = cmd_traj_acc_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_traj_acc_show, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_traj_acc_arg0, + (prog_void *)&cmd_traj_acc_show_arg, + NULL, + }, +}; + /**********************************************************/ /* circle coef configuration */ @@ -178,7 +244,7 @@ parse_pgm_inst_t cmd_circle_coef_show = { .data = NULL, /* 2nd arg of func */ .help_str = help_circle_coef_show, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_circle_coef_arg0, + (prog_void *)&cmd_circle_coef_arg0, (prog_void *)&cmd_circle_coef_show_arg, NULL, }, @@ -201,7 +267,7 @@ struct cmd_trajectory_result { static void cmd_trajectory_parsed(void * parsed_result, void * data) { struct cmd_trajectory_result * res = parsed_result; - + if (!strcmp_P(res->arg1, PSTR("set"))) { trajectory_set_windows(&mainboard.traj, res->d_win, res->a_win, res->a_start); @@ -225,11 +291,11 @@ parse_pgm_inst_t cmd_trajectory = { .data = NULL, /* 2nd arg of func */ .help_str = help_trajectory, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_trajectory_arg0, - (prog_void *)&cmd_trajectory_arg1, - (prog_void *)&cmd_trajectory_d, - (prog_void *)&cmd_trajectory_a, - (prog_void *)&cmd_trajectory_as, + (prog_void *)&cmd_trajectory_arg0, + (prog_void *)&cmd_trajectory_arg1, + (prog_void *)&cmd_trajectory_d, + (prog_void *)&cmd_trajectory_a, + (prog_void *)&cmd_trajectory_as, NULL, }, }; @@ -245,7 +311,7 @@ parse_pgm_inst_t cmd_trajectory_show = { .data = NULL, /* 2nd arg of func */ .help_str = help_trajectory_show, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_trajectory_arg0, + (prog_void *)&cmd_trajectory_arg0, (prog_void *)&cmd_trajectory_show_arg, NULL, }, @@ -271,9 +337,9 @@ static void cmd_rs_gains_parsed(void * parsed_result, void * data) struct cmd_rs_gains_result * res = parsed_result; if (!strcmp_P(res->arg1, PSTR("set"))) { - rs_set_left_ext_encoder(&mainboard.rs, encoders_spi_get_value, + rs_set_left_ext_encoder(&mainboard.rs, encoders_spi_get_value, LEFT_ENCODER, res->left); // en augmentant on tourne à gauche - rs_set_right_ext_encoder(&mainboard.rs, encoders_spi_get_value, + rs_set_right_ext_encoder(&mainboard.rs, encoders_spi_get_value, RIGHT_ENCODER, res->right); //en augmentant on tourne à droite } printf_P(PSTR("rs_gains set %2.2f %2.2f\r\n"), @@ -294,10 +360,10 @@ parse_pgm_inst_t cmd_rs_gains = { .data = NULL, /* 2nd arg of func */ .help_str = help_rs_gains, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_rs_gains_arg0, - (prog_void *)&cmd_rs_gains_arg1, - (prog_void *)&cmd_rs_gains_l, - (prog_void *)&cmd_rs_gains_r, + (prog_void *)&cmd_rs_gains_arg0, + (prog_void *)&cmd_rs_gains_arg1, + (prog_void *)&cmd_rs_gains_l, + (prog_void *)&cmd_rs_gains_r, NULL, }, }; @@ -313,7 +379,7 @@ parse_pgm_inst_t cmd_rs_gains_show = { .data = NULL, /* 2nd arg of func */ .help_str = help_rs_gains_show, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_rs_gains_arg0, + (prog_void *)&cmd_rs_gains_arg0, (prog_void *)&cmd_rs_gains_show_arg, NULL, }, @@ -352,9 +418,9 @@ parse_pgm_inst_t cmd_track = { .data = NULL, /* 2nd arg of func */ .help_str = help_track, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_track_arg0, - (prog_void *)&cmd_track_arg1, - (prog_void *)&cmd_track_val, + (prog_void *)&cmd_track_arg0, + (prog_void *)&cmd_track_arg1, + (prog_void *)&cmd_track_val, NULL, }, }; @@ -370,12 +436,69 @@ parse_pgm_inst_t cmd_track_show = { .data = NULL, /* 2nd arg of func */ .help_str = help_track_show, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_track_arg0, + (prog_void *)&cmd_track_arg0, (prog_void *)&cmd_track_show_arg, NULL, }, }; +/**********************************************************/ +/* centrifugal configuration */ + +/* this structure is filled when cmd_centrifugal is parsed successfully */ +struct cmd_centrifugal_result { + fixed_string_t arg0; + fixed_string_t arg1; + float val; +}; + +/* function called when cmd_centrifugal is parsed successfully */ +static void cmd_centrifugal_parsed(void * parsed_result, void * data) +{ + struct cmd_centrifugal_result * res = parsed_result; + + if (!strcmp_P(res->arg1, PSTR("set"))) { + position_set_centrifugal_coef(&mainboard.pos, res->val); + } + printf_P(PSTR("centrifugal set %f\r\n"), mainboard.pos.centrifugal_coef); +} + +prog_char str_centrifugal_arg0[] = "centrifugal"; +parse_pgm_token_string_t cmd_centrifugal_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_centrifugal_result, arg0, str_centrifugal_arg0); +prog_char str_centrifugal_arg1[] = "set"; +parse_pgm_token_string_t cmd_centrifugal_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_centrifugal_result, arg1, str_centrifugal_arg1); +parse_pgm_token_num_t cmd_centrifugal_val = TOKEN_NUM_INITIALIZER(struct cmd_centrifugal_result, val, FLOAT); + +prog_char help_centrifugal[] = "Set centrifugal coef"; +parse_pgm_inst_t cmd_centrifugal = { + .f = cmd_centrifugal_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_centrifugal, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_centrifugal_arg0, + (prog_void *)&cmd_centrifugal_arg1, + (prog_void *)&cmd_centrifugal_val, + NULL, + }, +}; + +/* show */ + +prog_char str_centrifugal_show_arg[] = "show"; +parse_pgm_token_string_t cmd_centrifugal_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_centrifugal_result, arg1, str_centrifugal_show_arg); + +prog_char help_centrifugal_show[] = "Show centrifugal"; +parse_pgm_inst_t cmd_centrifugal_show = { + .f = cmd_centrifugal_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_centrifugal_show, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_centrifugal_arg0, + (prog_void *)&cmd_centrifugal_show_arg, + NULL, + }, +}; + /**********************************************************/ @@ -399,7 +522,7 @@ static void cmd_pt_list_parsed(void * parsed_result, void * data) { struct cmd_pt_list_result * res = parsed_result; uint8_t i, why=0; - + if (!strcmp_P(res->arg1, PSTR("append"))) { res->arg2 = pt_list_len; } @@ -418,7 +541,7 @@ static void cmd_pt_list_parsed(void * parsed_result, void * data) printf_P(PSTR("List is too large\r\n")); return; } - memmove(&pt_list[res->arg2+1], &pt_list[res->arg2], + memmove(&pt_list[res->arg2+1], &pt_list[res->arg2], PT_LIST_SIZE-1-res->arg2); pt_list[res->arg2].x = res->arg3; pt_list[res->arg2].y = res->arg4; @@ -433,14 +556,14 @@ static void cmd_pt_list_parsed(void * parsed_result, void * data) printf_P(PSTR("Index too large\r\n")); return; } - memmove(&pt_list[res->arg2], &pt_list[res->arg2+1], + memmove(&pt_list[res->arg2], &pt_list[res->arg2+1], (PT_LIST_SIZE-1-res->arg2)*sizeof(struct xy_point)); pt_list_len--; } else if (!strcmp_P(res->arg1, PSTR("reset"))) { pt_list_len = 0; } - + /* else it is a "show" or a "start" */ if (pt_list_len == 0) { printf_P(PSTR("List empty\r\n")); @@ -492,11 +615,11 @@ parse_pgm_inst_t cmd_pt_list = { .data = NULL, /* 2nd arg of func */ .help_str = help_pt_list, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_pt_list_arg0, - (prog_void *)&cmd_pt_list_arg1, - (prog_void *)&cmd_pt_list_arg2, - (prog_void *)&cmd_pt_list_arg3, - (prog_void *)&cmd_pt_list_arg4, + (prog_void *)&cmd_pt_list_arg0, + (prog_void *)&cmd_pt_list_arg1, + (prog_void *)&cmd_pt_list_arg2, + (prog_void *)&cmd_pt_list_arg3, + (prog_void *)&cmd_pt_list_arg4, NULL, }, }; @@ -512,10 +635,10 @@ parse_pgm_inst_t cmd_pt_list_append = { .data = NULL, /* 2nd arg of func */ .help_str = help_pt_list_append, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_pt_list_arg0, - (prog_void *)&cmd_pt_list_arg1_append, - (prog_void *)&cmd_pt_list_arg3, - (prog_void *)&cmd_pt_list_arg4, + (prog_void *)&cmd_pt_list_arg0, + (prog_void *)&cmd_pt_list_arg1_append, + (prog_void *)&cmd_pt_list_arg3, + (prog_void *)&cmd_pt_list_arg4, NULL, }, }; @@ -531,8 +654,8 @@ parse_pgm_inst_t cmd_pt_list_del = { .data = NULL, /* 2nd arg of func */ .help_str = help_pt_list_del, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_pt_list_arg0, - (prog_void *)&cmd_pt_list_del_arg, + (prog_void *)&cmd_pt_list_arg0, + (prog_void *)&cmd_pt_list_del_arg, (prog_void *)&cmd_pt_list_arg2, NULL, }, @@ -548,7 +671,7 @@ parse_pgm_inst_t cmd_pt_list_show = { .data = NULL, /* 2nd arg of func */ .help_str = help_pt_list_show, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_pt_list_arg0, + (prog_void *)&cmd_pt_list_arg0, (prog_void *)&cmd_pt_list_show_arg, NULL, }, @@ -653,9 +776,9 @@ parse_pgm_inst_t cmd_goto1 = { .data = NULL, /* 2nd arg of func */ .help_str = help_goto1, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_goto_arg0, - (prog_void *)&cmd_goto_arg1_a, - (prog_void *)&cmd_goto_arg2, + (prog_void *)&cmd_goto_arg0, + (prog_void *)&cmd_goto_arg1_a, + (prog_void *)&cmd_goto_arg2, NULL, }, }; @@ -671,10 +794,10 @@ parse_pgm_inst_t cmd_goto2 = { .data = NULL, /* 2nd arg of func */ .help_str = help_goto2, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_goto_arg0, - (prog_void *)&cmd_goto_arg1_b, + (prog_void *)&cmd_goto_arg0, + (prog_void *)&cmd_goto_arg1_b, (prog_void *)&cmd_goto_arg2, - (prog_void *)&cmd_goto_arg3, + (prog_void *)&cmd_goto_arg3, NULL, }, }; @@ -700,44 +823,48 @@ static void auto_position(void) interrupt_traj_reset(); strat_get_speed(&old_spdd, &old_spda); strat_set_speed(AUTOPOS_SPEED_FAST, AUTOPOS_SPEED_FAST); + strat_set_acc(3, 3); - trajectory_d_rel(&mainboard.traj, -300); - err = wait_traj_end(END_INTR|END_TRAJ|END_BLOCKING); + err = strat_calib(300, END_INTR|END_TRAJ|END_BLOCKING); if (err == END_INTR) goto intr; - wait_ms(100); - strat_reset_pos(ROBOT_LENGTH/2, 0, 0); + strat_reset_pos(ROBOT_WIDTH/2 + 100, + COLOR_Y(ROBOT_HALF_LENGTH_FRONT), + COLOR_A(-90) + ROBOT_ANGLE_FRONT); + strat_hardstop(); - trajectory_d_rel(&mainboard.traj, 120); + trajectory_d_rel(&mainboard.traj, -180); err = wait_traj_end(END_INTR|END_TRAJ); if (err == END_INTR) goto intr; - trajectory_a_rel(&mainboard.traj, COLOR_A(90)); + time_wait_ms(250); + trajectory_a_rel(&mainboard.traj, COLOR_A(-90)); err = wait_traj_end(END_INTR|END_TRAJ); if (err == END_INTR) goto intr; - trajectory_d_rel(&mainboard.traj, -300); - err = wait_traj_end(END_INTR|END_TRAJ|END_BLOCKING); + time_wait_ms(250); + err = strat_calib(300, END_INTR|END_TRAJ|END_BLOCKING); if (err == END_INTR) goto intr; - wait_ms(100); - strat_reset_pos(DO_NOT_SET_POS, COLOR_Y(ROBOT_LENGTH/2), - COLOR_A(90)); + strat_reset_pos(ROBOT_HALF_LENGTH_FRONT, + DO_NOT_SET_POS, + 180 + ROBOT_ANGLE_FRONT); + strat_hardstop(); - trajectory_d_rel(&mainboard.traj, 120); + trajectory_d_rel(&mainboard.traj, -170); err = wait_traj_end(END_INTR|END_TRAJ); if (err == END_INTR) goto intr; - wait_ms(100); - - trajectory_a_rel(&mainboard.traj, COLOR_A(-40)); + time_wait_ms(250); + + trajectory_a_rel(&mainboard.traj, COLOR_A(-110)); err = wait_traj_end(END_INTR|END_TRAJ); if (err == END_INTR) goto intr; - wait_ms(100); - + time_wait_ms(250); + strat_set_speed(old_spdd, old_spda); return; @@ -750,7 +877,7 @@ intr: static void cmd_position_parsed(void * parsed_result, void * data) { struct cmd_position_result * res = parsed_result; - + /* display raw position values */ if (!strcmp_P(res->arg1, PSTR("reset"))) { position_set(&mainboard.pos, 0, 0, 0); @@ -758,7 +885,8 @@ static void cmd_position_parsed(void * parsed_result, void * data) else if (!strcmp_P(res->arg1, PSTR("set"))) { position_set(&mainboard.pos, res->arg2, res->arg3, res->arg4); } - else if (!strcmp_P(res->arg1, PSTR("autoset_blue"))) { + else if (!strcmp_P(res->arg1, PSTR("autoset_blue")) || + !strcmp_P(res->arg1, PSTR("autoset_or_blue"))) { mainboard.our_color = I2C_COLOR_BLUE; #ifndef HOST_VERSION i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_BLUE); @@ -766,7 +894,8 @@ static void cmd_position_parsed(void * parsed_result, void * data) #endif auto_position(); } - else if (!strcmp_P(res->arg1, PSTR("autoset_red"))) { + else if (!strcmp_P(res->arg1, PSTR("autoset_yellow")) || + !strcmp_P(res->arg1, PSTR("autoset_or_yellow"))) { mainboard.our_color = I2C_COLOR_YELLOW; #ifndef HOST_VERSION i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_YELLOW); @@ -775,8 +904,17 @@ static void cmd_position_parsed(void * parsed_result, void * data) auto_position(); } + if (!strcmp_P(res->arg1, PSTR("autoset_or_blue"))) { + strat_conf.flags |= STRAT_CONF_OUR_ORANGE; + prepare_hill(I2C_COLOR_BLUE, 340); + } + else if (!strcmp_P(res->arg1, PSTR("autoset_or_yellow"))) { + strat_conf.flags |= STRAT_CONF_OUR_ORANGE; + prepare_hill(I2C_COLOR_YELLOW, 340); + } + /* else it's just a "show" */ - printf_P(PSTR("x=%.2f y=%.2f a=%.2f\r\n"), + printf_P(PSTR("x=%.2f y=%.2f a=%.2f\r\n"), position_get_x_double(&mainboard.pos), position_get_y_double(&mainboard.pos), DEG(position_get_a_rad_double(&mainboard.pos))); @@ -784,7 +922,7 @@ static void cmd_position_parsed(void * parsed_result, void * data) prog_char str_position_arg0[] = "position"; parse_pgm_token_string_t cmd_position_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg0, str_position_arg0); -prog_char str_position_arg1[] = "show#reset#autoset_blue#autoset_red"; +prog_char str_position_arg1[] = "show#reset#autoset_blue#autoset_yellow#autoset_or_blue#autoset_or_yellow"; parse_pgm_token_string_t cmd_position_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg1, str_position_arg1); prog_char help_position[] = "Show/reset (x,y,a) position"; @@ -793,8 +931,8 @@ parse_pgm_inst_t cmd_position = { .data = NULL, /* 2nd arg of func */ .help_str = help_position, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_position_arg0, - (prog_void *)&cmd_position_arg1, + (prog_void *)&cmd_position_arg0, + (prog_void *)&cmd_position_arg1, NULL, }, }; @@ -812,11 +950,11 @@ parse_pgm_inst_t cmd_position_set = { .data = NULL, /* 2nd arg of func */ .help_str = help_position_set, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_position_arg0, - (prog_void *)&cmd_position_arg1_set, - (prog_void *)&cmd_position_arg2, - (prog_void *)&cmd_position_arg3, - (prog_void *)&cmd_position_arg4, + (prog_void *)&cmd_position_arg0, + (prog_void *)&cmd_position_arg1_set, + (prog_void *)&cmd_position_arg2, + (prog_void *)&cmd_position_arg3, + (prog_void *)&cmd_position_arg4, NULL, }, }; @@ -825,37 +963,37 @@ parse_pgm_inst_t cmd_position_set = { /**********************************************************/ /* strat configuration */ -/* this structure is filled when cmd_strat_infos is parsed successfully */ -struct cmd_strat_infos_result { +/* this structure is filled when cmd_strat_db is parsed successfully */ +struct cmd_strat_db_result { fixed_string_t arg0; fixed_string_t arg1; }; -/* function called when cmd_strat_infos is parsed successfully */ -static void cmd_strat_infos_parsed(void *parsed_result, void *data) +/* function called when cmd_strat_db is parsed successfully */ +static void cmd_strat_db_parsed(void *parsed_result, void *data) { - struct cmd_strat_infos_result *res = parsed_result; + struct cmd_strat_db_result *res = parsed_result; if (!strcmp_P(res->arg1, PSTR("reset"))) { - strat_reset_infos(); + strat_db_init(); } - strat_infos.dump_enabled = 1; - strat_dump_infos(__FUNCTION__); + strat_db.dump_enabled = 1; + strat_db_dump(__FUNCTION__); } -prog_char str_strat_infos_arg0[] = "strat_infos"; -parse_pgm_token_string_t cmd_strat_infos_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_infos_result, arg0, str_strat_infos_arg0); -prog_char str_strat_infos_arg1[] = "show#reset"; -parse_pgm_token_string_t cmd_strat_infos_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_infos_result, arg1, str_strat_infos_arg1); +prog_char str_strat_db_arg0[] = "strat_db"; +parse_pgm_token_string_t cmd_strat_db_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_db_result, arg0, str_strat_db_arg0); +prog_char str_strat_db_arg1[] = "show#reset"; +parse_pgm_token_string_t cmd_strat_db_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_db_result, arg1, str_strat_db_arg1); -prog_char help_strat_infos[] = "reset/show strat_infos"; -parse_pgm_inst_t cmd_strat_infos = { - .f = cmd_strat_infos_parsed, /* function to call */ +prog_char help_strat_db[] = "reset/show strat_db"; +parse_pgm_inst_t cmd_strat_db = { + .f = cmd_strat_db_parsed, /* function to call */ .data = NULL, /* 2nd arg of func */ - .help_str = help_strat_infos, + .help_str = help_strat_db, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_strat_infos_arg0, - (prog_void *)&cmd_strat_infos_arg1, + (prog_void *)&cmd_strat_db_arg0, + (prog_void *)&cmd_strat_db_arg1, NULL, }, }; @@ -873,9 +1011,8 @@ struct cmd_strat_conf_result { static void cmd_strat_conf_parsed(void *parsed_result, void *data) { // struct cmd_strat_conf_result *res = parsed_result; - - strat_infos.dump_enabled = 1; - strat_dump_conf(); + strat_conf.dump_enabled = 1; + strat_conf_dump(__FUNCTION__); } prog_char str_strat_conf_arg0[] = "strat_conf"; @@ -889,8 +1026,8 @@ parse_pgm_inst_t cmd_strat_conf = { .data = NULL, /* 2nd arg of func */ .help_str = help_strat_conf, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_strat_conf_arg0, - (prog_void *)&cmd_strat_conf_arg1, + (prog_void *)&cmd_strat_conf_arg0, + (prog_void *)&cmd_strat_conf_arg1, NULL, }, }; @@ -915,38 +1052,26 @@ static void cmd_strat_conf2_parsed(void *parsed_result, void *data) on = 1; else on = 0; - -#if 0 - if (!strcmp_P(res->arg1, PSTR("one_temple_on_disc"))) - bit = STRAT_CONF_ONLY_ONE_ON_DISC; - else if (!strcmp_P(res->arg1, PSTR("bypass_static2"))) - bit = STRAT_CONF_BYPASS_STATIC2; - else if (!strcmp_P(res->arg1, PSTR("take_one_lintel"))) - bit = STRAT_CONF_TAKE_ONE_LINTEL; - else if (!strcmp_P(res->arg1, PSTR("skip_when_check_fail"))) - bit = STRAT_CONF_TAKE_ONE_LINTEL; - else if (!strcmp_P(res->arg1, PSTR("store_static2"))) - bit = STRAT_CONF_STORE_STATIC2; - else if (!strcmp_P(res->arg1, PSTR("big3_temple"))) - bit = STRAT_CONF_BIG_3_TEMPLE; - else if (!strcmp_P(res->arg1, PSTR("early_opp_scan"))) - bit = STRAT_CONF_EARLY_SCAN; - else if (!strcmp_P(res->arg1, PSTR("push_opp_cols"))) - bit = STRAT_CONF_PUSH_OPP_COLS; -#endif + + if (!strcmp_P(res->arg1, PSTR("our_orange"))) + bit = STRAT_CONF_OUR_ORANGE; + else if (!strcmp_P(res->arg1, PSTR("wait_obstacle"))) + bit = STRAT_CONF_WAIT_OBSTACLE; + else if (!strcmp_P(res->arg1, PSTR("straight_begin"))) + bit = STRAT_CONF_STRAIGHT_BEGIN; if (on) - strat_infos.conf.flags |= bit; + strat_conf.flags |= bit; else - strat_infos.conf.flags &= (~bit); + strat_conf.flags &= (~bit); - strat_infos.dump_enabled = 1; - strat_dump_conf(); + strat_conf.dump_enabled = 1; + strat_conf_dump(__FUNCTION__); } prog_char str_strat_conf2_arg0[] = "strat_conf"; parse_pgm_token_string_t cmd_strat_conf2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg0, str_strat_conf2_arg0); -prog_char str_strat_conf2_arg1[] = "faux"; +prog_char str_strat_conf2_arg1[] = "our_orange#wait_obstacle#straight_begin"; parse_pgm_token_string_t cmd_strat_conf2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg1, str_strat_conf2_arg1); prog_char str_strat_conf2_arg2[] = "on#off"; parse_pgm_token_string_t cmd_strat_conf2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg2, str_strat_conf2_arg2); @@ -958,9 +1083,9 @@ parse_pgm_inst_t cmd_strat_conf2 = { .data = NULL, /* 2nd arg of func */ .help_str = help_strat_conf2, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_strat_conf2_arg0, - (prog_void *)&cmd_strat_conf2_arg1, - (prog_void *)&cmd_strat_conf2_arg2, + (prog_void *)&cmd_strat_conf2_arg0, + (prog_void *)&cmd_strat_conf2_arg1, + (prog_void *)&cmd_strat_conf2_arg2, NULL, }, }; @@ -978,45 +1103,27 @@ struct cmd_strat_conf3_result { /* function called when cmd_strat_conf3 is parsed successfully */ static void cmd_strat_conf3_parsed(void *parsed_result, void *data) { -#if 0 struct cmd_strat_conf3_result *res = parsed_result; - if (!strcmp_P(res->arg1, PSTR("scan_opponent_min_time"))) { - if (res->arg2 > 90) - res->arg2 = 90; - strat_infos.conf.scan_opp_min_time = res->arg2; - } - else if (!strcmp_P(res->arg1, PSTR("delay_between_opponent_scan"))) { - if (res->arg2 > 90) - res->arg2 = 90; - strat_infos.conf.delay_between_opp_scan = res->arg2; - } - else if (!strcmp_P(res->arg1, PSTR("scan_our_min_time"))) { + if (!strcmp_P(res->arg1, PSTR("orphan_tomato"))) { if (res->arg2 > 90) res->arg2 = 90; - strat_infos.conf.scan_our_min_time = res->arg2; + strat_conf.orphan_tomato = res->arg2; } - else if (!strcmp_P(res->arg1, PSTR("delay_between_our_scan"))) { + else if (!strcmp_P(res->arg1, PSTR("opp_orange"))) { if (res->arg2 > 90) res->arg2 = 90; - strat_infos.conf.delay_between_our_scan = res->arg2; - } - else if (!strcmp_P(res->arg1, PSTR("wait_opponent"))) { - strat_infos.conf.wait_opponent = res->arg2; + strat_conf.opp_orange = res->arg2; } - else if (!strcmp_P(res->arg1, PSTR("lintel_min_time"))) { - strat_infos.conf.lintel_min_time = res->arg2; - } -#endif - strat_infos.dump_enabled = 1; - strat_dump_conf(); + strat_conf.dump_enabled = 1; + strat_conf_dump(__FUNCTION__); } prog_char str_strat_conf3_arg0[] = "strat_conf"; parse_pgm_token_string_t cmd_strat_conf3_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf3_result, arg0, str_strat_conf3_arg0); -prog_char str_strat_conf3_arg1[] = "faux2"; +prog_char str_strat_conf3_arg1[] = "orphan_tomato#opp_orange"; parse_pgm_token_string_t cmd_strat_conf3_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf3_result, arg1, str_strat_conf3_arg1); -parse_pgm_token_num_t cmd_strat_conf3_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_strat_conf3_result, arg2, UINT16); +parse_pgm_token_num_t cmd_strat_conf3_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_strat_conf3_result, arg2, UINT8); prog_char help_strat_conf3[] = "configure strat options"; parse_pgm_inst_t cmd_strat_conf3 = { @@ -1024,9 +1131,9 @@ parse_pgm_inst_t cmd_strat_conf3 = { .data = NULL, /* 2nd arg of func */ .help_str = help_strat_conf3, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_strat_conf3_arg0, - (prog_void *)&cmd_strat_conf3_arg1, - (prog_void *)&cmd_strat_conf3_arg2, + (prog_void *)&cmd_strat_conf3_arg0, + (prog_void *)&cmd_strat_conf3_arg1, + (prog_void *)&cmd_strat_conf3_arg2, NULL, }, }; @@ -1035,6 +1142,13 @@ parse_pgm_inst_t cmd_strat_conf3 = { /**********************************************************/ /* Subtraj */ + +/* function called when cmd_test is parsed successfully */ +static void subtraj_test(void) +{ +} + + /* this structure is filled when cmd_subtraj is parsed successfully */ struct cmd_subtraj_result { fixed_string_t arg0; @@ -1048,15 +1162,26 @@ struct cmd_subtraj_result { /* function called when cmd_subtraj is parsed successfully */ static void cmd_subtraj_parsed(void *parsed_result, void *data) { -/* struct cmd_subtraj_result *res = parsed_result; */ + struct cmd_subtraj_result *res = parsed_result; + + if (!strcmp_P(res->arg1, PSTR("test"))) + subtraj_test(); + else if (!strcmp_P(res->arg1, PSTR("orange_yellow"))) + run_to_the_hills(I2C_COLOR_YELLOW); + else if (!strcmp_P(res->arg1, PSTR("orange_blue"))) + run_to_the_hills(I2C_COLOR_BLUE); + else if (!strcmp_P(res->arg1, PSTR("tomato"))) { + position_set(&mainboard.pos, 2625, + COLOR_Y(1847), COLOR_A(0.00)); + get_orphan_tomatoes(); + } - printf_P(PSTR("TODO\r\n")); trajectory_hardstop(&mainboard.traj); } prog_char str_subtraj_arg0[] = "subtraj"; parse_pgm_token_string_t cmd_subtraj_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_subtraj_result, arg0, str_subtraj_arg0); -prog_char str_subtraj_arg1[] = "faux"; +prog_char str_subtraj_arg1[] = "test#orange_yellow#orange_blue#tomato"; parse_pgm_token_string_t cmd_subtraj_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_subtraj_result, arg1, str_subtraj_arg1); parse_pgm_token_num_t cmd_subtraj_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg2, INT32); parse_pgm_token_num_t cmd_subtraj_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg3, INT32); @@ -1069,12 +1194,13 @@ parse_pgm_inst_t cmd_subtraj = { .data = NULL, /* 2nd arg of func */ .help_str = help_subtraj, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_subtraj_arg0, - (prog_void *)&cmd_subtraj_arg1, - (prog_void *)&cmd_subtraj_arg2, - (prog_void *)&cmd_subtraj_arg3, - (prog_void *)&cmd_subtraj_arg4, - (prog_void *)&cmd_subtraj_arg5, + (prog_void *)&cmd_subtraj_arg0, + (prog_void *)&cmd_subtraj_arg1, + (prog_void *)&cmd_subtraj_arg2, + (prog_void *)&cmd_subtraj_arg3, + (prog_void *)&cmd_subtraj_arg4, + (prog_void *)&cmd_subtraj_arg5, NULL, }, }; +