X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fmainboard%2Fstrat.c;h=6090ca8ef2829d72113360e771a5f9105aa673e5;hp=dc595815841cf7007f2e8575b46cc2bc419344c8;hb=HEAD;hpb=1aa73d20096ad6be733b2ddf1c890434bd82db75 diff --git a/projects/microb2010/mainboard/strat.c b/projects/microb2010/mainboard/strat.c index dc59581..6090ca8 100644 --- a/projects/microb2010/mainboard/strat.c +++ b/projects/microb2010/mainboard/strat.c @@ -112,12 +112,14 @@ void strat_conf_dump(const char *caller) return; printf_P(PSTR("-- conf --\r\n")); - 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\n"), + printf_P(PSTR("wait_obstacle = %s\r\n"), (strat_conf.flags & STRAT_CONF_WAIT_OBSTACLE) ? "y":"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("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) @@ -144,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); @@ -219,6 +223,20 @@ static void check_tomato(void) #endif } +static void remove_cob(uint8_t idx) +{ + if (strat_db.corn_table[idx]->corn.color == I2C_COB_BLACK) + return; + + if (strat_db.corn_table[idx]->time_removed == -1) { + strat_db.corn_table[idx]->time_removed = time_get_s(); +#ifdef HOST_VERSION + cobboard.cob_count ++; + printf("add cob %d\n", idx); +#endif + } +} + /* mark corn as not present and give correct commands to the cobboard * for spickles */ static void check_corn(void) @@ -230,6 +248,9 @@ static void check_corn(void) static uint8_t prev_check_time; uint8_t cur_time; uint8_t need_lpack, need_rpack; + int16_t l_xspickle, l_yspickle; + int16_t r_xspickle, r_yspickle; + uint8_t l_y_too_high_pack = 0, r_y_too_high_pack = 0; /* read sensors from ballboard */ IRQ_LOCK(flags); @@ -239,13 +260,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 && @@ -260,48 +280,58 @@ static void check_corn(void) } /* detect cob on left side */ - lcob_near = corn_is_near(&lidx, I2C_LEFT_SIDE); + lcob_near = corn_is_near(&lidx, I2C_LEFT_SIDE, + &l_xspickle, &l_yspickle); if (lcob_near && lcob != I2C_COB_NONE) { if (strat_db.corn_table[lidx]->corn.color == I2C_COB_UNKNOWN) DEBUG(E_USER_STRAT, "lcob %s %d", lcob == I2C_COB_WHITE ? "white" : "black", lidx); corn_set_color(strat_db.corn_table[lidx], lcob); } + if (cur_time > 5 && !__y_is_more_than(l_yspickle, 600)) + l_y_too_high_pack = 1; /* detect cob on right side */ - rcob_near = corn_is_near(&ridx, I2C_RIGHT_SIDE); + rcob_near = corn_is_near(&ridx, I2C_RIGHT_SIDE, + &r_xspickle, &r_yspickle); if (rcob_near && rcob != I2C_COB_NONE) { if (strat_db.corn_table[ridx]->corn.color == I2C_COB_UNKNOWN) DEBUG(E_USER_STRAT, "rcob %s %d", rcob == I2C_COB_WHITE ? "white" : "black", ridx); corn_set_color(strat_db.corn_table[ridx], rcob); } + if (cur_time > 5 && !__y_is_more_than(r_yspickle, 600)) + r_y_too_high_pack = 1; + + /* re-enable white cob */ + if (lcob == I2C_COB_WHITE && lcob_near && + strat_db.corn_table[lidx]->present == 0) { + strat_db.corn_table[lidx]->present = 1; + strat_db.corn_table[lidx]->time_removed = -1; + } + if (rcob == I2C_COB_WHITE && rcob_near && + strat_db.corn_table[ridx]->present == 0) { + strat_db.corn_table[ridx]->present = 1; + strat_db.corn_table[ridx]->time_removed = -1; + } /* 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) { + strat_lpack60 || strat_opponent_lpack || l_y_too_high_pack; + + if (lcob_near && + (strat_db.corn_table[lidx]->present || + strat_db.corn_table[lidx]->time_removed == -1)) { if (need_lpack) { /* nothing */ } else { /* deploy spickle and harvest white ones */ - if (strat_db.corn_table[lidx]->corn.color == I2C_COB_WHITE) { + if (strat_db.corn_table[lidx]->corn.color == I2C_COB_WHITE) i2c_cobboard_autoharvest_nomove(I2C_LEFT_SIDE); - if (strat_db.corn_table[lidx]->time_removed == -1 -#ifndef HOST_VERSION - && cobboard.status == I2C_COBBOARD_STATUS_LBUSY -#endif - ) { - strat_db.corn_table[lidx]->time_removed = time_get_s(); -#ifdef HOST_VERSION - cobboard.cob_count ++; - printf("add cob %d\n", lidx); -#endif - } - } else i2c_cobboard_deploy_nomove(I2C_LEFT_SIDE); + remove_cob(lidx); } } else { @@ -314,29 +344,20 @@ static void check_corn(void) /* 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) { + strat_rpack60 || strat_opponent_rpack || r_y_too_high_pack; + if (rcob_near && + (strat_db.corn_table[ridx]->present || + strat_db.corn_table[ridx]->time_removed == -1)) { if (need_rpack) { /* nothing */ } else { /* deploy spickle and harvest white ones */ - if (strat_db.corn_table[ridx]->corn.color == I2C_COB_WHITE) { + if (strat_db.corn_table[ridx]->corn.color == I2C_COB_WHITE) i2c_cobboard_autoharvest_nomove(I2C_RIGHT_SIDE); - if (strat_db.corn_table[ridx]->time_removed == -1 -#ifndef HOST_VERSION - && cobboard.status == I2C_COBBOARD_STATUS_RBUSY -#endif - ) { - strat_db.corn_table[ridx]->time_removed = time_get_s(); -#ifdef HOST_VERSION - cobboard.cob_count ++; - printf("add cob %d\n", ridx); -#endif - } - } else i2c_cobboard_deploy_nomove(I2C_RIGHT_SIDE); + remove_cob(ridx); } } else { @@ -494,6 +515,7 @@ static uint8_t strat_eject(void) /* ball ejection */ if (get_ball_count() > 0) { i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_EJECT); + time_wait_ms(300); trajectory_a_abs(&mainboard.traj, COLOR_A(70)); err = wait_traj_end(TRAJ_FLAGS_NO_NEAR); if (!TRAJ_SUCCESS(err)) @@ -504,35 +526,40 @@ static uint8_t strat_eject(void) #ifdef HOST_VERSION time_wait_ms(2000); #else - WAIT_COND_OR_TIMEOUT(ballboard.status == I2C_BALLBOARD_STATUS_F_BUSY, - 2000); WAIT_COND_OR_TIMEOUT(ballboard.status == I2C_BALLBOARD_STATUS_F_READY, 2000); #endif } + else { + /* to pack spickles */ + time_wait_ms(300); + } - /* 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 || + cobboard.status == I2C_COBBOARD_STATUS_LBUSY || + cobboard.status == I2C_COBBOARD_STATUS_RBUSY) { + /* 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; @@ -550,7 +577,9 @@ 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); + strat_set_speed(350, 40); trajectory_d_a_rel(&mainboard.traj, 1000, COLOR_A(20)); err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj), TRAJ_FLAGS_STD); @@ -594,7 +623,7 @@ static uint8_t strat_beginning2(uint8_t do_initturn) strat_set_acc(ACC_DIST, ACC_ANGLE); if (do_initturn) { - strat_set_speed(600, 90); /* OK */ + 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); @@ -751,7 +780,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); @@ -810,6 +839,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 @@ -866,7 +896,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); @@ -875,10 +904,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); @@ -889,31 +924,59 @@ uint8_t run_to_the_hills(uint8_t orange_color) strat_hardstop(); i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_TAKE_FORK); - time_wait_ms(1800); + time_wait_ms(500); + + trajectory_d_rel(&mainboard.traj, 15); + time_wait_ms(700); + strat_hardstop(); + time_wait_ms(200); /* reach top, go down */ - trajectory_d_rel(&mainboard.traj, -HILL_LEN); + trajectory_d_a_rel(&mainboard.traj, -HILL_LEN, -2); - 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); + + if (orange_color == I2C_COLOR_YELLOW) { + strat_set_acc(ad, aa); + strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST); + } + else { + strat_set_acc(ad, 0.4); + strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW); + trajectory_d_a_rel(&mainboard.traj, -500, 20); + } /* wait to be near the wall */ - err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos) < 200, - TRAJ_FLAGS_SMALL_DIST); + 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); - i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST); + strat_set_acc(ad, aa); + strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST); /* restore BD coefs */ bd_set_current_thresholds(&mainboard.distance.bd, 500, 8000, 1000000, 20); @@ -921,35 +984,45 @@ 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 0 + 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); - +#endif /* revert acceleration and speed */ pid_set_gains(&mainboard.angle.pid, p, i, d); pid_set_maximums(&mainboard.distance.pid, max_in, max_i, max_out);