2 * Copyright Droids Corporation, 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_base.c,v 1.8 2009-11-08 17:24:33 zer0 Exp $
27 #include <aversive/pgmspace.h>
28 #include <aversive/wait.h>
29 #include <aversive/error.h>
32 #include <clock_time.h>
36 #include <control_system_manager.h>
37 #include <trajectory_manager.h>
38 #include <vect_base.h>
41 #include <obstacle_avoidance.h>
42 #include <blocking_detection_manager.h>
43 #include <robot_system.h>
44 #include <position_manager.h>
51 #include "strat_utils.h"
52 #include "strat_base.h"
55 /* true if we want to interrupt a trajectory */
56 static uint8_t traj_intr=0;
58 /* filled when a END_OBSTACLE is returned */
59 struct opponent_obstacle opponent_obstacle;
62 static volatile int16_t strat_speed_a = SPEED_DIST_FAST;
63 static volatile int16_t strat_speed_d = SPEED_ANGLE_FAST;
64 static volatile uint16_t strat_limit_speed_a = 0; /* no limit */
65 static volatile uint16_t strat_limit_speed_d = 0;
67 static volatile uint8_t strat_limit_speed_enabled = 1;
70 /* Strings that match the end traj cause */
71 /* /!\ keep it sync with stat_base.h */
72 const char *err_tab []= {
83 /* return string from end traj type num */
84 const char *get_err(uint8_t err)
89 for (i=0 ; i<8; i++) {
96 void strat_hardstop(void)
98 trajectory_hardstop(&mainboard.traj);
99 pid_reset(&mainboard.angle.pid);
100 pid_reset(&mainboard.distance.pid);
101 bd_reset(&mainboard.angle.bd);
102 bd_reset(&mainboard.distance.bd);
104 while ((ABS(mainboard.speed_d) > 200) ||
105 (ABS(mainboard.speed_a) > 200))
107 trajectory_hardstop(&mainboard.traj);
108 pid_reset(&mainboard.angle.pid);
109 pid_reset(&mainboard.distance.pid);
110 bd_reset(&mainboard.angle.bd);
111 bd_reset(&mainboard.distance.bd);
114 /* go to an x,y point without checking for obstacle or blocking. It
115 * should be used for very small dist only. Return END_TRAJ if we
116 * reach destination, or END_BLOCKING if the robot blocked more than 3
118 uint8_t strat_goto_xy_force(int16_t x, int16_t y)
124 uint8_t hardstop = 0;
125 microseconds us = time_get_us2();
126 int16_t opp_a, opp_d, opp_x, opp_y;
129 serr = get_opponent_xyda(&opp_x, &opp_y,
139 if ((time_get_us2() - us) > 3000000L)
143 for (i=0; i<3; i++) {
144 trajectory_goto_xy_abs(&mainboard.traj, x, y);
145 err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
146 if (TRAJ_SUCCESS(err))
148 if (err == END_BLOCKING) {
157 void strat_reset_pos(int16_t x, int16_t y, int16_t a)
159 int16_t posx = position_get_x_s16(&mainboard.pos);
160 int16_t posy = position_get_y_s16(&mainboard.pos);
161 int16_t posa = position_get_a_deg_s16(&mainboard.pos);
163 if (x == DO_NOT_SET_POS)
165 if (y == DO_NOT_SET_POS)
167 if (a == DO_NOT_SET_POS)
170 DEBUG(E_USER_STRAT, "reset pos (%s%s%s)",
171 x == DO_NOT_SET_POS ? "" : "x",
172 y == DO_NOT_SET_POS ? "" : "y",
173 a == DO_NOT_SET_POS ? "" : "a");
174 position_set(&mainboard.pos, x, y, a);
175 DEBUG(E_USER_STRAT, "pos resetted", __FUNCTION__);
179 * decrease gain on angle PID, and go forward until we reach the
182 uint8_t strat_calib(int16_t dist, uint8_t flags)
184 int32_t p = pid_get_gain_P(&mainboard.angle.pid);
185 int32_t i = pid_get_gain_I(&mainboard.angle.pid);
186 int32_t d = pid_get_gain_D(&mainboard.angle.pid);
189 pid_set_gains(&mainboard.angle.pid, 150, 0, 2000);
190 trajectory_d_rel(&mainboard.traj, dist);
191 err = wait_traj_end(flags);
192 pid_set_gains(&mainboard.angle.pid, p, i, d);
196 /* escape from zone, and don't brake, so we can continue with another
198 uint8_t strat_escape(struct build_zone *zone, uint8_t flags)
201 uint16_t old_spdd, old_spda;
203 strat_get_speed(&old_spdd, &old_spda);
205 err = WAIT_COND_OR_TIMEOUT(!opponent_is_behind(), 1000);
207 strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST);
208 trajectory_d_rel(&mainboard.traj, -150);
209 err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
210 strat_set_speed(old_spdd, old_spda);
214 strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
216 if (zone->flags & ZONE_F_DISC) {
217 trajectory_d_rel(&mainboard.traj, -350);
218 err = WAIT_COND_OR_TRAJ_END(!robot_is_near_disc(), flags);
221 trajectory_d_rel(&mainboard.traj, -300);
222 err = wait_traj_end(flags);
225 strat_set_speed(old_spdd, old_spda);
231 static void strat_update_traj_speed(void)
236 if (strat_limit_speed_d && d > strat_limit_speed_d)
237 d = strat_limit_speed_d;
239 if (strat_limit_speed_a && a > strat_limit_speed_a)
240 a = strat_limit_speed_a;
242 trajectory_set_speed(&mainboard.traj, d, a);
245 void strat_set_speed(uint16_t d, uint16_t a)
251 strat_update_traj_speed();
255 void strat_get_speed(uint16_t *d, uint16_t *a)
264 void strat_limit_speed_enable(void)
266 strat_limit_speed_enabled = 1;
269 void strat_limit_speed_disable(void)
271 strat_limit_speed_enabled = 0;
274 /* called periodically */
275 void strat_limit_speed(void)
277 uint16_t lim_d = 0, lim_a = 0;
278 int16_t opp_d, opp_a;
280 if (strat_limit_speed_enabled == 0)
283 if (get_opponent_da(&opp_d, &opp_a) != 0)
293 if (mainboard.speed_d > 0 && (opp_a > 290 || opp_a < 70)) {
294 lim_d = SPEED_DIST_VERY_SLOW;
295 lim_a = SPEED_ANGLE_VERY_SLOW;
297 else if (mainboard.speed_d < 0 && (opp_a < 250 && opp_a > 110)) {
298 lim_d = SPEED_DIST_VERY_SLOW;
299 lim_a = SPEED_ANGLE_VERY_SLOW;
302 lim_d = SPEED_DIST_SLOW;
303 lim_a = SPEED_ANGLE_VERY_SLOW;
307 else if (opp_d < 800) {
308 if (mainboard.speed_d > 0 && (opp_a > 290 || opp_a < 70)) {
309 lim_d = SPEED_DIST_SLOW;
310 lim_a = SPEED_ANGLE_SLOW;
312 else if (mainboard.speed_d < 0 && (opp_a < 250 && opp_a > 110)) {
313 lim_d = SPEED_DIST_SLOW;
314 lim_a = SPEED_ANGLE_SLOW;
319 if (lim_d != strat_limit_speed_d ||
320 lim_a != strat_limit_speed_a) {
321 strat_limit_speed_d = lim_d;
322 strat_limit_speed_a = lim_a;
323 DEBUG(E_USER_STRAT, "new speed limit da=%d,%d", lim_d, lim_a);
324 strat_update_traj_speed();
328 /* start the strat */
329 void strat_start(void)
332 printf("not implemented\n");
339 /* if start sw not plugged */
340 if (sensor_get(S_START_SWITCH)) {
341 printf_P(PSTR("No start switch, press a key or plug it\r\n"));
343 /* while start sw not plugged */
344 while (sensor_get(S_START_SWITCH)) {
345 if (! cmdline_keypressed())
348 for (i=3; i>0; i--) {
349 printf_P(PSTR("%d\r\n"), i);
356 /* if start sw plugged */
357 if (!sensor_get(S_START_SWITCH)) {
358 printf_P(PSTR("Ready, unplug start switch to start\r\n"));
359 /* while start sw plugged */
360 while (!sensor_get(S_START_SWITCH));
365 NOTICE(E_USER_STRAT, "Finished !! returned %s", get_err(err));
370 /* return true if we have to brake due to an obstacle */
371 uint8_t strat_obstacle(void)
373 int16_t x_rel, y_rel;
374 int16_t opp_x, opp_y, opp_d, opp_a;
377 if (ABS(mainboard.speed_d) < 150)
380 /* no opponent detected */
381 if (get_opponent_xyda(&opp_x, &opp_y,
385 /* save obstacle position */
386 opponent_obstacle.x = opp_x;
387 opponent_obstacle.y = opp_y;
388 opponent_obstacle.d = opp_d;
389 opponent_obstacle.a = opp_a;
392 /* sensor are temporarily disabled */
393 if (sensor_obstacle_is_disabled())
397 /* relative position */
398 x_rel = cos(RAD(opp_a)) * (double)opp_d;
399 y_rel = sin(RAD(opp_a)) * (double)opp_d;
401 /* opponent too far */
405 /* opponent is in front of us */
406 if (mainboard.speed_d > 0 && (opp_a > 325 || opp_a < 35)) {
407 DEBUG(E_USER_STRAT, "opponent front d=%d, a=%d "
408 "xrel=%d yrel=%d (speed_d=%d)",
409 opp_d, opp_a, x_rel, y_rel, mainboard.speed_d);
411 sensor_obstacle_disable();
415 /* opponent is behind us */
416 if (mainboard.speed_d < 0 && (opp_a < 215 && opp_a > 145)) {
417 DEBUG(E_USER_STRAT, "opponent behind d=%d, a=%d xrel=%d yrel=%d",
418 opp_d, opp_a, x_rel, y_rel);
420 sensor_obstacle_disable();
428 void interrupt_traj(void)
433 void interrupt_traj_reset(void)
438 uint8_t test_traj_end(uint8_t why)
443 robot_pt.x = position_get_x_s16(&mainboard.pos);
444 robot_pt.y = position_get_y_s16(&mainboard.pos);
446 /* trigger an event at 3 sec before the end of the match if we
447 * have balls in the barrel */
448 cur_timer = time_get_s();
450 if ((mainboard.flags & DO_TIMER) && (why & END_TIMER)) {
452 if (cur_timer >= MATCH_TIME)
456 if ((why & END_INTR) && traj_intr) {
457 interrupt_traj_reset();
461 if ((why & END_TRAJ) && trajectory_finished(&mainboard.traj))
464 /* we are near the destination point (depends on current
465 * speed) AND the robot is in the area bounding box. */
466 if (why & END_NEAR) {
467 int16_t d_near = 100;
469 if (mainboard.speed_d > 2000)
472 if (trajectory_in_window(&mainboard.traj, d_near, RAD(5.0)) &&
473 is_in_boundingbox(&robot_pt))
477 if ((why & END_BLOCKING) && bd_get(&mainboard.angle.bd)) {
482 if ((why & END_BLOCKING) && bd_get(&mainboard.distance.bd)) {
487 if ((why & END_OBSTACLE) && strat_obstacle()) {
495 uint8_t __wait_traj_end_debug(uint8_t why, uint16_t line)
498 int16_t opp_x, opp_y, opp_d, opp_a;
501 ret = test_traj_end(why);
503 if (ret == END_OBSTACLE) {
504 if (get_opponent_xyda(&opp_x, &opp_y,
505 &opp_d, &opp_a) == 0)
506 DEBUG(E_USER_STRAT, "Got %s at line %d"
507 " xy=(%d,%d) da=(%d,%d)", get_err(ret),
508 line, opp_x, opp_y, opp_d, opp_a);
511 DEBUG(E_USER_STRAT, "Got %s at line %d",