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>
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>
51 #include <diagnostic.h>
56 #include "../common/i2c_commands.h"
57 #include "i2c_protocol.h"
60 #include "strat_base.h"
61 #include "strat_utils.h"
65 #define COL_DISP_MARGIN 400 /* stop 40 cm in front of dispenser */
66 #define COL_SCAN_PRE_MARGIN 250
71 #define BEACON_MAX_SAMPLES 100
72 struct beacon_sample {
81 static struct beacon_sample beacon_sample[BEACON_MAX_SAMPLES];
82 static uint8_t beacon_prev_time = 0;
83 static uint8_t beacon_cur_idx = 0;
85 static void beacon_update_samples(void)
87 int16_t opp_a, opp_d, opp_x, opp_y;
93 /* one sample per second max */
94 if (time <= beacon_prev_time)
96 /* limit max number of samples */
97 if (beacon_cur_idx >= BEACON_MAX_SAMPLES)
100 memset(&beacon_sample[beacon_cur_idx], 0, sizeof(beacon_sample[beacon_cur_idx]));
101 beacon_prev_time = time;
102 beacon_sample[beacon_cur_idx].time = time;
104 /* get opponent pos; if not found, just set struct to 0 */
105 err = get_opponent_xyda(&opp_x, &opp_y, &opp_d, &opp_a);
109 beacon_sample[beacon_cur_idx].posx = position_get_x_s16(&mainboard.pos);
110 beacon_sample[beacon_cur_idx].posy = position_get_y_s16(&mainboard.pos);
111 beacon_sample[beacon_cur_idx].posa = position_get_a_deg_s16(&mainboard.pos);
112 beacon_sample[beacon_cur_idx].oppx = opp_x;
113 beacon_sample[beacon_cur_idx].oppy = opp_y;
117 void beacon_dump_samples(void)
121 for (i=0; i<BEACON_MAX_SAMPLES; i++) {
122 printf_P(PSTR("%d: pos=(%d,%d,%d) opp=(%d,%d) time=%d\r\n"),
124 beacon_sample[i].posx,
125 beacon_sample[i].posy,
126 beacon_sample[i].posa,
127 beacon_sample[i].oppx,
128 beacon_sample[i].oppy,
129 beacon_sample[i].time);
134 struct strat_infos strat_infos = {
138 /* scanner disabled by default */
139 .scan_opp_min_time = 90,
140 .delay_between_opp_scan = 90,
141 .scan_our_min_time = 90,
142 .delay_between_our_scan = 90,
144 .lintel_min_time = 0,
145 .scan_opp_angle = -1,
154 /* column dispensers ; be carreful, positions are
155 * color-dependent, so COLOR_Y() and COLOR_A() should be
156 * used. All angles here are _absolute_ */
158 .checkpoint_x = 2711 - COL_SCAN_PRE_MARGIN,
159 .checkpoint_y = AREA_Y - COL_DISP_MARGIN,
164 .recalib_y = AREA_Y - (ROBOT_LENGTH/2 + DIST_BACK_DISPENSER),
169 .checkpoint_x = AREA_X - COL_DISP_MARGIN,
170 .checkpoint_y = 800 - COL_SCAN_PRE_MARGIN,
174 .recalib_x = AREA_X - (ROBOT_LENGTH/2 + DIST_BACK_DISPENSER),
180 .checkpoint_x = AREA_X-COL_DISP_MARGIN,
181 .checkpoint_y = 1300 + COL_SCAN_PRE_MARGIN,
185 .recalib_x = AREA_X - (ROBOT_LENGTH/2 + DIST_BACK_DISPENSER),
191 /* lintel dispensers */
193 .x = 912, /* XXX for red only */
197 .x = 1312, /* XXX for red only */
203 #define ZONE_DISC_NUM 0
205 .flags = ZONE_F_VALID | ZONE_F_DISC,
211 #define ZONE_1A_NUM 1
213 .flags = ZONE_F_VALID,
215 .checkpoint_x = 1385,
216 .checkpoint_y = 1700,
219 #define ZONE_1B_NUM 2
221 .flags = ZONE_F_VALID,
223 .checkpoint_x = 1615,
224 .checkpoint_y = 1700,
227 #define ZONE_0B_NUM 3
229 .flags = ZONE_F_VALID,
231 .checkpoint_x = 2100,
232 .checkpoint_y = 1700,
235 #define ZONE_0A_NUM 4
237 .flags = ZONE_F_VALID,
240 .checkpoint_y = 1700,
246 /*************************************************************/
250 /*************************************************************/
252 void strat_set_bounding_box(void)
254 if (get_color() == I2C_COLOR_RED) {
255 strat_infos.area_bbox.x1 = 300;
256 strat_infos.area_bbox.y1 = 200;
257 strat_infos.area_bbox.x2 = 2720; /* needed for c1 */
258 strat_infos.area_bbox.y2 = 1800;
261 strat_infos.area_bbox.x1 = 200;
262 strat_infos.area_bbox.y1 = 300;
263 strat_infos.area_bbox.x2 = 2720; /* needed for c1 */
264 strat_infos.area_bbox.y2 = 1900;
267 polygon_set_boundingbox(strat_infos.area_bbox.x1,
268 strat_infos.area_bbox.y1,
269 strat_infos.area_bbox.x2,
270 strat_infos.area_bbox.y2);
273 /* called before each strat, and before the start switch */
274 void strat_preinit(void)
277 interrupt_traj_reset();
278 mainboard.flags = DO_ENCODERS | DO_CS | DO_RS |
279 DO_POS | DO_BD | DO_POWER;
281 i2c_mechboard_mode_init();
282 if (get_color() == I2C_COLOR_RED)
283 i2c_mechboard_mode_prepare_pickup(I2C_LEFT_SIDE);
285 i2c_mechboard_mode_prepare_pickup(I2C_RIGHT_SIDE);
288 strat_dump_infos(__FUNCTION__);
291 void strat_dump_conf(void)
293 if (!strat_infos.dump_enabled)
296 printf_P(PSTR("-- conf --\r\n"));
298 printf_P(PSTR(" one build on disc: "));
299 if (strat_infos.conf.flags & STRAT_CONF_ONLY_ONE_ON_DISC)
300 printf_P(PSTR("on\r\n"));
302 printf_P(PSTR("off\r\n"));
304 printf_P(PSTR(" bypass static2: "));
305 if (strat_infos.conf.flags & STRAT_CONF_BYPASS_STATIC2)
306 printf_P(PSTR("on\r\n"));
308 printf_P(PSTR("off\r\n"));
310 printf_P(PSTR(" take one lintel: "));
311 if (strat_infos.conf.flags & STRAT_CONF_TAKE_ONE_LINTEL)
312 printf_P(PSTR("on\r\n"));
314 printf_P(PSTR("off\r\n"));
316 printf_P(PSTR(" skip this temple when temple check fails: "));
317 if (strat_infos.conf.flags & STRAT_CONF_SKIP_WHEN_CHECK_FAILS)
318 printf_P(PSTR("on\r\n"));
320 printf_P(PSTR("off\r\n"));
322 printf_P(PSTR(" store static2: "));
323 if (strat_infos.conf.flags & STRAT_CONF_STORE_STATIC2)
324 printf_P(PSTR("on\r\n"));
326 printf_P(PSTR("off\r\n"));
328 printf_P(PSTR(" (big3) try to build a temple with 3 lintels: "));
329 if (strat_infos.conf.flags & STRAT_CONF_BIG_3_TEMPLE)
330 printf_P(PSTR("on\r\n"));
332 printf_P(PSTR("off\r\n"));
334 printf_P(PSTR(" early opponent scan: "));
335 if (strat_infos.conf.flags & STRAT_CONF_EARLY_SCAN)
336 printf_P(PSTR("on\r\n"));
338 printf_P(PSTR("off\r\n"));
340 printf_P(PSTR(" push opponent columns: "));
341 if (strat_infos.conf.flags & STRAT_CONF_PUSH_OPP_COLS)
342 printf_P(PSTR("on\r\n"));
344 printf_P(PSTR("off\r\n"));
346 printf_P(PSTR(" scan opponent min time: %d\r\n"),
347 strat_infos.conf.scan_opp_min_time);
348 printf_P(PSTR(" delay between oppnent scan: %d\r\n"),
349 strat_infos.conf.delay_between_opp_scan);
350 printf_P(PSTR(" scan our min time: %d\r\n"),
351 strat_infos.conf.scan_our_min_time);
352 printf_P(PSTR(" delay between our scan: %d\r\n"),
353 strat_infos.conf.delay_between_our_scan);
354 printf_P(PSTR(" wait opponent gone before scan: %d\r\n"),
355 strat_infos.conf.wait_opponent);
356 printf_P(PSTR(" lintel min time: %d\r\n"),
357 strat_infos.conf.lintel_min_time);
358 printf_P(PSTR(" scan_opp_angle: %d\r\n"),
359 strat_infos.conf.scan_opp_angle);
362 void strat_dump_temple(struct temple *temple)
364 if (!strat_infos.dump_enabled)
367 printf_P(PSTR(" temple %p (%s): "), temple, temple->zone->name);
369 if (temple->flags & TEMPLE_F_MONOCOL)
370 printf_P(PSTR("MONOCOL "));
372 printf_P(PSTR("BICOL "));
374 if (temple->flags & TEMPLE_F_ON_DISC)
375 printf_P(PSTR("ON_DISC "));
377 printf_P(PSTR("ON_ZONE_0_1 "));
379 if (temple->flags & TEMPLE_F_OPPONENT)
380 printf_P(PSTR("OPPONENT "));
382 printf_P(PSTR("OURS "));
384 if (temple->flags & TEMPLE_F_LINTEL)
385 printf_P(PSTR("LIN_ON_TOP "));
387 printf_P(PSTR("COL_ON_TOP "));
389 printf_P(PSTR("\r\n"));
391 printf_P(PSTR(" pos=(%d,%d,%d) ckpt=(%d,%d) ltime=%d\r\n"),
392 temple->x, temple->y, temple->a,
393 temple->checkpoint_x, temple->checkpoint_y,
394 temple->last_try_time);
395 printf_P(PSTR(" L: lev=%d da=%d,%d\r\n"),
396 temple->level_l, temple->dist_l, temple->angle_l);
397 printf_P(PSTR(" R: lev=%d da=%d,%d\r\n"),
398 temple->level_l, temple->dist_l, temple->angle_l);
401 void strat_dump_zone(struct build_zone *zone)
403 if (!strat_infos.dump_enabled)
406 printf_P(PSTR(" zone %s: "), zone->name);
408 if (zone->flags & ZONE_F_DISC)
409 printf_P(PSTR("DISC "));
410 else if (zone->flags & ZONE_F_ZONE1)
411 printf_P(PSTR("ZONE1 "));
412 else if (zone->flags & ZONE_F_ZONE0)
413 printf_P(PSTR("ZONE0 "));
415 if (zone->flags & ZONE_F_BUSY)
416 printf_P(PSTR("BUSY "));
418 printf_P(PSTR("FREE "));
420 printf_P(PSTR("\r\n"));
422 printf_P(PSTR(" lev=%d ckpt=(%d,%d) ltime=%d\r\n"),
424 zone->checkpoint_x, zone->checkpoint_y,
425 zone->last_try_time);
428 void strat_dump_static_cols(void)
430 if (!strat_infos.dump_enabled)
433 printf_P(PSTR(" static cols: l0=%d l1=%d l2=%d\r\n"),
434 strat_infos.s_cols.flags & STATIC_COL_LINE0_DONE,
435 strat_infos.s_cols.flags & STATIC_COL_LINE1_DONE,
436 strat_infos.s_cols.flags & STATIC_COL_LINE2_DONE);
439 void strat_dump_col_disp(void)
441 if (!strat_infos.dump_enabled)
444 printf_P(PSTR(" c1 cnt=%d ltt=%d\r\n"),
445 strat_infos.c1.count, strat_infos.c1.last_try_time);
446 printf_P(PSTR(" c2 cnt=%d ltt=%d\r\n"),
447 strat_infos.c2.count, strat_infos.c2.last_try_time);
448 printf_P(PSTR(" c3 cnt=%d ltt=%d\r\n"),
449 strat_infos.c3.count, strat_infos.c3.last_try_time);
452 void strat_dump_lin_disp(void)
454 if (!strat_infos.dump_enabled)
456 printf_P(PSTR(" l1 cnt=%d ltt=%d\r\n"),
457 strat_infos.l1.count, strat_infos.l1.last_try_time);
458 printf_P(PSTR(" l2 cnt=%d ltt=%d\r\n"),
459 strat_infos.l2.count, strat_infos.l2.last_try_time);
463 void strat_dump_all_temples(void)
465 struct temple *temple;
468 if (!strat_infos.dump_enabled)
471 for (i=0; i<MAX_TEMPLE; i++) {
472 temple = &strat_infos.temple_list[i];
473 if (!(temple->flags & TEMPLE_F_VALID))
475 strat_dump_temple(temple);
479 void strat_dump_all_zones(void)
481 struct build_zone *zone;
484 if (!strat_infos.dump_enabled)
487 for (i=0; i<MAX_ZONE; i++) {
488 zone = &strat_infos.zone_list[i];
489 if (!(zone->flags & ZONE_F_VALID))
491 strat_dump_zone(zone);
495 /* display current information about the state of the game */
496 void strat_dump_infos(const char *caller)
498 if (!strat_infos.dump_enabled)
501 printf_P(PSTR("%s() dump strat infos:\r\n"), caller);
502 strat_dump_static_cols();
503 strat_dump_col_disp();
504 strat_dump_lin_disp();
505 strat_dump_all_temples();
506 strat_dump_all_zones();
509 /* init current area state before a match. Dump update user conf
511 void strat_reset_infos(void)
515 /* /!\ don't do a big memset() as there is static data */
516 strat_infos.s_cols.flags = 0;
517 strat_infos.c1.count = 5;
518 strat_infos.c1.last_try_time = 0;
519 strat_infos.c2.count = 5;
520 strat_infos.c2.last_try_time = 0;
521 strat_infos.c3.count = 5;
522 strat_infos.c3.last_try_time = 0;
523 strat_infos.l1.count = 1;
524 strat_infos.l1.last_try_time = 0;
525 strat_infos.l2.count = 1;
526 strat_infos.l2.last_try_time = 0;
528 strat_infos.taken_lintel = 0;
529 strat_infos.col_in_boobs = 0;
530 strat_infos.lazy_pickup_done = 0;
531 strat_infos.i2c_loaded_skipped = 0;
533 memset(strat_infos.temple_list, 0, sizeof(strat_infos.temple_list));
535 for (i=0; i<MAX_ZONE; i++)
536 strat_infos.zone_list[i].flags = ZONE_F_VALID;
537 strat_infos.zone_list[ZONE_DISC_NUM].flags |= ZONE_F_DISC;
538 strat_infos.zone_list[ZONE_1A_NUM].flags |= ZONE_F_ZONE1;
539 strat_infos.zone_list[ZONE_1B_NUM].flags |= ZONE_F_ZONE1;
540 strat_infos.zone_list[ZONE_0A_NUM].flags |= ZONE_F_ZONE0;
541 strat_infos.zone_list[ZONE_0B_NUM].flags |= ZONE_F_ZONE0;
543 strat_set_bounding_box();
545 /* set lintel position, depending on color */
546 if (mainboard.our_color == I2C_COLOR_RED) {
547 strat_infos.l1.x = 912;
548 strat_infos.l2.x = 1312;
551 strat_infos.l1.x = 888;
552 strat_infos.l2.x = 1288;
556 /* call it just before launching the strat */
557 void strat_init(void)
562 /* we consider that the color is correctly set */
564 strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
566 interrupt_traj_reset();
568 /* used in strat_base for END_TIMER */
569 mainboard.flags = DO_ENCODERS | DO_CS | DO_RS |
570 DO_POS | DO_BD | DO_TIMER | DO_POWER;
573 beacon_prev_time = 0;
579 /* call it after each strat */
580 void strat_exit(void)
585 mainboard.flags &= ~(DO_TIMER);
590 mainboard.flags &= ~(DO_CS);
591 pwm_ng_set(LEFT_PWM, 0);
592 pwm_ng_set(RIGHT_PWM, 0);
596 /* called periodically */
597 void strat_event(void *dummy)
599 /* limit speed when opponent is close */
603 beacon_update_samples();
607 /* do static cols + first temples */
608 static uint8_t strat_beginning(void)
612 /* don't limit the speed when opponent is near: it can change
613 * the radius of the curves */
614 strat_limit_speed_disable();
616 err = strat_static_columns(0);
618 strat_limit_speed_enable();
620 if (!TRAJ_SUCCESS(err))
623 /* go to disc to build the first temple */
625 /* XXX if opponent is near disc, go to zone1 */
626 err = strat_goto_disc(2);
627 if (!TRAJ_SUCCESS(err))
629 DEBUG(E_USER_STRAT, "disc reached");
631 /* can return END_ERROR or END_TIMER, should not happen
633 err = strat_build_new_temple(&strat_infos.zone_list[0]);
634 if (!TRAJ_SUCCESS(err))
637 /* bypass static2 if specified */
638 if (strat_infos.conf.flags & STRAT_CONF_BYPASS_STATIC2) {
639 err = strat_escape(&strat_infos.zone_list[0], TRAJ_FLAGS_STD);
643 /* get the last 2 columns, and build them on previous temple */
644 err = strat_static_columns_pass2();
645 if (!TRAJ_SUCCESS(err))
648 /* early opponent scan, for offensive strategy */
649 if (strat_infos.conf.flags & STRAT_CONF_EARLY_SCAN) {
650 err = strat_pickup_lintels();
653 /* try to build on opponent (scan must be enabled) */
654 err = strat_build_on_opponent_temple();
661 /* return true if we need to grab some more elements (lintel/cols) */
662 uint8_t need_more_elements(void)
664 if (time_get_s() <= 75) {
665 /* we have at least one col on each arm, build now */
666 if ((get_column_count_left() >= 1) &&
667 (get_column_count_right() >= 1))
671 if (get_column_count())
677 /* dump state (every 5 s max) */
678 #define DUMP_RATE_LIMIT(dump, last_print) \
680 if (time_get_s() - last_print > 5) { \
682 last_print = time_get_s(); \
687 uint8_t strat_main(void)
690 struct temple *temple = NULL;
691 struct build_zone *zone = NULL;
693 uint8_t last_print_cols = 0;
694 uint8_t last_print_lin = 0;
695 uint8_t last_print_temple = 0;
696 uint8_t last_print_zone = 0;
698 /* do static cols + first temple */
699 err = strat_beginning();
701 /* skip error code */
705 if (err == END_TIMER) {
706 DEBUG(E_USER_STRAT, "End of time");
711 /* we have at least one col on each arm, build now */
712 if (need_more_elements() == 0) {
714 /* try to build on opponent, will return
715 * END_TRAJ without doing anything if
717 err = strat_build_on_opponent_temple();
718 if (!TRAJ_SUCCESS(err))
720 if (need_more_elements())
723 /* try to scan and build on our temple, will
724 * return END_TRAJ without doing anything if
726 err = strat_check_temple_and_build();
727 if (!TRAJ_SUCCESS(err))
729 if (need_more_elements())
732 /* Else, do a simple build, as before */
734 temple = strat_get_best_temple();
736 /* one valid temple found */
738 DUMP_RATE_LIMIT(strat_dump_all_temples, last_print_temple);
740 err = strat_goto_temple(temple);
741 if (!TRAJ_SUCCESS(err))
744 /* can return END_ERROR or END_TIMER,
745 * should not happen here */
746 err = strat_grow_temple(temple);
747 if (!TRAJ_SUCCESS(err))
750 err = strat_escape(temple->zone, TRAJ_FLAGS_STD);
751 if (!TRAJ_SUCCESS(err))
757 zone = strat_get_best_zone();
759 DUMP_RATE_LIMIT(strat_dump_all_zones, last_print_zone);
761 DEBUG(E_USER_STRAT, "goto zone %s", zone->name);
762 err = strat_goto_build_zone(zone, zone->level);
763 if (!TRAJ_SUCCESS(err))
765 DEBUG(E_USER_STRAT, "zone reached");
767 /* no error code except END_ERROR, should not happen */
768 err = strat_build_new_temple(zone);
770 err = strat_escape(zone, TRAJ_FLAGS_STD);
771 if (!TRAJ_SUCCESS(err))
777 /* XXX hey what can we do here... :'( */
778 DEBUG(E_USER_STRAT, "panic :)");
783 /* else we need some elements (lintels, then columns) */
785 if (strat_infos.l1.count != 0 && strat_infos.l2.count != 0)
786 DUMP_RATE_LIMIT(strat_dump_lin_disp, last_print_lin);
788 err = strat_pickup_lintels();
789 /* can return an error code, but we have
790 * nothing to do because pickup_column()
791 * starts with a goto_and_avoid() */
792 if (!TRAJ_SUCCESS(err))
795 DUMP_RATE_LIMIT(strat_dump_col_disp, last_print_cols);
797 err = strat_pickup_columns();
798 if (!TRAJ_SUCCESS(err))
799 nop(); /* nothing to do */
801 /* XXX check here that we have elements, or do
803 /* if we cannot take elements, try to build */