--- /dev/null
+/*
+ * 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 <zer0@droids-corp.org>
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <math.h>
+
+#include <aversive/pgmspace.h>
+#include <aversive/queue.h>
+#include <aversive/wait.h>
+#include <aversive/error.h>
+
+#include <ax12.h>
+#include <uart.h>
+#include <pwm_ng.h>
+#include <time.h>
+#include <spi.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <trajectory_manager.h>
+#include <vect_base.h>
+#include <lines.h>
+#include <polygon.h>
+#include <obstacle_avoidance.h>
+#include <blocking_detection_manager.h>
+#include <robot_system.h>
+#include <position_manager.h>
+
+#include <rdline.h>
+#include <parse.h>
+
+
+#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<MAX_TEMPLE; i++) {
+ if (!(strat_infos.temple_list[i].flags & TEMPLE_F_VALID))
+ return &strat_infos.temple_list[i];
+ }
+ return NULL;
+}
+
+uint8_t strat_can_build_on_temple(struct temple *temple)
+{
+ uint8_t col_l, col_r, max_col, lintel;
+
+ col_l = get_column_count_left();
+ col_r = get_column_count_right();
+ max_col = (col_l > 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; i<MAX_TEMPLE; i++) {
+ temple = &strat_infos.temple_list[i];
+
+ if (!(temple->flags & 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; i<MAX_TEMPLE; i++) {
+ temple = &strat_infos.temple_list[i];
+
+ if (!(temple->flags & 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; i<MAX_ZONE; i++) {
+ zone = &strat_infos.zone_list[i];
+
+ if (zone->flags & ZONE_F_BUSY)
+ continue;
+ if ((time_get_s() - zone->last_try_time) < ZONE_DISABLE_TIME)
+ continue;
+
+ return zone;
+ }
+ return NULL;
+}
git.droids-corp.org / aversive.git / blobdiff