X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fmainboard%2Fcommands_traj.c;h=4da3b47259c4232603ec5c59334cf4b348cbf8f8;hp=d63f57ad6b7866e25aa57bac4ed9c2a5e2193ccf;hb=b6b61358268cccf0fe5cd3f808ceab6b8402b74c;hpb=4e7801883ed4076cb14b63a0571467747894c0f8 diff --git a/projects/microb2010/mainboard/commands_traj.c b/projects/microb2010/mainboard/commands_traj.c index d63f57a..4da3b47 100644 --- 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 */ @@ -442,6 +443,63 @@ parse_pgm_inst_t cmd_track_show = { }, }; +/**********************************************************/ +/* 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, + }, +}; + /**********************************************************/ @@ -766,6 +824,7 @@ 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); err = strat_calib(300, END_INTR|END_TRAJ|END_BLOCKING); if (err == END_INTR) @@ -780,11 +839,13 @@ static void auto_position(void) if (err == END_INTR) goto intr; + 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; + time_wait_ms(250); err = strat_calib(300, END_INTR|END_TRAJ|END_BLOCKING); if (err == END_INTR) goto intr; @@ -797,13 +858,13 @@ static void auto_position(void) err = wait_traj_end(END_INTR|END_TRAJ); if (err == END_INTR) goto intr; - wait_ms(100); + 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; @@ -825,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), @@ -851,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"; @@ -982,24 +1056,8 @@ static void cmd_strat_conf2_parsed(void *parsed_result, void *data) 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; if (on) strat_conf.flags |= bit; @@ -1012,7 +1070,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[] = "faux"; +prog_char str_strat_conf2_arg1[] = "our_orange"; 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); @@ -1044,45 +1102,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_conf.scan_opp_min_time = res->arg2; - } - else if (!strcmp_P(res->arg1, PSTR("delay_between_opponent_scan"))) { + if (!strcmp_P(res->arg1, PSTR("orphan_tomato"))) { if (res->arg2 > 90) res->arg2 = 90; - strat_conf.delay_between_opp_scan = res->arg2; + strat_conf.orphan_tomato = res->arg2; } - else if (!strcmp_P(res->arg1, PSTR("scan_our_min_time"))) { + else if (!strcmp_P(res->arg1, PSTR("opp_orange"))) { if (res->arg2 > 90) res->arg2 = 90; - strat_conf.scan_our_min_time = res->arg2; + strat_conf.opp_orange = res->arg2; } - else if (!strcmp_P(res->arg1, PSTR("delay_between_our_scan"))) { - if (res->arg2 > 90) - res->arg2 = 90; - strat_conf.delay_between_our_scan = res->arg2; - } - else if (!strcmp_P(res->arg1, PSTR("wait_opponent"))) { - strat_conf.wait_opponent = res->arg2; - } - else if (!strcmp_P(res->arg1, PSTR("lintel_min_time"))) { - strat_conf.lintel_min_time = res->arg2; - } -#endif 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 = { @@ -1101,221 +1141,10 @@ parse_pgm_inst_t cmd_strat_conf3 = { /**********************************************************/ /* Subtraj */ -////////////////////// - -// 500 -- 5 -// 400 -- 3 -#define TEST_SPEED 400 -#define TEST_ACC 3 - -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; - 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_P(PSTR("%s()\r\n"), __FUNCTION__); - - strat_set_speed(TEST_SPEED, TEST_SPEED); - quadramp_set_2nd_order_vars(&mainboard.angle.qr, TEST_ACC, TEST_ACC); - - 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; - angle = simple_modulo_2pi(angle); - angle = DEG(angle); - printf_P(PSTR("(%d,%d,%d) "), - position_get_x_s16(&mainboard.pos), - position_get_y_s16(&mainboard.pos), - position_get_a_deg_s16(&mainboard.pos)); - printf_P(PSTR("circle distance=%2.2f angle=%2.2f\r\n"), - distance, angle); - - /* take some margin on dist to avoid deceleration */ - trajectory_d_a_rel(&mainboard.traj, distance + 250, angle); - - /* circle exit condition */ - err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj), - TRAJ_FLAGS_NO_NEAR); - - strat_set_speed(500, 500); - printf_P(PSTR("(%d,%d,%d) "), - position_get_x_s16(&mainboard.pos), - position_get_y_s16(&mainboard.pos), - position_get_a_deg_s16(&mainboard.pos)); - printf_P(PSTR("line\r\n")); - trajectory_line_abs(&mainboard.traj, - line2x1, line2y1, - line2x2, line2y2, 150.); -} - -static void halfturn(double line1x1, double line1y1, - double line1x2, double line1y2, - double line2x1, double line2y1, - double line2x2, double line2y2, - double radius, double dist, double dir) -{ - uint8_t err; - double speed_d, speed_a; - double distance, angle; - - printf_P(PSTR("%s()\r\n"), __FUNCTION__); - - strat_set_speed(TEST_SPEED, TEST_SPEED); - quadramp_set_2nd_order_vars(&mainboard.angle.qr, TEST_ACC, TEST_ACC); - - 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 = dir * M_PI/2.; - distance = angle * radius; - if (distance < 0) - distance = -distance; - angle = simple_modulo_2pi(angle); - angle = DEG(angle); - - /* take some margin on dist to avoid deceleration */ - DEBUG(E_USER_STRAT, "circle1 distance=%2.2f angle=%2.2f", - distance, angle); - trajectory_d_a_rel(&mainboard.traj, distance + 500, angle); - - /* circle exit condition */ - err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj), - TRAJ_FLAGS_NO_NEAR); - - DEBUG(E_USER_STRAT, "miniline"); - err = WAIT_COND_OR_TRAJ_END(distance_from_robot(line2x1, line2y1) < - dist, TRAJ_FLAGS_NO_NEAR); - DEBUG(E_USER_STRAT, "circle2"); - /* take some margin on dist to avoid deceleration */ - trajectory_d_a_rel(&mainboard.traj, distance + 500, angle); - - err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj), - TRAJ_FLAGS_NO_NEAR); - - strat_set_speed(500, 500); - DEBUG(E_USER_STRAT, "line"); - trajectory_line_abs(&mainboard.traj, - line2x1, line2y1, - line2x2, line2y2, 150.); -} - /* function called when cmd_test is parsed successfully */ static void subtraj_test(void) { -#ifdef HOST_VERSION - strat_reset_pos(400, 400, 90); - mainboard.angle.on = 1; - mainboard.distance.on = 1; -#endif - printf_P(PSTR("%s()\r\n"), __FUNCTION__); - while (!cmdline_keypressed()) { - /****** PASS1 */ - -#define DIST_HARD_TURN 260 -#define RADIUS_HARD_TURN 100 -#define DIST_EASY_TURN 190 -#define RADIUS_EASY_TURN 190 -#define DIST_HALF_TURN 225 -#define RADIUS_HALF_TURN 130 - - /* hard turn */ - line2line(375, 597, 375, 1847, - 375, 1847, 1050, 1472, - RADIUS_HARD_TURN, DIST_HARD_TURN); - - /* easy left and easy right !*/ - line2line(825, 1596, 1050, 1472, - 1050, 1472, 1500, 1722, - RADIUS_EASY_TURN, DIST_EASY_TURN); - line2line(1050, 1472, 1500, 1722, - 1500, 1722, 2175, 1347, - RADIUS_EASY_TURN, DIST_EASY_TURN); - line2line(1500, 1722, 2175, 1347, - 2175, 1347, 2175, 847, - RADIUS_EASY_TURN, DIST_EASY_TURN); - - /* half turns */ - halfturn(2175, 1347, 2175, 722, - 2625, 722, 2625, 1597, - RADIUS_HALF_TURN, DIST_HALF_TURN, 1.); - halfturn(2625, 847, 2625, 1722, - 2175, 1722, 2175, 1097, - RADIUS_HALF_TURN, DIST_HALF_TURN, 1.); - - /* easy turns */ - line2line(2175, 1597, 2175, 1097, - 2175, 1097, 1500, 722, - RADIUS_EASY_TURN, DIST_EASY_TURN); - line2line(2175, 1097, 1500, 722, - 1500, 722, 1050, 972, - RADIUS_EASY_TURN, DIST_EASY_TURN); - line2line(1500, 722, 1050, 972, - 1050, 972, 375, 597, - RADIUS_EASY_TURN, DIST_EASY_TURN); - - /* hard turn */ - line2line(1050, 972, 375, 597, - 375, 597, 375, 1097, - RADIUS_HARD_TURN, DIST_HARD_TURN); - - /****** PASS2 */ - - /* easy turn */ - line2line(375, 597, 375, 1097, - 375, 1097, 1050, 1472, - RADIUS_EASY_TURN, DIST_EASY_TURN); - - /* hard turn */ - line2line(375, 1097, 1050, 1472, - 1050, 1472, 375, 1847, - RADIUS_HARD_TURN, DIST_HARD_TURN); - - /* hard turn */ - line2line(1050, 1472, 375, 1847, - 375, 1847, 375, 1347, - RADIUS_HARD_TURN, DIST_HARD_TURN); - - /* easy turn */ - line2line(375, 1847, 375, 1347, - 375, 1347, 1050, 972, - RADIUS_EASY_TURN, DIST_EASY_TURN); - - /* hard turn */ - line2line(375, 1347, 1050, 972, - 1050, 972, 375, 597, - RADIUS_HARD_TURN, DIST_HARD_TURN); - - /* hard turn */ - line2line(1050, 972, 375, 597, - 375, 597, 375, 1847, - RADIUS_HARD_TURN, DIST_HARD_TURN); - - } - trajectory_hardstop(&mainboard.traj); } @@ -1336,13 +1165,22 @@ 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_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(); + } 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);