X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fmainboard%2Fstrat.c;h=3ef6675f5837a3535a9e001a497fb5c28f55a77a;hp=38397387431d4aed1aac4720d1482ef2d05b8cac;hb=0a6dcb3bfa4b8b424975f91b8bcd57f0832deb9e;hpb=d776dc0dab6dd3ddf92041f1ad42e521847ea9cf diff --git a/projects/microb2010/mainboard/strat.c b/projects/microb2010/mainboard/strat.c index 3839738..3ef6675 100644 --- a/projects/microb2010/mainboard/strat.c +++ b/projects/microb2010/mainboard/strat.c @@ -61,13 +61,17 @@ #include "strat_base.h" #include "strat_corn.h" #include "strat_utils.h" +#include "strat_avoid.h" #include "sensor.h" #include "actuator.h" #define COL_DISP_MARGIN 400 /* stop 40 cm in front of dispenser */ #define COL_SCAN_PRE_MARGIN 250 -static uint8_t strat_running = 0; +static volatile uint8_t strat_running = 0; +static volatile uint8_t want_pack = 0; +volatile uint8_t strat_lpack60 = 0; +volatile uint8_t strat_rpack60 = 0; struct strat_conf strat_conf; /*************************************************************/ @@ -84,7 +88,7 @@ void strat_preinit(void) mainboard.flags = DO_ENCODERS | DO_CS | DO_RS | DO_POS | DO_BD | DO_POWER; - //i2c_cobboard_mode_init(); + strat_db_init(); strat_conf_dump(__FUNCTION__); strat_db_dump(__FUNCTION__); } @@ -98,6 +102,16 @@ 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) { @@ -108,7 +122,6 @@ void strat_init(void) /* 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(); @@ -121,7 +134,6 @@ void strat_init(void) DO_POS | DO_BD | DO_TIMER | DO_POWER; } - /* call it after each strat */ void strat_exit(void) { @@ -147,6 +159,7 @@ void strat_exit(void) void strat_event(void *dummy) { uint8_t flags; + int8_t lcob_near, rcob_near; uint8_t lcob, rcob; uint8_t lidx, ridx; @@ -154,6 +167,7 @@ void strat_event(void *dummy) if (strat_running == 0) return; + /* read sensors from ballboard */ IRQ_LOCK(flags); lcob = ballboard.lcob; ballboard.lcob = I2C_COB_NONE; @@ -161,139 +175,192 @@ void strat_event(void *dummy) ballboard.rcob = I2C_COB_NONE; IRQ_UNLOCK(flags); +/* if (lcob != I2C_COB_NONE) */ +/* DEBUG(E_USER_STRAT, "XXX lcob %s", */ +/* lcob == I2C_COB_WHITE ? "white" : "black"); */ +/* if (rcob != I2C_COB_NONE) */ +/* DEBUG(E_USER_STRAT, "XXX rcob %s", */ +/* rcob == I2C_COB_WHITE ? "white" : "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; +#endif /* detect cob on left side */ - if (corn_is_near(&lidx, I2C_LEFT_SIDE)) { - if (lcob != I2C_COB_NONE) { - corn_set_color(strat_db.corn_table[lidx], lcob); + 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); - } - if (strat_db.corn_table[lidx]->corn.color == I2C_COB_WHITE) - i2c_cobboard_autoharvest(I2C_LEFT_SIDE); - else - i2c_cobboard_deploy(I2C_LEFT_SIDE); - } - else { - i2c_cobboard_deploy(I2C_LEFT_SIDE); + corn_set_color(strat_db.corn_table[lidx], lcob); } /* detect cob on right side */ - if (corn_is_near(&ridx, I2C_RIGHT_SIDE)) { - if (rcob != I2C_COB_NONE) { - corn_set_color(strat_db.corn_table[ridx], rcob); + 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); + } + + /* control the cobboard mode for left spickle */ + if (lcob_near && strat_db.corn_table[lidx]->present) { + if (get_cob_count() >= 5 || want_pack || strat_lpack60) { + /* nothing */ + } + else { + /* deploy spickle and harvest white ones */ + if (strat_db.corn_table[lidx]->corn.color == I2C_COB_WHITE) { + i2c_cobboard_autoharvest_nomove(I2C_LEFT_SIDE); + if (cobboard.status == I2C_COBBOARD_STATUS_LBUSY) + strat_db.corn_table[lidx]->present = 0; + } + else + i2c_cobboard_deploy_nomove(I2C_LEFT_SIDE); } - if (strat_db.corn_table[ridx]->corn.color == I2C_COB_WHITE) - i2c_cobboard_autoharvest(I2C_RIGHT_SIDE); - else - i2c_cobboard_deploy(I2C_RIGHT_SIDE); } else { - i2c_cobboard_deploy(I2C_RIGHT_SIDE); + /* no cob near us, we can pack or deploy freely */ + if (get_cob_count() >= 5 || want_pack || strat_lpack60) + i2c_cobboard_pack(I2C_LEFT_SIDE); + else + i2c_cobboard_deploy(I2C_LEFT_SIDE); } + /* control the cobboard mode for right spickle */ + if (rcob_near && strat_db.corn_table[ridx]->present) { + if (get_cob_count() >= 5 || want_pack || strat_rpack60) { + /* nothing */ + } + else { + /* deploy spickle and harvest white ones */ + if (strat_db.corn_table[ridx]->corn.color == I2C_COB_WHITE) { + i2c_cobboard_autoharvest_nomove(I2C_RIGHT_SIDE); + if (cobboard.status == I2C_COBBOARD_STATUS_RBUSY) + strat_db.corn_table[ridx]->present = 0; + } + else + i2c_cobboard_deploy_nomove(I2C_RIGHT_SIDE); + } + } + else { + /* no cob near us, we can pack or deploy freely */ + if (get_cob_count() >= 5 || want_pack || strat_rpack60) + i2c_cobboard_pack(I2C_RIGHT_SIDE); + else + i2c_cobboard_deploy(I2C_RIGHT_SIDE); + } - /* limit speed when opponent is close */ + /* limit speed when opponent is near */ strat_limit_speed(); } -static uint8_t strat_beginning(void) + +static uint8_t strat_harvest(void) +{ + return 0; +} + +/* must be called from a terminal line */ +static uint8_t strat_eject(void) { uint8_t err; + //XXX return vals - strat_set_acc(ACC_DIST, ACC_ANGLE); + /* go to eject point */ + trajectory_goto_xy_abs(&mainboard.traj, 2625, COLOR_Y(1847)); + err = WAIT_COND_OR_TRAJ_END(get_cob_count() >= 5, + TRAJ_FLAGS_NO_NEAR); + if (err == 0) { + want_pack = 1; + strat_set_speed(SPEED_CLITOID_FAST, SPEED_ANGLE_SLOW); + err = wait_traj_end(TRAJ_FLAGS_NO_NEAR); + } + + /* pack arms */ + strat_event_disable(); + i2c_cobboard_pack(I2C_LEFT_SIDE); + i2c_cobboard_pack(I2C_RIGHT_SIDE); + + /* ball ejection */ + i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_EJECT); + 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(); #ifdef HOST_VERSION - strat_set_speed(600, SPEED_ANGLE_FAST); + time_wait_ms(2000); #else - /* 250 */ - strat_set_speed(250, SPEED_ANGLE_FAST); + 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 + /* half turn */ + trajectory_a_rel(&mainboard.traj, COLOR_A(180)); + err = wait_traj_end(END_INTR|END_TRAJ); + + /* cob ejection */ + trajectory_d_rel(&mainboard.traj, -70); + err = wait_traj_end(END_INTR|END_TRAJ); + + i2c_cobboard_set_mode(I2C_COBBOARD_MODE_EJECT); + strat_db_dump(__FUNCTION__); + time_wait_ms(2000); - // strat_set_speed(600, 60); /* OK */ - strat_set_speed(250, 28); /* OK */ + strat_event_enable(); + want_pack = 0; + return 0; +} + +static uint8_t strat_beginning(void) +{ + uint8_t err; + + strat_set_acc(ACC_DIST, ACC_ANGLE); + strat_set_speed(600, 60); /* OK */ + //strat_set_speed(250, 28); /* OK */ trajectory_d_a_rel(&mainboard.traj, 1000, COLOR_A(20)); err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj), TRAJ_FLAGS_STD); strat_set_acc(ACC_DIST, ACC_ANGLE); + strat_set_speed(SPEED_CLITOID_SLOW, SPEED_ANGLE_SLOW); -#if 1 l1: - if (get_cob_count() >= 5) - strat_set_speed(600, SPEED_ANGLE_FAST); - - err = line2line(LINE_UP, 0, LINE_R_DOWN, 2); + DEBUG(E_USER_STRAT, "%s():%d count=%d", __FUNCTION__, __LINE__, get_cob_count()); + err = line2line(0, LINE_UP, 2, LINE_R_DOWN, TRAJ_FLAGS_NO_NEAR); if (!TRAJ_SUCCESS(err)) { - trajectory_hardstop(&mainboard.traj); + strat_hardstop(); time_wait_ms(2000); goto l1; } l2: - if (get_cob_count() >= 5) - strat_set_speed(600, SPEED_ANGLE_FAST); - - err = line2line(LINE_R_DOWN, 2, LINE_R_UP, 2); + DEBUG(E_USER_STRAT, "%s():%d count=%d", __FUNCTION__, __LINE__, get_cob_count()); + err = line2line(2, LINE_R_DOWN, 2, LINE_R_UP, TRAJ_FLAGS_NO_NEAR); 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); + strat_eject(); - err = line2line(LINE_R_UP, 2, LINE_UP, 5); - if (!TRAJ_SUCCESS(err)) { - trajectory_hardstop(&mainboard.traj); - time_wait_ms(2000); - goto l3; + while (1) { + strat_set_speed(SPEED_CLITOID_SLOW, SPEED_ANGLE_SLOW); + strat_harvest_circuit(); + strat_eject(); } -#else - strat_set_speed(600, SPEED_ANGLE_FAST); - err = line2line(LINE_UP, 0, LINE_R_DOWN, 3); - err = line2line(LINE_R_DOWN, 3, LINE_R_UP, 2); - err = line2line(LINE_R_UP, 2, LINE_R_DOWN, 2); - err = line2line(LINE_R_DOWN, 2, LINE_R_UP, 3); - err = line2line(LINE_R_UP, 3, LINE_UP, 5); - err = line2line(LINE_UP, 5, LINE_L_DOWN, 2); - err = line2line(LINE_L_DOWN, 2, LINE_L_UP, 1); - 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); - return END_TRAJ; -#endif - - trajectory_hardstop(&mainboard.traj); - /* ball ejection */ - trajectory_a_abs(&mainboard.traj, COLOR_A(90)); - i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_EJECT); - time_wait_ms(2000); - - /* 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); - - /* 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); - - trajectory_hardstop(&mainboard.traj); return END_TRAJ; } @@ -330,16 +397,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;