another beginning

[aversive.git] / projects / microb2010 / mainboard / commands_traj.c

diff --git a/projects/microb2010/mainboard/commands_traj.c b/projects/microb2010/mainboard/commands_traj.c
index 0dff9ed..0bd04b5 100644 (file)
--- a/projects/microb2010/mainboard/commands_traj.c
+++ b/projects/microb2010/mainboard/commands_traj.c
@@ -62,6 +62,7 @@
 #include "strat_db.h"
 #include "../common/i2c_commands.h"
 #include "i2c_protocol.h"
+#include "beacon.h"
 
 /**********************************************************/
 /* Traj_Speeds for trajectory_manager */
@@ -885,23 +886,36 @@ 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);
                i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_BLUE);
+               beacon_set_color(I2C_COLOR_YELLOW);
 #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);
                i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_YELLOW);
+               beacon_set_color(I2C_COLOR_BLUE);
 #endif
                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 +925,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 +1058,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 +1074,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 +1126,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 +1169,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 +1184,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);