X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fmainboard%2Fstrat.c;h=883ce6533645a0b29d9c5a7ba45e9f1bdcafb409;hp=3f54716cc0294d6a6743f373fdb058f8a03c69f3;hb=90945acf48061ebc3960195cabcb11175475e426;hpb=5918edd6f4f713ef3c8b0b0020dd30a4fb8222ae diff --git a/projects/microb2010/mainboard/strat.c b/projects/microb2010/mainboard/strat.c index 3f54716..883ce65 100644 --- a/projects/microb2010/mainboard/strat.c +++ b/projects/microb2010/mainboard/strat.c @@ -1,6 +1,6 @@ -/* +/* * Copyright Droids, Microb Technology (2009) - * + * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation; either version 2 of the License, or @@ -17,7 +17,7 @@ * * Revision : $Id: strat.c,v 1.6 2009-11-08 17:24:33 zer0 Exp $ * - * Olivier MATZ + * Olivier MATZ */ #include @@ -33,7 +33,7 @@ #include #include #include -#include +#include #include #include @@ -57,191 +57,22 @@ #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" +#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 - -#ifdef TEST_BEACON - -#define BEACON_MAX_SAMPLES 100 -struct beacon_sample { - int16_t posx; - int16_t posy; - int16_t posa; - int16_t oppx; - int16_t oppy; - uint8_t time; -}; - -static struct beacon_sample beacon_sample[BEACON_MAX_SAMPLES]; -static uint8_t beacon_prev_time = 0; -static uint8_t beacon_cur_idx = 0; - -static void beacon_update_samples(void) -{ - int16_t opp_a, opp_d, opp_x, opp_y; - int8_t err; - uint8_t time; - - time = time_get_s(); - - /* one sample per second max */ - if (time <= beacon_prev_time) - return; - /* limit max number of samples */ - if (beacon_cur_idx >= BEACON_MAX_SAMPLES) - return; - - memset(&beacon_sample[beacon_cur_idx], 0, sizeof(beacon_sample[beacon_cur_idx])); - beacon_prev_time = time; - beacon_sample[beacon_cur_idx].time = time; - - /* get opponent pos; if not found, just set struct to 0 */ - err = get_opponent_xyda(&opp_x, &opp_y, &opp_d, &opp_a); - if (err == -1) - return; - - beacon_sample[beacon_cur_idx].posx = position_get_x_s16(&mainboard.pos); - beacon_sample[beacon_cur_idx].posy = position_get_y_s16(&mainboard.pos); - beacon_sample[beacon_cur_idx].posa = position_get_a_deg_s16(&mainboard.pos); - beacon_sample[beacon_cur_idx].oppx = opp_x; - beacon_sample[beacon_cur_idx].oppy = opp_y; - beacon_cur_idx++; -} - -void beacon_dump_samples(void) -{ - uint16_t i; - - for (i=0; izone->name); - - if (temple->flags & TEMPLE_F_MONOCOL) - printf_P(PSTR("MONOCOL ")); - else - printf_P(PSTR("BICOL ")); - - if (temple->flags & TEMPLE_F_ON_DISC) - printf_P(PSTR("ON_DISC ")); - else - printf_P(PSTR("ON_ZONE_0_1 ")); - - if (temple->flags & TEMPLE_F_OPPONENT) - printf_P(PSTR("OPPONENT ")); - else - printf_P(PSTR("OURS ")); - - if (temple->flags & TEMPLE_F_LINTEL) - printf_P(PSTR("LIN_ON_TOP ")); - else - printf_P(PSTR("COL_ON_TOP ")); - - printf_P(PSTR("\r\n")); - - printf_P(PSTR(" pos=(%d,%d,%d) ckpt=(%d,%d) ltime=%d\r\n"), - temple->x, temple->y, temple->a, - temple->checkpoint_x, temple->checkpoint_y, - temple->last_try_time); - printf_P(PSTR(" L: lev=%d da=%d,%d\r\n"), - temple->level_l, temple->dist_l, temple->angle_l); - printf_P(PSTR(" R: lev=%d da=%d,%d\r\n"), - temple->level_l, temple->dist_l, temple->angle_l); + strat_running = 1; } -void strat_dump_zone(struct build_zone *zone) +void strat_event_disable(void) { - if (!strat_infos.dump_enabled) - return; - - printf_P(PSTR(" zone %s: "), zone->name); - - if (zone->flags & ZONE_F_DISC) - printf_P(PSTR("DISC ")); - else if (zone->flags & ZONE_F_ZONE1) - printf_P(PSTR("ZONE1 ")); - else if (zone->flags & ZONE_F_ZONE0) - printf_P(PSTR("ZONE0 ")); - - if (zone->flags & ZONE_F_BUSY) - printf_P(PSTR("BUSY ")); - else - printf_P(PSTR("FREE ")); - - printf_P(PSTR("\r\n")); - - printf_P(PSTR(" lev=%d ckpt=(%d,%d) ltime=%d\r\n"), - zone->level, - zone->checkpoint_x, zone->checkpoint_y, - zone->last_try_time); + strat_running = 0; } -void strat_dump_static_cols(void) +/* call it just before launching the strat */ +void strat_init(void) { - if (!strat_infos.dump_enabled) - return; +#ifdef HOST_VERSION + position_set(&mainboard.pos, 258., + COLOR_Y(246.), COLOR_A(45.)); +#endif - printf_P(PSTR(" static cols: l0=%d l1=%d l2=%d\r\n"), - strat_infos.s_cols.flags & STATIC_COL_LINE0_DONE, - strat_infos.s_cols.flags & STATIC_COL_LINE1_DONE, - strat_infos.s_cols.flags & STATIC_COL_LINE2_DONE); + /* we consider that the color is correctly set */ + strat_running = 1; + strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST); + time_reset(); + interrupt_traj_reset(); + + i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST); + i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_HARVEST); + + /* used in strat_base for END_TIMER */ + mainboard.flags = DO_ENCODERS | DO_CS | DO_RS | + DO_POS | DO_BD | DO_TIMER | DO_POWER; } -void strat_dump_col_disp(void) +/* call it after each strat */ +void strat_exit(void) { - if (!strat_infos.dump_enabled) - return; +#ifndef HOST_VERSION + uint8_t flags; +#endif - printf_P(PSTR(" c1 cnt=%d ltt=%d\r\n"), - strat_infos.c1.count, strat_infos.c1.last_try_time); - printf_P(PSTR(" c2 cnt=%d ltt=%d\r\n"), - strat_infos.c2.count, strat_infos.c2.last_try_time); - printf_P(PSTR(" c3 cnt=%d ltt=%d\r\n"), - strat_infos.c3.count, strat_infos.c3.last_try_time); + strat_running = 0; + mainboard.flags &= ~(DO_TIMER); + strat_hardstop(); + time_reset(); + 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); +#endif } -void strat_dump_lin_disp(void) +/* mark tomato as not present */ +static void check_tomato(void) { - if (!strat_infos.dump_enabled) + 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; - printf_P(PSTR(" l1 cnt=%d ltt=%d\r\n"), - strat_infos.l1.count, strat_infos.l1.last_try_time); - printf_P(PSTR(" l2 cnt=%d ltt=%d\r\n"), - strat_infos.l2.count, strat_infos.l2.last_try_time); + strat_db.wp_table[i][j].present = 0; } -void strat_dump_all_temples(void) +/* mark corn as not present and give correct commands to the cobboard + * for spickles */ +static void check_corn(void) { - struct temple *temple; - uint8_t i; + uint8_t flags; + int8_t lcob_near, rcob_near; + uint8_t lcob, rcob; + uint8_t lidx, ridx; - if (!strat_infos.dump_enabled) - return; + /* read sensors from ballboard */ + IRQ_LOCK(flags); + lcob = ballboard.lcob; + ballboard.lcob = I2C_COB_NONE; + rcob = ballboard.rcob; + ballboard.rcob = I2C_COB_NONE; + IRQ_UNLOCK(flags); - for (i=0; iflags & TEMPLE_F_VALID)) - continue; - strat_dump_temple(temple); +/* 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 */ + 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); } -} -void strat_dump_all_zones(void) -{ - struct build_zone *zone; - uint8_t i; + /* 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); + } - if (!strat_infos.dump_enabled) - return; + /* control the cobboard mode for left spickle */ + if (lcob_near && strat_db.corn_table[lidx]->present) { + if (get_cob_count() >= 5 || strat_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); + } + } + else { + /* 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); + } - for (i=0; iflags & ZONE_F_VALID)) - continue; - strat_dump_zone(zone); + /* control the cobboard mode for right spickle */ + if (rcob_near && strat_db.corn_table[ridx]->present) { + if (get_cob_count() >= 5 || strat_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 || strat_want_pack || strat_rpack60) + i2c_cobboard_pack_weak(I2C_RIGHT_SIDE); + else + i2c_cobboard_deploy(I2C_RIGHT_SIDE); } } -/* display current information about the state of the game */ -void strat_dump_infos(const char *caller) +/* called periodically (10ms) */ +void strat_event(void *dummy) { - if (!strat_infos.dump_enabled) + /* ignore when strat is not running */ + if (strat_running == 0) return; - printf_P(PSTR("%s() dump strat infos:\r\n"), caller); - strat_dump_static_cols(); - strat_dump_col_disp(); - strat_dump_lin_disp(); - strat_dump_all_temples(); - strat_dump_all_zones(); + check_tomato(); + check_corn(); + /* limit speed when opponent is near */ + //strat_limit_speed(); } -/* init current area state before a match. Dump update user conf - * here */ -void strat_reset_infos(void) + +static uint8_t strat_harvest(void) { - uint8_t i; - - /* /!\ don't do a big memset() as there is static data */ - strat_infos.s_cols.flags = 0; - strat_infos.c1.count = 5; - strat_infos.c1.last_try_time = 0; - strat_infos.c2.count = 5; - strat_infos.c2.last_try_time = 0; - strat_infos.c3.count = 5; - strat_infos.c3.last_try_time = 0; - strat_infos.l1.count = 1; - strat_infos.l1.last_try_time = 0; - strat_infos.l2.count = 1; - strat_infos.l2.last_try_time = 0; - - strat_infos.taken_lintel = 0; - strat_infos.col_in_boobs = 0; - strat_infos.lazy_pickup_done = 0; - strat_infos.i2c_loaded_skipped = 0; - - memset(strat_infos.temple_list, 0, sizeof(strat_infos.temple_list)); - - for (i=0; i= 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); + } - strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST); - time_reset(); - interrupt_traj_reset(); + /* pack arms */ + strat_event_disable(); + i2c_cobboard_pack(I2C_LEFT_SIDE); + i2c_cobboard_pack(I2C_RIGHT_SIDE); - /* used in strat_base for END_TIMER */ - mainboard.flags = DO_ENCODERS | DO_CS | DO_RS | - DO_POS | DO_BD | DO_TIMER | DO_POWER; + /* 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); -#ifdef TEST_BEACON - beacon_prev_time = 0; - beacon_cur_idx = 0; + 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 */ + trajectory_a_rel(&mainboard.traj, COLOR_A(180)); + err = wait_traj_end(END_INTR|END_TRAJ); -/* call it after each strat */ -void strat_exit(void) -{ - uint8_t flags; + /* cob ejection */ + trajectory_d_rel(&mainboard.traj, -70); + err = wait_traj_end(END_INTR|END_TRAJ); - pickup_wheels_off(); - mainboard.flags &= ~(DO_TIMER); - strat_hardstop(); - time_reset(); - wait_ms(1000); - IRQ_LOCK(flags); - mainboard.flags &= ~(DO_CS); - pwm_ng_set(LEFT_PWM, 0); - pwm_ng_set(RIGHT_PWM, 0); - IRQ_UNLOCK(flags); -} + i2c_cobboard_set_mode(I2C_COBBOARD_MODE_EJECT); + strat_db_dump(__FUNCTION__); + time_wait_ms(2000); -/* called periodically */ -void strat_event(void *dummy) -{ - /* limit speed when opponent is close */ - strat_limit_speed(); - -#ifdef TEST_BEACON - beacon_update_samples(); -#endif + strat_event_enable(); + strat_want_pack = 0; + return 0; } -/* do static cols + first temples */ static uint8_t strat_beginning(void) { uint8_t err; - /* don't limit the speed when opponent is near: it can change - * the radius of the curves */ - strat_limit_speed_disable(); - - err = strat_static_columns(0); - - strat_limit_speed_enable(); - - if (!TRAJ_SUCCESS(err)) - return err; + strat_set_acc(ACC_DIST, ACC_ANGLE); + strat_set_speed(600, 60); /* OK */ + //strat_set_speed(250, 28); /* OK */ - /* go to disc to build the first temple */ + trajectory_d_a_rel(&mainboard.traj, 1000, COLOR_A(20)); + err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj), + TRAJ_FLAGS_STD); - /* XXX if opponent is near disc, go to zone1 */ - err = strat_goto_disc(2); - if (!TRAJ_SUCCESS(err)) - return err; - DEBUG(E_USER_STRAT, "disc reached"); + strat_set_acc(ACC_DIST, ACC_ANGLE); + strat_set_speed(SPEED_CLITOID_SLOW, SPEED_ANGLE_SLOW); - /* can return END_ERROR or END_TIMER, should not happen - * here */ - err = strat_build_new_temple(&strat_infos.zone_list[0]); - if (!TRAJ_SUCCESS(err)) - return err; - - /* bypass static2 if specified */ - if (strat_infos.conf.flags & STRAT_CONF_BYPASS_STATIC2) { - err = strat_escape(&strat_infos.zone_list[0], TRAJ_FLAGS_STD); - return err; + 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; } - /* get the last 2 columns, and build them on previous temple */ - err = strat_static_columns_pass2(); - if (!TRAJ_SUCCESS(err)) - return err; - - /* early opponent scan, for offensive strategy */ - if (strat_infos.conf.flags & STRAT_CONF_EARLY_SCAN) { - err = strat_pickup_lintels(); - /* ignore code */ - - /* try to build on opponent (scan must be enabled) */ - err = strat_build_on_opponent_temple(); - /* ignore code */ + 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; } - return err; -} + strat_eject(); -/* return true if we need to grab some more elements (lintel/cols) */ -uint8_t need_more_elements(void) -{ - if (time_get_s() <= 75) { - /* we have at least one col on each arm, build now */ - if ((get_column_count_left() >= 1) && - (get_column_count_right() >= 1)) - return 0; - } - else { - if (get_column_count()) - return 0; + while (1) { + strat_set_speed(SPEED_CLITOID_SLOW, SPEED_ANGLE_SLOW); + strat_harvest_circuit(); + strat_eject(); } - return 1; + + return END_TRAJ; } /* dump state (every 5 s max) */ @@ -684,124 +399,57 @@ uint8_t need_more_elements(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; + } +} + uint8_t strat_main(void) { uint8_t err; - struct temple *temple = NULL; - struct build_zone *zone = NULL; - - uint8_t last_print_cols = 0; - uint8_t last_print_lin = 0; - uint8_t last_print_temple = 0; - uint8_t last_print_zone = 0; - /* do static cols + first temple */ + /* harvest the first cobs + balls */ err = strat_beginning(); - /* skip error code */ - while (1) { - + /* end of time exit ! */ if (err == END_TIMER) { DEBUG(E_USER_STRAT, "End of time"); strat_exit(); break; } - /* we have at least one col on each arm, build now */ if (need_more_elements() == 0) { - - /* try to build on opponent, will return - * END_TRAJ without doing anything if - * disabled */ - err = strat_build_on_opponent_temple(); + /* we have enough elements, go to eject */ + err = strat_eject(); if (!TRAJ_SUCCESS(err)) continue; - if (need_more_elements()) - continue; - - /* try to scan and build on our temple, will - * return END_TRAJ without doing anything if - * disabled */ - err = strat_check_temple_and_build(); - if (!TRAJ_SUCCESS(err)) - continue; - if (need_more_elements()) - continue; - - /* Else, do a simple build, as before */ - - temple = strat_get_best_temple(); - - /* one valid temple found */ - if (temple) { - DUMP_RATE_LIMIT(strat_dump_all_temples, last_print_temple); - - err = strat_goto_temple(temple); - if (!TRAJ_SUCCESS(err)) - continue; - - /* can return END_ERROR or END_TIMER, - * should not happen here */ - err = strat_grow_temple(temple); - if (!TRAJ_SUCCESS(err)) - continue; - - err = strat_escape(temple->zone, TRAJ_FLAGS_STD); - if (!TRAJ_SUCCESS(err)) - continue; - - continue; - } - - zone = strat_get_best_zone(); - if (zone) { - DUMP_RATE_LIMIT(strat_dump_all_zones, last_print_zone); - - DEBUG(E_USER_STRAT, "goto zone %s", zone->name); - err = strat_goto_build_zone(zone, zone->level); - if (!TRAJ_SUCCESS(err)) - continue; - DEBUG(E_USER_STRAT, "zone reached"); - - /* no error code except END_ERROR, should not happen */ - err = strat_build_new_temple(zone); - - err = strat_escape(zone, TRAJ_FLAGS_STD); - if (!TRAJ_SUCCESS(err)) - continue; - - continue; - } - - /* XXX hey what can we do here... :'( */ - DEBUG(E_USER_STRAT, "panic :)"); - time_wait_ms(1000); - continue; } - - /* else we need some elements (lintels, then columns) */ else { - if (strat_infos.l1.count != 0 && strat_infos.l2.count != 0) - DUMP_RATE_LIMIT(strat_dump_lin_disp, last_print_lin); - - err = strat_pickup_lintels(); - /* can return an error code, but we have - * nothing to do because pickup_column() - * starts with a goto_and_avoid() */ - if (!TRAJ_SUCCESS(err)) - nop(); - - DUMP_RATE_LIMIT(strat_dump_col_disp, last_print_cols); - - err = strat_pickup_columns(); + /* harvest */ + err = strat_harvest(); if (!TRAJ_SUCCESS(err)) - nop(); /* nothing to do */ - - /* XXX check here that we have elements, or do - * something else */ - /* if we cannot take elements, try to build */ + continue; } } - return END_TRAJ; + + return err; }