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>
34 #include <clock_time.h>
39 #include <control_system_manager.h>
40 #include <trajectory_manager.h>
41 #include <trajectory_manager_utils.h>
42 #include <vect_base.h>
45 #include <obstacle_avoidance.h>
46 #include <blocking_detection_manager.h>
47 #include <robot_system.h>
48 #include <position_manager.h>
53 #include "../common/i2c_commands.h"
57 #include "strat_utils.h"
58 #include "strat_base.h"
61 #include "i2c_protocol.h"
64 /* true if we want to interrupt a trajectory */
65 static uint8_t traj_intr=0;
67 /* to debug state of strat_obstacle() */
68 static uint8_t strat_obstacle_debug = 0;
70 /* filled when a END_OBSTACLE is returned */
71 struct opponent_obstacle opponent_obstacle;
74 static volatile int16_t strat_speed_a = SPEED_DIST_FAST;
75 static volatile int16_t strat_speed_d = SPEED_ANGLE_FAST;
76 static volatile uint16_t strat_limit_speed_a = 0; /* no limit */
77 static volatile uint16_t strat_limit_speed_d = 0;
79 static volatile uint8_t strat_limit_speed_enabled = 1;
82 /* Strings that match the end traj cause */
83 /* /!\ keep it sync with stat_base.h */
84 const char *err_tab []= {
95 /* return string from end traj type num */
96 const char *get_err(uint8_t err)
101 for (i=0 ; i<8; i++) {
105 return "END_UNKNOWN";
108 void strat_hardstop(void)
110 DEBUG(E_USER_STRAT, "strat_hardstop");
112 trajectory_hardstop(&mainboard.traj);
113 pid_reset(&mainboard.angle.pid);
114 pid_reset(&mainboard.distance.pid);
115 bd_reset(&mainboard.angle.bd);
116 bd_reset(&mainboard.distance.bd);
118 while ((ABS(mainboard.speed_d) > 200) ||
119 (ABS(mainboard.speed_a) > 200))
121 trajectory_hardstop(&mainboard.traj);
122 pid_reset(&mainboard.angle.pid);
123 pid_reset(&mainboard.distance.pid);
124 bd_reset(&mainboard.angle.bd);
125 bd_reset(&mainboard.distance.bd);
128 /* go to an x,y point without checking for obstacle or blocking. It
129 * should be used for very small dist only. Return END_TRAJ if we
130 * reach destination, or END_BLOCKING if the robot blocked more than 3
132 uint8_t strat_goto_xy_force(int16_t x, int16_t y)
136 for (i=0; i<3; i++) {
137 trajectory_goto_xy_abs(&mainboard.traj, x, y);
138 err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
139 if (TRAJ_SUCCESS(err))
141 if (err == END_BLOCKING) {
150 void strat_reset_pos(int16_t x, int16_t y, double a)
152 double posx = position_get_x_double(&mainboard.pos);
153 double posy = position_get_y_double(&mainboard.pos);
154 double posa = position_get_a_rad_double(&mainboard.pos);
156 if (x == DO_NOT_SET_POS)
158 if (y == DO_NOT_SET_POS)
160 if (a == DO_NOT_SET_POS)
163 /* some issues with blocking on simulator */
165 if (x == ROBOT_HALF_LENGTH_REAR)
166 x = ROBOT_HALF_LENGTH_REAR + 10;
167 if (x == AREA_X - ROBOT_HALF_LENGTH_REAR)
168 x = AREA_X - ROBOT_HALF_LENGTH_REAR - 10;
169 if (y == ROBOT_HALF_LENGTH_REAR)
170 y = ROBOT_HALF_LENGTH_REAR + 10;
171 if (y == AREA_Y - ROBOT_HALF_LENGTH_REAR)
172 y = AREA_Y - ROBOT_HALF_LENGTH_REAR - 10;
173 if (x == ROBOT_HALF_LENGTH_FRONT)
174 x = ROBOT_HALF_LENGTH_FRONT + 10;
175 if (x == AREA_X - ROBOT_HALF_LENGTH_FRONT)
176 x = AREA_X - ROBOT_HALF_LENGTH_FRONT - 10;
177 if (y == ROBOT_HALF_LENGTH_FRONT)
178 y = ROBOT_HALF_LENGTH_FRONT + 10;
179 if (y == AREA_Y - ROBOT_HALF_LENGTH_FRONT)
180 y = AREA_Y - ROBOT_HALF_LENGTH_FRONT - 10;
182 DEBUG(E_USER_STRAT, "reset pos (%s%s%s)",
183 x == DO_NOT_SET_POS ? "" : "x",
184 y == DO_NOT_SET_POS ? "" : "y",
185 a == DO_NOT_SET_POS ? "" : "a");
186 position_set(&mainboard.pos, x, y, a);
187 DEBUG(E_USER_STRAT, "pos resetted", __FUNCTION__);
191 * decrease gain on angle PID, and go forward until we reach the
194 uint8_t strat_calib(int16_t dist, uint8_t flags)
196 int32_t p = pid_get_gain_P(&mainboard.angle.pid);
197 int32_t i = pid_get_gain_I(&mainboard.angle.pid);
198 int32_t d = pid_get_gain_D(&mainboard.angle.pid);
199 int32_t max_in = pid_get_max_in(&mainboard.angle.pid);
200 int32_t max_i = pid_get_max_I(&mainboard.angle.pid);
201 int32_t max_out = pid_get_max_out(&mainboard.angle.pid);
202 uint32_t i_thres = mainboard.distance.bd.i_thres;
203 int32_t k1 = mainboard.distance.bd.k1;
204 int32_t k2 = mainboard.distance.bd.k2;
205 uint16_t cpt_thres = mainboard.distance.bd.cpt_thres;
209 /* go with a lower angle pid, and with a sensible blocking
211 bd_set_current_thresholds(&mainboard.distance.bd, 500, 8000, 400000, 20);
212 pid_set_maximums(&mainboard.distance.pid, 0, 20000, 1000);
213 pid_set_gains(&mainboard.angle.pid, 500, 10, 4000);
214 trajectory_d_rel(&mainboard.traj, dist);
215 err = wait_traj_end(flags);
216 if (err != END_BLOCKING)
219 /* we detected a blocking, reset bd, remove angle pid and
221 trajectory_d_rel(&mainboard.traj, dist);
222 pid_set_maximums(&mainboard.distance.pid, max_in, max_i, 4095);
223 pid_set_gains(&mainboard.angle.pid, 1, 0, 0);
228 /* issue with block on simulator */
230 trajectory_d_rel(&mainboard.traj, -10);
232 trajectory_d_rel(&mainboard.traj, 10);
233 wait_traj_end(END_TRAJ);
237 pid_set_gains(&mainboard.angle.pid, p, i, d);
238 pid_set_maximums(&mainboard.distance.pid, max_in, max_i, max_out);
239 bd_set_current_thresholds(&mainboard.distance.bd, k1, k2, i_thres, cpt_thres);
244 static void strat_update_traj_speed(void)
249 if (strat_limit_speed_d && d > strat_limit_speed_d)
250 d = strat_limit_speed_d;
252 if (strat_limit_speed_a && a > strat_limit_speed_a)
253 a = strat_limit_speed_a;
255 trajectory_set_speed(&mainboard.traj, d, a);
258 void strat_set_speed(uint16_t d, uint16_t a)
264 strat_update_traj_speed();
268 void strat_set_acc(double d, double a)
270 trajectory_set_acc(&mainboard.traj, d, a);
273 void strat_get_speed(uint16_t *d, uint16_t *a)
282 void strat_get_acc(double *d, double *a)
286 *d = mainboard.traj.d_acc;
287 *a = mainboard.traj.a_acc;
291 void strat_limit_speed_enable(void)
293 strat_limit_speed_enabled = 1;
296 void strat_limit_speed_disable(void)
298 strat_limit_speed_enabled = 0;
301 /* called periodically (note: disabled in 2010) */
302 void strat_limit_speed(void)
304 uint16_t lim_d = 0, lim_a = 0;
305 int16_t opp_d, opp_a;
307 if (strat_limit_speed_enabled == 0)
310 if (get_opponent_da(&opp_d, &opp_a) != 0)
314 if (mainboard.speed_d > 0 && (opp_a > 290 || opp_a < 70)) {
315 lim_d = SPEED_DIST_VERY_SLOW;
316 lim_a = SPEED_ANGLE_VERY_SLOW;
318 else if (mainboard.speed_d < 0 && (opp_a < 250 && opp_a > 110)) {
319 lim_d = SPEED_DIST_VERY_SLOW;
320 lim_a = SPEED_ANGLE_VERY_SLOW;
323 lim_d = SPEED_DIST_SLOW;
324 lim_a = SPEED_ANGLE_VERY_SLOW;
327 else if (opp_d < 800) {
328 if (mainboard.speed_d > 0 && (opp_a > 290 || opp_a < 70)) {
329 lim_d = SPEED_DIST_SLOW;
330 lim_a = SPEED_ANGLE_SLOW;
332 else if (mainboard.speed_d < 0 && (opp_a < 250 && opp_a > 110)) {
333 lim_d = SPEED_DIST_SLOW;
334 lim_a = SPEED_ANGLE_SLOW;
339 if (lim_d != strat_limit_speed_d ||
340 lim_a != strat_limit_speed_a) {
341 strat_limit_speed_d = lim_d;
342 strat_limit_speed_a = lim_a;
343 DEBUG(E_USER_STRAT, "new speed limit da=%d,%d", lim_d, lim_a);
344 strat_update_traj_speed();
348 /* start the strat */
349 void strat_start(void)
356 /* if start sw not plugged */
357 if (sensor_get(S_START_SWITCH)) {
360 printf_P(PSTR("No start switch, press a key or plug it\r\n"));
362 /* while start sw not plugged */
363 while (sensor_get(S_START_SWITCH)) {
364 if (! cmdline_keypressed())
367 for (i=3; i>0; i--) {
368 printf_P(PSTR("%d\r\n"), i);
375 /* if start sw plugged */
376 if (!sensor_get(S_START_SWITCH)) {
377 printf_P(PSTR("Ready, unplug start switch to start\r\n"));
378 /* while start sw plugged */
379 while (!sensor_get(S_START_SWITCH));
385 NOTICE(E_USER_STRAT, "Finished !! returned %s", get_err(err));
390 /* return true if we have to brake due to an obstacle */
391 uint8_t strat_obstacle(void)
393 int16_t x_rel, y_rel;
394 int16_t opp_x, opp_y, opp_d, opp_a;
397 if (ABS(mainboard.speed_d) < 100) {
398 if (strat_obstacle_debug != 1) {
399 DEBUG(E_USER_STRAT, "disable opp, too slow");
400 strat_obstacle_debug = 1;
405 /* no opponent detected */
406 if (get_opponent_xyda(&opp_x, &opp_y, &opp_d, &opp_a) < 0) {
407 if (strat_obstacle_debug != 2) {
408 DEBUG(E_USER_STRAT, "no opponent found");
409 strat_obstacle_debug = 2;
414 /* save obstacle position */
415 opponent_obstacle.x = opp_x;
416 opponent_obstacle.y = opp_y;
417 opponent_obstacle.d = opp_d;
418 opponent_obstacle.a = opp_a;
420 if (!is_in_area(opp_x, opp_y, 250)) {
421 if (strat_obstacle_debug != 3) {
423 "opponent not in area : d=%d, a=%d "
424 "x=%d y=%d (speed_d=%d)",
425 opp_d, opp_a, opp_x, opp_y, mainboard.speed_d);
426 strat_obstacle_debug = 3;
431 /* sensor are temporarily disabled */
432 if (sensor_obstacle_is_disabled()) {
433 if (strat_obstacle_debug != 4) {
435 "sensor are disabled: opponent d=%d, a=%d "
436 "x=%d y=%d (speed_d=%d)",
437 opp_d, opp_a, opp_x, opp_y, mainboard.speed_d);
438 strat_obstacle_debug = 4;
443 /* relative position */
444 x_rel = cos(RAD(opp_a)) * (double)opp_d;
445 y_rel = sin(RAD(opp_a)) * (double)opp_d;
447 /* opponent too far */
449 if (strat_obstacle_debug != 5) {
450 DEBUG(E_USER_STRAT, "opponent too far d=%d, a=%d "
451 "x=%d y=%d (speed_d=%d)",
452 opp_d, opp_a, opp_x, opp_y, mainboard.speed_d);
453 strat_obstacle_debug = 5;
458 /* opponent is in front of us */
459 if ((mainboard.speed_d >= 0 && opp_d < 600 && (opp_a > 315 || opp_a < 45)) ||
460 (mainboard.speed_d >= 0 && opp_d < 800 && (opp_a > 330 || opp_a < 30))) {
461 DEBUG(E_USER_STRAT, "opponent front d=%d, a=%d "
462 "xrel=%d yrel=%d (speed_d=%d)",
463 opp_d, opp_a, x_rel, y_rel, mainboard.speed_d);
464 sensor_obstacle_disable();
467 /* opponent is behind us */
468 if ((mainboard.speed_d < 0 && opp_d < 600 && (opp_a < 225 && opp_a > 135)) ||
469 (mainboard.speed_d < 0 && opp_d < 800 && (opp_a < 210 && opp_a > 150))) {
470 DEBUG(E_USER_STRAT, "opponent behind d=%d, a=%d xrel=%d yrel=%d",
471 opp_d, opp_a, x_rel, y_rel);
472 sensor_obstacle_disable();
476 if (strat_obstacle_debug != 6) {
477 DEBUG(E_USER_STRAT, "opponent not in cone d=%d, a=%d "
478 "x=%d y=%d (speed_d=%d)",
479 opp_d, opp_a, opp_x, opp_y, mainboard.speed_d);
480 strat_obstacle_debug = 6;
486 void interrupt_traj(void)
491 void interrupt_traj_reset(void)
496 uint8_t test_traj_end(uint8_t why)
501 robot_pt.x = position_get_x_s16(&mainboard.pos);
502 robot_pt.y = position_get_y_s16(&mainboard.pos);
504 /* trigger an event at 3 sec before the end of the match if we
505 * have balls in the barrel */
506 cur_timer = time_get_s();
508 if ((mainboard.flags & DO_TIMER) && (why & END_TIMER)) {
510 if (cur_timer >= MATCH_TIME)
514 if ((why & END_INTR) && traj_intr) {
515 interrupt_traj_reset();
519 if ((why & END_TRAJ) && trajectory_finished(&mainboard.traj))
522 /* we are near the destination point (depends on current
523 * speed) AND the robot is in the area bounding box. */
524 if (why & END_NEAR) {
525 int16_t d_near = 100;
527 if (mainboard.speed_d > 2000)
530 if (trajectory_in_window(&mainboard.traj, d_near, RAD(5.0)) &&
531 is_in_boundingbox(&robot_pt))
535 if ((why & END_BLOCKING) && bd_get(&mainboard.angle.bd)) {
540 if ((why & END_BLOCKING) && bd_get(&mainboard.distance.bd)) {
545 if ((why & END_OBSTACLE) && strat_obstacle()) {
551 if (robotsim_blocking) {
552 robotsim_blocking = 0;
559 uint8_t __wait_traj_end_debug(uint8_t why, uint16_t line)
562 int16_t opp_x, opp_y, opp_d, opp_a;
565 ret = test_traj_end(why);
567 if (ret == END_OBSTACLE) {
568 if (get_opponent_xyda(&opp_x, &opp_y,
569 &opp_d, &opp_a) == 0)
570 DEBUG(E_USER_STRAT, "Got %s at line %d"
571 " xy=(%d,%d) da=(%d,%d)", get_err(ret),
572 line, opp_x, opp_y, opp_d, opp_a);
575 DEBUG(E_USER_STRAT, "Got %s at line %d",