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.h,v 1.4.4.10 2009-05-02 10:03:04 zer0 Exp $
22 #ifndef TRAJECTORY_MANAGER
23 #define TRAJECTORY_MANAGER
27 #include <robot_system.h>
28 #include <vect_base.h>
31 enum trajectory_state {
34 /* simple trajectories */
39 /* trajectories using events */
45 RUNNING_XY_F_ANGLE_OK,
48 RUNNING_XY_B_ANGLE_OK,
58 RUNNING_CLITOID_CURVE,
61 struct circle_target {
62 vect2_cart center; /**< center of the circle */
63 double radius; /**< radius of the circle */
64 int32_t dest_angle; /**< dst angle in inc */
66 #define TRIGO 1 /* rotation is counterclockwise */
67 #define FORWARD 2 /* go forward or backward */
68 uint8_t flags; /**< flags for this trajectory */
71 /* for line and clitoid */
77 /* only for clitoid */
86 enum trajectory_state state; /*<< describe the type of target, and if we reached the target */
89 vect2_cart cart; /**<< target, if it is a x,y vector */
90 struct rs_polar pol; /**<< target, if it is a d,a vector */
91 struct circle_target circle; /**<< target, if it is a circle */
92 struct line_target line; /**<< target, if it is a line */
95 double d_win; /**<< distance window (for END_NEAR) */
96 double a_win_rad; /**<< angle window (for END_NEAR) */
97 double a_start_rad;/**<< in xy consigns, start to move in distance
98 * when a_target < a_start */
99 double circle_coef;/**<< corrective circle coef */
101 double d_speed; /**<< distance speed consign */
102 double a_speed; /**<< angle speed consign */
104 double d_acc; /**<< distance acceleration consign */
105 double a_acc; /**<< angle acceleration consign */
107 struct robot_position *position; /**<< associated robot_position */
108 struct robot_system *robot; /**<< associated robot_system */
109 struct cs *csm_angle; /**<< associated control system (angle) */
110 struct cs *csm_distance; /**<< associated control system (distance) */
114 int8_t scheduler_task; /**<< id of current task (-1 if no running task) */
117 /** structure initialization */
118 void trajectory_init(struct trajectory *traj, double cs_hz);
120 /** structure initialization */
121 void trajectory_set_cs(struct trajectory *traj, struct cs *cs_d,
124 /** structure initialization */
125 void trajectory_set_robot_params(struct trajectory *traj,
126 struct robot_system *rs,
127 struct robot_position *pos) ;
129 /** set speed consign */
130 void trajectory_set_speed(struct trajectory *traj, double d_speed, double a_speed);
132 /** set speed consign */
133 void trajectory_set_acc(struct trajectory *traj, double d_acc, double a_acc);
136 * set windows for trajectory.
137 * params: distance window, angle window: we the robot enters this
138 * position window, we deletes the event and the last consign is
140 * a_start_deg used in xy consigns (start to move in distance when
141 * a_target < a_start)
143 void trajectory_set_windows(struct trajectory *traj, double d_win,
144 double a_win_deg, double a_start_deg);
147 * Set coef for circle trajectory. The objective of this value is to
148 * fix the radius of the circle which is not correctly what we asked.
150 void trajectory_set_circle_coef(struct trajectory *traj, double coef);
153 * return trajectory state
155 enum trajectory_state trajectory_get_state(struct trajectory *traj);
157 /** return true if the position consign is equal to the filtered
158 * position consign (after quadramp filter), for angle and
160 uint8_t trajectory_finished(struct trajectory *traj);
161 uint8_t trajectory_angle_finished(struct trajectory *traj);
162 uint8_t trajectory_distance_finished(struct trajectory *traj);
164 /** return true if traj is nearly finished depending on specified
166 uint8_t trajectory_in_window(struct trajectory *traj, double d_win, double a_win_rad);
168 /* simple commands */
170 /** set relative angle and distance consign to 0 */
171 void trajectory_stop(struct trajectory *traj);
173 /** set relative angle and distance consign to 0, and break any
174 * deceleration ramp in quadramp filter */
175 void trajectory_hardstop(struct trajectory *traj);
177 /** go straight forward (d is in mm) */
178 void trajectory_d_rel(struct trajectory *traj, double d_mm);
180 /** update distance consign without changing angle consign */
181 void trajectory_only_d_rel(struct trajectory *traj, double d_mm);
183 /** turn by 'a' degrees */
184 void trajectory_a_rel(struct trajectory *traj, double a_deg);
186 /** go to angle 'a' in degrees */
187 void trajectory_a_abs(struct trajectory *traj, double a_deg);
189 /** turn the robot until the point x,y is in front of us */
190 void trajectory_turnto_xy(struct trajectory*traj, double x_abs_mm, double y_abs_mm);
192 /** turn the robot until the point x,y is behind us */
193 void trajectory_turnto_xy_behind(struct trajectory*traj, double x_abs_mm, double y_abs_mm);
195 /** update angle consign without changing distance consign */
196 void trajectory_only_a_rel(struct trajectory *traj, double a_deg);
198 /** update angle consign without changing distance consign */
199 void trajectory_only_a_abs(struct trajectory *traj, double a_deg);
201 /** turn by 'a' degrees and go by 'd' mm */
202 void trajectory_d_a_rel(struct trajectory *traj, double d_mm, double a_deg);
204 /* commands using events */
206 /** goto a x,y point, using a trajectory event */
207 void trajectory_goto_xy_abs(struct trajectory *traj, double x_abs_mm, double y_abs_mm);
209 /** go forward to a x,y point, using a trajectory event */
210 void trajectory_goto_forward_xy_abs(struct trajectory *traj, double x_abs_mm, double y_abs_mm);
212 /** go backward to a x,y point, using a trajectory event */
213 void trajectory_goto_backward_xy_abs(struct trajectory *traj, double x_abs_mm, double y_abs_mm);
215 /** go forward to a d,a point, using a trajectory event */
216 void trajectory_goto_d_a_rel(struct trajectory *traj, double d, double a);
218 /** go forward to a x,y relative point, using a trajectory event */
219 void trajectory_goto_xy_rel(struct trajectory *traj, double x_rel_mm, double y_rel_mm);
221 /** make the robot orbiting around (x,y) on a circle whose radius is
222 * radius_mm, and exit when relative destination angle is reached. The
223 * flags set if we go forward or backwards, and CW/CCW. */
224 void trajectory_circle_rel(struct trajectory *traj, double x, double y,
225 double radius_mm, double rel_a_deg, uint8_t flags);
228 * Compute the fastest distance and angle speeds matching the radius
229 * from current traj_speed
231 void circle_get_da_speed_from_radius(struct trajectory *traj,
237 void trajectory_line_abs(struct trajectory *traj, double x1, double y1,
238 double x2, double y2, double advance);
240 #endif //TRAJECTORY_MANAGER