X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fmainboard%2Fstrat.c;h=152c1735a86a3e0ea094bb15cc5200643cce3f73;hp=0405844d3117ee64ddfb26f848e4fd33e199f385;hb=017dbe8be9a515265aa85d9df2b469df7593c4b0;hpb=878d0fc33d940d5d02fc65dfd0e56798c97de18d diff --git a/projects/microb2010/mainboard/strat.c b/projects/microb2010/mainboard/strat.c index 0405844..152c173 100644 --- a/projects/microb2010/mainboard/strat.c +++ b/projects/microb2010/mainboard/strat.c @@ -69,7 +69,7 @@ #define COL_SCAN_PRE_MARGIN 250 static volatile uint8_t strat_running = 0; -static volatile uint8_t want_pack = 0; +volatile uint8_t strat_want_pack = 0; volatile uint8_t strat_lpack60 = 0; volatile uint8_t strat_rpack60 = 0; struct strat_conf strat_conf; @@ -225,7 +225,7 @@ static void check_corn(void) /* 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) { + if (get_cob_count() >= 5 || strat_want_pack || strat_lpack60) { /* nothing */ } else { @@ -241,15 +241,15 @@ static void check_corn(void) } else { /* 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); + if (get_cob_count() >= 5 || strat_want_pack || strat_lpack60) + i2c_cobboard_pack_weak(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) { + if (get_cob_count() >= 5 || strat_want_pack || strat_rpack60) { /* nothing */ } else { @@ -265,8 +265,8 @@ static void check_corn(void) } 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); + if (get_cob_count() >= 5 || strat_want_pack || strat_rpack60) + i2c_cobboard_pack_weak(I2C_RIGHT_SIDE); else i2c_cobboard_deploy(I2C_RIGHT_SIDE); } @@ -286,63 +286,78 @@ void strat_event(void *dummy) } -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 MUST be a on the line !! */ //XXX return vals + DEBUG(E_USER_STRAT, "%s()", __FUNCTION__); + /* 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 && !TRAJ_SUCCESS(err)) + return err; + + strat_want_pack = 1; 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); + i2c_cobboard_pack_weak(I2C_LEFT_SIDE); + i2c_cobboard_pack_weak(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); + if (get_ball_count() > 0) { + i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_EJECT); + trajectory_a_abs(&mainboard.traj, COLOR_A(70)); + err = wait_traj_end(TRAJ_FLAGS_NO_NEAR); + if (!TRAJ_SUCCESS(err)) + goto fail; - DEBUG(E_USER_STRAT, "%s():%d", __FUNCTION__, __LINE__); - strat_hardstop(); + DEBUG(E_USER_STRAT, "%s():%d", __FUNCTION__, __LINE__); + strat_hardstop(); #ifdef HOST_VERSION - time_wait_ms(2000); + 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); + 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)); + 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; - i2c_cobboard_set_mode(I2C_COBBOARD_MODE_EJECT); - strat_db_dump(__FUNCTION__); - time_wait_ms(2000); + if (get_cob_count() > 0) { + i2c_cobboard_set_mode(I2C_COBBOARD_MODE_EJECT); + strat_db_dump(__FUNCTION__); + time_wait_ms(2000); + } + + err = END_TRAJ; + fail: strat_event_enable(); - want_pack = 0; - return 0; + strat_want_pack = 0; + return err; } static uint8_t strat_beginning(void) @@ -351,7 +366,6 @@ static uint8_t strat_beginning(void) 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), @@ -360,30 +374,13 @@ static uint8_t strat_beginning(void) strat_set_acc(ACC_DIST, ACC_ANGLE); strat_set_speed(SPEED_CLITOID_SLOW, SPEED_ANGLE_SLOW); - l1: - 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)) { - strat_hardstop(); - time_wait_ms(2000); - goto l1; - } + if (!TRAJ_SUCCESS(err)) + return err; - l2: - 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)) { - strat_hardstop(); - time_wait_ms(2000); - goto l2; - } - - strat_eject(); - - while (1) { - strat_set_speed(SPEED_CLITOID_SLOW, SPEED_ANGLE_SLOW); - strat_harvest_circuit(); - strat_eject(); + return err; } return END_TRAJ; @@ -399,6 +396,7 @@ static uint8_t strat_beginning(void) } while (0) +#if 0 /* return true if we need to grab some more elements */ static uint8_t need_more_elements(void) { @@ -421,6 +419,7 @@ static uint8_t need_more_elements(void) return 1; } } +#endif uint8_t strat_main(void) { @@ -428,26 +427,44 @@ uint8_t strat_main(void) /* harvest the first cobs + balls */ err = strat_beginning(); + if (!TRAJ_SUCCESS(err)) + strat_unblock(); + else + err = strat_eject(); while (1) { - /* end of time exit ! */ + + /**********************/ + /* harvest on circuit */ + /**********************/ + + DEBUG(E_USER_STRAT, "start main loop"); + + err = strat_harvest_circuit(); if (err == END_TIMER) { DEBUG(E_USER_STRAT, "End of time"); strat_exit(); break; } + if (!TRAJ_SUCCESS(err)) { + strat_unblock(); + continue; + } + + /***********************/ + /* eject game elements */ + /***********************/ - if (need_more_elements() == 0) { - /* we have enough elements, go to eject */ - err = strat_eject(); - if (!TRAJ_SUCCESS(err)) - continue; + err = strat_eject(); + /* end of time exit ! */ + if (err == END_TIMER) { + DEBUG(E_USER_STRAT, "End of time"); + strat_exit(); + break; } - else { - /* harvest */ - err = strat_harvest(); - if (!TRAJ_SUCCESS(err)) - continue; + if (!TRAJ_SUCCESS(err)) { + strat_unblock(); + continue; } }