X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fmainboard%2Fstrat.c;h=5ae37974ec5538cb789f58a284e81d312997c5e4;hp=2c6042828a6b74e1593388d8e8b7acdfc5acc315;hb=aa68be2d5d596693bda1109e4ccd6489f613b4f7;hpb=9d056416278ae5d772a0fc1f7d2d11a4461fb673 diff --git a/projects/microb2010/mainboard/strat.c b/projects/microb2010/mainboard/strat.c index 2c60428..5ae3797 100644 --- a/projects/microb2010/mainboard/strat.c +++ b/projects/microb2010/mainboard/strat.c @@ -67,6 +67,8 @@ #define COL_DISP_MARGIN 400 /* stop 40 cm in front of dispenser */ #define COL_SCAN_PRE_MARGIN 250 +static volatile uint8_t strat_running = 0; +static volatile uint8_t want_pack = 0; struct strat_conf strat_conf; /*************************************************************/ @@ -97,25 +99,32 @@ void strat_conf_dump(const char *caller) } +void strat_event_enable(void) +{ + strat_running = 1; +} + +void strat_event_disable(void) +{ + strat_running = 0; +} + /* call it just before launching the strat */ void strat_init(void) { +#ifdef HOST_VERSION position_set(&mainboard.pos, 298.16, COLOR_Y(308.78), COLOR_A(70.00)); - - /* XXX init rollers, .. */ - - strat_db_init(); +#endif /* we consider that the color is correctly set */ - + strat_running = 1; + strat_db_init(); strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST); time_reset(); interrupt_traj_reset(); i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST); - i2c_cobboard_harvest(I2C_LEFT_SIDE); - i2c_cobboard_harvest(I2C_RIGHT_SIDE); i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_HARVEST); /* used in strat_base for END_TIMER */ @@ -123,7 +132,6 @@ void strat_init(void) DO_POS | DO_BD | DO_TIMER | DO_POWER; } - /* call it after each strat */ void strat_exit(void) { @@ -131,6 +139,7 @@ void strat_exit(void) uint8_t flags; #endif + strat_running = 0; mainboard.flags &= ~(DO_TIMER); strat_hardstop(); time_reset(); @@ -144,11 +153,17 @@ void strat_exit(void) #endif } -/* called periodically */ +/* called periodically (10ms) */ void strat_event(void *dummy) { uint8_t flags; + int8_t lcob_near, rcob_near; uint8_t lcob, rcob; + uint8_t lidx, ridx; + + /* ignore when strat is not running */ + if (strat_running == 0) + return; IRQ_LOCK(flags); lcob = ballboard.lcob; @@ -157,19 +172,111 @@ void strat_event(void *dummy) ballboard.rcob = I2C_COB_NONE; IRQ_UNLOCK(flags); - if (lcob == I2C_COB_WHITE) - DEBUG(E_USER_STRAT, "lcob white"); - if (lcob == I2C_COB_BLACK) - DEBUG(E_USER_STRAT, "lcob black"); - if (rcob == I2C_COB_WHITE) - DEBUG(E_USER_STRAT, "rcob white"); - if (rcob == I2C_COB_BLACK) - DEBUG(E_USER_STRAT, "rcob black"); + /* XXX take opponent position into account */ + +#ifdef HOST_VERSION + if (time_get_s() == 15) + cobboard.cob_count = 5; + if (time_get_s() == 16) + cobboard.cob_count = 0; + if (time_get_s() == 25) + cobboard.cob_count = 5; +#endif + + /* detect cob on left side */ + lcob_near = corn_is_near(&lidx, I2C_LEFT_SIDE); + 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); + } + + /* detect cob on right side */ + rcob_near = corn_is_near(&ridx, I2C_RIGHT_SIDE); + 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); + } - /* limit speed when opponent is close */ + /* control the cobboard mode for left spickle */ + if (get_cob_count() >= 5 || want_pack) { + if (strat_db.corn_table[lidx]->corn.color != I2C_COB_WHITE && + strat_db.corn_table[lidx]->corn.color != I2C_COB_BLACK) + i2c_cobboard_pack(I2C_LEFT_SIDE); + } + else { + if (strat_db.corn_table[lidx]->corn.color == I2C_COB_WHITE) + i2c_cobboard_autoharvest(I2C_LEFT_SIDE); + else if (strat_db.corn_table[lidx]->corn.color == I2C_COB_BLACK) + i2c_cobboard_deploy_nomove(I2C_LEFT_SIDE); + else + i2c_cobboard_deploy(I2C_LEFT_SIDE); + } + + /* control the cobboard mode for right spickle */ + if (get_cob_count() >= 5 || want_pack) { + if (strat_db.corn_table[lidx]->corn.color != I2C_COB_WHITE && + strat_db.corn_table[lidx]->corn.color != I2C_COB_BLACK) + i2c_cobboard_pack(I2C_RIGHT_SIDE); + } + else { + if (strat_db.corn_table[lidx]->corn.color == I2C_COB_WHITE) + i2c_cobboard_autoharvest(I2C_RIGHT_SIDE); + else if (strat_db.corn_table[lidx]->corn.color == I2C_COB_BLACK) + i2c_cobboard_deploy_nomove(I2C_RIGHT_SIDE); + else + i2c_cobboard_deploy(I2C_RIGHT_SIDE); + } + + /* limit speed when opponent is near */ strat_limit_speed(); } + +static uint8_t strat_harvest(void) +{ + return 0; +} + +static uint8_t strat_eject(void) +{ + uint8_t err; + + //XXX return vals + strat_set_speed(600, SPEED_ANGLE_SLOW); + + trajectory_goto_xy_abs(&mainboard.traj, 2625, COLOR_Y(1847)); + err = wait_traj_end(TRAJ_FLAGS_NO_NEAR); + trajectory_a_abs(&mainboard.traj, COLOR_A(70)); + err = wait_traj_end(TRAJ_FLAGS_NO_NEAR); + + DEBUG(E_USER_STRAT, "%s():%d", __FUNCTION__, __LINE__); + strat_hardstop(); + + /* ball ejection */ + trajectory_a_abs(&mainboard.traj, COLOR_A(90)); + i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_EJECT); + time_wait_ms(2000); + + /* half turn */ + i2c_cobboard_pack(I2C_LEFT_SIDE); + i2c_cobboard_pack(I2C_RIGHT_SIDE); + trajectory_a_rel(&mainboard.traj, COLOR_A(180)); + err = wait_traj_end(END_INTR|END_TRAJ); + + /* cob ejection */ + trajectory_d_rel(&mainboard.traj, -100); + err = wait_traj_end(END_INTR|END_TRAJ); + strat_running = ; + i2c_cobboard_set_mode(I2C_COBBOARD_MODE_EJECT); + time_wait_ms(2000); + + return 0; +} + static uint8_t strat_beginning(void) { uint8_t err; @@ -182,7 +289,6 @@ static uint8_t strat_beginning(void) strat_set_speed(250, SPEED_ANGLE_FAST); #endif - // strat_set_speed(600, 60); /* OK */ strat_set_speed(250, 28); /* OK */ @@ -191,40 +297,33 @@ static uint8_t strat_beginning(void) TRAJ_FLAGS_STD); strat_set_acc(ACC_DIST, ACC_ANGLE); + strat_set_speed(250, SPEED_ANGLE_SLOW); #if 1 l1: + DEBUG(E_USER_STRAT, "%s():%d", __FUNCTION__, __LINE__); if (get_cob_count() >= 5) strat_set_speed(600, SPEED_ANGLE_FAST); err = line2line(LINE_UP, 0, LINE_R_DOWN, 2); if (!TRAJ_SUCCESS(err)) { - trajectory_hardstop(&mainboard.traj); + strat_hardstop(); time_wait_ms(2000); goto l1; } l2: + DEBUG(E_USER_STRAT, "%s():%d", __FUNCTION__, __LINE__); if (get_cob_count() >= 5) strat_set_speed(600, SPEED_ANGLE_FAST); err = line2line(LINE_R_DOWN, 2, LINE_R_UP, 2); if (!TRAJ_SUCCESS(err)) { - trajectory_hardstop(&mainboard.traj); + strat_hardstop(); time_wait_ms(2000); goto l2; } - l3: - if (get_cob_count() >= 5) - strat_set_speed(600, SPEED_ANGLE_FAST); - - err = line2line(LINE_R_UP, 2, LINE_UP, 5); - if (!TRAJ_SUCCESS(err)) { - trajectory_hardstop(&mainboard.traj); - time_wait_ms(2000); - goto l3; - } #else strat_set_speed(600, SPEED_ANGLE_FAST); err = line2line(LINE_UP, 0, LINE_R_DOWN, 3); @@ -237,32 +336,46 @@ static uint8_t strat_beginning(void) err = line2line(LINE_L_UP, 1, LINE_L_DOWN, 1); err = line2line(LINE_L_DOWN, 1, LINE_DOWN, 0); wait_ms(500); - trajectory_hardstop(&mainboard.traj); + strat_hardstop(); return END_TRAJ; #endif - trajectory_hardstop(&mainboard.traj); + strat_eject(); - /* ball ejection */ - trajectory_a_abs(&mainboard.traj, COLOR_A(90)); - i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_EJECT); - time_wait_ms(2000); + strat_set_speed(250, SPEED_ANGLE_FAST); - /* half turn */ - trajectory_goto_xy_abs(&mainboard.traj, 2625, COLOR_Y(1847)); - err = wait_traj_end(END_INTR|END_TRAJ); - i2c_cobboard_pack(I2C_LEFT_SIDE); - i2c_cobboard_pack(I2C_RIGHT_SIDE); - trajectory_a_rel(&mainboard.traj, COLOR_A(180)); - err = wait_traj_end(END_INTR|END_TRAJ); + l4: + DEBUG(E_USER_STRAT, "%s():%d", __FUNCTION__, __LINE__); + if (get_cob_count() >= 5) + strat_set_speed(600, SPEED_ANGLE_FAST); - /* cob ejection */ - trajectory_d_rel(&mainboard.traj, -100); - err = wait_traj_end(END_INTR|END_TRAJ); - i2c_cobboard_set_mode(I2C_COBBOARD_MODE_EJECT); - time_wait_ms(2000); + err = line2line(LINE_DOWN, 5, LINE_L_UP, 2); + if (!TRAJ_SUCCESS(err)) { + strat_hardstop(); + time_wait_ms(2000); + goto l4; + } + + l5: + DEBUG(E_USER_STRAT, "%s():%d", __FUNCTION__, __LINE__); + if (get_cob_count() >= 5) + strat_set_speed(600, SPEED_ANGLE_FAST); + + err = line2line(LINE_L_UP, 2, LINE_R_UP, 2); + if (!TRAJ_SUCCESS(err)) { + strat_hardstop(); + time_wait_ms(2000); + goto l5; + } + + DEBUG(E_USER_STRAT, "%s():%d", __FUNCTION__, __LINE__); + if (get_cob_count() >= 5) + strat_set_speed(600, SPEED_ANGLE_FAST); + + WAIT_COND_OR_TRAJ_END(distance_from_robot(2625, COLOR_Y(1847)) < 100, + TRAJ_FLAGS_STD); + strat_eject(); - trajectory_hardstop(&mainboard.traj); return END_TRAJ; } @@ -299,16 +412,6 @@ static uint8_t need_more_elements(void) } } -static uint8_t strat_harvest(void) -{ - return 0; -} - -static uint8_t strat_eject(void) -{ - return 0; -} - uint8_t strat_main(void) { uint8_t err;