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.6 2009-05-02 10:08:09 zer0 Exp $
27 #include <aversive/pgmspace.h>
28 #include <aversive/wait.h>
29 #include <aversive/error.h>
33 #include <clock_time.h>
37 #include <control_system_manager.h>
38 #include <trajectory_manager.h>
39 #include <blocking_detection_manager.h>
40 #include <robot_system.h>
41 #include <position_manager.h>
48 #include "strat_base.h"
51 /* true if we want to interrupt a trajectory */
52 static uint8_t traj_intr=0;
54 /* filled when a END_OBSTACLE is returned */
55 struct opponent_obstacle opponent_obstacle;
58 static volatile int16_t strat_speed_a = SPEED_DIST_FAST;
59 static volatile int16_t strat_speed_d = SPEED_ANGLE_FAST;
60 static volatile uint16_t strat_limit_speed_a = 0; /* no limit */
61 static volatile uint16_t strat_limit_speed_d = 0;
63 static volatile uint8_t strat_limit_speed_enabled = 1;
66 /* Strings that match the end traj cause */
67 /* /!\ keep it sync with stat_base.h */
68 const char *err_tab []= {
79 /* return string from end traj type num */
80 const char *get_err(uint8_t err)
85 for (i=0 ; i<8; i++) {
92 void strat_hardstop(void)
94 trajectory_hardstop(&mainboard.traj);
95 pid_reset(&mainboard.angle.pid);
96 pid_reset(&mainboard.distance.pid);
97 bd_reset(&mainboard.angle.bd);
98 bd_reset(&mainboard.distance.bd);
100 while ((ABS(mainboard.speed_d) > 200) ||
101 (ABS(mainboard.speed_a) > 200))
103 trajectory_hardstop(&mainboard.traj);
104 pid_reset(&mainboard.angle.pid);
105 pid_reset(&mainboard.distance.pid);
106 bd_reset(&mainboard.angle.bd);
107 bd_reset(&mainboard.distance.bd);
111 void strat_reset_pos(int16_t x, int16_t y, int16_t a)
113 int16_t posx = position_get_x_s16(&mainboard.pos);
114 int16_t posy = position_get_y_s16(&mainboard.pos);
115 int16_t posa = position_get_a_deg_s16(&mainboard.pos);
117 if (x == DO_NOT_SET_POS)
119 if (y == DO_NOT_SET_POS)
121 if (a == DO_NOT_SET_POS)
124 DEBUG(E_USER_STRAT, "reset pos (%s%s%s)",
125 x == DO_NOT_SET_POS ? "" : "x",
126 y == DO_NOT_SET_POS ? "" : "y",
127 a == DO_NOT_SET_POS ? "" : "a");
128 position_set(&mainboard.pos, x, y, a);
129 DEBUG(E_USER_STRAT, "pos resetted", __FUNCTION__);
133 * decrease gain on angle PID, and go forward until we reach the
136 uint8_t strat_calib(int16_t dist, uint8_t flags)
138 int32_t p = pid_get_gain_P(&mainboard.angle.pid);
139 int32_t i = pid_get_gain_I(&mainboard.angle.pid);
140 int32_t d = pid_get_gain_D(&mainboard.angle.pid);
143 pid_set_gains(&mainboard.angle.pid, 150, 0, 2000);
144 trajectory_d_rel(&mainboard.traj, dist);
145 err = wait_traj_end(flags);
146 pid_set_gains(&mainboard.angle.pid, p, i, d);
150 static void strat_update_traj_speed(void)
155 if (strat_limit_speed_d && d > strat_limit_speed_d)
156 d = strat_limit_speed_d;
158 if (strat_limit_speed_a && a > strat_limit_speed_a)
159 a = strat_limit_speed_a;
161 trajectory_set_speed(&mainboard.traj, d, a);
164 void strat_set_speed(uint16_t d, uint16_t a)
170 strat_update_traj_speed();
174 void strat_get_speed(uint16_t *d, uint16_t *a)
184 void interrupt_traj(void)
189 void interrupt_traj_reset(void)
194 uint8_t test_traj_end(uint8_t why)
197 /* point_t robot_pt; */
199 /* robot_pt.x = position_get_x_s16(&mainboard.pos); */
200 /* robot_pt.y = position_get_y_s16(&mainboard.pos); */
202 /* trigger an event at 3 sec before the end of the match if we
203 * have balls in the barrel */
204 cur_timer = time_get_s();
206 if ((mainboard.flags & DO_TIMER) && (why & END_TIMER)) {
208 if (cur_timer >= MATCH_TIME)
212 if ((why & END_INTR) && traj_intr) {
213 interrupt_traj_reset();
217 if ((why & END_TRAJ) && trajectory_finished(&mainboard.traj))
220 /* we are near the destination point (depends on current
221 * speed) AND the robot is in the area bounding box. */
222 if (why & END_NEAR) {
223 int16_t d_near = 100;
225 if (mainboard.speed_d > 2000)
228 /* if (trajectory_in_window(&mainboard.traj, d_near, RAD(5.0)) && */
229 /* is_in_boundingbox(&robot_pt)) */
230 /* return END_NEAR; */
233 if ((why & END_BLOCKING) && bd_get(&mainboard.angle.bd)) {
238 if ((why & END_BLOCKING) && bd_get(&mainboard.distance.bd)) {
243 /* if ((why & END_OBSTACLE) && strat_obstacle()) { */
244 /* strat_hardstop(); */
245 /* return END_OBSTACLE; */
251 uint8_t __wait_traj_end_debug(uint8_t why, uint16_t line)
254 /* int16_t opp_x, opp_y, opp_d, opp_a; */
257 ret = test_traj_end(why);
259 /* if (ret == END_OBSTACLE) { */
260 /* if (get_opponent_xyda(&opp_x, &opp_y, */
261 /* &opp_d, &opp_a) == 0) */
262 /* DEBUG(E_USER_STRAT, "Got %s at line %d" */
263 /* " xy=(%d,%d) da=(%d,%d)", get_err(ret), */
264 /* line, opp_x, opp_y, opp_d, opp_a); */
267 /* DEBUG(E_USER_STRAT, "Got %s at line %d", */
268 /* get_err(ret), line); */