X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fmainboard%2Fstrat.c;h=64562f530c77ebc69a50f448ea6989ed486c1b56;hp=dd7b23c43cc48be4557e6b8c8171cd29f01afc66;hb=f1fab48d45873bc23dccbae5db393afd2942570c;hpb=03e4f34e90f71a08b671193c8a3ac9e587f83cd6 diff --git a/projects/microb2010/mainboard/strat.c b/projects/microb2010/mainboard/strat.c index dd7b23c..64562f5 100644 --- a/projects/microb2010/mainboard/strat.c +++ b/projects/microb2010/mainboard/strat.c @@ -75,10 +75,14 @@ static volatile uint8_t strat_running = 0; volatile uint8_t strat_want_pack = 0; volatile uint8_t strat_lpack60 = 0; volatile uint8_t strat_rpack60 = 0; + +volatile uint8_t strat_opponent_lpack = 0; +volatile uint8_t strat_opponent_rpack = 0; + struct strat_conf strat_conf = { .dump_enabled = 0, .opp_orange = 90, - .orphan_tomato = 50, + .orphan_tomato = 45, .flags = 0, }; @@ -97,6 +101,7 @@ void strat_preinit(void) DO_POS | DO_BD | DO_POWER; strat_db_init(); + strat_conf.prev_wait_obstacle = -5; strat_conf_dump(__FUNCTION__); strat_db_dump(__FUNCTION__); } @@ -107,10 +112,14 @@ void strat_conf_dump(const char *caller) return; printf_P(PSTR("-- conf --\r\n")); - printf_P(PSTR("opp_orange = %d\n"), strat_conf.opp_orange); - printf_P(PSTR("orphan_tomato = %d\n"), strat_conf.orphan_tomato); - printf_P(PSTR("our_orange = %s\n"), + printf_P(PSTR("our_orange = %s\r\n"), (strat_conf.flags & STRAT_CONF_OUR_ORANGE) ? "y":"n"); + printf_P(PSTR("wait_obstacle = %s\r\n"), + (strat_conf.flags & STRAT_CONF_WAIT_OBSTACLE) ? "y":"n"); + printf_P(PSTR("straight begin = %s\r\n"), + (strat_conf.flags & STRAT_CONF_STRAIGHT_BEGIN) ? "y":"n"); + printf_P(PSTR("opp_orange = %d\r\n"), strat_conf.opp_orange); + printf_P(PSTR("orphan_tomato = %d\r\n"), strat_conf.orphan_tomato); } void strat_event_enable(void) @@ -137,6 +146,8 @@ void strat_init(void) time_reset(); interrupt_traj_reset(); + i2c_cobboard_deploy(I2C_LEFT_SIDE); + i2c_cobboard_deploy(I2C_RIGHT_SIDE); i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST); i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_HARVEST); @@ -222,6 +233,7 @@ static void check_corn(void) uint8_t lidx, ridx; static uint8_t prev_check_time; uint8_t cur_time; + uint8_t need_lpack, need_rpack; /* read sensors from ballboard */ IRQ_LOCK(flags); @@ -231,13 +243,12 @@ static void check_corn(void) ballboard.rcob = I2C_COB_NONE; IRQ_UNLOCK(flags); - /* XXX take opponent position into account */ - /* check present cobs once per second */ cur_time = time_get_s(); if (cur_time != prev_check_time) { uint8_t i; + /* only useful for simu */ for (i = 0; i < CORN_NB; i++) { if (strat_db.corn_table[i]->present == 1 && strat_db.corn_table[i]->time_removed != -1 && @@ -270,8 +281,10 @@ static void check_corn(void) } /* control the cobboard mode for left spickle */ + need_lpack = get_cob_count() >= 5 || strat_want_pack || + strat_lpack60 || strat_opponent_lpack; if (lcob_near && strat_db.corn_table[lidx]->present) { - if (get_cob_count() >= 5 || strat_want_pack || strat_lpack60) { + if (need_lpack) { /* nothing */ } else { @@ -287,6 +300,8 @@ static void check_corn(void) #ifdef HOST_VERSION cobboard.cob_count ++; printf("add cob %d\n", lidx); +#else + strat_db.corn_table[lidx]->present = 0; #endif } } @@ -296,15 +311,17 @@ static void check_corn(void) } else { /* no cob near us, we can pack or deploy freely */ - if (get_cob_count() >= 5 || strat_want_pack || strat_lpack60) + if (need_lpack) i2c_cobboard_pack_weak(I2C_LEFT_SIDE); else i2c_cobboard_deploy(I2C_LEFT_SIDE); } /* control the cobboard mode for right spickle */ + need_rpack = get_cob_count() >= 5 || strat_want_pack || + strat_rpack60 || strat_opponent_rpack; if (rcob_near && strat_db.corn_table[ridx]->present) { - if (get_cob_count() >= 5 || strat_want_pack || strat_rpack60) { + if (need_rpack) { /* nothing */ } else { @@ -320,6 +337,8 @@ static void check_corn(void) #ifdef HOST_VERSION cobboard.cob_count ++; printf("add cob %d\n", ridx); +#else + strat_db.corn_table[ridx]->present = 0; #endif } } @@ -329,13 +348,57 @@ static void check_corn(void) } else { /* no cob near us, we can pack or deploy freely */ - if (get_cob_count() >= 5 || strat_want_pack || strat_rpack60) + if (need_rpack) i2c_cobboard_pack_weak(I2C_RIGHT_SIDE); else i2c_cobboard_deploy(I2C_RIGHT_SIDE); } } +/* check opponent position */ +void check_opponent(void) +{ + int16_t opp_x, opp_y; + int16_t opp_d, opp_a; + uint8_t i, j; + + strat_opponent_lpack = 0; + strat_opponent_rpack = 0; + + if (get_opponent_xyda(&opp_x, &opp_y, &opp_d, &opp_a) < 0) + return; + + /* pack spickles if opponent too close */ + if (opp_d < 600) { + if (opp_a > 45 && opp_a < 135) + strat_opponent_lpack = 1; + if (opp_a > 225 && opp_a < 315) + strat_opponent_rpack = 1; + } + + /* check for oranges after 5 seconds */ + if (time_get_s() > 5) { + if (mainboard.our_color == I2C_COLOR_YELLOW) { + if (opp_y < 500 && opp_x < 500) + strat_db.our_oranges_count = 0; + if (opp_y < 500 && opp_x > AREA_X - 500) + strat_db.opp_oranges_count = 0; + } + else { + if (opp_y > AREA_Y - 500 && opp_x < 500) + strat_db.our_oranges_count = 0; + if (opp_y > AREA_Y - 500 && opp_x > AREA_X - 500) + strat_db.opp_oranges_count = 0; + } + } + + /* malus for some tomatoes and cobs, visited by opponent */ + if (xycoord_to_ijcoord(&opp_x, &opp_y, &i, &j) < 0) + return; + + strat_db.wp_table[i][j].opp_visited = 1; +} + /* called periodically (10ms) */ void strat_event(void *dummy) { @@ -343,6 +406,7 @@ void strat_event(void *dummy) if (strat_running == 0) return; + check_opponent(); check_tomato(); check_corn(); @@ -360,7 +424,7 @@ static uint8_t robot_is_on_eject_line(void) x = position_get_x_s16(&mainboard.pos); y = position_get_y_s16(&mainboard.pos); - if (xycoord_to_ijcoord(&x, &y, &i, &j) < 0) + if (xycoord_to_ijcoord_not_corn(&x, &y, &i, &j) < 0) return 0; if (!wp_belongs_to_line(i, j, 5, LINE_UP) && @@ -384,7 +448,7 @@ static uint8_t eject_select_speed(void) return 0; /* fast */ } - if (xycoord_to_ijcoord(&x, &y, &i, &j) < 0) { + if (xycoord_to_ijcoord_not_corn(&x, &y, &i, &j) < 0) { DEBUG(E_USER_STRAT, "%s(): cannot find waypoint at %d,%d", __FUNCTION__, x, y); return 1; /* slow */ @@ -454,28 +518,29 @@ static uint8_t strat_eject(void) #endif } - /* half turn */ - trajectory_a_abs(&mainboard.traj, COLOR_A(-110)); - err = wait_traj_end(END_INTR|END_TRAJ); - if (!TRAJ_SUCCESS(err)) - goto fail; + if (get_cob_count() > 0) { + /* half turn */ + trajectory_a_abs(&mainboard.traj, COLOR_A(-110)); + err = wait_traj_end(END_INTR|END_TRAJ); + if (!TRAJ_SUCCESS(err)) + goto fail; - /* cob ejection */ - trajectory_d_rel(&mainboard.traj, -70); - err = wait_traj_end(END_INTR|END_TRAJ); - if (!TRAJ_SUCCESS(err)) - goto fail; + /* cob ejection */ + trajectory_d_rel(&mainboard.traj, -70); + err = wait_traj_end(END_INTR|END_TRAJ); + if (!TRAJ_SUCCESS(err)) + goto fail; - if (get_cob_count() > 0) { i2c_cobboard_set_mode(I2C_COBBOARD_MODE_EJECT); time_wait_ms(2000); - } - /* cob ejection */ - trajectory_d_rel(&mainboard.traj, 70); - err = wait_traj_end(END_INTR|END_TRAJ); - if (!TRAJ_SUCCESS(err)) - goto fail; + + /* cob ejection */ + trajectory_d_rel(&mainboard.traj, 70); + err = wait_traj_end(END_INTR|END_TRAJ); + if (!TRAJ_SUCCESS(err)) + goto fail; + } strat_db_dump(__FUNCTION__); err = END_TRAJ; @@ -493,7 +558,8 @@ static uint8_t strat_beginning(uint8_t do_initturn) strat_set_acc(ACC_DIST, ACC_ANGLE); if (do_initturn) { - strat_set_speed(600, 60); /* OK */ + //strat_set_speed(600, 60); + strat_set_speed(450, 50); trajectory_d_a_rel(&mainboard.traj, 1000, COLOR_A(20)); err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj), TRAJ_FLAGS_STD); @@ -502,11 +568,27 @@ static uint8_t strat_beginning(uint8_t do_initturn) strat_set_acc(ACC_DIST, ACC_ANGLE); strat_set_speed(SPEED_CLITOID_SLOW, SPEED_ANGLE_SLOW); + l1: err = line2line(0, LINE_UP, 2, LINE_R_DOWN, TRAJ_FLAGS_NO_NEAR); + if (err == END_OBSTACLE && + strat_conf.flags & STRAT_CONF_WAIT_OBSTACLE && + time_get_s() > strat_conf.prev_wait_obstacle + 5) { + strat_conf.prev_wait_obstacle = time_get_s(); + time_wait_ms(2000); + goto l1; + } if (!TRAJ_SUCCESS(err)) return err; + l2: err = line2line(2, LINE_R_DOWN, 2, LINE_R_UP, TRAJ_FLAGS_NO_NEAR); + if (err == END_OBSTACLE && + strat_conf.flags & STRAT_CONF_WAIT_OBSTACLE && + time_get_s() > strat_conf.prev_wait_obstacle + 5) { + strat_conf.prev_wait_obstacle = time_get_s(); + time_wait_ms(2000); + goto l2; + } if (!TRAJ_SUCCESS(err)) { return err; } @@ -514,6 +596,28 @@ static uint8_t strat_beginning(uint8_t do_initturn) return END_TRAJ; } +static uint8_t strat_beginning2(uint8_t do_initturn) +{ + uint8_t err; + + strat_set_acc(ACC_DIST, ACC_ANGLE); + + if (do_initturn) { + strat_set_speed(600, 95); /* OK */ + trajectory_d_a_rel(&mainboard.traj, 1000, COLOR_A(-40)); + err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj), + TRAJ_FLAGS_STD); + if (err == 0) + return END_TRAJ; + } + else { + trajectory_goto_forward_xy_abs(&mainboard.traj, + 375, COLOR_Y(597)); + err = wait_traj_end(TRAJ_FLAGS_STD); + } + return err; +} + /* dump state (every 5 s max) */ #define DUMP_RATE_LIMIT(dump, last_print) \ do { \ @@ -566,7 +670,7 @@ uint8_t get_opp_oranges(void) x = position_get_x_s16(&mainboard.pos); y = position_get_y_s16(&mainboard.pos); - if (xycoord_to_ijcoord(&x, &y, &i, &j) < 0) + if (xycoord_to_ijcoord_not_corn(&x, &y, &i, &j) < 0) return END_ERROR; /* not on eject point */ @@ -604,7 +708,7 @@ uint8_t get_opp_oranges(void) uint8_t get_orphan_tomatoes(void) { #define CLITOID_TOMATO_RADIUS 100. -#define TOMATO_BACK_X 2760 +#define TOMATO_BACK_X 2780 #define TOMATO_BACK_LEN 200 int16_t x, y, a; @@ -623,7 +727,7 @@ uint8_t get_orphan_tomatoes(void) x = position_get_x_s16(&mainboard.pos); y = position_get_y_s16(&mainboard.pos); - if (xycoord_to_ijcoord(&x, &y, &i, &j) < 0) + if (xycoord_to_ijcoord_not_corn(&x, &y, &i, &j) < 0) return END_ERROR; /* not on eject point */ @@ -656,7 +760,7 @@ uint8_t get_orphan_tomatoes(void) if (ABS(a) < 10) strat_reset_pos(AREA_X - ROBOT_HALF_LENGTH_FRONT, DO_NOT_SET_POS, - COLOR_A(0)); + COLOR_A(0) + ROBOT_ANGLE_FRONT); strat_set_speed(SPEED_CLITOID_FAST, SPEED_ANGLE_FAST); trajectory_d_rel(&mainboard.traj, -250); @@ -715,6 +819,7 @@ uint8_t prepare_hill(uint8_t orange_color, int16_t posx) uint8_t our_color = get_color(); uint8_t err; + strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW); if (orange_color == I2C_COLOR_YELLOW) starty = HILL_POSY_YELLOW; else @@ -771,7 +876,6 @@ uint8_t run_to_the_hills(uint8_t orange_color) bd_set_current_thresholds(&mainboard.angle.bd, 500, 8000, 2000000, 80); bd_set_speed_threshold(&mainboard.distance.bd, 10); support_balls_pack(); - i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_PREP_FORK); /* decrease angle gains */ pid_set_gains(&mainboard.angle.pid, 200, 0, 2000); @@ -780,10 +884,16 @@ uint8_t run_to_the_hills(uint8_t orange_color) * are on the hill */ i2c_cobboard_set_mode(I2C_COBBOARD_MODE_KICKSTAND_DOWN); trajectory_d_rel(&mainboard.traj, HILL_LEN); - err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos) > - HILL_POSX_BALLS_DOWN1, - TRAJ_FLAGS_SMALL_DIST); + if (orange_color == our_color) + err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos) > + HILL_POSX_BALLS_DOWN1, + TRAJ_FLAGS_SMALL_DIST); + else + err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos) < + AREA_X - HILL_POSX_BALLS_DOWN1, + TRAJ_FLAGS_SMALL_DIST); DEBUG(E_USER_STRAT, "deploy support balls"); + i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_PREP_FORK); support_balls_deploy(); i2c_cobboard_set_mode(I2C_COBBOARD_MODE_KICKSTAND_UP); trajectory_only_a_rel(&mainboard.traj, 2); @@ -799,26 +909,38 @@ uint8_t run_to_the_hills(uint8_t orange_color) /* reach top, go down */ trajectory_d_rel(&mainboard.traj, -HILL_LEN); - err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos) < - HILL_POSX_BALLS_DOWN2, - TRAJ_FLAGS_SMALL_DIST); + if (orange_color == our_color) + err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos) < + HILL_POSX_BALLS_DOWN2, + TRAJ_FLAGS_SMALL_DIST); + else + err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos) > + AREA_X - HILL_POSX_BALLS_DOWN2, + TRAJ_FLAGS_SMALL_DIST); DEBUG(E_USER_STRAT, "pack support balls"); support_balls_pack(); - err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos) < - HILL_POSX_BALLS_DOWN3, - TRAJ_FLAGS_SMALL_DIST); + if (orange_color == our_color) + err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos) < + HILL_POSX_BALLS_DOWN3, + TRAJ_FLAGS_SMALL_DIST); + else + err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos) > + AREA_X - HILL_POSX_BALLS_DOWN3, + TRAJ_FLAGS_SMALL_DIST); + DEBUG(E_USER_STRAT, "deploy support balls"); strat_set_acc(ad, aa); strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST); support_balls_deploy(); - err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); - i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST); /* wait to be near the wall */ - err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos) < 200, - TRAJ_FLAGS_SMALL_DIST); - - i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST); + if (orange_color == our_color) + err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos) < 300, + TRAJ_FLAGS_SMALL_DIST); + else + err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos) > + AREA_X - 300, + TRAJ_FLAGS_SMALL_DIST); /* restore BD coefs */ bd_set_current_thresholds(&mainboard.distance.bd, 500, 8000, 1000000, 20); @@ -826,33 +948,42 @@ uint8_t run_to_the_hills(uint8_t orange_color) bd_set_speed_threshold(&mainboard.distance.bd, 60); /* calibrate position on the wall */ - strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST); - err = strat_calib(-400, TRAJ_FLAGS_SMALL_DIST); - if (orange_color == our_color) - strat_reset_pos(ROBOT_HALF_LENGTH_REAR, - DO_NOT_SET_POS, COLOR_A(0)); - else - strat_reset_pos(AREA_X - ROBOT_HALF_LENGTH_REAR, - DO_NOT_SET_POS, COLOR_A(180)); + strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_SLOW); + + i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST); + + trajectory_a_abs(&mainboard.traj, COLOR_A(-90)); + err = wait_traj_end(TRAJ_FLAGS_NO_NEAR); + time_wait_ms(200); + + err = strat_calib(400, TRAJ_FLAGS_SMALL_DIST); + strat_reset_pos(DO_NOT_SET_POS, + COLOR_Y(ROBOT_HALF_LENGTH_FRONT), + COLOR_A(-90) + ROBOT_ANGLE_FRONT); strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST); - trajectory_d_rel(&mainboard.traj, 250); + trajectory_d_rel(&mainboard.traj, -250); err = wait_traj_end(TRAJ_FLAGS_NO_NEAR); - if (orange_color == I2C_COLOR_YELLOW) - trajectory_a_rel(&mainboard.traj, 90); + if (orange_color == our_color) + trajectory_a_abs(&mainboard.traj, 180); else - trajectory_a_rel(&mainboard.traj, -90); + trajectory_a_abs(&mainboard.traj, 0); err = wait_traj_end(TRAJ_FLAGS_NO_NEAR); time_wait_ms(200); strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST); - err = strat_calib(-400, TRAJ_FLAGS_SMALL_DIST); - strat_reset_pos(DO_NOT_SET_POS, - COLOR_Y(ROBOT_HALF_LENGTH_REAR), - COLOR_A(90)); + err = strat_calib(400, TRAJ_FLAGS_SMALL_DIST); + if (orange_color == our_color) + strat_reset_pos(ROBOT_HALF_LENGTH_FRONT, + DO_NOT_SET_POS, + 180 + ROBOT_ANGLE_FRONT); + else + strat_reset_pos(AREA_X - ROBOT_HALF_LENGTH_FRONT, + DO_NOT_SET_POS, + 0 + ROBOT_ANGLE_FRONT); strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST); - trajectory_d_rel(&mainboard.traj, 250); + trajectory_d_rel(&mainboard.traj, -250); err = wait_traj_end(TRAJ_FLAGS_NO_NEAR); /* revert acceleration and speed */ @@ -876,7 +1007,10 @@ uint8_t strat_main(void) } /* harvest the first cobs + balls */ - err = strat_beginning(do_initturn); + if (strat_conf.flags & STRAT_CONF_STRAIGHT_BEGIN) + err = strat_beginning2(do_initturn); + else + err = strat_beginning(do_initturn); if (!TRAJ_SUCCESS(err)) strat_unblock();