X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fmainboard%2Fstrat.c;h=dc595815841cf7007f2e8575b46cc2bc419344c8;hp=3e29f3a22385011fa7ffce0d72debdd88db40c47;hb=1aa73d20096ad6be733b2ddf1c890434bd82db75;hpb=28da5858ac871c626153f47566e968ecb05ff52b diff --git a/projects/microb2010/mainboard/strat.c b/projects/microb2010/mainboard/strat.c index 3e29f3a..dc59581 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,12 @@ 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"), (strat_conf.flags & STRAT_CONF_OUR_ORANGE) ? "y":"n"); + printf_P(PSTR("wait_obstacle = %s\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); } void strat_event_enable(void) @@ -222,6 +229,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); @@ -270,8 +278,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 { @@ -296,15 +306,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 { @@ -329,13 +341,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 +399,7 @@ void strat_event(void *dummy) if (strat_running == 0) return; + check_opponent(); check_tomato(); check_corn(); @@ -360,7 +417,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 +441,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 */ @@ -502,11 +559,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 +587,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, 90); /* 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 +661,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 +699,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 +718,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 */ @@ -808,8 +903,8 @@ uint8_t run_to_the_hills(uint8_t orange_color) HILL_POSX_BALLS_DOWN3, TRAJ_FLAGS_SMALL_DIST); DEBUG(E_USER_STRAT, "deploy support balls"); - strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST); 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); @@ -876,7 +971,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();