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 /* called periodically (10ms) */
340 void strat_event(void *dummy)
342 /* ignore when strat is not running */
343 if (strat_running == 0)
349 /* limit speed when opponent is near */
350 /* disabled for 2010, we are already slow :) */
351 //strat_limit_speed();
354 /* check that we are on an eject line */
355 static uint8_t robot_is_on_eject_line(void)
360 x = position_get_x_s16(&mainboard.pos);
361 y = position_get_y_s16(&mainboard.pos);
363 if (xycoord_to_ijcoord(&x, &y, &i, &j) < 0)
366 if (!wp_belongs_to_line(i, j, 5, LINE_UP) &&
367 !wp_belongs_to_line(i, j, 2, LINE_R_UP))
373 /* 0 = fast, 1 = slow */
374 static uint8_t eject_select_speed(void)
379 x = position_get_x_s16(&mainboard.pos);
380 y = position_get_y_s16(&mainboard.pos);
382 if (get_cob_count() >= 5) {
387 if (xycoord_to_ijcoord(&x, &y, &i, &j) < 0) {
388 DEBUG(E_USER_STRAT, "%s(): cannot find waypoint at %d,%d",
393 if (corn_count_neigh(i, j) == 2)
399 /* called multiple times while we are waiting to reach the ejection
401 static uint8_t speedify_eject(void)
403 if (eject_select_speed())
404 strat_set_speed(SPEED_CLITOID_SLOW, SPEED_ANGLE_SLOW);
406 strat_set_speed(SPEED_CLITOID_FAST, SPEED_ANGLE_SLOW);
410 /* must be called from a terminal line */
411 static uint8_t strat_eject(void)
415 DEBUG(E_USER_STRAT, "%s() cob_count=%d ball_count=%d",
416 __FUNCTION__, get_cob_count(), get_ball_count());
418 /* check that we are called from an eject line */
419 if (!robot_is_on_eject_line()) {
420 DEBUG(E_USER_STRAT, "%s() not on eject line", __FUNCTION__);
424 /* go to eject point */
425 trajectory_goto_forward_xy_abs(&mainboard.traj, 2625, COLOR_Y(1847));
426 err = WAIT_COND_OR_TRAJ_END(speedify_eject(),
428 /* err is never == 0 because speedify_eject() always return 0 */
429 if (!TRAJ_SUCCESS(err))
432 /* pack arms (force), and disable strat_event */
433 strat_event_disable();
434 i2c_cobboard_pack_weak(I2C_LEFT_SIDE);
435 i2c_cobboard_pack_weak(I2C_RIGHT_SIDE);
438 if (get_ball_count() > 0) {
439 i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_EJECT);
440 trajectory_a_abs(&mainboard.traj, COLOR_A(70));
441 err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
442 if (!TRAJ_SUCCESS(err))
445 DEBUG(E_USER_STRAT, "%s():%d", __FUNCTION__, __LINE__);
450 WAIT_COND_OR_TIMEOUT(ballboard.status == I2C_BALLBOARD_STATUS_F_BUSY,
452 WAIT_COND_OR_TIMEOUT(ballboard.status == I2C_BALLBOARD_STATUS_F_READY,
458 trajectory_a_abs(&mainboard.traj, COLOR_A(-110));
459 err = wait_traj_end(END_INTR|END_TRAJ);
460 if (!TRAJ_SUCCESS(err))
464 trajectory_d_rel(&mainboard.traj, -70);
465 err = wait_traj_end(END_INTR|END_TRAJ);
466 if (!TRAJ_SUCCESS(err))
469 if (get_cob_count() > 0) {
470 i2c_cobboard_set_mode(I2C_COBBOARD_MODE_EJECT);
475 trajectory_d_rel(&mainboard.traj, 70);
476 err = wait_traj_end(END_INTR|END_TRAJ);
477 if (!TRAJ_SUCCESS(err))
480 strat_db_dump(__FUNCTION__);
484 strat_event_enable();
489 static uint8_t strat_beginning(uint8_t do_initturn)
493 strat_set_acc(ACC_DIST, ACC_ANGLE);
496 strat_set_speed(600, 60); /* OK */
497 trajectory_d_a_rel(&mainboard.traj, 1000, COLOR_A(20));
498 err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj),
502 strat_set_acc(ACC_DIST, ACC_ANGLE);
503 strat_set_speed(SPEED_CLITOID_SLOW, SPEED_ANGLE_SLOW);
505 err = line2line(0, LINE_UP, 2, LINE_R_DOWN, TRAJ_FLAGS_NO_NEAR);
506 if (!TRAJ_SUCCESS(err))
509 err = line2line(2, LINE_R_DOWN, 2, LINE_R_UP, TRAJ_FLAGS_NO_NEAR);
510 if (!TRAJ_SUCCESS(err)) {
517 /* dump state (every 5 s max) */
518 #define DUMP_RATE_LIMIT(dump, last_print) \
520 if (time_get_s() - last_print > 5) { \
522 last_print = time_get_s(); \
528 /* return true if we need to grab some more elements */
529 static uint8_t need_more_elements(void)
531 if (time_get_s() <= 75) {
532 /* we have enough time left */
533 if (get_ball_count() >= 4)
535 if (get_cob_count() >= 4)
537 if ((get_ball_count() >= 2) &&
538 (get_cob_count() >= 2))
543 /* not much time remaining */
544 if ((get_ball_count() >= 1) &&
545 (get_cob_count() >= 1))
552 /* get tomatoes near our goals (12,5 and 12,3) */
553 uint8_t get_opp_oranges(void)
559 DEBUG(E_USER_STRAT, "%s()", __FUNCTION__);
561 /* only if oranges are present */
562 if (strat_db.opp_oranges_count == 0)
565 strat_db.opp_oranges_count = 0;
566 x = position_get_x_s16(&mainboard.pos);
567 y = position_get_y_s16(&mainboard.pos);
569 if (xycoord_to_ijcoord(&x, &y, &i, &j) < 0)
572 /* not on eject point */
573 if (i != 11 || j != 6)
577 strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
579 /* turn in the correct direction */
580 trajectory_a_abs(&mainboard.traj, COLOR_A(-90));
581 err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
582 if (!TRAJ_SUCCESS(err))
585 trajectory_goto_forward_xy_abs(&mainboard.traj, 2625, COLOR_Y(597));
586 err = wait_traj_end(TRAJ_FLAGS_STD);
587 if (!TRAJ_SUCCESS(err))
590 strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW);
591 trajectory_goto_forward_xy_abs(&mainboard.traj, 2750, COLOR_Y(250));
592 err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
593 if (!TRAJ_SUCCESS(err))
596 err = run_to_the_hills(get_opponent_color());
603 /* get tomatoes near our goals (12,5 and 12,3) */
604 uint8_t get_orphan_tomatoes(void)
606 #define CLITOID_TOMATO_RADIUS 100.
607 #define TOMATO_BACK_X 2760
608 #define TOMATO_BACK_LEN 200
615 DEBUG(E_USER_STRAT, "%s()", __FUNCTION__);
617 /* only go if both tomatoes are present */
618 if (!strat_db.wp_table[12][5].present ||
619 !strat_db.wp_table[12][3].present) {
623 x = position_get_x_s16(&mainboard.pos);
624 y = position_get_y_s16(&mainboard.pos);
626 if (xycoord_to_ijcoord(&x, &y, &i, &j) < 0)
629 /* not on eject point */
630 if (i != 11 || j != 6)
634 strat_set_speed(SPEED_CLITOID_FAST, SPEED_ANGLE_SLOW);
636 /* turn in the correct direction */
637 trajectory_a_abs(&mainboard.traj, COLOR_A(-90));
638 err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
640 /* clitoid to turn and take the first ball */
641 ret = trajectory_clitoid(&mainboard.traj, 2625, COLOR_Y(1847),
642 COLOR_A(-90), 150., COLOR_A(90), 0,
643 CLITOID_TOMATO_RADIUS, 3*125);
649 err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
650 if (!TRAJ_SUCCESS(err))
653 strat_set_speed(SPEED_CLITOID_SLOW, SPEED_ANGLE_SLOW);
654 err = strat_calib(300, END_TRAJ|END_BLOCKING);
655 a = position_get_a_deg_s16(&mainboard.pos);
657 strat_reset_pos(AREA_X - ROBOT_HALF_LENGTH_FRONT,
661 strat_set_speed(SPEED_CLITOID_FAST, SPEED_ANGLE_FAST);
662 trajectory_d_rel(&mainboard.traj, -250);
663 err = WAIT_COND_OR_TRAJ_END(!x_is_more_than(TOMATO_BACK_X),
666 if (err != 0 && !TRAJ_SUCCESS(err))
669 trajectory_d_a_rel(&mainboard.traj, -TOMATO_BACK_LEN, COLOR_A(-90));
670 err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
674 /* clitoid to turn and take the first ball */
675 strat_set_speed(SPEED_CLITOID_FAST, SPEED_ANGLE_SLOW);
676 ret = trajectory_clitoid(&mainboard.traj, 2625, COLOR_Y(1847),
677 COLOR_A(-90), 150., COLOR_A(90), 0,
678 CLITOID_TOMATO_RADIUS, 7*125);
683 err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
684 if (!TRAJ_SUCCESS(err))
688 strat_set_speed(SPEED_CLITOID_FAST, SPEED_ANGLE_FAST);
689 trajectory_d_rel(&mainboard.traj, -250);
690 err = WAIT_COND_OR_TRAJ_END(!x_is_more_than(TOMATO_BACK_X),
692 trajectory_d_a_rel(&mainboard.traj, -TOMATO_BACK_LEN, COLOR_A(-90));
693 err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
701 #define HILL_LEN 1000
704 #define HILL_POSY_YELLOW 310
705 #define HILL_POSY_BLUE 190
707 #define HILL_POSX_BALLS_DOWN1 830
708 #define HILL_POSX_BALLS_DOWN2 920
709 #define HILL_POSX_BALLS_DOWN3 730
710 #define HILL_START_POSX 580
712 uint8_t prepare_hill(uint8_t orange_color, int16_t posx)
714 int16_t startx, starty;
715 uint8_t our_color = get_color();
718 if (orange_color == I2C_COLOR_YELLOW)
719 starty = HILL_POSY_YELLOW;
721 starty = HILL_POSY_BLUE;
722 if (orange_color == our_color)
725 startx = AREA_X - posx;
726 trajectory_goto_forward_xy_abs(&mainboard.traj, startx, COLOR_Y(starty));
727 err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
728 if (!TRAJ_SUCCESS(err))
731 /* turn to the hills */
732 if (orange_color == our_color)
733 trajectory_a_abs(&mainboard.traj, COLOR_A(HILL_ANGLE));
735 trajectory_a_abs(&mainboard.traj, COLOR_A(-180+HILL_ANGLE));
736 err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
737 if (!TRAJ_SUCCESS(err))
743 /* get oranges, must be called near game area */
744 uint8_t run_to_the_hills(uint8_t orange_color)
749 uint8_t our_color = get_color();
750 int32_t p = pid_get_gain_P(&mainboard.angle.pid);
751 int32_t i = pid_get_gain_I(&mainboard.angle.pid);
752 int32_t d = pid_get_gain_D(&mainboard.angle.pid);
753 int32_t max_in = pid_get_max_in(&mainboard.angle.pid);
754 int32_t max_i = pid_get_max_I(&mainboard.angle.pid);
755 int32_t max_out = pid_get_max_out(&mainboard.angle.pid);
758 strat_get_acc(&ad, &aa);
759 strat_get_speed(&sd, &sa);
761 DEBUG(E_USER_STRAT, "%s()", __FUNCTION__);
763 strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
764 err = prepare_hill(orange_color, HILL_START_POSX);
765 if (!TRAJ_SUCCESS(err))
768 strat_set_acc(5, ACC_ANGLE);
769 strat_set_speed(300, SPEED_ANGLE_SLOW);
770 bd_set_current_thresholds(&mainboard.distance.bd, 500, 8000, 2000000, 80);
771 bd_set_current_thresholds(&mainboard.angle.bd, 500, 8000, 2000000, 80);
772 bd_set_speed_threshold(&mainboard.distance.bd, 10);
773 support_balls_pack();
774 i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_PREP_FORK);
776 /* decrease angle gains */
777 pid_set_gains(&mainboard.angle.pid, 200, 0, 2000);
779 /* here it is difficult to handle return values, because we
781 i2c_cobboard_set_mode(I2C_COBBOARD_MODE_KICKSTAND_DOWN);
782 trajectory_d_rel(&mainboard.traj, HILL_LEN);
783 err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos) >
784 HILL_POSX_BALLS_DOWN1,
785 TRAJ_FLAGS_SMALL_DIST);
786 DEBUG(E_USER_STRAT, "deploy support balls");
787 support_balls_deploy();
788 i2c_cobboard_set_mode(I2C_COBBOARD_MODE_KICKSTAND_UP);
789 trajectory_only_a_rel(&mainboard.traj, 2);
790 err = WAIT_COND_OR_TE_TO(0, 0, 2200);
792 i2c_cobboard_set_mode(I2C_COBBOARD_MODE_KICKSTAND_DOWN);
795 i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_TAKE_FORK);
799 /* reach top, go down */
800 trajectory_d_rel(&mainboard.traj, -HILL_LEN);
802 err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos) <
803 HILL_POSX_BALLS_DOWN2,
804 TRAJ_FLAGS_SMALL_DIST);
805 DEBUG(E_USER_STRAT, "pack support balls");
806 support_balls_pack();
807 err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos) <
808 HILL_POSX_BALLS_DOWN3,
809 TRAJ_FLAGS_SMALL_DIST);
810 DEBUG(E_USER_STRAT, "deploy support balls");
811 strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
812 strat_set_acc(ad, aa);
813 support_balls_deploy();
814 err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
815 i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST);
817 /* wait to be near the wall */
818 err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos) < 200,
819 TRAJ_FLAGS_SMALL_DIST);
821 i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST);
823 /* restore BD coefs */
824 bd_set_current_thresholds(&mainboard.distance.bd, 500, 8000, 1000000, 20);
825 bd_set_current_thresholds(&mainboard.distance.bd, 500, 8000, 1000000, 20);
826 bd_set_speed_threshold(&mainboard.distance.bd, 60);
828 /* calibrate position on the wall */
829 strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST);
830 err = strat_calib(-400, TRAJ_FLAGS_SMALL_DIST);
831 if (orange_color == our_color)
832 strat_reset_pos(ROBOT_HALF_LENGTH_REAR,
833 DO_NOT_SET_POS, COLOR_A(0));
835 strat_reset_pos(AREA_X - ROBOT_HALF_LENGTH_REAR,
836 DO_NOT_SET_POS, COLOR_A(180));
837 strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
839 trajectory_d_rel(&mainboard.traj, 250);
840 err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
841 if (orange_color == I2C_COLOR_YELLOW)
842 trajectory_a_rel(&mainboard.traj, 90);
844 trajectory_a_rel(&mainboard.traj, -90);
845 err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
848 strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST);
849 err = strat_calib(-400, TRAJ_FLAGS_SMALL_DIST);
850 strat_reset_pos(DO_NOT_SET_POS,
851 COLOR_Y(ROBOT_HALF_LENGTH_REAR),
853 strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
855 trajectory_d_rel(&mainboard.traj, 250);
856 err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
858 /* revert acceleration and speed */
859 pid_set_gains(&mainboard.angle.pid, p, i, d);
860 pid_set_maximums(&mainboard.distance.pid, max_in, max_i, max_out);
862 strat_set_speed(sd, sa);
863 support_balls_deploy();
867 uint8_t strat_main(void)
869 uint8_t err, do_initturn = 1;
872 if (strat_conf.flags & STRAT_CONF_OUR_ORANGE) {
873 err = run_to_the_hills(get_color());
874 strat_db.our_oranges_count = 0;
878 /* harvest the first cobs + balls */
879 err = strat_beginning(do_initturn);
881 if (!TRAJ_SUCCESS(err))
886 /* choose circuit, and harvest on it */
889 DEBUG(E_USER_STRAT, "start main loop");
891 /* if it's time to get tomatoes, do it */
892 if (time_get_s() > strat_conf.orphan_tomato) {
893 err = get_orphan_tomatoes();
894 if (err == END_ERROR) {
896 "get_orphan_tomatoes returned END_ERROR");
898 else if (err == END_TIMER) {
899 DEBUG(E_USER_STRAT, "End of time");
903 else if (!TRAJ_SUCCESS(err)) {
904 /* don't retry these tomatoes if it failed */
905 strat_conf.orphan_tomato = 90;
910 /* if it's time to get opponent oranges, do it */
911 if (time_get_s() > strat_conf.opp_orange) {
912 err = get_opp_oranges();
913 if (err == END_ERROR) {
915 "get_opp_oranges returned END_ERROR");
917 else if (err == END_TIMER) {
918 DEBUG(E_USER_STRAT, "End of time");
922 else if (!TRAJ_SUCCESS(err)) {
923 /* don't retry oranges if it failed */
924 strat_conf.opp_orange = 90;
929 /**********************/
930 /* harvest on circuit */
931 /**********************/
933 err = strat_harvest_circuit();
934 if (err == END_TIMER) {
935 DEBUG(E_USER_STRAT, "End of time");
939 if (!TRAJ_SUCCESS(err)) {
944 /***********************/
945 /* eject game elements */
946 /***********************/
949 /* end of time exit ! */
950 if (err == END_TIMER) {
951 DEBUG(E_USER_STRAT, "End of time");
955 if (!TRAJ_SUCCESS(err)) {