X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fmainboard%2Fstrat_building.c;fp=projects%2Fmicrob2010%2Fmainboard%2Fstrat_building.c;h=9abdb5a22bb1c987cf600b9f0cee40cb6a88f32e;hp=0000000000000000000000000000000000000000;hb=5918edd6f4f713ef3c8b0b0020dd30a4fb8222ae;hpb=9d2d9100592e18fed985730298215884127fc568 diff --git a/projects/microb2010/mainboard/strat_building.c b/projects/microb2010/mainboard/strat_building.c new file mode 100644 index 0000000..9abdb5a --- /dev/null +++ b/projects/microb2010/mainboard/strat_building.c @@ -0,0 +1,907 @@ +/* + * Copyright Droids, Microb Technology (2008) + * + * 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 + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + * Revision : $Id: strat_building.c,v 1.5 2009-11-08 17:24:33 zer0 Exp $ + * + * Olivier MATZ + */ + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + + +#include "../common/i2c_commands.h" +#include "main.h" +#include "cmdline.h" +#include "i2c_protocol.h" +#include "strat.h" +#include "strat_base.h" +#include "strat_utils.h" +#include "strat_avoid.h" +#include "sensor.h" + + +#define DISC_DIST_NEED_GOTO_AVOID 1000 +#define DISC_DIST_PREPARE_BUILD 700 +#define DISC_DIST_SLOW 500 + +#define ERROUT(e) do { \ + err = e; \ + goto end; \ + } while(0) + +static uint8_t is_ready_for_prepare_build(void) +{ + double d, a; + if (distance_from_robot(CENTER_X, CENTER_Y) > + DISC_DIST_PREPARE_BUILD) + return 0; + abs_xy_to_rel_da(CENTER_X, CENTER_Y, &d, &a); + if (a < RAD(-30)) + return 0; + if (a > RAD(30)) + return 0; + return 1; +} + +/* go to the nearest place on the disc. Also prepare the arms for + * building at the correct level. If level==-1, don't move the + * arms. */ +uint8_t strat_goto_disc(int8_t level) +{ + uint8_t err; + uint16_t old_spdd, old_spda; + double d, a, x, y; + + DEBUG(E_USER_STRAT, "%s()", __FUNCTION__); + + strat_get_speed(&old_spdd, &old_spda); + strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST); + + /* workaround for some static cols configurations */ + if ((strat_infos.conf.flags & STRAT_CONF_EARLY_SCAN) == 0) { + if (time_get_s() > 15) + i2c_mechboard_mode_loaded(); + } + + /* if we are far from the disc, goto backward faster */ + abs_xy_to_rel_da(CENTER_X, CENTER_Y, &d, &a); + if (d > DISC_DIST_NEED_GOTO_AVOID) { + rel_da_to_abs_xy(d - DISC_DIST_PREPARE_BUILD, a, &x, &y); + err = goto_and_avoid(x, y, TRAJ_FLAGS_STD, + TRAJ_FLAGS_NO_NEAR); + if (!TRAJ_SUCCESS(err)) + ERROUT(err); + } + +#ifdef HOMOLOGATION + { + int16_t opp_d, opp_a; + trajectory_turnto_xy(&mainboard.traj, + CENTER_X, CENTER_Y); + err = wait_traj_end(TRAJ_FLAGS_NO_NEAR); + + time_wait_ms(500); + + err = get_opponent_da(&opp_d, &opp_a); + if (err == 0 && opp_d < 600 && + (opp_a > 325 || opp_a < 35)) + return END_ERROR; + } +#endif + + trajectory_goto_forward_xy_abs(&mainboard.traj, + CENTER_X, CENTER_Y); + err = WAIT_COND_OR_TRAJ_END(is_ready_for_prepare_build(), + TRAJ_FLAGS_NO_NEAR); + + if (err == END_BLOCKING) + ERROUT(END_BLOCKING); + if (TRAJ_SUCCESS(err)) /* should not reach dest */ + ERROUT(END_ERROR); + + strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW); + if (level != -1) + i2c_mechboard_mode_prepare_build_both(level); + + err = WAIT_COND_OR_TRAJ_END(distance_from_robot(CENTER_X, + CENTER_Y) < DISC_DIST_SLOW, + TRAJ_FLAGS_NO_NEAR); + + if (err == END_BLOCKING) + ERROUT(END_BLOCKING); + if (TRAJ_SUCCESS(err)) /* should not reach dest */ + ERROUT(END_ERROR); + + strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_VERY_SLOW); + err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + + if (err == END_BLOCKING) + ERROUT(END_TRAJ); + if (TRAJ_SUCCESS(err)) /* should not reach dest */ + ERROUT(END_ERROR); + + ERROUT(err); + end: + strat_set_speed(old_spdd, old_spda); + return err; +} + +/* must be called from the checkpoint before zone 1. */ +static uint8_t strat_goto_build_zone1_near(uint8_t level) +{ + uint8_t err; + + /* turn to build zone */ + strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST); + trajectory_a_abs(&mainboard.traj, COLOR_A(90)); + err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + if (!TRAJ_SUCCESS(err)) + return err; + + /* move forward to reach the build zone */ + strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_SLOW); + i2c_mechboard_mode_prepare_build_both(level); + err = strat_calib(500, TRAJ_FLAGS_SMALL_DIST); + if (err == END_BLOCKING) { + err = END_TRAJ; + } + + DEBUG(E_USER_STRAT, "build zone reached"); + return err; +} + +/* must be called from the checkpoint before zone 0 */ +static uint8_t strat_goto_build_zone0_near(uint8_t level) +{ + uint8_t err; +#ifdef OLD_STYLE + int16_t cur_y, diff_y, dst_y; +#endif + + /* turn to build zone */ + strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST); + trajectory_a_abs(&mainboard.traj, COLOR_A(90)); + err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + if (!TRAJ_SUCCESS(err)) + return err; + +#ifdef OLD_STYLE + cur_y = position_get_y_s16(&mainboard.pos); + dst_y = COLOR_Y(AREA_Y - (ROBOT_LENGTH/2) - 100); + diff_y = ABS(cur_y - dst_y); + + /* move forward to reach the build zone */ + strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_SLOW); + i2c_mechboard_mode_prepare_build_both(level); + trajectory_d_rel(&mainboard.traj, diff_y); + err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + if (err == END_BLOCKING) { /* not very good for z0 but... */ + err = END_TRAJ; + } +#else + /* move forward to reach the build zone */ + strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_SLOW); + i2c_mechboard_mode_prepare_build_both(level); + err = strat_calib(500, TRAJ_FLAGS_SMALL_DIST); + if (err == END_BLOCKING) { + err = END_TRAJ; + } +#endif + + DEBUG(E_USER_STRAT, "build zone reached"); + return err; +} + +/* Go to any build zone: disc, 1a or 1b. Doesn't work with zone 0 for + * now... */ +uint8_t strat_goto_build_zone(struct build_zone *zone, uint8_t level) +{ + uint8_t err = END_TRAJ; + uint16_t old_spdd, old_spda; + int16_t checkpoint_x, checkpoint_y; + int16_t errx; + + zone->last_try_time = time_get_s(); + + if (zone->flags & ZONE_F_DISC) + return strat_goto_disc(level); + + DEBUG(E_USER_STRAT, "goto build zone x=%d", zone->checkpoint_x); + + /* workaround for some static cols configurations */ + if (time_get_s() > 15) + i2c_mechboard_mode_loaded(); + + strat_get_speed(&old_spdd, &old_spda); + strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST); + + checkpoint_x = zone->checkpoint_x; + checkpoint_y = COLOR_Y(zone->checkpoint_y); + errx = position_get_x_s16(&mainboard.pos) - checkpoint_x; + + /* goto checkpoint if we are too far from it, or if error on x + * is too big. */ + if (distance_from_robot(checkpoint_x, checkpoint_y) > 300 || + ABS(errx) > 15) { + err = goto_and_avoid(checkpoint_x, checkpoint_y, + TRAJ_FLAGS_STD, + TRAJ_FLAGS_NO_NEAR); + if (!TRAJ_SUCCESS(err)) + ERROUT(err); + } + + if (zone->flags & ZONE_F_ZONE1) + err = strat_goto_build_zone1_near(level); + else if (zone->flags & ZONE_F_ZONE0) + err = strat_goto_build_zone0_near(level); + if (!TRAJ_SUCCESS(err)) + ERROUT(err); + + end: + strat_set_speed(old_spdd, old_spda); + return err; +} + +/* return a free temple structure */ +struct temple *strat_get_free_temple(void) +{ + uint8_t i; + + for (i=0; i col_r ? col_l : col_r); + lintel = (get_lintel_count() > 0); + + if (strat_infos.conf.flags & STRAT_CONF_ONLY_ONE_ON_DISC) { + if ((temple->level_r > 5) && + (temple->flags & TEMPLE_F_ON_DISC)) + return 0; + } + + /* return symetric temples only */ + if (temple->level_l != temple->level_r) + return 0; + + if ((time_get_s() - temple->last_try_time) < TEMPLE_DISABLE_TIME) + return 0; + + /* we could do better to work on non-symetric temples */ + if (temple->level_l + max_col + lintel > 9) + return 0; + + if (temple->flags & TEMPLE_F_MONOCOL) + return 0; + + /* XXX don't allow to build on opponent temple. For that we + * must support the little back_mm. */ + if (temple->flags & TEMPLE_F_OPPONENT) + return 0; + + return 1; +} + + +/* return the best existing temple for building */ +struct temple *strat_get_best_temple(void) +{ + uint8_t i; + struct temple *best = NULL; + struct temple *temple = NULL; + + for (i=0; iflags & TEMPLE_F_VALID)) + continue; + + if (strat_can_build_on_temple(temple) == 0) + continue; + + if (best == NULL) { + best = temple; + continue; + } + + /* take the higher temple between 'best' and 'temple' */ + if (best->level_l < temple->level_l) + best = temple; + } + + DEBUG(E_USER_STRAT, "%s() return %p", __FUNCTION__, best); + return best; +} + +/* return the temple we built on the disc if any. If valid == 1, the + * temple must be buildable. */ +struct temple *strat_get_our_temple_on_disc(uint8_t valid) +{ + uint8_t i; + struct temple *temple = NULL; + + if (strat_infos.conf.flags & STRAT_CONF_ONLY_ONE_ON_DISC) { + return NULL; + } + + for (i=0; iflags & TEMPLE_F_VALID)) + continue; + + if (valid == 1 && strat_can_build_on_temple(temple) == 0) + continue; + + if (temple->flags & TEMPLE_F_ON_DISC) + return temple; + } + return NULL; +} + +#define COL_MAX(x,y) (((x)>(y)) ? (x) : (y)) + +#define TIME_FOR_LINTEL 3000L +#define TIME_FOR_BUILD 0L +#define TIME_FOR_COL 800L +#define TIME_MARGIN 2000L + +#define CHECKPOINT_DISC_DIST 380 +#define CHECKPOINT_OTHER_DIST 200 +/* Grow a temple. It will update temple list. */ +uint8_t strat_grow_temple(struct temple *temple) +{ + double checkpoint_x, checkpoint_y; + uint8_t add_level = 0; + uint8_t do_lintel = 1; + uint8_t col_l, col_r, col_max; + uint8_t err; + uint16_t timeout; + + /* XXX temple must be symetric */ + uint8_t level = temple->level_l; + + DEBUG(E_USER_STRAT, "%s()", __FUNCTION__); + + if (temple->level_l >= 9) + return END_ERROR; + + if ( (temple->zone->flags & ZONE_F_ZONE1) || + (temple->zone->flags & ZONE_F_ZONE0) ) { + strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW); + trajectory_d_rel(&mainboard.traj, -17); + wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + } + + col_l = get_column_count_left(); + col_r = get_column_count_right(); + + if (time_get_s() < 75) { + /* make temple symetric: if we have 1 col on left and 2 cols + * on right, only build one on both sides. */ + if (col_l > col_r) { + col_r = col_l; + do_lintel = 0; + } + if (col_r > col_l) { + col_r = col_l; + do_lintel = 0; + } + if (get_lintel_count() == 0) + do_lintel = 0; + } + else if (col_l != col_r) + do_lintel = 0; + + if (col_l == 0 || col_r == 0) { + if (temple->flags & TEMPLE_F_LINTEL) + do_lintel = 0; + } + + if (col_l == 0 && col_r == 0 && do_lintel == 0) { + DEBUG(E_USER_STRAT, "nothing to do"); + return END_ERROR; + } + + add_level = do_lintel + col_l; + while (level + add_level > 9) { + if (do_lintel) { + do_lintel = 0; + add_level = do_lintel + col_l; + continue; + } + /* we know col_r and col_l are > 0 */ + col_r--; + col_l--; + } + + col_max = COL_MAX(col_r, col_l); + + /* Reduce nb elts if we don't have time */ + timeout = (!!col_max) * TIME_FOR_BUILD; + timeout += col_max * TIME_FOR_COL; + timeout += do_lintel * TIME_FOR_LINTEL; + if ((timeout / 1000L) + time_get_s() > 89 && do_lintel) { + do_lintel = 0; + timeout -= TIME_FOR_LINTEL; + } + if ((timeout / 1000L) + time_get_s() > 89 && col_max) { + if (col_r > 0) + col_r--; + if (col_l > 0) + col_l--; + col_max--; + timeout -= TIME_FOR_COL; + } + + /* take a margin for timeout */ + timeout += (!!col_max) * TIME_MARGIN; + + if (col_l == 0 && col_r == 0 && do_lintel == 0) { + DEBUG(E_USER_STRAT, "nothing to do (2)"); + return END_ERROR; + } + + DEBUG(E_USER_STRAT, "Autobuild: left=%d,%d right=%d,%d lintel=%d", + level, col_l, level, col_r, do_lintel); + + i2c_mechboard_mode_autobuild(level, col_l, I2C_AUTOBUILD_DEFAULT_DIST, + level, col_r, I2C_AUTOBUILD_DEFAULT_DIST, + do_lintel); + WAIT_COND_OR_TIMEOUT(get_mechboard_mode() == + I2C_MECHBOARD_MODE_AUTOBUILD, 100); + err = WAIT_COND_OR_TIMEOUT(get_mechboard_mode() != + I2C_MECHBOARD_MODE_AUTOBUILD, timeout); + if (err == 0) { + DEBUG(E_USER_STRAT, "timeout building temple (timeout was %d)", timeout); + temple->flags = 0; /* remove temple from list */ + return END_TRAJ; + } + else + DEBUG(E_USER_STRAT, "temple built"); + + /* position of the robot when build the new temple */ + temple->x = position_get_x_s16(&mainboard.pos); + temple->y = position_get_y_s16(&mainboard.pos); + temple->a = position_get_a_deg_s16(&mainboard.pos); + + /* checkpoint is a bit behind us */ + if (temple->zone->flags & ZONE_F_DISC) { + rel_da_to_abs_xy(CHECKPOINT_DISC_DIST, M_PI, + &checkpoint_x, &checkpoint_y); + } + else { + rel_da_to_abs_xy(CHECKPOINT_OTHER_DIST, M_PI, + &checkpoint_x, &checkpoint_y); + } + temple->checkpoint_x = checkpoint_x; + temple->checkpoint_y = checkpoint_y; + + temple->level_l = level + add_level; + temple->dist_l = 0; + temple->angle_l = 0; + + temple->level_r = level + add_level; + temple->dist_r = 0; + temple->angle_r = 0; + + temple->flags = TEMPLE_F_VALID; + + if (distance_from_robot(CENTER_X, CENTER_Y) < 400) + temple->flags |= TEMPLE_F_ON_DISC; + + if (do_lintel) + temple->flags |= TEMPLE_F_LINTEL; + + /* we must push the temple */ + if ( ((temple->zone->flags & ZONE_F_ZONE1) || + (temple->zone->flags & ZONE_F_ZONE0)) && + level <= 1) { + strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW); + trajectory_d_rel(&mainboard.traj, -100); + wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + i2c_mechboard_mode_push_temple(level); + time_wait_ms(400); + strat_set_speed(200, SPEED_ANGLE_SLOW); + trajectory_d_rel(&mainboard.traj, 100); + wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + } + + /* Special case for big 3 */ + if (strat_infos.col_in_boobs) { + uint16_t old_spdd, old_spda; + strat_get_speed(&old_spdd, &old_spda); + strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_SLOW); + DEBUG(E_USER_STRAT, "%s() big 3", __FUNCTION__); + strat_infos.col_in_boobs = 0; + trajectory_d_rel(&mainboard.traj, -120); + wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + i2c_mechboard_mode_prepare_pickup_next(I2C_AUTO_SIDE, + I2C_MECHBOARD_MODE_CLEAR); + WAIT_COND_OR_TIMEOUT(get_column_count() >= 2, 4000L); + i2c_mechboard_mode_prepare_build_both(level + add_level); + time_wait_ms(800); + strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW); + trajectory_d_rel(&mainboard.traj, 120); + wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + err = strat_grow_temple(temple); + strat_set_speed(old_spdd, old_spda); + return err; + } + + return END_TRAJ; +} + +#define COL_BACK_DIST 70 +#define COL_ANGLE 20 +#define COL_ARM_DIST 220 + +#define COL_BACK_DIST_ZONE1 35 +#define COL_ARM_DIST_ZONE1 230 +#define COL_ANGLE_ZONE1 19 + +static uint8_t try_build_col(uint8_t l, uint8_t r, + uint8_t lp, uint8_t rp, + uint8_t lvl) +{ + uint8_t max_lvl = lvl + r + l; + + if (l == 0 && r == 0) + return 0; + if (lp - l == 2 && rp - r == 0) + return 0; + if (lp - l == 0 && rp - r == 2) + return 0; + if (max_lvl > 9) + return 0; + if (max_lvl == 9 && rp == 2 && r == 1) + return 0; + return max_lvl; +} + +/* Grow a temple by building a column on it. It will update temple + * list. */ +uint8_t strat_grow_temple_column(struct temple *temple) +{ + uint16_t old_spdd, old_spda; + double checkpoint_x, checkpoint_y; + uint8_t add_level = 0; + uint8_t col_l, col_r; + uint8_t col_l_before, col_r_before; + uint8_t err; + int16_t a_abs, a; + uint8_t level = temple->level_l; + uint8_t lvl_ok = 0, col_l_ok = 0, col_r_ok = 0; + uint8_t tmp_lvl; + int16_t col_arm_dist = COL_ARM_DIST; + int16_t col_back_dist = COL_BACK_DIST; + int16_t col_angle = COL_ANGLE; + + DEBUG(E_USER_STRAT, "%s()", __FUNCTION__); + + if (level >= 9) + return END_ERROR; + + strat_get_speed(&old_spdd, &old_spda); + + if ( (temple->zone->flags & ZONE_F_ZONE1) || + (temple->zone->flags & ZONE_F_ZONE0) ) { + if (level == 1) + col_arm_dist = COL_ARM_DIST_ZONE1; + col_back_dist = COL_BACK_DIST_ZONE1; + col_angle = COL_ANGLE_ZONE1; + } + + a_abs = position_get_a_deg_s16(&mainboard.pos); + + col_l_before = get_column_count_left(); + col_r_before = get_column_count_right(); + col_l = col_l_before; + col_r = col_r_before; + + /* check number of cols */ + for (col_l = 0; col_l < col_l_before + 1; col_l++) { + for (col_r = 0; col_r < col_r_before + 1; col_r++) { + tmp_lvl = try_build_col(col_l, col_r, + col_l_before, + col_r_before, level); + if (tmp_lvl > lvl_ok) { + lvl_ok = tmp_lvl; + col_l_ok = col_l; + col_r_ok = col_r; + } + } + } + + col_l = col_l_ok; + col_r = col_r_ok; + add_level = col_l + col_r; + + strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW); + + if (col_l == 0 && col_r == 0) + ERROUT(END_ERROR); + + DEBUG(E_USER_STRAT, "Build col: left=%d right=%d", + col_l, col_r); + + i2c_mechboard_mode_prepare_inside_both(level); + trajectory_d_rel(&mainboard.traj, -col_back_dist); + err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + if (!TRAJ_SUCCESS(err)) + ERROUT(err); + + /* build with left arm */ + if (col_l) { + a = a_abs - col_angle; + if (a < -180) + a += 360; + trajectory_a_abs(&mainboard.traj, a); + err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + if (!TRAJ_SUCCESS(err)) + ERROUT(err); + + if (time_get_s() > 88) + return END_TIMER; + + if (level >= 7 && get_column_count_left() == 2) + i2c_mechboard_mode_prepare_build_select(level+1, -1); + else + i2c_mechboard_mode_prepare_build_select(level, -1); + time_wait_ms(200); + i2c_mechboard_mode_autobuild(level, col_l, col_arm_dist, + 0, 0, col_arm_dist, 0); + while (get_mechboard_mode() != I2C_MECHBOARD_MODE_AUTOBUILD); + while (get_mechboard_mode() == I2C_MECHBOARD_MODE_AUTOBUILD); + + if ((strat_infos.conf.flags & STRAT_CONF_PUSH_OPP_COLS) && + (col_r == 0) && + (temple->flags & TEMPLE_F_OPPONENT)) { + strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST); + trajectory_d_rel(&mainboard.traj, -100); + wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + i2c_mechboard_mode_push_temple_disc(I2C_LEFT_SIDE); + time_wait_ms(500); + trajectory_d_rel(&mainboard.traj, 100); + wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + } + else if ((level == 1 || level == 0) && (col_r == 0)) { + trajectory_d_rel(&mainboard.traj, -100); + wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + i2c_mechboard_mode_push_temple(level); + time_wait_ms(400); + strat_set_speed(200, SPEED_ANGLE_SLOW); + trajectory_d_rel(&mainboard.traj, 120); + wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + } + + i2c_mechboard_mode_prepare_inside_select(level+col_l, -1); + } + + /* build with right arm */ + if (col_r) { + a = a_abs + col_angle; + if (a > 180) + a -= 360; + trajectory_a_abs(&mainboard.traj, a); + err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + if (!TRAJ_SUCCESS(err)) + ERROUT(err); + + if (time_get_s() > 88) + return END_TIMER; + + if ((level+col_l) >= 7 && get_column_count_right() == 2) + i2c_mechboard_mode_prepare_build_select(-1, level + col_l + 1); + else + i2c_mechboard_mode_prepare_build_select(-1, level + col_l); + time_wait_ms(200); + i2c_mechboard_mode_autobuild(0, 0, col_arm_dist, + level + col_l, col_r, + col_arm_dist, 0); + while (get_mechboard_mode() != I2C_MECHBOARD_MODE_AUTOBUILD); + while (get_mechboard_mode() == I2C_MECHBOARD_MODE_AUTOBUILD); + + if ((strat_infos.conf.flags & STRAT_CONF_PUSH_OPP_COLS) && + (temple->flags & TEMPLE_F_OPPONENT)) { + strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST); + trajectory_d_rel(&mainboard.traj, -100); + wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + i2c_mechboard_mode_push_temple_disc(I2C_RIGHT_SIDE); + time_wait_ms(500); + trajectory_d_rel(&mainboard.traj, 100); + wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + } + else if (level == 1 || level == 0) { + trajectory_d_rel(&mainboard.traj, -100); + wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + i2c_mechboard_mode_push_temple(level); + time_wait_ms(400); + strat_set_speed(200, SPEED_ANGLE_SLOW); + trajectory_d_rel(&mainboard.traj, 120); + wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + } + + i2c_mechboard_mode_prepare_inside_select(-1, level + col_l + col_r); + + } + + trajectory_a_abs(&mainboard.traj, a_abs); + err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + if (!TRAJ_SUCCESS(err)) + ERROUT(err); + + /* position of the robot when build the new temple */ + temple->x = position_get_x_s16(&mainboard.pos); + temple->y = position_get_y_s16(&mainboard.pos); + temple->a = position_get_a_deg_s16(&mainboard.pos); + + /* checkpoint is a bit behind us */ + if (temple->zone->flags | ZONE_F_DISC) { + rel_da_to_abs_xy(CHECKPOINT_DISC_DIST, M_PI, + &checkpoint_x, &checkpoint_y); + } + else { + rel_da_to_abs_xy(CHECKPOINT_OTHER_DIST, M_PI, + &checkpoint_x, &checkpoint_y); + } + temple->checkpoint_x = checkpoint_x; + temple->checkpoint_y = checkpoint_y; + + temple->level_l = level + add_level; + temple->dist_l = 0; /* XXX */ + temple->angle_l = 0; + + temple->level_r = level + add_level; + temple->dist_r = 0; + temple->angle_r = 0; + + temple->flags = TEMPLE_F_VALID | TEMPLE_F_MONOCOL; + + if (distance_from_robot(CENTER_X, CENTER_Y) < 400) + temple->flags |= TEMPLE_F_ON_DISC; + + if ( (temple->zone->flags & ZONE_F_ZONE1) || + (temple->zone->flags & ZONE_F_ZONE0) ) { + + } + return END_TRAJ; + end: + strat_set_speed(old_spdd, old_spda); + return err; +} + +uint8_t strat_build_new_temple(struct build_zone *zone) +{ + struct temple *temple; + uint8_t level = zone->level; + uint8_t err; + + /* create a dummy temple */ + temple = strat_get_free_temple(); + if (!temple) + return END_ERROR; + + memset(temple, 0, sizeof(*temple)); + temple->level_l = level; + temple->level_r = level; + temple->flags = TEMPLE_F_VALID | TEMPLE_F_LINTEL; + temple->zone = zone; + + zone->flags |= ZONE_F_BUSY; + + if (time_get_s() > 50 && time_get_s() < 85 && + get_lintel_count() == 0) + err = strat_grow_temple_column(temple); + else + err = strat_grow_temple(temple); + + if (!TRAJ_SUCCESS(err)) + temple->flags = 0; + return err; +} + +uint8_t strat_goto_temple(struct temple *temple) +{ + uint16_t old_spdd, old_spda; + uint8_t err; + + DEBUG(E_USER_STRAT, "goto temple %p checkpoint=%d,%d", + temple, temple->checkpoint_x, temple->checkpoint_y); + + temple->last_try_time = time_get_s(); + + strat_get_speed(&old_spdd, &old_spda); + strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST); + + i2c_mechboard_mode_loaded(); + + err = goto_and_avoid(temple->checkpoint_x, + temple->checkpoint_y, + TRAJ_FLAGS_STD, + TRAJ_FLAGS_NO_NEAR); + if (!TRAJ_SUCCESS(err)) + ERROUT(err); + + err = strat_goto_build_zone(temple->zone, temple->level_r); + if (!TRAJ_SUCCESS(err)) + ERROUT(err); + + DEBUG(E_USER_STRAT, "zone reached"); + ERROUT(END_TRAJ); + + end: + strat_set_speed(old_spdd, old_spda); + return err; +} + +/* return the best existing temple for building */ +struct build_zone *strat_get_best_zone(void) +{ + uint8_t i; + struct build_zone *zone = NULL; + + for (i=0; iflags & ZONE_F_BUSY) + continue; + if ((time_get_s() - zone->last_try_time) < ZONE_DISABLE_TIME) + continue; + + return zone; + } + return NULL; +}