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, double d_speed, double a_speed);
35 /** set acc consign in quadramp filter */
36 void set_quadramp_acc(struct trajectory *traj, double d_acc, double a_acc);
38 /** get angle speed consign in quadramp filter */
39 double get_quadramp_angle_speed(struct trajectory *traj);
41 /** get distance speed consign in quadramp filter */
42 double get_quadramp_distance_speed(struct trajectory *traj);
44 /** remove event if any */
45 void delete_event(struct trajectory *traj);
47 /** schedule the trajectory event */
48 void schedule_event(struct trajectory *traj);
50 /** do a modulo 2.pi -> [-Pi,+Pi], knowing that 'a' is in [-3Pi,+3Pi] */
51 double simple_modulo_2pi(double a);
53 /** do a modulo 2.pi -> [-Pi,+Pi] */
54 double modulo_2pi(double a);
56 /** near the target (dist) ? */
57 uint8_t is_robot_in_dist_window(struct trajectory *traj, double d_win);
59 /** near the target (dist in x,y) ? */
60 uint8_t is_robot_in_xy_window(struct trajectory *traj, double d_win);
62 /** near the angle target in radian ? Only valid if
63 * traj->target.pol.angle is set (i.e. an angle command, not an xy
65 uint8_t is_robot_in_angle_window(struct trajectory *traj, double a_win_rad);
67 double pos_mm2imp(struct trajectory *traj, double pos);
68 double pos_imp2mm(struct trajectory *traj, double pos);
69 double speed_mm2imp(struct trajectory *traj, double speed);
70 double speed_imp2mm(struct trajectory *traj, double speed);
71 double acc_mm2imp(struct trajectory *traj, double acc);
72 double acc_imp2mm(struct trajectory *traj, double acc);
73 double pos_rd2imp(struct trajectory *traj, double pos);
74 double pos_imp2rd(struct trajectory *traj, double pos);
75 double speed_rd2imp(struct trajectory *traj, double speed);
76 double speed_imp2rd(struct trajectory *traj, double speed);
77 double acc_rd2imp(struct trajectory *traj, double acc);
78 double acc_imp2rd(struct trajectory *traj, double acc);