X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fmainboard%2Fstrat.c;h=64562f530c77ebc69a50f448ea6989ed486c1b56;hp=14aaf652ae8397d3233200c802d3902221c71d86;hb=f1fab48d45873bc23dccbae5db393afd2942570c;hpb=90bef45081332f3780e26bf9f838b668df99fb26 diff --git a/projects/microb2010/mainboard/strat.c b/projects/microb2010/mainboard/strat.c index 14aaf65..64562f5 100644 --- a/projects/microb2010/mainboard/strat.c +++ b/projects/microb2010/mainboard/strat.c @@ -82,7 +82,7 @@ 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, }; @@ -101,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__); } @@ -111,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) @@ -141,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); @@ -236,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 && @@ -294,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 } } @@ -329,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 } } @@ -508,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; @@ -547,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); @@ -556,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; } @@ -568,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 { \ @@ -658,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; @@ -710,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); @@ -769,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 @@ -825,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); @@ -834,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); @@ -853,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); @@ -880,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 */ @@ -930,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();