2 * Copyright Droids Corporation, Microb Technology, Eirbot (2005)
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: trajectory_manager.c,v 1.4.4.17 2009-05-18 12:28:36 zer0 Exp $
22 /* Trajectory Manager v3 - zer0 - for Eurobot 2010 */
24 #define M_2PI (2*M_PI)
26 #define DEG(x) ((x) * (180.0 / M_PI))
27 #define RAD(x) ((x) * (M_PI / 180.0))
30 #define TRAJ_EVT_PERIOD (25000UL/SCHEDULER_UNIT)
32 /** set speed consign in quadramp filter */
33 void set_quadramp_speed(struct trajectory *traj, int16_t d_speed, int16_t a_speed);
35 /** get angle speed consign in quadramp filter */
36 uint32_t get_quadramp_angle_speed(struct trajectory *traj);
38 /** get distance speed consign in quadramp filter */
39 uint32_t get_quadramp_distance_speed(struct trajectory *traj);
41 /** remove event if any */
42 void delete_event(struct trajectory *traj);
44 /** schedule the trajectory event */
45 void schedule_event(struct trajectory *traj);
47 /** do a modulo 2.pi -> [-Pi,+Pi], knowing that 'a' is in [-3Pi,+3Pi] */
48 double simple_modulo_2pi(double a);
50 /** do a modulo 2.pi -> [-Pi,+Pi] */
51 double modulo_2pi(double a);
53 /** near the target (dist) ? */
54 uint8_t is_robot_in_dist_window(struct trajectory *traj, double d_win);
56 /** near the target (dist in x,y) ? */
57 uint8_t is_robot_in_xy_window(struct trajectory *traj, double d_win);
59 /** near the angle target in radian ? Only valid if
60 * traj->target.pol.angle is set (i.e. an angle command, not an xy
62 uint8_t is_robot_in_angle_window(struct trajectory *traj, double a_win_rad);