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=0000000000000000000000000000000000000000;hp=9abdb5a22bb1c987cf600b9f0cee40cb6a88f32e;hb=1714f4ee916fca95ce24120ea6e698237913f947;hpb=8d6a47e9e21a9a31f4bc12d32fb3d11091a4b305 diff --git a/projects/microb2010/mainboard/strat_building.c b/projects/microb2010/mainboard/strat_building.c deleted file mode 100644 index 9abdb5a..0000000 --- a/projects/microb2010/mainboard/strat_building.c +++ /dev/null @@ -1,907 +0,0 @@ -/* - * 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; -}