2 * Copyright Droids, Microb Technology (2009)
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.c,v 1.6 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>
36 #include <clock_time.h>
41 #include <control_system_manager.h>
42 #include <trajectory_manager.h>
43 #include <trajectory_manager_utils.h>
44 #include <trajectory_manager_core.h>
45 #include <vect_base.h>
48 #include <obstacle_avoidance.h>
49 #include <blocking_detection_manager.h>
50 #include <robot_system.h>
51 #include <position_manager.h>
53 #include <diagnostic.h>
58 #include "../common/i2c_commands.h"
59 #include "i2c_protocol.h"
64 #include "strat_base.h"
65 #include "strat_corn.h"
66 #include "strat_utils.h"
67 #include "strat_avoid.h"
71 #define COL_DISP_MARGIN 400 /* stop 40 cm in front of dispenser */
72 #define COL_SCAN_PRE_MARGIN 250
74 static volatile uint8_t strat_running = 0;
75 volatile uint8_t strat_want_pack = 0;
76 volatile uint8_t strat_lpack60 = 0;
77 volatile uint8_t strat_rpack60 = 0;
78 struct strat_conf strat_conf = {
85 /*************************************************************/
89 /*************************************************************/
91 /* called before each strat, and before the start switch */
92 void strat_preinit(void)
95 interrupt_traj_reset();
96 mainboard.flags = DO_ENCODERS | DO_CS | DO_RS |
97 DO_POS | DO_BD | DO_POWER;
100 strat_conf_dump(__FUNCTION__);
101 strat_db_dump(__FUNCTION__);
104 void strat_conf_dump(const char *caller)
106 if (!strat_conf.dump_enabled)
109 printf_P(PSTR("-- conf --\r\n"));
110 printf_P(PSTR("opp_orange = %d\n"), strat_conf.opp_orange);
111 printf_P(PSTR("orphan_tomato = %d\n"), strat_conf.orphan_tomato);
112 printf_P(PSTR("our_orange = %s\n"),
113 (strat_conf.flags & STRAT_CONF_OUR_ORANGE) ? "y":"n");
116 void strat_event_enable(void)
121 void strat_event_disable(void)
126 /* call it just before launching the strat */
127 void strat_init(void)
130 position_set(&mainboard.pos, 298.16,
131 COLOR_Y(308.78), COLOR_A(70.00));
134 /* we consider that the color is correctly set */
136 strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
138 interrupt_traj_reset();
140 i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST);
141 i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_HARVEST);
143 /* used in strat_base for END_TIMER */
144 mainboard.flags = DO_ENCODERS | DO_CS | DO_RS |
145 DO_POS | DO_BD | DO_TIMER | DO_POWER;
148 /* call it after each strat */
149 void strat_exit(void)
156 mainboard.flags &= ~(DO_TIMER);
162 mainboard.flags &= ~(DO_CS);
164 pwm_ng_set(LEFT_PWM, 0);
165 pwm_ng_set(RIGHT_PWM, 0);
169 /* mark tomato as not present */
170 static void check_tomato(void)
174 static uint8_t prev_check_time;
177 /* check present tomatoes once per second */
178 cur_time = time_get_s();
179 if (cur_time != prev_check_time) {
182 for (k = 0; k < TOMATO_NB; k++) {
183 if (strat_db.tomato_table[k]->present == 1 &&
184 strat_db.tomato_table[k]->time_removed != -1 &&
185 strat_db.tomato_table[k]->time_removed + 2 <= time_get_s()) {
187 printf("remove tomato %d\n", k);
189 strat_db.tomato_table[k]->present = 0;
192 prev_check_time = cur_time;
195 x = position_get_x_s16(&mainboard.pos);
196 y = position_get_y_s16(&mainboard.pos);
198 if (xycoord_to_ijcoord(&x, &y, &i, &j) < 0)
201 if (strat_db.wp_table[i][j].type != WP_TYPE_TOMATO)
204 if (strat_db.wp_table[i][j].present == 0)
207 strat_db.wp_table[i][j].time_removed = time_get_s();
208 strat_db.wp_table[i][j].present = 0;
210 ballboard.ball_count ++;
211 printf("add ball %d,%d\n", i, j);
215 /* mark corn as not present and give correct commands to the cobboard
217 static void check_corn(void)
220 int8_t lcob_near, rcob_near;
223 static uint8_t prev_check_time;
226 /* read sensors from ballboard */
228 lcob = ballboard.lcob;
229 ballboard.lcob = I2C_COB_NONE;
230 rcob = ballboard.rcob;
231 ballboard.rcob = I2C_COB_NONE;
234 /* XXX take opponent position into account */
236 /* check present cobs once per second */
237 cur_time = time_get_s();
238 if (cur_time != prev_check_time) {
241 for (i = 0; i < CORN_NB; i++) {
242 if (strat_db.corn_table[i]->present == 1 &&
243 strat_db.corn_table[i]->time_removed != -1 &&
244 strat_db.corn_table[i]->time_removed + 2 <= time_get_s()) {
246 printf("remove cob %d\n", i);
248 strat_db.corn_table[i]->present = 0;
251 prev_check_time = cur_time;
254 /* detect cob on left side */
255 lcob_near = corn_is_near(&lidx, I2C_LEFT_SIDE);
256 if (lcob_near && lcob != I2C_COB_NONE) {
257 if (strat_db.corn_table[lidx]->corn.color == I2C_COB_UNKNOWN)
258 DEBUG(E_USER_STRAT, "lcob %s %d",
259 lcob == I2C_COB_WHITE ? "white" : "black", lidx);
260 corn_set_color(strat_db.corn_table[lidx], lcob);
263 /* detect cob on right side */
264 rcob_near = corn_is_near(&ridx, I2C_RIGHT_SIDE);
265 if (rcob_near && rcob != I2C_COB_NONE) {
266 if (strat_db.corn_table[ridx]->corn.color == I2C_COB_UNKNOWN)
267 DEBUG(E_USER_STRAT, "rcob %s %d",
268 rcob == I2C_COB_WHITE ? "white" : "black", ridx);
269 corn_set_color(strat_db.corn_table[ridx], rcob);
272 /* control the cobboard mode for left spickle */
273 if (lcob_near && strat_db.corn_table[lidx]->present) {
274 if (get_cob_count() >= 5 || strat_want_pack || strat_lpack60) {
278 /* deploy spickle and harvest white ones */
279 if (strat_db.corn_table[lidx]->corn.color == I2C_COB_WHITE) {
280 i2c_cobboard_autoharvest_nomove(I2C_LEFT_SIDE);
281 if (strat_db.corn_table[lidx]->time_removed == -1
283 && cobboard.status == I2C_COBBOARD_STATUS_LBUSY
286 strat_db.corn_table[lidx]->time_removed = time_get_s();
288 cobboard.cob_count ++;
289 printf("add cob %d\n", lidx);
294 i2c_cobboard_deploy_nomove(I2C_LEFT_SIDE);
298 /* no cob near us, we can pack or deploy freely */
299 if (get_cob_count() >= 5 || strat_want_pack || strat_lpack60)
300 i2c_cobboard_pack_weak(I2C_LEFT_SIDE);
302 i2c_cobboard_deploy(I2C_LEFT_SIDE);
305 /* control the cobboard mode for right spickle */
306 if (rcob_near && strat_db.corn_table[ridx]->present) {
307 if (get_cob_count() >= 5 || strat_want_pack || strat_rpack60) {
311 /* deploy spickle and harvest white ones */
312 if (strat_db.corn_table[ridx]->corn.color == I2C_COB_WHITE) {
313 i2c_cobboard_autoharvest_nomove(I2C_RIGHT_SIDE);
314 if (strat_db.corn_table[ridx]->time_removed == -1
316 && cobboard.status == I2C_COBBOARD_STATUS_RBUSY
319 strat_db.corn_table[ridx]->time_removed = time_get_s();
321 cobboard.cob_count ++;
322 printf("add cob %d\n", ridx);
327 i2c_cobboard_deploy_nomove(I2C_RIGHT_SIDE);
331 /* no cob near us, we can pack or deploy freely */
332 if (get_cob_count() >= 5 || strat_want_pack || strat_rpack60)
333 i2c_cobboard_pack_weak(I2C_RIGHT_SIDE);
335 i2c_cobboard_deploy(I2C_RIGHT_SIDE);
339 /* check opponent position */
340 void check_opponent(void)
345 if (get_opponent_xy(&x, &y) < 0)
348 /* check for oranges after 5 seconds */
349 if (time_get_s() > 5) {
350 if (mainboard.our_color == I2C_COLOR_YELLOW) {
351 if (y < 500 && x < 500)
352 strat_db.our_oranges_count = 0;
353 if (y < 500 && x > AREA_X - 500)
354 strat_db.opp_oranges_count = 0;
357 if (y > AREA_Y - 500 && x < 500)
358 strat_db.our_oranges_count = 0;
359 if (y > AREA_Y - 500 && x > AREA_X - 500)
360 strat_db.opp_oranges_count = 0;
364 /* malus for some tomatoes and cobs, visited by opponent */
365 if (xycoord_to_ijcoord(&x, &y, &i, &j) < 0)
368 strat_db.wp_table[i][j].opp_visited = 1;
371 /* called periodically (10ms) */
372 void strat_event(void *dummy)
374 /* ignore when strat is not running */
375 if (strat_running == 0)
382 /* limit speed when opponent is near */
383 /* disabled for 2010, we are already slow :) */
384 //strat_limit_speed();
387 /* check that we are on an eject line */
388 static uint8_t robot_is_on_eject_line(void)
393 x = position_get_x_s16(&mainboard.pos);
394 y = position_get_y_s16(&mainboard.pos);
396 if (xycoord_to_ijcoord_not_corn(&x, &y, &i, &j) < 0)
399 if (!wp_belongs_to_line(i, j, 5, LINE_UP) &&
400 !wp_belongs_to_line(i, j, 2, LINE_R_UP))
406 /* 0 = fast, 1 = slow */
407 static uint8_t eject_select_speed(void)
412 x = position_get_x_s16(&mainboard.pos);
413 y = position_get_y_s16(&mainboard.pos);
415 if (get_cob_count() >= 5) {
420 if (xycoord_to_ijcoord_not_corn(&x, &y, &i, &j) < 0) {
421 DEBUG(E_USER_STRAT, "%s(): cannot find waypoint at %d,%d",
426 if (corn_count_neigh(i, j) == 2)
432 /* called multiple times while we are waiting to reach the ejection
434 static uint8_t speedify_eject(void)
436 if (eject_select_speed())
437 strat_set_speed(SPEED_CLITOID_SLOW, SPEED_ANGLE_SLOW);
439 strat_set_speed(SPEED_CLITOID_FAST, SPEED_ANGLE_SLOW);
443 /* must be called from a terminal line */
444 static uint8_t strat_eject(void)
448 DEBUG(E_USER_STRAT, "%s() cob_count=%d ball_count=%d",
449 __FUNCTION__, get_cob_count(), get_ball_count());
451 /* check that we are called from an eject line */
452 if (!robot_is_on_eject_line()) {
453 DEBUG(E_USER_STRAT, "%s() not on eject line", __FUNCTION__);
457 /* go to eject point */
458 trajectory_goto_forward_xy_abs(&mainboard.traj, 2625, COLOR_Y(1847));
459 err = WAIT_COND_OR_TRAJ_END(speedify_eject(),
461 /* err is never == 0 because speedify_eject() always return 0 */
462 if (!TRAJ_SUCCESS(err))
465 /* pack arms (force), and disable strat_event */
466 strat_event_disable();
467 i2c_cobboard_pack_weak(I2C_LEFT_SIDE);
468 i2c_cobboard_pack_weak(I2C_RIGHT_SIDE);
471 if (get_ball_count() > 0) {
472 i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_EJECT);
473 trajectory_a_abs(&mainboard.traj, COLOR_A(70));
474 err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
475 if (!TRAJ_SUCCESS(err))
478 DEBUG(E_USER_STRAT, "%s():%d", __FUNCTION__, __LINE__);
483 WAIT_COND_OR_TIMEOUT(ballboard.status == I2C_BALLBOARD_STATUS_F_BUSY,
485 WAIT_COND_OR_TIMEOUT(ballboard.status == I2C_BALLBOARD_STATUS_F_READY,
491 trajectory_a_abs(&mainboard.traj, COLOR_A(-110));
492 err = wait_traj_end(END_INTR|END_TRAJ);
493 if (!TRAJ_SUCCESS(err))
497 trajectory_d_rel(&mainboard.traj, -70);
498 err = wait_traj_end(END_INTR|END_TRAJ);
499 if (!TRAJ_SUCCESS(err))
502 if (get_cob_count() > 0) {
503 i2c_cobboard_set_mode(I2C_COBBOARD_MODE_EJECT);
508 trajectory_d_rel(&mainboard.traj, 70);
509 err = wait_traj_end(END_INTR|END_TRAJ);
510 if (!TRAJ_SUCCESS(err))
513 strat_db_dump(__FUNCTION__);
517 strat_event_enable();
522 static uint8_t strat_beginning(uint8_t do_initturn)
526 strat_set_acc(ACC_DIST, ACC_ANGLE);
529 strat_set_speed(600, 60); /* OK */
530 trajectory_d_a_rel(&mainboard.traj, 1000, COLOR_A(20));
531 err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj),
535 strat_set_acc(ACC_DIST, ACC_ANGLE);
536 strat_set_speed(SPEED_CLITOID_SLOW, SPEED_ANGLE_SLOW);
538 err = line2line(0, LINE_UP, 2, LINE_R_DOWN, TRAJ_FLAGS_NO_NEAR);
539 if (!TRAJ_SUCCESS(err))
542 err = line2line(2, LINE_R_DOWN, 2, LINE_R_UP, TRAJ_FLAGS_NO_NEAR);
543 if (!TRAJ_SUCCESS(err)) {
550 /* dump state (every 5 s max) */
551 #define DUMP_RATE_LIMIT(dump, last_print) \
553 if (time_get_s() - last_print > 5) { \
555 last_print = time_get_s(); \
561 /* return true if we need to grab some more elements */
562 static uint8_t need_more_elements(void)
564 if (time_get_s() <= 75) {
565 /* we have enough time left */
566 if (get_ball_count() >= 4)
568 if (get_cob_count() >= 4)
570 if ((get_ball_count() >= 2) &&
571 (get_cob_count() >= 2))
576 /* not much time remaining */
577 if ((get_ball_count() >= 1) &&
578 (get_cob_count() >= 1))
585 /* get tomatoes near our goals (12,5 and 12,3) */
586 uint8_t get_opp_oranges(void)
592 DEBUG(E_USER_STRAT, "%s()", __FUNCTION__);
594 /* only if oranges are present */
595 if (strat_db.opp_oranges_count == 0)
598 strat_db.opp_oranges_count = 0;
599 x = position_get_x_s16(&mainboard.pos);
600 y = position_get_y_s16(&mainboard.pos);
602 if (xycoord_to_ijcoord_not_corn(&x, &y, &i, &j) < 0)
605 /* not on eject point */
606 if (i != 11 || j != 6)
610 strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
612 /* turn in the correct direction */
613 trajectory_a_abs(&mainboard.traj, COLOR_A(-90));
614 err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
615 if (!TRAJ_SUCCESS(err))
618 trajectory_goto_forward_xy_abs(&mainboard.traj, 2625, COLOR_Y(597));
619 err = wait_traj_end(TRAJ_FLAGS_STD);
620 if (!TRAJ_SUCCESS(err))
623 strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW);
624 trajectory_goto_forward_xy_abs(&mainboard.traj, 2750, COLOR_Y(250));
625 err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
626 if (!TRAJ_SUCCESS(err))
629 err = run_to_the_hills(get_opponent_color());
636 /* get tomatoes near our goals (12,5 and 12,3) */
637 uint8_t get_orphan_tomatoes(void)
639 #define CLITOID_TOMATO_RADIUS 100.
640 #define TOMATO_BACK_X 2760
641 #define TOMATO_BACK_LEN 200
648 DEBUG(E_USER_STRAT, "%s()", __FUNCTION__);
650 /* only go if both tomatoes are present */
651 if (!strat_db.wp_table[12][5].present ||
652 !strat_db.wp_table[12][3].present) {
656 x = position_get_x_s16(&mainboard.pos);
657 y = position_get_y_s16(&mainboard.pos);
659 if (xycoord_to_ijcoord_not_corn(&x, &y, &i, &j) < 0)
662 /* not on eject point */
663 if (i != 11 || j != 6)
667 strat_set_speed(SPEED_CLITOID_FAST, SPEED_ANGLE_SLOW);
669 /* turn in the correct direction */
670 trajectory_a_abs(&mainboard.traj, COLOR_A(-90));
671 err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
673 /* clitoid to turn and take the first ball */
674 ret = trajectory_clitoid(&mainboard.traj, 2625, COLOR_Y(1847),
675 COLOR_A(-90), 150., COLOR_A(90), 0,
676 CLITOID_TOMATO_RADIUS, 3*125);
682 err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
683 if (!TRAJ_SUCCESS(err))
686 strat_set_speed(SPEED_CLITOID_SLOW, SPEED_ANGLE_SLOW);
687 err = strat_calib(300, END_TRAJ|END_BLOCKING);
688 a = position_get_a_deg_s16(&mainboard.pos);
690 strat_reset_pos(AREA_X - ROBOT_HALF_LENGTH_FRONT,
694 strat_set_speed(SPEED_CLITOID_FAST, SPEED_ANGLE_FAST);
695 trajectory_d_rel(&mainboard.traj, -250);
696 err = WAIT_COND_OR_TRAJ_END(!x_is_more_than(TOMATO_BACK_X),
699 if (err != 0 && !TRAJ_SUCCESS(err))
702 trajectory_d_a_rel(&mainboard.traj, -TOMATO_BACK_LEN, COLOR_A(-90));
703 err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
707 /* clitoid to turn and take the first ball */
708 strat_set_speed(SPEED_CLITOID_FAST, SPEED_ANGLE_SLOW);
709 ret = trajectory_clitoid(&mainboard.traj, 2625, COLOR_Y(1847),
710 COLOR_A(-90), 150., COLOR_A(90), 0,
711 CLITOID_TOMATO_RADIUS, 7*125);
716 err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
717 if (!TRAJ_SUCCESS(err))
721 strat_set_speed(SPEED_CLITOID_FAST, SPEED_ANGLE_FAST);
722 trajectory_d_rel(&mainboard.traj, -250);
723 err = WAIT_COND_OR_TRAJ_END(!x_is_more_than(TOMATO_BACK_X),
725 trajectory_d_a_rel(&mainboard.traj, -TOMATO_BACK_LEN, COLOR_A(-90));
726 err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
734 #define HILL_LEN 1000
737 #define HILL_POSY_YELLOW 310
738 #define HILL_POSY_BLUE 190
740 #define HILL_POSX_BALLS_DOWN1 830
741 #define HILL_POSX_BALLS_DOWN2 920
742 #define HILL_POSX_BALLS_DOWN3 730
743 #define HILL_START_POSX 580
745 uint8_t prepare_hill(uint8_t orange_color, int16_t posx)
747 int16_t startx, starty;
748 uint8_t our_color = get_color();
751 if (orange_color == I2C_COLOR_YELLOW)
752 starty = HILL_POSY_YELLOW;
754 starty = HILL_POSY_BLUE;
755 if (orange_color == our_color)
758 startx = AREA_X - posx;
759 trajectory_goto_forward_xy_abs(&mainboard.traj, startx, COLOR_Y(starty));
760 err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
761 if (!TRAJ_SUCCESS(err))
764 /* turn to the hills */
765 if (orange_color == our_color)
766 trajectory_a_abs(&mainboard.traj, COLOR_A(HILL_ANGLE));
768 trajectory_a_abs(&mainboard.traj, COLOR_A(-180+HILL_ANGLE));
769 err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
770 if (!TRAJ_SUCCESS(err))
776 /* get oranges, must be called near game area */
777 uint8_t run_to_the_hills(uint8_t orange_color)
782 uint8_t our_color = get_color();
783 int32_t p = pid_get_gain_P(&mainboard.angle.pid);
784 int32_t i = pid_get_gain_I(&mainboard.angle.pid);
785 int32_t d = pid_get_gain_D(&mainboard.angle.pid);
786 int32_t max_in = pid_get_max_in(&mainboard.angle.pid);
787 int32_t max_i = pid_get_max_I(&mainboard.angle.pid);
788 int32_t max_out = pid_get_max_out(&mainboard.angle.pid);
791 strat_get_acc(&ad, &aa);
792 strat_get_speed(&sd, &sa);
794 DEBUG(E_USER_STRAT, "%s()", __FUNCTION__);
796 strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
797 err = prepare_hill(orange_color, HILL_START_POSX);
798 if (!TRAJ_SUCCESS(err))
801 strat_set_acc(5, ACC_ANGLE);
802 strat_set_speed(300, SPEED_ANGLE_SLOW);
803 bd_set_current_thresholds(&mainboard.distance.bd, 500, 8000, 2000000, 80);
804 bd_set_current_thresholds(&mainboard.angle.bd, 500, 8000, 2000000, 80);
805 bd_set_speed_threshold(&mainboard.distance.bd, 10);
806 support_balls_pack();
807 i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_PREP_FORK);
809 /* decrease angle gains */
810 pid_set_gains(&mainboard.angle.pid, 200, 0, 2000);
812 /* here it is difficult to handle return values, because we
814 i2c_cobboard_set_mode(I2C_COBBOARD_MODE_KICKSTAND_DOWN);
815 trajectory_d_rel(&mainboard.traj, HILL_LEN);
816 err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos) >
817 HILL_POSX_BALLS_DOWN1,
818 TRAJ_FLAGS_SMALL_DIST);
819 DEBUG(E_USER_STRAT, "deploy support balls");
820 support_balls_deploy();
821 i2c_cobboard_set_mode(I2C_COBBOARD_MODE_KICKSTAND_UP);
822 trajectory_only_a_rel(&mainboard.traj, 2);
823 err = WAIT_COND_OR_TE_TO(0, 0, 2200);
825 i2c_cobboard_set_mode(I2C_COBBOARD_MODE_KICKSTAND_DOWN);
828 i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_TAKE_FORK);
832 /* reach top, go down */
833 trajectory_d_rel(&mainboard.traj, -HILL_LEN);
835 err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos) <
836 HILL_POSX_BALLS_DOWN2,
837 TRAJ_FLAGS_SMALL_DIST);
838 DEBUG(E_USER_STRAT, "pack support balls");
839 support_balls_pack();
840 err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos) <
841 HILL_POSX_BALLS_DOWN3,
842 TRAJ_FLAGS_SMALL_DIST);
843 DEBUG(E_USER_STRAT, "deploy support balls");
844 strat_set_acc(ad, aa);
845 strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
846 support_balls_deploy();
847 err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
848 i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST);
850 /* wait to be near the wall */
851 err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos) < 200,
852 TRAJ_FLAGS_SMALL_DIST);
854 i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST);
856 /* restore BD coefs */
857 bd_set_current_thresholds(&mainboard.distance.bd, 500, 8000, 1000000, 20);
858 bd_set_current_thresholds(&mainboard.distance.bd, 500, 8000, 1000000, 20);
859 bd_set_speed_threshold(&mainboard.distance.bd, 60);
861 /* calibrate position on the wall */
862 strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST);
863 err = strat_calib(-400, TRAJ_FLAGS_SMALL_DIST);
864 if (orange_color == our_color)
865 strat_reset_pos(ROBOT_HALF_LENGTH_REAR,
866 DO_NOT_SET_POS, COLOR_A(0));
868 strat_reset_pos(AREA_X - ROBOT_HALF_LENGTH_REAR,
869 DO_NOT_SET_POS, COLOR_A(180));
870 strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
872 trajectory_d_rel(&mainboard.traj, 250);
873 err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
874 if (orange_color == I2C_COLOR_YELLOW)
875 trajectory_a_rel(&mainboard.traj, 90);
877 trajectory_a_rel(&mainboard.traj, -90);
878 err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
881 strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST);
882 err = strat_calib(-400, TRAJ_FLAGS_SMALL_DIST);
883 strat_reset_pos(DO_NOT_SET_POS,
884 COLOR_Y(ROBOT_HALF_LENGTH_REAR),
886 strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
888 trajectory_d_rel(&mainboard.traj, 250);
889 err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
891 /* revert acceleration and speed */
892 pid_set_gains(&mainboard.angle.pid, p, i, d);
893 pid_set_maximums(&mainboard.distance.pid, max_in, max_i, max_out);
895 strat_set_speed(sd, sa);
896 support_balls_deploy();
900 uint8_t strat_main(void)
902 uint8_t err, do_initturn = 1;
905 if (strat_conf.flags & STRAT_CONF_OUR_ORANGE) {
906 err = run_to_the_hills(get_color());
907 strat_db.our_oranges_count = 0;
911 /* harvest the first cobs + balls */
912 err = strat_beginning(do_initturn);
914 if (!TRAJ_SUCCESS(err))
919 /* choose circuit, and harvest on it */
922 DEBUG(E_USER_STRAT, "start main loop");
924 /* if it's time to get tomatoes, do it */
925 if (time_get_s() > strat_conf.orphan_tomato) {
926 err = get_orphan_tomatoes();
927 if (err == END_ERROR) {
929 "get_orphan_tomatoes returned END_ERROR");
931 else if (err == END_TIMER) {
932 DEBUG(E_USER_STRAT, "End of time");
936 else if (!TRAJ_SUCCESS(err)) {
937 /* don't retry these tomatoes if it failed */
938 strat_conf.orphan_tomato = 90;
943 /* if it's time to get opponent oranges, do it */
944 if (time_get_s() > strat_conf.opp_orange) {
945 err = get_opp_oranges();
946 if (err == END_ERROR) {
948 "get_opp_oranges returned END_ERROR");
950 else if (err == END_TIMER) {
951 DEBUG(E_USER_STRAT, "End of time");
955 else if (!TRAJ_SUCCESS(err)) {
956 /* don't retry oranges if it failed */
957 strat_conf.opp_orange = 90;
962 /**********************/
963 /* harvest on circuit */
964 /**********************/
966 err = strat_harvest_circuit();
967 if (err == END_TIMER) {
968 DEBUG(E_USER_STRAT, "End of time");
972 if (!TRAJ_SUCCESS(err)) {
977 /***********************/
978 /* eject game elements */
979 /***********************/
982 /* end of time exit ! */
983 if (err == END_TIMER) {
984 DEBUG(E_USER_STRAT, "End of time");
988 if (!TRAJ_SUCCESS(err)) {