X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fmainboard%2Fstrat.c;h=883ce6533645a0b29d9c5a7ba45e9f1bdcafb409;hp=7ea7b88ab068b2465b13301ce30c92af4fdb87eb;hb=90945acf48061ebc3960195cabcb11175475e426;hpb=17aadc4c8c3e60c2b5e6bbba91c8542849addbd7 diff --git a/projects/microb2010/mainboard/strat.c b/projects/microb2010/mainboard/strat.c index 7ea7b88..883ce65 100644 --- a/projects/microb2010/mainboard/strat.c +++ b/projects/microb2010/mainboard/strat.c @@ -69,7 +69,9 @@ #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; /*************************************************************/ @@ -114,8 +116,8 @@ void strat_event_disable(void) void strat_init(void) { #ifdef HOST_VERSION - position_set(&mainboard.pos, 298.16, - COLOR_Y(308.78), COLOR_A(70.00)); + position_set(&mainboard.pos, 258., + COLOR_Y(246.), COLOR_A(45.)); #endif /* we consider that the color is correctly set */ @@ -153,18 +155,33 @@ void strat_exit(void) #endif } -/* called periodically (10ms) */ -void strat_event(void *dummy) +/* mark tomato as not present */ +static void check_tomato(void) +{ + int16_t x, y; + uint8_t i, j; + + x = position_get_x_s16(&mainboard.pos); + y = position_get_y_s16(&mainboard.pos); + + if (xycoord_to_ijcoord(&x, &y, &i, &j) < 0) + return; + + if (strat_db.wp_table[i][j].type != WP_TYPE_TOMATO) + return; + + strat_db.wp_table[i][j].present = 0; +} + +/* mark corn as not present and give correct commands to the cobboard + * for spickles */ +static void check_corn(void) { 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; - /* read sensors from ballboard */ IRQ_LOCK(flags); lcob = ballboard.lcob; @@ -186,8 +203,6 @@ void strat_event(void *dummy) 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 */ @@ -209,53 +224,65 @@ void strat_event(void *dummy) } /* control the cobboard mode for left spickle */ - if (lcob_near) { - 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); + if (lcob_near && strat_db.corn_table[lidx]->present) { + if (get_cob_count() >= 5 || strat_want_pack || strat_lpack60) { + /* nothing */ } 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); + /* 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(I2C_LEFT_SIDE); + i2c_cobboard_deploy_nomove(I2C_LEFT_SIDE); } } else { - if (get_cob_count() >= 5 || want_pack) - i2c_cobboard_pack(I2C_LEFT_SIDE); + /* no cob near us, we can pack or deploy freely */ + 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) { - if (get_cob_count() >= 5 || want_pack) { - if (strat_db.corn_table[ridx]->corn.color != I2C_COB_WHITE && - strat_db.corn_table[ridx]->corn.color != I2C_COB_BLACK) - i2c_cobboard_pack(I2C_RIGHT_SIDE); + if (rcob_near && strat_db.corn_table[ridx]->present) { + if (get_cob_count() >= 5 || strat_want_pack || strat_rpack60) { + /* nothing */ } else { - if (strat_db.corn_table[ridx]->corn.color == I2C_COB_WHITE) - i2c_cobboard_autoharvest(I2C_RIGHT_SIDE); - else if (strat_db.corn_table[ridx]->corn.color == I2C_COB_BLACK) - i2c_cobboard_deploy_nomove(I2C_RIGHT_SIDE); + /* 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(I2C_RIGHT_SIDE); + i2c_cobboard_deploy_nomove(I2C_RIGHT_SIDE); } } else { - if (get_cob_count() >= 5 || want_pack) - i2c_cobboard_pack(I2C_RIGHT_SIDE); + /* no cob near us, we can pack or deploy freely */ + if (get_cob_count() >= 5 || strat_want_pack || strat_rpack60) + i2c_cobboard_pack_weak(I2C_RIGHT_SIDE); else i2c_cobboard_deploy(I2C_RIGHT_SIDE); } +} + +/* called periodically (10ms) */ +void strat_event(void *dummy) +{ + /* ignore when strat is not running */ + if (strat_running == 0) + return; + check_tomato(); + check_corn(); /* limit speed when opponent is near */ - strat_limit_speed(); + //strat_limit_speed(); } @@ -264,16 +291,26 @@ 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_speed(600, SPEED_ANGLE_SLOW); - want_pack = 1; + /* go to eject point */ trajectory_goto_xy_abs(&mainboard.traj, 2625, COLOR_Y(1847)); - err = wait_traj_end(TRAJ_FLAGS_NO_NEAR); + err = WAIT_COND_OR_TRAJ_END(get_cob_count() >= 5, + TRAJ_FLAGS_NO_NEAR); + if (err == 0) { + strat_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); @@ -282,13 +319,16 @@ static uint8_t strat_eject(void) DEBUG(E_USER_STRAT, "%s():%d", __FUNCTION__, __LINE__); strat_hardstop(); +#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 /* half turn */ - strat_event_disable(); - 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); @@ -301,7 +341,7 @@ static uint8_t strat_eject(void) time_wait_ms(2000); strat_event_enable(); - want_pack = 0; + strat_want_pack = 0; return 0; } @@ -310,30 +350,19 @@ static uint8_t strat_beginning(void) uint8_t err; strat_set_acc(ACC_DIST, ACC_ANGLE); -#ifdef HOST_VERSION - strat_set_speed(600, SPEED_ANGLE_FAST); -#else - /* 250 */ - strat_set_speed(250, SPEED_ANGLE_FAST); -#endif - - // strat_set_speed(600, 60); /* OK */ - strat_set_speed(250, 28); /* OK */ + 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(250, SPEED_ANGLE_SLOW); + strat_set_speed(SPEED_CLITOID_SLOW, SPEED_ANGLE_SLOW); -#if 1 l1: DEBUG(E_USER_STRAT, "%s():%d count=%d", __FUNCTION__, __LINE__, get_cob_count()); - if (get_cob_count() >= 5) - strat_set_speed(600, SPEED_ANGLE_FAST); - - err = line2line(0, LINE_UP, 2, LINE_R_DOWN); + err = line2line(0, LINE_UP, 2, LINE_R_DOWN, TRAJ_FLAGS_NO_NEAR); if (!TRAJ_SUCCESS(err)) { strat_hardstop(); time_wait_ms(2000); @@ -342,78 +371,21 @@ static uint8_t strat_beginning(void) l2: DEBUG(E_USER_STRAT, "%s():%d count=%d", __FUNCTION__, __LINE__, get_cob_count()); - if (get_cob_count() >= 5) - strat_set_speed(600, SPEED_ANGLE_FAST); - - err = line2line(2, LINE_R_DOWN, 2, LINE_R_UP); + 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; } -#else -/* strat_set_speed(600, SPEED_ANGLE_FAST); */ -/* err = line2line(0, LINE_UP, 3, LINE_R_DOWN); */ -/* err = line2line(3, LINE_R_DOWN, 2, LINE_R_UP); */ -/* err = line2line(2, LINE_R_UP, 2, LINE_R_DOWN); */ -/* err = line2line(2, LINE_R_DOWN, 3, LINE_R_UP); */ -/* err = line2line(3, LINE_R_UP, 5, LINE_UP); */ -/* err = line2line(5, LINE_UP, 2, LINE_L_DOWN); */ -/* err = line2line(2, LINE_L_DOWN, 1, LINE_L_UP); */ -/* err = line2line(1, LINE_L_UP, 1, LINE_L_DOWN); */ -/* err = line2line(1, LINE_L_DOWN, 0, LINE_DOWN); */ - - strat_set_speed(300, SPEED_ANGLE_FAST); - err = line2line(0, LINE_UP, 0, LINE_R_UP); - err = line2line(0, LINE_R_UP, 1, LINE_DOWN); - err = line2line(1, LINE_DOWN, 1, LINE_L_DOWN); - err = line2line(1, LINE_L_DOWN, 0, LINE_DOWN); - wait_ms(500); - strat_hardstop(); - return END_TRAJ; -#endif - strat_eject(); - strat_set_speed(250, SPEED_ANGLE_FAST); - -#if 0 - l4: - DEBUG(E_USER_STRAT, "%s():%d count=%d", __FUNCTION__, __LINE__, get_cob_count()); - if (get_cob_count() >= 5) - strat_set_speed(600, SPEED_ANGLE_FAST); - - err = line2line(5, LINE_DOWN, 2, LINE_L_UP); - if (!TRAJ_SUCCESS(err)) { - strat_hardstop(); - time_wait_ms(2000); - goto l4; - } - - l5: - DEBUG(E_USER_STRAT, "%s():%d count=%d", __FUNCTION__, __LINE__, get_cob_count()); - if (get_cob_count() >= 5) - strat_set_speed(600, SPEED_ANGLE_FAST); - - err = line2line(2, LINE_L_UP, 2, LINE_R_UP); - if (!TRAJ_SUCCESS(err)) { - strat_hardstop(); - time_wait_ms(2000); - goto l5; + while (1) { + strat_set_speed(SPEED_CLITOID_SLOW, SPEED_ANGLE_SLOW); + strat_harvest_circuit(); + strat_eject(); } - DEBUG(E_USER_STRAT, "%s():%d count=%d", __FUNCTION__, __LINE__, get_cob_count()); - if (get_cob_count() >= 5) - strat_set_speed(600, SPEED_ANGLE_FAST); -#else - strat_harvest_circuit(); -#endif - - WAIT_COND_OR_TRAJ_END(distance_from_robot(2625, COLOR_Y(1847)) < 100, - TRAJ_FLAGS_STD); - strat_eject(); - return END_TRAJ; }