X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fmainboard%2Fstrat.c;h=2c6042828a6b74e1593388d8e8b7acdfc5acc315;hp=c253e5f63c9f7d06eac6f5d391df44bb27bb6888;hb=9d056416278ae5d772a0fc1f7d2d11a4461fb673;hpb=78150017ab8c5615af414df706a0525fe7c262ae diff --git a/projects/microb2010/mainboard/strat.c b/projects/microb2010/mainboard/strat.c index c253e5f..2c60428 100644 --- a/projects/microb2010/mainboard/strat.c +++ b/projects/microb2010/mainboard/strat.c @@ -57,6 +57,7 @@ #include "i2c_protocol.h" #include "main.h" #include "strat.h" +#include "strat_db.h" #include "strat_base.h" #include "strat_corn.h" #include "strat_utils.h" @@ -66,16 +67,7 @@ #define COL_DISP_MARGIN 400 /* stop 40 cm in front of dispenser */ #define COL_SCAN_PRE_MARGIN 250 -struct strat_infos strat_infos = { - /* conf */ - .conf = { - .flags = 0, - }, - /* status */ - .status = { - .flags = 0, - }, -}; +struct strat_conf strat_conf; /*************************************************************/ @@ -83,27 +75,6 @@ struct strat_infos strat_infos = { /*************************************************************/ -void strat_set_bounding_box(void) -{ - if (get_color() == I2C_COLOR_YELLOW) { - strat_infos.area_bbox.x1 = 300; - strat_infos.area_bbox.y1 = 200; - strat_infos.area_bbox.x2 = 2720; /* needed for c1 */ - strat_infos.area_bbox.y2 = 1800; - } - else { - strat_infos.area_bbox.x1 = 200; - strat_infos.area_bbox.y1 = 300; - strat_infos.area_bbox.x2 = 2720; /* needed for c1 */ - strat_infos.area_bbox.y2 = 1900; - } - - polygon_set_boundingbox(strat_infos.area_bbox.x1, - strat_infos.area_bbox.y1, - strat_infos.area_bbox.x2, - strat_infos.area_bbox.y2); -} - /* called before each strat, and before the start switch */ void strat_preinit(void) { @@ -113,41 +84,28 @@ void strat_preinit(void) DO_POS | DO_BD | DO_POWER; //i2c_cobboard_mode_init(); - strat_dump_conf(); - strat_dump_infos(__FUNCTION__); + strat_conf_dump(__FUNCTION__); + strat_db_dump(__FUNCTION__); } -void strat_dump_conf(void) +void strat_conf_dump(const char *caller) { - if (!strat_infos.dump_enabled) + if (!strat_conf.dump_enabled) return; printf_P(PSTR("-- conf --\r\n")); } -/* display current information about the state of the game */ -void strat_dump_infos(const char *caller) -{ - if (!strat_infos.dump_enabled) - return; - - printf_P(PSTR("%s() dump strat infos:\r\n"), caller); -} - -/* init current area state before a match. Dump update user conf - * here */ -void strat_reset_infos(void) -{ - init_corn_table(-1, -1); -} - /* call it just before launching the strat */ void strat_init(void) { + position_set(&mainboard.pos, 298.16, + COLOR_Y(308.78), COLOR_A(70.00)); + /* XXX init rollers, .. */ - strat_reset_infos(); + strat_db_init(); /* we consider that the color is correctly set */ @@ -155,8 +113,9 @@ void strat_init(void) time_reset(); interrupt_traj_reset(); - i2c_cobboard_mode_harvest(I2C_LEFT_SIDE); - i2c_cobboard_mode_harvest(I2C_RIGHT_SIDE); + 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 */ @@ -168,33 +127,45 @@ void strat_init(void) /* call it after each strat */ void strat_exit(void) { +#ifndef HOST_VERSION uint8_t flags; +#endif mainboard.flags &= ~(DO_TIMER); strat_hardstop(); time_reset(); - wait_ms(1000); + wait_ms(100); +#ifndef HOST_VERSION IRQ_LOCK(flags); mainboard.flags &= ~(DO_CS); + IRQ_UNLOCK(flags); pwm_ng_set(LEFT_PWM, 0); pwm_ng_set(RIGHT_PWM, 0); - IRQ_UNLOCK(flags); +#endif } /* called periodically */ void strat_event(void *dummy) { -#if 0 - /* pack or deploy spickle */ - if (strat_infos.status.flags & STRAT_STATUS_LHARVEST) { - if (sensor_get(S_LCOB_PRESENT)) { - if (sensor_get(S_LCOB_WHITE)) - i2c_ballboard_set_mode(); - else - ; - } - } -#endif + uint8_t flags; + uint8_t lcob, rcob; + + IRQ_LOCK(flags); + lcob = ballboard.lcob; + ballboard.lcob = I2C_COB_NONE; + rcob = ballboard.rcob; + 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"); + /* limit speed when opponent is close */ strat_limit_speed(); } @@ -203,12 +174,73 @@ 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); - //init_corn_table(0, 0); +#endif + + + // 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); + +#if 1 + l1: + 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); + 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); + if (!TRAJ_SUCCESS(err)) { + trajectory_hardstop(&mainboard.traj); + 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); + 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 */ @@ -219,17 +251,18 @@ static uint8_t strat_beginning(void) /* half turn */ trajectory_goto_xy_abs(&mainboard.traj, 2625, COLOR_Y(1847)); err = wait_traj_end(END_INTR|END_TRAJ); - i2c_cobboard_mode_pack(I2C_LEFT_SIDE); - i2c_cobboard_mode_pack(I2C_RIGHT_SIDE); + 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_mode_eject(); + i2c_cobboard_set_mode(I2C_COBBOARD_MODE_EJECT); time_wait_ms(2000); + trajectory_hardstop(&mainboard.traj); return END_TRAJ; } @@ -243,12 +276,67 @@ static uint8_t strat_beginning(void) } while (0) +/* return true if we need to grab some more elements */ +static uint8_t need_more_elements(void) +{ + if (time_get_s() <= 75) { + /* we have enough time left */ + if (get_ball_count() >= 4) + return 0; + if (get_cob_count() >= 4) + return 0; + if ((get_ball_count() >= 2) && + (get_cob_count() >= 2)) + return 0; + return 1; + } + else { + /* not much time remaining */ + if ((get_ball_count() >= 1) && + (get_cob_count() >= 1)) + return 0; + return 1; + } +} + +static uint8_t strat_harvest(void) +{ + return 0; +} + +static uint8_t strat_eject(void) +{ + return 0; +} + uint8_t strat_main(void) { uint8_t err; - /* */ + /* harvest the first cobs + balls */ err = strat_beginning(); + while (1) { + /* end of time exit ! */ + if (err == END_TIMER) { + DEBUG(E_USER_STRAT, "End of time"); + strat_exit(); + break; + } + + if (need_more_elements() == 0) { + /* we have enough elements, go to eject */ + err = strat_eject(); + if (!TRAJ_SUCCESS(err)) + continue; + } + else { + /* harvest */ + err = strat_harvest(); + if (!TRAJ_SUCCESS(err)) + continue; + } + } + return err; }