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"
63 #include "strat_base.h"
64 #include "strat_corn.h"
65 #include "strat_utils.h"
66 #include "strat_avoid.h"
70 #define COL_DISP_MARGIN 400 /* stop 40 cm in front of dispenser */
71 #define COL_SCAN_PRE_MARGIN 250
73 static volatile uint8_t strat_running = 0;
74 volatile uint8_t strat_want_pack = 0;
75 volatile uint8_t strat_lpack60 = 0;
76 volatile uint8_t strat_rpack60 = 0;
77 struct strat_conf strat_conf = {
84 /*************************************************************/
88 /*************************************************************/
90 /* called before each strat, and before the start switch */
91 void strat_preinit(void)
94 interrupt_traj_reset();
95 mainboard.flags = DO_ENCODERS | DO_CS | DO_RS |
96 DO_POS | DO_BD | DO_POWER;
99 strat_conf_dump(__FUNCTION__);
100 strat_db_dump(__FUNCTION__);
103 void strat_conf_dump(const char *caller)
105 if (!strat_conf.dump_enabled)
108 printf_P(PSTR("-- conf --\r\n"));
109 printf_P(PSTR("opp_orange = %d\n"), strat_conf.opp_orange);
110 printf_P(PSTR("orphan_tomato = %d\n"), strat_conf.orphan_tomato);
111 printf_P(PSTR("our_orange = %s\n"),
112 (strat_conf.flags & STRAT_CONF_OUR_ORANGE) ? "y":"n");
115 void strat_event_enable(void)
120 void strat_event_disable(void)
125 /* call it just before launching the strat */
126 void strat_init(void)
129 position_set(&mainboard.pos, 298.16,
130 COLOR_Y(308.78), COLOR_A(70.00));
133 /* we consider that the color is correctly set */
135 strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
137 interrupt_traj_reset();
139 i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST);
140 i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_HARVEST);
142 /* used in strat_base for END_TIMER */
143 mainboard.flags = DO_ENCODERS | DO_CS | DO_RS |
144 DO_POS | DO_BD | DO_TIMER | DO_POWER;
147 /* call it after each strat */
148 void strat_exit(void)
155 mainboard.flags &= ~(DO_TIMER);
161 mainboard.flags &= ~(DO_CS);
163 pwm_ng_set(LEFT_PWM, 0);
164 pwm_ng_set(RIGHT_PWM, 0);
168 /* mark tomato as not present */
169 static void check_tomato(void)
173 static uint8_t prev_check_time;
176 /* check present tomatoes once per second */
177 cur_time = time_get_s();
178 if (cur_time != prev_check_time) {
181 for (k = 0; k < TOMATO_NB; k++) {
182 if (strat_db.tomato_table[k]->time_removed != -1 &&
183 strat_db.tomato_table[k]->time_removed + 2 == time_get_s()) {
185 printf("remove tomato %d\n", k);
187 strat_db.tomato_table[k]->present = 0;
190 prev_check_time = cur_time;
193 x = position_get_x_s16(&mainboard.pos);
194 y = position_get_y_s16(&mainboard.pos);
196 if (xycoord_to_ijcoord(&x, &y, &i, &j) < 0)
199 if (strat_db.wp_table[i][j].type != WP_TYPE_TOMATO)
202 if (strat_db.wp_table[i][j].present == 0)
205 strat_db.wp_table[i][j].time_removed = time_get_s();
206 strat_db.wp_table[i][j].present = 0;
208 ballboard.ball_count ++;
209 printf("add ball %d,%d\n", i, j);
213 /* mark corn as not present and give correct commands to the cobboard
215 static void check_corn(void)
218 int8_t lcob_near, rcob_near;
221 static uint8_t prev_check_time;
224 /* read sensors from ballboard */
226 lcob = ballboard.lcob;
227 ballboard.lcob = I2C_COB_NONE;
228 rcob = ballboard.rcob;
229 ballboard.rcob = I2C_COB_NONE;
232 /* XXX take opponent position into account */
234 /* check present cobs once per second */
235 cur_time = time_get_s();
236 if (cur_time != prev_check_time) {
239 for (i = 0; i < CORN_NB; i++) {
240 if (strat_db.corn_table[i]->time_removed != -1 &&
241 strat_db.corn_table[i]->time_removed + 2 == time_get_s()) {
243 printf("remove cob %d\n", i);
245 strat_db.corn_table[i]->present = 0;
248 prev_check_time = cur_time;
251 /* detect cob on left side */
252 lcob_near = corn_is_near(&lidx, I2C_LEFT_SIDE);
253 if (lcob_near && lcob != I2C_COB_NONE) {
254 if (strat_db.corn_table[lidx]->corn.color == I2C_COB_UNKNOWN)
255 DEBUG(E_USER_STRAT, "lcob %s %d",
256 lcob == I2C_COB_WHITE ? "white" : "black", lidx);
257 corn_set_color(strat_db.corn_table[lidx], lcob);
260 /* detect cob on right side */
261 rcob_near = corn_is_near(&ridx, I2C_RIGHT_SIDE);
262 if (rcob_near && rcob != I2C_COB_NONE) {
263 if (strat_db.corn_table[ridx]->corn.color == I2C_COB_UNKNOWN)
264 DEBUG(E_USER_STRAT, "rcob %s %d",
265 rcob == I2C_COB_WHITE ? "white" : "black", ridx);
266 corn_set_color(strat_db.corn_table[ridx], rcob);
269 /* control the cobboard mode for left spickle */
270 if (lcob_near && strat_db.corn_table[lidx]->present) {
271 if (get_cob_count() >= 5 || strat_want_pack || strat_lpack60) {
275 /* deploy spickle and harvest white ones */
276 if (strat_db.corn_table[lidx]->corn.color == I2C_COB_WHITE) {
277 i2c_cobboard_autoharvest_nomove(I2C_LEFT_SIDE);
278 if (strat_db.corn_table[lidx]->time_removed == -1
280 && cobboard.status == I2C_COBBOARD_STATUS_LBUSY
283 strat_db.corn_table[lidx]->time_removed = time_get_s();
285 cobboard.cob_count ++;
286 printf("add cob %d\n", lidx);
291 i2c_cobboard_deploy_nomove(I2C_LEFT_SIDE);
295 /* no cob near us, we can pack or deploy freely */
296 if (get_cob_count() >= 5 || strat_want_pack || strat_lpack60)
297 i2c_cobboard_pack_weak(I2C_LEFT_SIDE);
299 i2c_cobboard_deploy(I2C_LEFT_SIDE);
302 /* control the cobboard mode for right spickle */
303 if (rcob_near && strat_db.corn_table[ridx]->present) {
304 if (get_cob_count() >= 5 || strat_want_pack || strat_rpack60) {
308 /* deploy spickle and harvest white ones */
309 if (strat_db.corn_table[ridx]->corn.color == I2C_COB_WHITE) {
310 i2c_cobboard_autoharvest_nomove(I2C_RIGHT_SIDE);
311 if (strat_db.corn_table[ridx]->time_removed == -1
313 && cobboard.status == I2C_COBBOARD_STATUS_RBUSY
316 strat_db.corn_table[ridx]->time_removed = time_get_s();
318 cobboard.cob_count ++;
319 printf("add cob %d\n", ridx);
324 i2c_cobboard_deploy_nomove(I2C_RIGHT_SIDE);
328 /* no cob near us, we can pack or deploy freely */
329 if (get_cob_count() >= 5 || strat_want_pack || strat_rpack60)
330 i2c_cobboard_pack_weak(I2C_RIGHT_SIDE);
332 i2c_cobboard_deploy(I2C_RIGHT_SIDE);
336 /* called periodically (10ms) */
337 void strat_event(void *dummy)
339 /* ignore when strat is not running */
340 if (strat_running == 0)
346 /* limit speed when opponent is near */
347 /* disabled for 2010, we are already slow :) */
348 //strat_limit_speed();
351 /* check that we are on an eject line */
352 static uint8_t robot_is_on_eject_line(void)
357 x = position_get_x_s16(&mainboard.pos);
358 y = position_get_y_s16(&mainboard.pos);
360 if (xycoord_to_ijcoord(&x, &y, &i, &j) < 0)
363 if (!wp_belongs_to_line(i, j, 5, LINE_UP) &&
364 !wp_belongs_to_line(i, j, 2, LINE_R_UP))
370 /* 0 = fast, 1 = slow */
371 static uint8_t eject_select_speed(void)
376 x = position_get_x_s16(&mainboard.pos);
377 y = position_get_y_s16(&mainboard.pos);
379 if (get_cob_count() >= 5) {
384 if (xycoord_to_ijcoord(&x, &y, &i, &j) < 0) {
385 DEBUG(E_USER_STRAT, "%s(): cannot find waypoint at %d,%d",
390 if (corn_count_neigh(i, j) == 2)
396 /* called multiple times while we are waiting to reach the ejection
398 static uint8_t speedify_eject(void)
400 if (eject_select_speed())
401 strat_set_speed(SPEED_CLITOID_SLOW, SPEED_ANGLE_SLOW);
403 strat_set_speed(SPEED_CLITOID_FAST, SPEED_ANGLE_SLOW);
407 /* must be called from a terminal line */
408 static uint8_t strat_eject(void)
412 DEBUG(E_USER_STRAT, "%s() cob_count=%d ball_count=%d",
413 __FUNCTION__, get_cob_count(), get_ball_count());
415 /* check that we are called from an eject line */
416 if (!robot_is_on_eject_line()) {
417 DEBUG(E_USER_STRAT, "%s() not on eject line", __FUNCTION__);
421 /* go to eject point */
422 trajectory_goto_xy_abs(&mainboard.traj, 2625, COLOR_Y(1847));
423 err = WAIT_COND_OR_TRAJ_END(speedify_eject(),
425 /* err is never == 0 because speedify_eject() always return 0 */
426 if (!TRAJ_SUCCESS(err))
429 /* pack arms (force), and disable strat_event */
430 strat_event_disable();
431 i2c_cobboard_pack_weak(I2C_LEFT_SIDE);
432 i2c_cobboard_pack_weak(I2C_RIGHT_SIDE);
435 if (get_ball_count() > 0) {
436 i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_EJECT);
437 trajectory_a_abs(&mainboard.traj, COLOR_A(70));
438 err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
439 if (!TRAJ_SUCCESS(err))
442 DEBUG(E_USER_STRAT, "%s():%d", __FUNCTION__, __LINE__);
447 WAIT_COND_OR_TIMEOUT(ballboard.status == I2C_BALLBOARD_STATUS_F_BUSY,
449 WAIT_COND_OR_TIMEOUT(ballboard.status == I2C_BALLBOARD_STATUS_F_READY,
455 trajectory_a_abs(&mainboard.traj, COLOR_A(-110));
456 err = wait_traj_end(END_INTR|END_TRAJ);
457 if (!TRAJ_SUCCESS(err))
461 trajectory_d_rel(&mainboard.traj, -70);
462 err = wait_traj_end(END_INTR|END_TRAJ);
463 if (!TRAJ_SUCCESS(err))
466 if (get_cob_count() > 0) {
467 i2c_cobboard_set_mode(I2C_COBBOARD_MODE_EJECT);
472 trajectory_d_rel(&mainboard.traj, 70);
473 err = wait_traj_end(END_INTR|END_TRAJ);
474 if (!TRAJ_SUCCESS(err))
477 strat_db_dump(__FUNCTION__);
481 strat_event_enable();
486 static uint8_t strat_beginning(uint8_t do_initturn)
490 strat_set_acc(ACC_DIST, ACC_ANGLE);
493 strat_set_speed(600, 60); /* OK */
494 trajectory_d_a_rel(&mainboard.traj, 1000, COLOR_A(20));
495 err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj),
499 strat_set_acc(ACC_DIST, ACC_ANGLE);
500 strat_set_speed(SPEED_CLITOID_SLOW, SPEED_ANGLE_SLOW);
502 err = line2line(0, LINE_UP, 2, LINE_R_DOWN, TRAJ_FLAGS_NO_NEAR);
503 if (!TRAJ_SUCCESS(err))
506 err = line2line(2, LINE_R_DOWN, 2, LINE_R_UP, TRAJ_FLAGS_NO_NEAR);
507 if (!TRAJ_SUCCESS(err)) {
514 /* dump state (every 5 s max) */
515 #define DUMP_RATE_LIMIT(dump, last_print) \
517 if (time_get_s() - last_print > 5) { \
519 last_print = time_get_s(); \
525 /* return true if we need to grab some more elements */
526 static uint8_t need_more_elements(void)
528 if (time_get_s() <= 75) {
529 /* we have enough time left */
530 if (get_ball_count() >= 4)
532 if (get_cob_count() >= 4)
534 if ((get_ball_count() >= 2) &&
535 (get_cob_count() >= 2))
540 /* not much time remaining */
541 if ((get_ball_count() >= 1) &&
542 (get_cob_count() >= 1))
549 /* get tomatoes near our goals (12,5 and 12,3) */
550 uint8_t get_orphan_tomatoes(void)
552 #define CLITOID_TOMATO_RADIUS 100.
553 #define TOMATO_BACK_X 2760
554 #define TOMATO_BACK_LEN 200
561 DEBUG(E_USER_STRAT, "%s()", __FUNCTION__);
563 /* only go if both tomatoes are present */
564 if (!strat_db.wp_table[12][5].present ||
565 !strat_db.wp_table[12][3].present) {
569 x = position_get_x_s16(&mainboard.pos);
570 y = position_get_y_s16(&mainboard.pos);
572 if (xycoord_to_ijcoord(&x, &y, &i, &j) < 0)
575 /* not on eject point */
576 if (i != 11 || j != 6)
580 strat_set_speed(SPEED_CLITOID_FAST, SPEED_ANGLE_SLOW);
582 /* turn in the correct direction */
583 trajectory_a_abs(&mainboard.traj, COLOR_A(-90));
584 err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
586 /* clitoid to turn and take the first ball */
587 ret = trajectory_clitoid(&mainboard.traj, 2625, COLOR_Y(1847),
588 COLOR_A(-90), 150., COLOR_A(90), 0,
589 CLITOID_TOMATO_RADIUS, 3*125);
595 err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
596 if (!TRAJ_SUCCESS(err))
599 strat_set_speed(SPEED_CLITOID_SLOW, SPEED_ANGLE_SLOW);
600 err = strat_calib(300, END_TRAJ|END_BLOCKING);
601 a = position_get_a_deg_s16(&mainboard.pos);
603 strat_reset_pos(AREA_X - ROBOT_HALF_LENGTH_FRONT,
607 strat_set_speed(SPEED_CLITOID_FAST, SPEED_ANGLE_FAST);
608 trajectory_d_rel(&mainboard.traj, -250);
609 err = WAIT_COND_OR_TRAJ_END(!x_is_more_than(TOMATO_BACK_X),
611 #ifdef HOST_VERSION /* simulator sees 2 blockings */
612 if (err == END_BLOCKING) {
613 trajectory_d_rel(&mainboard.traj, -250);
614 err = WAIT_COND_OR_TRAJ_END(!x_is_more_than(TOMATO_BACK_X),
619 if (err != 0 && !TRAJ_SUCCESS(err))
622 trajectory_d_a_rel(&mainboard.traj, -TOMATO_BACK_LEN, COLOR_A(-90));
623 err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
627 /* clitoid to turn and take the first ball */
628 strat_set_speed(SPEED_CLITOID_FAST, SPEED_ANGLE_SLOW);
629 ret = trajectory_clitoid(&mainboard.traj, 2625, COLOR_Y(1847),
630 COLOR_A(-90), 150., COLOR_A(90), 0,
631 CLITOID_TOMATO_RADIUS, 7*125);
636 err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
637 if (!TRAJ_SUCCESS(err))
641 strat_set_speed(SPEED_CLITOID_FAST, SPEED_ANGLE_FAST);
642 trajectory_d_rel(&mainboard.traj, -250);
643 err = WAIT_COND_OR_TRAJ_END(!x_is_more_than(TOMATO_BACK_X),
645 trajectory_d_a_rel(&mainboard.traj, -TOMATO_BACK_LEN, COLOR_A(-90));
646 err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
653 /* get oranges, must be called near game area */
654 uint8_t run_to_the_hills(uint8_t orange_color)
657 #define HILL_POSY_YELLOW 300
658 #define HILL_POSY_BLUE 200
659 #define HILL_POSX_BALLS_DOWN 900
660 #define HILL_START_POSX 580
665 uint8_t our_color = get_color();
666 int16_t startx, starty;
669 strat_get_acc(&ad, &aa);
670 strat_get_speed(&sd, &sa);
672 DEBUG(E_USER_STRAT, "%s()", __FUNCTION__);
674 strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
675 if (orange_color == I2C_COLOR_YELLOW)
676 starty = HILL_POSY_YELLOW;
678 starty = HILL_POSY_BLUE;
679 if (orange_color == our_color)
680 startx = HILL_START_POSX;
682 startx = AREA_X - HILL_START_POSX;
683 trajectory_goto_xy_abs(&mainboard.traj, startx, COLOR_Y(starty));
684 err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
686 /* turn to the hills */
687 if (orange_color == our_color)
688 trajectory_a_abs(&mainboard.traj, COLOR_A(0));
690 trajectory_a_abs(&mainboard.traj, COLOR_A(180));
691 err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
694 strat_set_speed(300, 500);
695 bd_set_current_thresholds(&mainboard.distance.bd, 500, 8000, 2000000, 40);
696 support_balls_pack();
698 /* here it is difficult to handle return values, because we
700 trajectory_d_rel(&mainboard.traj, HILL_LEN);
701 err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos) > HILL_POSX_BALLS_DOWN,
703 printf_P(PSTR("deploy\r\n"));
704 support_balls_deploy();
705 i2c_cobboard_set_mode(I2C_COBBOARD_MODE_KICKSTAND);
706 err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
710 /* reach top, go down */
711 trajectory_d_rel(&mainboard.traj, -HILL_LEN);
713 err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos) < HILL_POSX_BALLS_DOWN,
715 printf_P(PSTR("pack\r\n"));
716 support_balls_pack();
717 err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
718 printf_P(PSTR("deploy\r\n"));
719 support_balls_deploy();
721 /* calibrate position on the wall */
722 strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
723 strat_set_acc(ad, aa);
724 trajectory_goto_xy_abs(&mainboard.traj, 150, COLOR_Y(starty));
725 err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos) < 300,
727 strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST);
728 err = strat_calib(-300, TRAJ_FLAGS_NO_NEAR);
729 if (orange_color == our_color)
730 position_set(&mainboard.pos, ROBOT_HALF_LENGTH_REAR,
731 DO_NOT_SET_POS, COLOR_A(0));
733 position_set(&mainboard.pos, AREA_X - ROBOT_HALF_LENGTH_REAR,
734 DO_NOT_SET_POS, COLOR_A(180));
735 strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
736 trajectory_goto_xy_abs(&mainboard.traj, 150, COLOR_Y(starty));
737 err = wait_traj_end(TRAJ_FLAGS_STD);
739 /* revert acceleration and speed */
740 i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST);
742 strat_set_speed(sd, sa);
743 bd_set_current_thresholds(&mainboard.distance.bd, 500, 8000, 1000000, 20);
744 support_balls_deploy();
748 uint8_t strat_main(void)
750 uint8_t err, do_initturn = 1;
753 if (strat_conf.flags & STRAT_CONF_OUR_ORANGE) {
754 err = run_to_the_hills(get_color());
758 /* harvest the first cobs + balls */
759 err = strat_beginning(do_initturn);
761 if (!TRAJ_SUCCESS(err))
766 /* choose circuit, and harvest on it */
769 DEBUG(E_USER_STRAT, "start main loop");
771 /* if it's time to get tomatoes, do it */
772 if (time_get_s() > strat_conf.orphan_tomato) {
773 err = get_orphan_tomatoes();
774 if (err == END_ERROR) {
776 "get_orphan_tomatoes returned END_ERROR");
778 else if (err == END_TIMER) {
779 DEBUG(E_USER_STRAT, "End of time");
783 else if (!TRAJ_SUCCESS(err)) {
784 /* don't retry these tomatoes if it failed */
785 strat_conf.orphan_tomato = 90;
790 /**********************/
791 /* harvest on circuit */
792 /**********************/
794 err = strat_harvest_circuit();
795 if (err == END_TIMER) {
796 DEBUG(E_USER_STRAT, "End of time");
800 if (!TRAJ_SUCCESS(err)) {
805 /***********************/
806 /* eject game elements */
807 /***********************/
810 /* end of time exit ! */
811 if (err == END_TIMER) {
812 DEBUG(E_USER_STRAT, "End of time");
816 if (!TRAJ_SUCCESS(err)) {