X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fmainboard%2Fcommands_traj.c;h=0dff9edf9d3c7715876a98a4448b8fbf2d78b545;hp=be9150b3b2fa6bdfff3bef92a5d325a93332c190;hb=b699727efceb661be65fbd22e975565b800c628e;hpb=78150017ab8c5615af414df706a0525fe7c262ae diff --git a/projects/microb2010/mainboard/commands_traj.c b/projects/microb2010/mainboard/commands_traj.c index be9150b..0dff9ed 100644 --- a/projects/microb2010/mainboard/commands_traj.c +++ b/projects/microb2010/mainboard/commands_traj.c @@ -59,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" @@ -441,6 +442,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, + }, +}; + /**********************************************************/ @@ -765,6 +823,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) @@ -779,11 +838,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; @@ -796,13 +857,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; @@ -891,37 +952,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, }, }; @@ -939,9 +1000,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"; @@ -982,37 +1042,21 @@ 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_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"; 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,43 +1088,25 @@ 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 (!strcmp_P(res->arg1, PSTR("orphan_tomato"))) { if (res->arg2 > 90) res->arg2 = 90; - strat_infos.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_infos.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_infos.conf.delay_between_our_scan = res->arg2; - } - else if (!strcmp_P(res->arg1, PSTR("wait_opponent"))) { - strat_infos.conf.wait_opponent = 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); @@ -1101,221 +1127,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 +1151,20 @@ 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("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#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);