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 */
25 * update angle and/or distance
26 * this function is not called directly by the user
27 * traj : pointer to the trajectory structure
28 * d_mm : distance in mm
29 * a_rad : angle in radian
30 * flags : what to update (UPDATE_A, UPDATE_D)
32 void __trajectory_goto_d_a_rel(struct trajectory *traj, double d_mm,
33 double a_rad, uint8_t state, uint8_t flags);
35 /** go straight forward (d is in mm) */
36 void trajectory_d_rel(struct trajectory *traj, double d_mm);
38 /** update distance consign without changing angle consign */
39 void trajectory_only_d_rel(struct trajectory *traj, double d_mm);
41 /** turn by 'a' degrees */
42 void trajectory_a_rel(struct trajectory *traj, double a_deg_rel);
44 /** turn by 'a' degrees */
45 void trajectory_a_abs(struct trajectory *traj, double a_deg_abs);
47 /** turn the robot until the point x,y is in front of us */
48 void trajectory_turnto_xy(struct trajectory *traj, double x_abs_mm, double y_abs_mm);
51 /** turn the robot until the point x,y is behind us */
52 void trajectory_turnto_xy_behind(struct trajectory *traj, double x_abs_mm, double y_abs_mm);
55 /** update angle consign without changing distance consign */
56 void trajectory_only_a_rel(struct trajectory *traj, double a_deg);
58 /** update angle consign without changing distance consign */
59 void trajectory_only_a_abs(struct trajectory *traj, double a_deg_abs);
62 /** turn by 'a' degrees */
63 void trajectory_d_a_rel(struct trajectory *traj, double d_mm, double a_deg);
65 /** set relative angle and distance consign to 0 */
66 void trajectory_stop(struct trajectory *traj);
68 /** set relative angle and distance consign to 0, and break any
69 * deceleration ramp in quadramp filter */
70 void trajectory_hardstop(struct trajectory *traj);
73 /************ GOTO XY, USE EVENTS */
75 /** goto a x,y point, using a trajectory event */
76 void trajectory_goto_xy_abs(struct trajectory *traj, double x, double y);
78 /** go forward to a x,y point, using a trajectory event */
79 void trajectory_goto_forward_xy_abs(struct trajectory *traj, double x, double y);
81 /** go backward to a x,y point, using a trajectory event */
82 void trajectory_goto_backward_xy_abs(struct trajectory *traj, double x, double y);
84 /** go forward to a d,a point, using a trajectory event */
85 void trajectory_goto_d_a_rel(struct trajectory *traj, double d, double a);
87 /** go forward to a x,y relative point, using a trajectory event */
88 void trajectory_goto_xy_rel(struct trajectory *traj, double x_rel_mm, double y_rel_mm);
90 /************ FUNCS FOR GETTING TRAJ STATE */
92 uint8_t trajectory_angle_finished(struct trajectory *traj);
93 uint8_t trajectory_distance_finished(struct trajectory *traj);
95 /** return true if the position consign is equal to the filtered
96 * position consign (after quadramp filter), for angle and
98 uint8_t trajectory_finished(struct trajectory *traj);
100 /** return true if traj is nearly finished */
101 uint8_t trajectory_in_window(struct trajectory *traj, double d_win, double a_win_rad);
103 /*********** *TRAJECTORY EVENT FUNC */
105 /** event called for xy trajectories */
106 void trajectory_manager_xy_event(struct trajectory *traj);
108 /* trajectory event for circles */
109 void trajectory_manager_circle_event(struct trajectory *traj);
111 /* trajectory event */
112 void trajectory_manager_event(void * param);
114 /*********** *CIRCLE */
118 /*********** CLITOID */
121 * do a superb curve joining line1 to line2 which is composed of:
122 * - a clothoid starting from line1
124 * - another clothoid up to line2
125 * this curve is called a clitoid (hehe)
127 * the function assumes that the initial linear speed is Vd and
128 * angular speed is 0.
130 * - x,y,a: starting position
131 * - advance: parameter for line following
132 * - alpha: total angle
133 * - beta: circular part of angle (lower than alpha)
134 * - R: the radius of the circle (must be != 0)
135 * - Vd: linear speed to use (in imp per cs period)
136 * - Amax: maximum angular acceleration
137 * - d_inter: distance in mm until the intersection of the
140 * return 0 if trajectory can be loaded, then it is processed in
143 int8_t trajectory_clitoid(struct trajectory *traj,
144 double x, double y, double a, double advance,
145 double alpha_deg, double beta_deg, double R_mm,