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 */
29 #include <aversive/error.h>
30 #include <scheduler.h>
33 #include <position_manager.h>
34 #include <robot_system.h>
35 #include <control_system_manager.h>
38 #include <trajectory_manager.h>
39 #include "trajectory_manager_utils.h"
40 #include "trajectory_manager_core.h"
42 /************ SIMPLE TRAJS, NO EVENT */
50 * update angle and/or distance
51 * this function is not called directly by the user
52 * traj : pointer to the trajectory structure
53 * d_mm : distance in mm
54 * a_rad : angle in radian
55 * flags : what to update (UPDATE_A, UPDATE_D)
57 void __trajectory_goto_d_a_rel(struct trajectory *traj, double d_mm,
58 double a_rad, uint8_t state, uint8_t flags)
60 int32_t a_consign, d_consign;
62 DEBUG(E_TRAJECTORY, "Goto DA/RS rel to d=%f a_rad=%f", d_mm, a_rad);
65 if (flags & UPDATE_A) {
66 if (flags & RESET_A) {
70 a_consign = (int32_t)(a_rad * (traj->position->phys.distance_imp_per_mm) *
71 (traj->position->phys.track_mm) / 2);
73 a_consign += rs_get_angle(traj->robot);
74 traj->target.pol.angle = a_consign;
75 cs_set_consign(traj->csm_angle, a_consign);
77 if (flags & UPDATE_D) {
78 if (flags & RESET_D) {
82 d_consign = (int32_t)((d_mm) * (traj->position->phys.distance_imp_per_mm));
84 d_consign += rs_get_distance(traj->robot);
85 traj->target.pol.distance = d_consign;
86 cs_set_consign(traj->csm_distance, d_consign);
90 /** go straight forward (d is in mm) */
91 void trajectory_d_rel(struct trajectory *traj, double d_mm)
93 __trajectory_goto_d_a_rel(traj, d_mm, 0, RUNNING_D,
94 UPDATE_D | UPDATE_A | RESET_A);
97 /** update distance consign without changing angle consign */
98 void trajectory_only_d_rel(struct trajectory *traj, double d_mm)
100 __trajectory_goto_d_a_rel(traj, d_mm, 0, RUNNING_D, UPDATE_D);
103 /** turn by 'a' degrees */
104 void trajectory_a_rel(struct trajectory *traj, double a_deg_rel)
106 __trajectory_goto_d_a_rel(traj, 0, RAD(a_deg_rel), RUNNING_A,
107 UPDATE_A | UPDATE_D | RESET_D);
110 /** turn by 'a' degrees */
111 void trajectory_a_abs(struct trajectory *traj, double a_deg_abs)
113 double posa = position_get_a_rad_double(traj->position);
116 a = RAD(a_deg_abs) - posa;
118 __trajectory_goto_d_a_rel(traj, 0, a, RUNNING_A,
119 UPDATE_A | UPDATE_D | RESET_D);
122 /** turn the robot until the point x,y is in front of us */
123 void trajectory_turnto_xy(struct trajectory *traj, double x_abs_mm, double y_abs_mm)
125 double posx = position_get_x_double(traj->position);
126 double posy = position_get_y_double(traj->position);
127 double posa = position_get_a_rad_double(traj->position);
129 DEBUG(E_TRAJECTORY, "Goto Turn To xy %f %f", x_abs_mm, y_abs_mm);
130 __trajectory_goto_d_a_rel(traj, 0,
131 simple_modulo_2pi(atan2(y_abs_mm - posy, x_abs_mm - posx) - posa),
133 UPDATE_A | UPDATE_D | RESET_D);
136 /** turn the robot until the point x,y is behind us */
137 void trajectory_turnto_xy_behind(struct trajectory *traj, double x_abs_mm, double y_abs_mm)
139 double posx = position_get_x_double(traj->position);
140 double posy = position_get_y_double(traj->position);
141 double posa = position_get_a_rad_double(traj->position);
143 DEBUG(E_TRAJECTORY, "Goto Turn To xy %f %f", x_abs_mm, y_abs_mm);
144 __trajectory_goto_d_a_rel(traj, 0,
145 modulo_2pi(atan2(y_abs_mm - posy, x_abs_mm - posx) - posa + M_PI),
147 UPDATE_A | UPDATE_D | RESET_D);
150 /** update angle consign without changing distance consign */
151 void trajectory_only_a_rel(struct trajectory *traj, double a_deg)
153 __trajectory_goto_d_a_rel(traj, 0, RAD(a_deg), RUNNING_A,
157 /** update angle consign without changing distance consign */
158 void trajectory_only_a_abs(struct trajectory *traj, double a_deg_abs)
160 double posa = position_get_a_rad_double(traj->position);
163 a = RAD(a_deg_abs) - posa;
165 __trajectory_goto_d_a_rel(traj, 0, a, RUNNING_A, UPDATE_A);
168 /** turn by 'a' degrees */
169 void trajectory_d_a_rel(struct trajectory *traj, double d_mm, double a_deg)
171 __trajectory_goto_d_a_rel(traj, d_mm, RAD(a_deg),
172 RUNNING_AD, UPDATE_A | UPDATE_D);
175 /** set relative angle and distance consign to 0 */
176 void trajectory_stop(struct trajectory *traj)
178 __trajectory_goto_d_a_rel(traj, 0, 0, READY,
179 UPDATE_A | UPDATE_D | RESET_D | RESET_A);
182 /** set relative angle and distance consign to 0, and break any
183 * deceleration ramp in quadramp filter */
184 void trajectory_hardstop(struct trajectory *traj)
186 struct quadramp_filter *q_d, *q_a;
188 q_d = traj->csm_distance->consign_filter_params;
189 q_a = traj->csm_angle->consign_filter_params;
190 __trajectory_goto_d_a_rel(traj, 0, 0, READY,
191 UPDATE_A | UPDATE_D | RESET_D | RESET_A);
193 q_d->previous_var = 0;
194 q_d->previous_out = rs_get_distance(traj->robot);
195 q_a->previous_var = 0;
196 q_a->previous_out = rs_get_angle(traj->robot);
200 /************ GOTO XY, USE EVENTS */
202 /** goto a x,y point, using a trajectory event */
203 void trajectory_goto_xy_abs(struct trajectory *traj, double x, double y)
205 DEBUG(E_TRAJECTORY, "Goto XY");
207 traj->target.cart.x = x;
208 traj->target.cart.y = y;
209 traj->state = RUNNING_XY_START;
210 trajectory_manager_event(traj);
211 schedule_event(traj);
214 /** go forward to a x,y point, using a trajectory event */
215 void trajectory_goto_forward_xy_abs(struct trajectory *traj, double x, double y)
217 DEBUG(E_TRAJECTORY, "Goto XY_F");
219 traj->target.cart.x = x;
220 traj->target.cart.y = y;
221 traj->state = RUNNING_XY_F_START;
222 trajectory_manager_event(traj);
223 schedule_event(traj);
226 /** go backward to a x,y point, using a trajectory event */
227 void trajectory_goto_backward_xy_abs(struct trajectory *traj, double x, double y)
229 DEBUG(E_TRAJECTORY, "Goto XY_B");
231 traj->target.cart.x = x;
232 traj->target.cart.y = y;
233 traj->state = RUNNING_XY_B_START;
234 trajectory_manager_event(traj);
235 schedule_event(traj);
238 /** go forward to a d,a point, using a trajectory event */
239 void trajectory_goto_d_a_rel(struct trajectory *traj, double d, double a)
242 double x = position_get_x_double(traj->position);
243 double y = position_get_y_double(traj->position);
245 DEBUG(E_TRAJECTORY, "Goto DA rel");
249 p.theta = RAD(a) + position_get_a_rad_double(traj->position);
250 vect2_pol2cart(&p, &traj->target.cart);
251 traj->target.cart.x += x;
252 traj->target.cart.y += y;
254 traj->state = RUNNING_XY_START;
255 trajectory_manager_event(traj);
256 schedule_event(traj);
259 /** go forward to a x,y relative point, using a trajectory event */
260 void trajectory_goto_xy_rel(struct trajectory *traj, double x_rel_mm, double y_rel_mm)
264 double x = position_get_x_double(traj->position);
265 double y = position_get_y_double(traj->position);
267 DEBUG(E_TRAJECTORY, "Goto XY rel");
273 vect2_cart2pol(&c, &p);
274 p.theta += position_get_a_rad_double(traj->position);;
275 vect2_pol2cart(&p, &traj->target.cart);
277 traj->target.cart.x += x;
278 traj->target.cart.y += y;
280 traj->state = RUNNING_XY_START;
281 trajectory_manager_event(traj);
282 schedule_event(traj);
285 /************ FUNCS FOR GETTING TRAJ STATE */
287 /** return true if the position consign is equal to the filtered
288 * position consign (after quadramp filter), for angle and
290 uint8_t trajectory_finished(struct trajectory *traj)
292 return cs_get_consign(traj->csm_angle) == cs_get_filtered_consign(traj->csm_angle) &&
293 cs_get_consign(traj->csm_distance) == cs_get_filtered_consign(traj->csm_distance) ;
296 /** return true if traj is nearly finished */
297 uint8_t trajectory_in_window(struct trajectory *traj, double d_win, double a_win_rad)
299 switch(traj->state) {
301 case RUNNING_XY_ANGLE_OK:
302 case RUNNING_XY_F_ANGLE_OK:
303 case RUNNING_XY_B_ANGLE_OK:
304 /* if robot coordinates are near the x,y target */
305 return is_robot_in_xy_window(traj, d_win);
308 return is_robot_in_angle_window(traj, a_win_rad);
311 return is_robot_in_dist_window(traj, d_win);
314 return is_robot_in_dist_window(traj, d_win) &&
315 is_robot_in_angle_window(traj, a_win_rad);
317 case RUNNING_XY_START:
318 case RUNNING_XY_F_START:
319 case RUNNING_XY_B_START:
320 case RUNNING_XY_ANGLE:
321 case RUNNING_XY_F_ANGLE:
322 case RUNNING_XY_B_ANGLE:
328 /*********** *TRAJECTORY EVENT FUNC */
330 /** event called for xy trajectories */
331 void trajectory_manager_xy_event(struct trajectory *traj)
334 double x = position_get_x_double(traj->position);
335 double y = position_get_y_double(traj->position);
336 double a = position_get_a_rad_double(traj->position);
337 int32_t d_consign=0, a_consign=0;
339 /* These vectors contain target position of the robot in
340 * its own coordinates */
341 vect2_cart v2cart_pos;
342 vect2_pol v2pol_target;
344 /* step 1 : process new commands to quadramps */
346 switch (traj->state) {
347 case RUNNING_XY_START:
348 case RUNNING_XY_ANGLE:
349 case RUNNING_XY_ANGLE_OK:
350 case RUNNING_XY_F_START:
351 case RUNNING_XY_F_ANGLE:
352 case RUNNING_XY_F_ANGLE_OK:
353 case RUNNING_XY_B_START:
354 case RUNNING_XY_B_ANGLE:
355 case RUNNING_XY_B_ANGLE_OK:
357 /* process the command vector from current position to
358 * absolute target, or to the center of the circle. */
359 v2cart_pos.x = traj->target.cart.x - x;
360 v2cart_pos.y = traj->target.cart.y - y;
361 vect2_cart2pol(&v2cart_pos, &v2pol_target);
362 v2pol_target.theta = simple_modulo_2pi(v2pol_target.theta - a);
364 /* asked to go backwards */
365 if (traj->state >= RUNNING_XY_B_START &&
366 traj->state <= RUNNING_XY_B_ANGLE_OK ) {
367 v2pol_target.r = -v2pol_target.r;
368 v2pol_target.theta = simple_modulo_2pi(v2pol_target.theta + M_PI);
371 /* if we don't need to go forward */
372 if (traj->state >= RUNNING_XY_START &&
373 traj->state <= RUNNING_XY_ANGLE_OK ) {
374 /* If the target is behind the robot, we need to go
375 * backwards. 0.52 instead of 0.5 because we prefer to
377 if ((v2pol_target.theta > 0.52*M_PI) ||
378 (v2pol_target.theta < -0.52*M_PI ) ) {
379 v2pol_target.r = -v2pol_target.r;
380 v2pol_target.theta = simple_modulo_2pi(v2pol_target.theta + M_PI);
386 /* If the robot is correctly oriented to start moving in distance */
387 /* here limit dist speed depending on v2pol_target.theta */
388 if (ABS(v2pol_target.theta) > traj->a_start_rad) // || ABS(v2pol_target.r) < traj->d_win)
389 set_quadramp_speed(traj, 0, traj->a_speed);
391 coef = (traj->a_start_rad - ABS(v2pol_target.theta)) / traj->a_start_rad;
392 set_quadramp_speed(traj, traj->d_speed * coef, traj->a_speed);
395 d_consign = (int32_t)(v2pol_target.r * (traj->position->phys.distance_imp_per_mm));
396 d_consign += rs_get_distance(traj->robot);
399 /* XXX here we specify 2.2 instead of 2.0 to avoid oscillations */
400 a_consign = (int32_t)(v2pol_target.theta *
401 (traj->position->phys.distance_imp_per_mm) *
402 (traj->position->phys.track_mm) / 2.2);
403 a_consign += rs_get_angle(traj->robot);
408 /* hmmm quite odd, delete the event */
409 DEBUG(E_TRAJECTORY, "GNI ???");
415 /* step 2 : update state, or delete event if we reached the
418 /* XXX if target is our pos !! */
420 switch (traj->state) {
421 case RUNNING_XY_START:
422 case RUNNING_XY_F_START:
423 case RUNNING_XY_B_START:
425 DEBUG(E_TRAJECTORY, "-> ANGLE");
429 case RUNNING_XY_ANGLE:
430 case RUNNING_XY_F_ANGLE:
431 case RUNNING_XY_B_ANGLE: {
432 struct quadramp_filter *q_a;
433 q_a = traj->csm_angle->consign_filter_params;
434 /* if d_speed is not 0, we are in start_angle_win */
435 if (get_quadramp_distance_speed(traj)) {
436 if(is_robot_in_xy_window(traj, traj->d_win)) {
439 /* ANGLE -> ANGLE_OK */
441 DEBUG(E_TRAJECTORY, "-> ANGLE_OK");
446 case RUNNING_XY_ANGLE_OK:
447 case RUNNING_XY_F_ANGLE_OK:
448 case RUNNING_XY_B_ANGLE_OK:
449 /* If we reached the destination */
450 if(is_robot_in_xy_window(traj, traj->d_win)) {
459 /* step 3 : send the processed commands to cs */
461 DEBUG(E_TRAJECTORY, "EVENT XY cur=(%f,%f,%f) cart=(%f,%f) pol=(%f,%f)",
462 x, y, a, v2cart_pos.x, v2cart_pos.y, v2pol_target.r, v2pol_target.theta);
464 DEBUG(E_TRAJECTORY,"d_cur=%" PRIi32 ", d_consign=%" PRIi32 ", d_speed=%" PRIi32 ", "
465 "a_cur=%" PRIi32 ", a_consign=%" PRIi32 ", a_speed=%" PRIi32,
466 rs_get_distance(traj->robot), d_consign, get_quadramp_distance_speed(traj),
467 rs_get_angle(traj->robot), a_consign, get_quadramp_angle_speed(traj));
469 cs_set_consign(traj->csm_angle, a_consign);
470 cs_set_consign(traj->csm_distance, d_consign);
473 /* trajectory event for circles */
474 void trajectory_manager_circle_event(struct trajectory *traj)
477 double x = position_get_x_double(traj->position);
478 double y = position_get_y_double(traj->position);
479 double a = position_get_a_rad_double(traj->position);
480 int32_t d_consign=0, a_consign=0;
484 /* trajectory event */
485 void trajectory_manager_event(void * param)
487 struct trajectory *traj = (struct trajectory *)param;
489 switch (traj->state) {
490 case RUNNING_XY_START:
491 case RUNNING_XY_ANGLE:
492 case RUNNING_XY_ANGLE_OK:
493 case RUNNING_XY_F_START:
494 case RUNNING_XY_F_ANGLE:
495 case RUNNING_XY_F_ANGLE_OK:
496 case RUNNING_XY_B_START:
497 case RUNNING_XY_B_ANGLE:
498 case RUNNING_XY_B_ANGLE_OK:
499 trajectory_manager_xy_event(traj);
503 trajectory_manager_circle_event(traj);
511 /*********** *CIRCLE */
514 * Compute the fastest distance and angle speeds matching the radius
515 * from current traj_speed
517 /* static */void circle_get_da_speed_from_radius(struct trajectory *traj,
522 /* speed_d = coef * speed_a */
524 double speed_d2, speed_a2;
526 coef = 2. * radius_mm / traj->position->phys.track_mm;
528 speed_d2 = traj->a_speed * coef;
529 if (speed_d2 < traj->d_speed) {
531 *speed_a = traj->a_speed;
534 speed_a2 = traj->d_speed / coef;
535 *speed_d = traj->d_speed;
540 /* return the distance in millimeters that corresponds to an angle in
541 * degree and a radius in mm */
542 /* static */double circle_get_dist_from_degrees(double radius_mm, double a_deg)
544 double a_rad = RAD(a_deg);
545 return a_rad * radius_mm;
549 * Start a circle of specified radius around the specified center
550 * (relative with d,a). The distance is specified in mm.
552 void trajectory_circle(struct trajectory *traj,
553 double center_d_mm, double center_a_rad,
554 double radius_mm, double dist_mm)
558 /* DEBUG(E_TRAJECTORY, "CIRCLE to d=%f a_rad=%f", center_d_mm, */
560 /* delete_event(traj); */
561 /* traj->state = RUNNING_CIRCLE; */
567 * Start a circle of specified radius around the specified center
568 * (absolute). The distance is specified in mm.
570 void trajectory_circle_abs_dist_mm(struct trajectory *traj,
571 double x_rel_mm, double y_rel_mm,
572 double radius_mm, double dist_mm)
577 * Start a circle of specified radius around the specified center
578 * (absolute). The distance is specified in degrees.
580 void trajectory_circle_abs_dist_deg(struct trajectory *traj,
581 double x_rel_mm, double y_rel_mm,
582 double radius_mm, double dist_degrees)