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);
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);
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);
#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);
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),
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";
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;
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);
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 = {
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));
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);