X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fmainboard%2Fcommands_traj.c;h=d08b4e5d47392693aaa44bc4cc593bdcc7113c99;hp=0dff9edf9d3c7715876a98a4448b8fbf2d78b545;hb=ad466314d3aff8f661654d4701b9d12fdeb7811f;hpb=b699727efceb661be65fbd22e975565b800c628e diff --git a/projects/microb2010/mainboard/commands_traj.c b/projects/microb2010/mainboard/commands_traj.c index 0dff9ed..d08b4e5 100644 --- a/projects/microb2010/mainboard/commands_traj.c +++ b/projects/microb2010/mainboard/commands_traj.c @@ -830,7 +830,7 @@ static void auto_position(void) goto intr; strat_reset_pos(ROBOT_WIDTH/2 + 100, COLOR_Y(ROBOT_HALF_LENGTH_FRONT), - COLOR_A(-90)); + COLOR_A(-90) + ROBOT_ANGLE_FRONT); strat_hardstop(); trajectory_d_rel(&mainboard.traj, -180); @@ -850,7 +850,7 @@ static void auto_position(void) goto intr; strat_reset_pos(ROBOT_HALF_LENGTH_FRONT, DO_NOT_SET_POS, - 180); + 180 + ROBOT_ANGLE_FRONT); strat_hardstop(); trajectory_d_rel(&mainboard.traj, -170); @@ -885,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); @@ -893,7 +894,8 @@ static void cmd_position_parsed(void * parsed_result, void * data) #endif auto_position(); } - else if (!strcmp_P(res->arg1, PSTR("autoset_yellow"))) { + 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); @@ -902,6 +904,15 @@ 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"), position_get_x_double(&mainboard.pos), @@ -911,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_yellow"; +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"; @@ -1044,6 +1055,10 @@ static void cmd_strat_conf2_parsed(void *parsed_result, void *data) 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_conf.flags |= bit; @@ -1056,7 +1071,7 @@ static void cmd_strat_conf2_parsed(void *parsed_result, void *data) 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[] = "our_orange"; +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); @@ -1108,7 +1123,7 @@ 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[] = "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 = { @@ -1151,8 +1166,10 @@ static void cmd_subtraj_parsed(void *parsed_result, void *data) if (!strcmp_P(res->arg1, PSTR("test"))) subtraj_test(); - else if (!strcmp_P(res->arg1, PSTR("orange"))) - 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)); @@ -1164,7 +1181,7 @@ static void cmd_subtraj_parsed(void *parsed_result, void *data) 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[] = "test#orange#tomato"; +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);