2 * Copyright Droids, Microb Technology (2008)
4 * This program is free software; you can redistribute it and/or modify
5 * it under the terms of the GNU General Public License as published by
6 * the Free Software Foundation; either version 2 of the License, or
7 * (at your option) any later version.
9 * This program is distributed in the hope that it will be useful,
10 * but WITHOUT ANY WARRANTY; without even the implied warranty of
11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 * GNU General Public License for more details.
14 * You should have received a copy of the GNU General Public License
15 * along with this program; if not, write to the Free Software
16 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
18 * Revision : $Id: strat_building.c,v 1.5 2009-11-08 17:24:33 zer0 Exp $
20 * Olivier MATZ <zer0@droids-corp.org>
28 #include <aversive/pgmspace.h>
29 #include <aversive/queue.h>
30 #include <aversive/wait.h>
31 #include <aversive/error.h>
41 #include <control_system_manager.h>
42 #include <trajectory_manager.h>
43 #include <vect_base.h>
46 #include <obstacle_avoidance.h>
47 #include <blocking_detection_manager.h>
48 #include <robot_system.h>
49 #include <position_manager.h>
55 #include "../common/i2c_commands.h"
58 #include "i2c_protocol.h"
60 #include "strat_base.h"
61 #include "strat_utils.h"
62 #include "strat_avoid.h"
66 #define DISC_DIST_NEED_GOTO_AVOID 1000
67 #define DISC_DIST_PREPARE_BUILD 700
68 #define DISC_DIST_SLOW 500
70 #define ERROUT(e) do { \
75 static uint8_t is_ready_for_prepare_build(void)
78 if (distance_from_robot(CENTER_X, CENTER_Y) >
79 DISC_DIST_PREPARE_BUILD)
81 abs_xy_to_rel_da(CENTER_X, CENTER_Y, &d, &a);
89 /* go to the nearest place on the disc. Also prepare the arms for
90 * building at the correct level. If level==-1, don't move the
92 uint8_t strat_goto_disc(int8_t level)
95 uint16_t old_spdd, old_spda;
98 DEBUG(E_USER_STRAT, "%s()", __FUNCTION__);
100 strat_get_speed(&old_spdd, &old_spda);
101 strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
103 /* workaround for some static cols configurations */
104 if ((strat_infos.conf.flags & STRAT_CONF_EARLY_SCAN) == 0) {
105 if (time_get_s() > 15)
106 i2c_mechboard_mode_loaded();
109 /* if we are far from the disc, goto backward faster */
110 abs_xy_to_rel_da(CENTER_X, CENTER_Y, &d, &a);
111 if (d > DISC_DIST_NEED_GOTO_AVOID) {
112 rel_da_to_abs_xy(d - DISC_DIST_PREPARE_BUILD, a, &x, &y);
113 err = goto_and_avoid(x, y, TRAJ_FLAGS_STD,
115 if (!TRAJ_SUCCESS(err))
121 int16_t opp_d, opp_a;
122 trajectory_turnto_xy(&mainboard.traj,
124 err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
128 err = get_opponent_da(&opp_d, &opp_a);
129 if (err == 0 && opp_d < 600 &&
130 (opp_a > 325 || opp_a < 35))
135 trajectory_goto_forward_xy_abs(&mainboard.traj,
137 err = WAIT_COND_OR_TRAJ_END(is_ready_for_prepare_build(),
140 if (err == END_BLOCKING)
141 ERROUT(END_BLOCKING);
142 if (TRAJ_SUCCESS(err)) /* should not reach dest */
145 strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW);
147 i2c_mechboard_mode_prepare_build_both(level);
149 err = WAIT_COND_OR_TRAJ_END(distance_from_robot(CENTER_X,
150 CENTER_Y) < DISC_DIST_SLOW,
153 if (err == END_BLOCKING)
154 ERROUT(END_BLOCKING);
155 if (TRAJ_SUCCESS(err)) /* should not reach dest */
158 strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_VERY_SLOW);
159 err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
161 if (err == END_BLOCKING)
163 if (TRAJ_SUCCESS(err)) /* should not reach dest */
168 strat_set_speed(old_spdd, old_spda);
172 /* must be called from the checkpoint before zone 1. */
173 static uint8_t strat_goto_build_zone1_near(uint8_t level)
177 /* turn to build zone */
178 strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
179 trajectory_a_abs(&mainboard.traj, COLOR_A(90));
180 err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
181 if (!TRAJ_SUCCESS(err))
184 /* move forward to reach the build zone */
185 strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_SLOW);
186 i2c_mechboard_mode_prepare_build_both(level);
187 err = strat_calib(500, TRAJ_FLAGS_SMALL_DIST);
188 if (err == END_BLOCKING) {
192 DEBUG(E_USER_STRAT, "build zone reached");
196 /* must be called from the checkpoint before zone 0 */
197 static uint8_t strat_goto_build_zone0_near(uint8_t level)
201 int16_t cur_y, diff_y, dst_y;
204 /* turn to build zone */
205 strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
206 trajectory_a_abs(&mainboard.traj, COLOR_A(90));
207 err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
208 if (!TRAJ_SUCCESS(err))
212 cur_y = position_get_y_s16(&mainboard.pos);
213 dst_y = COLOR_Y(AREA_Y - (ROBOT_LENGTH/2) - 100);
214 diff_y = ABS(cur_y - dst_y);
216 /* move forward to reach the build zone */
217 strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_SLOW);
218 i2c_mechboard_mode_prepare_build_both(level);
219 trajectory_d_rel(&mainboard.traj, diff_y);
220 err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
221 if (err == END_BLOCKING) { /* not very good for z0 but... */
225 /* move forward to reach the build zone */
226 strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_SLOW);
227 i2c_mechboard_mode_prepare_build_both(level);
228 err = strat_calib(500, TRAJ_FLAGS_SMALL_DIST);
229 if (err == END_BLOCKING) {
234 DEBUG(E_USER_STRAT, "build zone reached");
238 /* Go to any build zone: disc, 1a or 1b. Doesn't work with zone 0 for
240 uint8_t strat_goto_build_zone(struct build_zone *zone, uint8_t level)
242 uint8_t err = END_TRAJ;
243 uint16_t old_spdd, old_spda;
244 int16_t checkpoint_x, checkpoint_y;
247 zone->last_try_time = time_get_s();
249 if (zone->flags & ZONE_F_DISC)
250 return strat_goto_disc(level);
252 DEBUG(E_USER_STRAT, "goto build zone x=%d", zone->checkpoint_x);
254 /* workaround for some static cols configurations */
255 if (time_get_s() > 15)
256 i2c_mechboard_mode_loaded();
258 strat_get_speed(&old_spdd, &old_spda);
259 strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
261 checkpoint_x = zone->checkpoint_x;
262 checkpoint_y = COLOR_Y(zone->checkpoint_y);
263 errx = position_get_x_s16(&mainboard.pos) - checkpoint_x;
265 /* goto checkpoint if we are too far from it, or if error on x
267 if (distance_from_robot(checkpoint_x, checkpoint_y) > 300 ||
269 err = goto_and_avoid(checkpoint_x, checkpoint_y,
272 if (!TRAJ_SUCCESS(err))
276 if (zone->flags & ZONE_F_ZONE1)
277 err = strat_goto_build_zone1_near(level);
278 else if (zone->flags & ZONE_F_ZONE0)
279 err = strat_goto_build_zone0_near(level);
280 if (!TRAJ_SUCCESS(err))
284 strat_set_speed(old_spdd, old_spda);
288 /* return a free temple structure */
289 struct temple *strat_get_free_temple(void)
293 for (i=0; i<MAX_TEMPLE; i++) {
294 if (!(strat_infos.temple_list[i].flags & TEMPLE_F_VALID))
295 return &strat_infos.temple_list[i];
300 uint8_t strat_can_build_on_temple(struct temple *temple)
302 uint8_t col_l, col_r, max_col, lintel;
304 col_l = get_column_count_left();
305 col_r = get_column_count_right();
306 max_col = (col_l > col_r ? col_l : col_r);
307 lintel = (get_lintel_count() > 0);
309 if (strat_infos.conf.flags & STRAT_CONF_ONLY_ONE_ON_DISC) {
310 if ((temple->level_r > 5) &&
311 (temple->flags & TEMPLE_F_ON_DISC))
315 /* return symetric temples only */
316 if (temple->level_l != temple->level_r)
319 if ((time_get_s() - temple->last_try_time) < TEMPLE_DISABLE_TIME)
322 /* we could do better to work on non-symetric temples */
323 if (temple->level_l + max_col + lintel > 9)
326 if (temple->flags & TEMPLE_F_MONOCOL)
329 /* XXX don't allow to build on opponent temple. For that we
330 * must support the little back_mm. */
331 if (temple->flags & TEMPLE_F_OPPONENT)
338 /* return the best existing temple for building */
339 struct temple *strat_get_best_temple(void)
342 struct temple *best = NULL;
343 struct temple *temple = NULL;
345 for (i=0; i<MAX_TEMPLE; i++) {
346 temple = &strat_infos.temple_list[i];
348 if (!(temple->flags & TEMPLE_F_VALID))
351 if (strat_can_build_on_temple(temple) == 0)
359 /* take the higher temple between 'best' and 'temple' */
360 if (best->level_l < temple->level_l)
364 DEBUG(E_USER_STRAT, "%s() return %p", __FUNCTION__, best);
368 /* return the temple we built on the disc if any. If valid == 1, the
369 * temple must be buildable. */
370 struct temple *strat_get_our_temple_on_disc(uint8_t valid)
373 struct temple *temple = NULL;
375 if (strat_infos.conf.flags & STRAT_CONF_ONLY_ONE_ON_DISC) {
379 for (i=0; i<MAX_TEMPLE; i++) {
380 temple = &strat_infos.temple_list[i];
382 if (!(temple->flags & TEMPLE_F_VALID))
385 if (valid == 1 && strat_can_build_on_temple(temple) == 0)
388 if (temple->flags & TEMPLE_F_ON_DISC)
394 #define COL_MAX(x,y) (((x)>(y)) ? (x) : (y))
396 #define TIME_FOR_LINTEL 3000L
397 #define TIME_FOR_BUILD 0L
398 #define TIME_FOR_COL 800L
399 #define TIME_MARGIN 2000L
401 #define CHECKPOINT_DISC_DIST 380
402 #define CHECKPOINT_OTHER_DIST 200
403 /* Grow a temple. It will update temple list. */
404 uint8_t strat_grow_temple(struct temple *temple)
406 double checkpoint_x, checkpoint_y;
407 uint8_t add_level = 0;
408 uint8_t do_lintel = 1;
409 uint8_t col_l, col_r, col_max;
413 /* XXX temple must be symetric */
414 uint8_t level = temple->level_l;
416 DEBUG(E_USER_STRAT, "%s()", __FUNCTION__);
418 if (temple->level_l >= 9)
421 if ( (temple->zone->flags & ZONE_F_ZONE1) ||
422 (temple->zone->flags & ZONE_F_ZONE0) ) {
423 strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW);
424 trajectory_d_rel(&mainboard.traj, -17);
425 wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
428 col_l = get_column_count_left();
429 col_r = get_column_count_right();
431 if (time_get_s() < 75) {
432 /* make temple symetric: if we have 1 col on left and 2 cols
433 * on right, only build one on both sides. */
442 if (get_lintel_count() == 0)
445 else if (col_l != col_r)
448 if (col_l == 0 || col_r == 0) {
449 if (temple->flags & TEMPLE_F_LINTEL)
453 if (col_l == 0 && col_r == 0 && do_lintel == 0) {
454 DEBUG(E_USER_STRAT, "nothing to do");
458 add_level = do_lintel + col_l;
459 while (level + add_level > 9) {
462 add_level = do_lintel + col_l;
465 /* we know col_r and col_l are > 0 */
470 col_max = COL_MAX(col_r, col_l);
472 /* Reduce nb elts if we don't have time */
473 timeout = (!!col_max) * TIME_FOR_BUILD;
474 timeout += col_max * TIME_FOR_COL;
475 timeout += do_lintel * TIME_FOR_LINTEL;
476 if ((timeout / 1000L) + time_get_s() > 89 && do_lintel) {
478 timeout -= TIME_FOR_LINTEL;
480 if ((timeout / 1000L) + time_get_s() > 89 && col_max) {
486 timeout -= TIME_FOR_COL;
489 /* take a margin for timeout */
490 timeout += (!!col_max) * TIME_MARGIN;
492 if (col_l == 0 && col_r == 0 && do_lintel == 0) {
493 DEBUG(E_USER_STRAT, "nothing to do (2)");
497 DEBUG(E_USER_STRAT, "Autobuild: left=%d,%d right=%d,%d lintel=%d",
498 level, col_l, level, col_r, do_lintel);
500 i2c_mechboard_mode_autobuild(level, col_l, I2C_AUTOBUILD_DEFAULT_DIST,
501 level, col_r, I2C_AUTOBUILD_DEFAULT_DIST,
503 WAIT_COND_OR_TIMEOUT(get_mechboard_mode() ==
504 I2C_MECHBOARD_MODE_AUTOBUILD, 100);
505 err = WAIT_COND_OR_TIMEOUT(get_mechboard_mode() !=
506 I2C_MECHBOARD_MODE_AUTOBUILD, timeout);
508 DEBUG(E_USER_STRAT, "timeout building temple (timeout was %d)", timeout);
509 temple->flags = 0; /* remove temple from list */
513 DEBUG(E_USER_STRAT, "temple built");
515 /* position of the robot when build the new temple */
516 temple->x = position_get_x_s16(&mainboard.pos);
517 temple->y = position_get_y_s16(&mainboard.pos);
518 temple->a = position_get_a_deg_s16(&mainboard.pos);
520 /* checkpoint is a bit behind us */
521 if (temple->zone->flags & ZONE_F_DISC) {
522 rel_da_to_abs_xy(CHECKPOINT_DISC_DIST, M_PI,
523 &checkpoint_x, &checkpoint_y);
526 rel_da_to_abs_xy(CHECKPOINT_OTHER_DIST, M_PI,
527 &checkpoint_x, &checkpoint_y);
529 temple->checkpoint_x = checkpoint_x;
530 temple->checkpoint_y = checkpoint_y;
532 temple->level_l = level + add_level;
536 temple->level_r = level + add_level;
540 temple->flags = TEMPLE_F_VALID;
542 if (distance_from_robot(CENTER_X, CENTER_Y) < 400)
543 temple->flags |= TEMPLE_F_ON_DISC;
546 temple->flags |= TEMPLE_F_LINTEL;
548 /* we must push the temple */
549 if ( ((temple->zone->flags & ZONE_F_ZONE1) ||
550 (temple->zone->flags & ZONE_F_ZONE0)) &&
552 strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW);
553 trajectory_d_rel(&mainboard.traj, -100);
554 wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
555 i2c_mechboard_mode_push_temple(level);
557 strat_set_speed(200, SPEED_ANGLE_SLOW);
558 trajectory_d_rel(&mainboard.traj, 100);
559 wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
562 /* Special case for big 3 */
563 if (strat_infos.col_in_boobs) {
564 uint16_t old_spdd, old_spda;
565 strat_get_speed(&old_spdd, &old_spda);
566 strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_SLOW);
567 DEBUG(E_USER_STRAT, "%s() big 3", __FUNCTION__);
568 strat_infos.col_in_boobs = 0;
569 trajectory_d_rel(&mainboard.traj, -120);
570 wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
571 i2c_mechboard_mode_prepare_pickup_next(I2C_AUTO_SIDE,
572 I2C_MECHBOARD_MODE_CLEAR);
573 WAIT_COND_OR_TIMEOUT(get_column_count() >= 2, 4000L);
574 i2c_mechboard_mode_prepare_build_both(level + add_level);
576 strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW);
577 trajectory_d_rel(&mainboard.traj, 120);
578 wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
579 err = strat_grow_temple(temple);
580 strat_set_speed(old_spdd, old_spda);
587 #define COL_BACK_DIST 70
589 #define COL_ARM_DIST 220
591 #define COL_BACK_DIST_ZONE1 35
592 #define COL_ARM_DIST_ZONE1 230
593 #define COL_ANGLE_ZONE1 19
595 static uint8_t try_build_col(uint8_t l, uint8_t r,
596 uint8_t lp, uint8_t rp,
599 uint8_t max_lvl = lvl + r + l;
601 if (l == 0 && r == 0)
603 if (lp - l == 2 && rp - r == 0)
605 if (lp - l == 0 && rp - r == 2)
609 if (max_lvl == 9 && rp == 2 && r == 1)
614 /* Grow a temple by building a column on it. It will update temple
616 uint8_t strat_grow_temple_column(struct temple *temple)
618 uint16_t old_spdd, old_spda;
619 double checkpoint_x, checkpoint_y;
620 uint8_t add_level = 0;
621 uint8_t col_l, col_r;
622 uint8_t col_l_before, col_r_before;
625 uint8_t level = temple->level_l;
626 uint8_t lvl_ok = 0, col_l_ok = 0, col_r_ok = 0;
628 int16_t col_arm_dist = COL_ARM_DIST;
629 int16_t col_back_dist = COL_BACK_DIST;
630 int16_t col_angle = COL_ANGLE;
632 DEBUG(E_USER_STRAT, "%s()", __FUNCTION__);
637 strat_get_speed(&old_spdd, &old_spda);
639 if ( (temple->zone->flags & ZONE_F_ZONE1) ||
640 (temple->zone->flags & ZONE_F_ZONE0) ) {
642 col_arm_dist = COL_ARM_DIST_ZONE1;
643 col_back_dist = COL_BACK_DIST_ZONE1;
644 col_angle = COL_ANGLE_ZONE1;
647 a_abs = position_get_a_deg_s16(&mainboard.pos);
649 col_l_before = get_column_count_left();
650 col_r_before = get_column_count_right();
651 col_l = col_l_before;
652 col_r = col_r_before;
654 /* check number of cols */
655 for (col_l = 0; col_l < col_l_before + 1; col_l++) {
656 for (col_r = 0; col_r < col_r_before + 1; col_r++) {
657 tmp_lvl = try_build_col(col_l, col_r,
659 col_r_before, level);
660 if (tmp_lvl > lvl_ok) {
670 add_level = col_l + col_r;
672 strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW);
674 if (col_l == 0 && col_r == 0)
677 DEBUG(E_USER_STRAT, "Build col: left=%d right=%d",
680 i2c_mechboard_mode_prepare_inside_both(level);
681 trajectory_d_rel(&mainboard.traj, -col_back_dist);
682 err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
683 if (!TRAJ_SUCCESS(err))
686 /* build with left arm */
688 a = a_abs - col_angle;
691 trajectory_a_abs(&mainboard.traj, a);
692 err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
693 if (!TRAJ_SUCCESS(err))
696 if (time_get_s() > 88)
699 if (level >= 7 && get_column_count_left() == 2)
700 i2c_mechboard_mode_prepare_build_select(level+1, -1);
702 i2c_mechboard_mode_prepare_build_select(level, -1);
704 i2c_mechboard_mode_autobuild(level, col_l, col_arm_dist,
705 0, 0, col_arm_dist, 0);
706 while (get_mechboard_mode() != I2C_MECHBOARD_MODE_AUTOBUILD);
707 while (get_mechboard_mode() == I2C_MECHBOARD_MODE_AUTOBUILD);
709 if ((strat_infos.conf.flags & STRAT_CONF_PUSH_OPP_COLS) &&
711 (temple->flags & TEMPLE_F_OPPONENT)) {
712 strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST);
713 trajectory_d_rel(&mainboard.traj, -100);
714 wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
715 i2c_mechboard_mode_push_temple_disc(I2C_LEFT_SIDE);
717 trajectory_d_rel(&mainboard.traj, 100);
718 wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
720 else if ((level == 1 || level == 0) && (col_r == 0)) {
721 trajectory_d_rel(&mainboard.traj, -100);
722 wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
723 i2c_mechboard_mode_push_temple(level);
725 strat_set_speed(200, SPEED_ANGLE_SLOW);
726 trajectory_d_rel(&mainboard.traj, 120);
727 wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
730 i2c_mechboard_mode_prepare_inside_select(level+col_l, -1);
733 /* build with right arm */
735 a = a_abs + col_angle;
738 trajectory_a_abs(&mainboard.traj, a);
739 err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
740 if (!TRAJ_SUCCESS(err))
743 if (time_get_s() > 88)
746 if ((level+col_l) >= 7 && get_column_count_right() == 2)
747 i2c_mechboard_mode_prepare_build_select(-1, level + col_l + 1);
749 i2c_mechboard_mode_prepare_build_select(-1, level + col_l);
751 i2c_mechboard_mode_autobuild(0, 0, col_arm_dist,
752 level + col_l, col_r,
754 while (get_mechboard_mode() != I2C_MECHBOARD_MODE_AUTOBUILD);
755 while (get_mechboard_mode() == I2C_MECHBOARD_MODE_AUTOBUILD);
757 if ((strat_infos.conf.flags & STRAT_CONF_PUSH_OPP_COLS) &&
758 (temple->flags & TEMPLE_F_OPPONENT)) {
759 strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST);
760 trajectory_d_rel(&mainboard.traj, -100);
761 wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
762 i2c_mechboard_mode_push_temple_disc(I2C_RIGHT_SIDE);
764 trajectory_d_rel(&mainboard.traj, 100);
765 wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
767 else if (level == 1 || level == 0) {
768 trajectory_d_rel(&mainboard.traj, -100);
769 wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
770 i2c_mechboard_mode_push_temple(level);
772 strat_set_speed(200, SPEED_ANGLE_SLOW);
773 trajectory_d_rel(&mainboard.traj, 120);
774 wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
777 i2c_mechboard_mode_prepare_inside_select(-1, level + col_l + col_r);
781 trajectory_a_abs(&mainboard.traj, a_abs);
782 err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
783 if (!TRAJ_SUCCESS(err))
786 /* position of the robot when build the new temple */
787 temple->x = position_get_x_s16(&mainboard.pos);
788 temple->y = position_get_y_s16(&mainboard.pos);
789 temple->a = position_get_a_deg_s16(&mainboard.pos);
791 /* checkpoint is a bit behind us */
792 if (temple->zone->flags | ZONE_F_DISC) {
793 rel_da_to_abs_xy(CHECKPOINT_DISC_DIST, M_PI,
794 &checkpoint_x, &checkpoint_y);
797 rel_da_to_abs_xy(CHECKPOINT_OTHER_DIST, M_PI,
798 &checkpoint_x, &checkpoint_y);
800 temple->checkpoint_x = checkpoint_x;
801 temple->checkpoint_y = checkpoint_y;
803 temple->level_l = level + add_level;
804 temple->dist_l = 0; /* XXX */
807 temple->level_r = level + add_level;
811 temple->flags = TEMPLE_F_VALID | TEMPLE_F_MONOCOL;
813 if (distance_from_robot(CENTER_X, CENTER_Y) < 400)
814 temple->flags |= TEMPLE_F_ON_DISC;
816 if ( (temple->zone->flags & ZONE_F_ZONE1) ||
817 (temple->zone->flags & ZONE_F_ZONE0) ) {
822 strat_set_speed(old_spdd, old_spda);
826 uint8_t strat_build_new_temple(struct build_zone *zone)
828 struct temple *temple;
829 uint8_t level = zone->level;
832 /* create a dummy temple */
833 temple = strat_get_free_temple();
837 memset(temple, 0, sizeof(*temple));
838 temple->level_l = level;
839 temple->level_r = level;
840 temple->flags = TEMPLE_F_VALID | TEMPLE_F_LINTEL;
843 zone->flags |= ZONE_F_BUSY;
845 if (time_get_s() > 50 && time_get_s() < 85 &&
846 get_lintel_count() == 0)
847 err = strat_grow_temple_column(temple);
849 err = strat_grow_temple(temple);
851 if (!TRAJ_SUCCESS(err))
856 uint8_t strat_goto_temple(struct temple *temple)
858 uint16_t old_spdd, old_spda;
861 DEBUG(E_USER_STRAT, "goto temple %p checkpoint=%d,%d",
862 temple, temple->checkpoint_x, temple->checkpoint_y);
864 temple->last_try_time = time_get_s();
866 strat_get_speed(&old_spdd, &old_spda);
867 strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
869 i2c_mechboard_mode_loaded();
871 err = goto_and_avoid(temple->checkpoint_x,
872 temple->checkpoint_y,
875 if (!TRAJ_SUCCESS(err))
878 err = strat_goto_build_zone(temple->zone, temple->level_r);
879 if (!TRAJ_SUCCESS(err))
882 DEBUG(E_USER_STRAT, "zone reached");
886 strat_set_speed(old_spdd, old_spda);
890 /* return the best existing temple for building */
891 struct build_zone *strat_get_best_zone(void)
894 struct build_zone *zone = NULL;
896 for (i=0; i<MAX_ZONE; i++) {
897 zone = &strat_infos.zone_list[i];
899 if (zone->flags & ZONE_F_BUSY)
901 if ((time_get_s() - zone->last_try_time) < ZONE_DISABLE_TIME)