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 v2 - zer0 - for Eurobot 2008 */
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>
40 #define M_2PI (2*M_PI)
42 #define DEG(x) ((x) * (180.0 / M_PI))
43 #define RAD(x) ((x) * (M_PI / 180.0))
45 static void trajectory_manager_event(void *param);
47 /************ INIT FUNCS */
49 /** structure initialization */
50 void trajectory_init(struct trajectory *traj)
55 memset(traj, 0, sizeof(struct trajectory));
57 traj->scheduler_task = -1;
61 /** structure initialization */
62 void trajectory_set_cs(struct trajectory *traj, struct cs *cs_d,
68 traj->csm_distance = cs_d;
69 traj->csm_angle = cs_a;
73 /** structure initialization */
74 void trajectory_set_robot_params(struct trajectory *traj,
75 struct robot_system *rs,
76 struct robot_position *pos)
85 /** set speed consign */
86 void trajectory_set_speed( struct trajectory *traj, int16_t d_speed, int16_t a_speed)
90 traj->d_speed = d_speed;
91 traj->a_speed = a_speed;
95 /** set windows for trajectory */
96 void trajectory_set_windows(struct trajectory *traj, double d_win,
97 double a_win_deg, double a_start_deg)
101 traj->d_win = d_win ;
102 traj->a_win_rad = RAD(a_win_deg);
103 traj->a_start_rad = RAD(a_start_deg);
107 /************ STATIC [ AND USEFUL ] FUNCS */
109 /** set speed consign in quadramp filter */
110 static void set_quadramp_speed(struct trajectory *traj, int16_t d_speed, int16_t a_speed)
112 struct quadramp_filter * q_d, * q_a;
113 q_d = traj->csm_distance->consign_filter_params;
114 q_a = traj->csm_angle->consign_filter_params;
115 quadramp_set_1st_order_vars(q_d, ABS(d_speed), ABS(d_speed));
116 quadramp_set_1st_order_vars(q_a, ABS(a_speed), ABS(a_speed));
119 /** get angle speed consign in quadramp filter */
120 static uint32_t get_quadramp_angle_speed(struct trajectory *traj)
122 struct quadramp_filter *q_a;
123 q_a = traj->csm_angle->consign_filter_params;
124 return q_a->var_1st_ord_pos;
127 /** get distance speed consign in quadramp filter */
128 static uint32_t get_quadramp_distance_speed(struct trajectory *traj)
130 struct quadramp_filter *q_d;
131 q_d = traj->csm_distance->consign_filter_params;
132 return q_d->var_1st_ord_pos;
135 /** remove event if any */
136 static void delete_event(struct trajectory *traj)
138 set_quadramp_speed(traj, traj->d_speed, traj->a_speed);
139 if ( traj->scheduler_task != -1) {
140 DEBUG(E_TRAJECTORY, "Delete event");
141 scheduler_del_event(traj->scheduler_task);
142 traj->scheduler_task = -1;
146 /** schedule the trajectory event */
147 static void schedule_event(struct trajectory *traj)
149 if ( traj->scheduler_task != -1) {
150 DEBUG(E_TRAJECTORY, "Schedule event, already scheduled");
153 traj->scheduler_task =
154 scheduler_add_periodical_event_priority(&trajectory_manager_event,
156 100000L/SCHEDULER_UNIT, 30);
160 /** do a modulo 2.pi -> [-Pi,+Pi], knowing that 'a' is in [-3Pi,+3Pi] */
161 static double simple_modulo_2pi(double a)
172 /** do a modulo 2.pi -> [-Pi,+Pi] */
173 static double modulo_2pi(double a)
175 double res = a - (((int32_t) (a/M_2PI)) * M_2PI);
176 return simple_modulo_2pi(res);
179 #include <aversive/wait.h>
180 /** near the target (dist) ? */
181 static uint8_t is_robot_in_dist_window(struct trajectory *traj, double d_win)
183 double d = traj->target.pol.distance - rs_get_distance(traj->robot);
185 d = d / traj->position->phys.distance_imp_per_mm;
189 /** near the target (dist in x,y) ? */
190 static uint8_t is_robot_in_xy_window(struct trajectory *traj, double d_win)
192 double x1 = traj->target.cart.x;
193 double y1 = traj->target.cart.y;
194 double x2 = position_get_x_double(traj->position);
195 double y2 = position_get_y_double(traj->position);
196 return ( sqrt ((x2-x1) * (x2-x1) + (y2-y1) * (y2-y1)) < d_win );
199 /** near the angle target in radian ? Only valid if
200 * traj->target.pol.angle is set (i.e. an angle command, not an xy
202 static uint8_t is_robot_in_angle_window(struct trajectory *traj, double a_win_rad)
206 /* convert relative angle from imp to rad */
207 a = traj->target.pol.angle - rs_get_angle(traj->robot);
208 a /= traj->position->phys.distance_imp_per_mm;
209 a /= traj->position->phys.track_mm;
211 return ABS(a) < a_win_rad;
215 /************ SIMPLE TRAJS, NO EVENT */
223 * update angle and/or distance
224 * this function is not called directly by the user
225 * traj : pointer to the trajectory structure
226 * d_mm : distance in mm
227 * a_rad : angle in radian
228 * flags : what to update (UPDATE_A, UPDATE_D)
230 void __trajectory_goto_d_a_rel(struct trajectory *traj, double d_mm,
231 double a_rad, uint8_t state, uint8_t flags)
233 int32_t a_consign, d_consign;
235 DEBUG(E_TRAJECTORY, "Goto DA/RS rel to d=%f a_rad=%f", d_mm, a_rad);
238 if (flags & UPDATE_A) {
239 if (flags & RESET_A) {
243 a_consign = (int32_t)(a_rad * (traj->position->phys.distance_imp_per_mm) *
244 (traj->position->phys.track_mm) / 2);
246 a_consign += rs_get_angle(traj->robot);
247 traj->target.pol.angle = a_consign;
248 cs_set_consign(traj->csm_angle, a_consign);
250 if (flags & UPDATE_D) {
251 if (flags & RESET_D) {
255 d_consign = (int32_t)((d_mm) * (traj->position->phys.distance_imp_per_mm));
257 d_consign += rs_get_distance(traj->robot);
258 traj->target.pol.distance = d_consign;
259 cs_set_consign(traj->csm_distance, d_consign);
263 /** go straight forward (d is in mm) */
264 void trajectory_d_rel(struct trajectory *traj, double d_mm)
266 __trajectory_goto_d_a_rel(traj, d_mm, 0, RUNNING_D,
267 UPDATE_D | UPDATE_A | RESET_A);
270 /** update distance consign without changing angle consign */
271 void trajectory_only_d_rel(struct trajectory *traj, double d_mm)
273 __trajectory_goto_d_a_rel(traj, d_mm, 0, RUNNING_D, UPDATE_D);
276 /** turn by 'a' degrees */
277 void trajectory_a_rel(struct trajectory *traj, double a_deg_rel)
279 __trajectory_goto_d_a_rel(traj, 0, RAD(a_deg_rel), RUNNING_A,
280 UPDATE_A | UPDATE_D | RESET_D);
283 /** turn by 'a' degrees */
284 void trajectory_a_abs(struct trajectory *traj, double a_deg_abs)
286 double posa = position_get_a_rad_double(traj->position);
289 a = RAD(a_deg_abs) - posa;
291 __trajectory_goto_d_a_rel(traj, 0, a, RUNNING_A,
292 UPDATE_A | UPDATE_D | RESET_D);
295 /** turn the robot until the point x,y is in front of us */
296 void trajectory_turnto_xy(struct trajectory *traj, double x_abs_mm, double y_abs_mm)
298 double posx = position_get_x_double(traj->position);
299 double posy = position_get_y_double(traj->position);
300 double posa = position_get_a_rad_double(traj->position);
302 DEBUG(E_TRAJECTORY, "Goto Turn To xy %f %f", x_abs_mm, y_abs_mm);
303 __trajectory_goto_d_a_rel(traj, 0,
304 simple_modulo_2pi(atan2(y_abs_mm - posy, x_abs_mm - posx) - posa),
306 UPDATE_A | UPDATE_D | RESET_D);
309 /** turn the robot until the point x,y is behind us */
310 void trajectory_turnto_xy_behind(struct trajectory *traj, double x_abs_mm, double y_abs_mm)
312 double posx = position_get_x_double(traj->position);
313 double posy = position_get_y_double(traj->position);
314 double posa = position_get_a_rad_double(traj->position);
316 DEBUG(E_TRAJECTORY, "Goto Turn To xy %f %f", x_abs_mm, y_abs_mm);
317 __trajectory_goto_d_a_rel(traj, 0,
318 modulo_2pi(atan2(y_abs_mm - posy, x_abs_mm - posx) - posa + M_PI),
320 UPDATE_A | UPDATE_D | RESET_D);
323 /** update angle consign without changing distance consign */
324 void trajectory_only_a_rel(struct trajectory *traj, double a_deg)
326 __trajectory_goto_d_a_rel(traj, 0, RAD(a_deg), RUNNING_A,
330 /** update angle consign without changing distance consign */
331 void trajectory_only_a_abs(struct trajectory *traj, double a_deg_abs)
333 double posa = position_get_a_rad_double(traj->position);
336 a = RAD(a_deg_abs) - posa;
338 __trajectory_goto_d_a_rel(traj, 0, a, RUNNING_A, UPDATE_A);
341 /** turn by 'a' degrees */
342 void trajectory_d_a_rel(struct trajectory *traj, double d_mm, double a_deg)
344 __trajectory_goto_d_a_rel(traj, d_mm, RAD(a_deg),
345 RUNNING_AD, UPDATE_A | UPDATE_D);
348 /** set relative angle and distance consign to 0 */
349 void trajectory_stop(struct trajectory *traj)
351 __trajectory_goto_d_a_rel(traj, 0, 0, READY,
352 UPDATE_A | UPDATE_D | RESET_D | RESET_A);
355 /** set relative angle and distance consign to 0, and break any
356 * deceleration ramp in quadramp filter */
357 void trajectory_hardstop(struct trajectory *traj)
359 struct quadramp_filter *q_d, *q_a;
361 q_d = traj->csm_distance->consign_filter_params;
362 q_a = traj->csm_angle->consign_filter_params;
363 __trajectory_goto_d_a_rel(traj, 0, 0, READY,
364 UPDATE_A | UPDATE_D | RESET_D | RESET_A);
366 q_d->previous_var = 0;
367 q_d->previous_out = rs_get_distance(traj->robot);
368 q_a->previous_var = 0;
369 q_a->previous_out = rs_get_angle(traj->robot);
373 /************ GOTO XY, USE EVENTS */
375 /** goto a x,y point, using a trajectory event */
376 void trajectory_goto_xy_abs(struct trajectory *traj, double x, double y)
378 DEBUG(E_TRAJECTORY, "Goto XY");
380 traj->target.cart.x = x;
381 traj->target.cart.y = y;
382 traj->state = RUNNING_XY_START;
383 trajectory_manager_event(traj);
384 schedule_event(traj);
387 /** go forward to a x,y point, using a trajectory event */
388 void trajectory_goto_forward_xy_abs(struct trajectory *traj, double x, double y)
390 DEBUG(E_TRAJECTORY, "Goto XY_F");
392 traj->target.cart.x = x;
393 traj->target.cart.y = y;
394 traj->state = RUNNING_XY_F_START;
395 trajectory_manager_event(traj);
396 schedule_event(traj);
399 /** go backward to a x,y point, using a trajectory event */
400 void trajectory_goto_backward_xy_abs(struct trajectory *traj, double x, double y)
402 DEBUG(E_TRAJECTORY, "Goto XY_B");
404 traj->target.cart.x = x;
405 traj->target.cart.y = y;
406 traj->state = RUNNING_XY_B_START;
407 trajectory_manager_event(traj);
408 schedule_event(traj);
411 /** go forward to a d,a point, using a trajectory event */
412 void trajectory_goto_d_a_rel(struct trajectory *traj, double d, double a)
415 double x = position_get_x_double(traj->position);
416 double y = position_get_y_double(traj->position);
418 DEBUG(E_TRAJECTORY, "Goto DA rel");
422 p.theta = RAD(a) + position_get_a_rad_double(traj->position);
423 vect2_pol2cart(&p, &traj->target.cart);
424 traj->target.cart.x += x;
425 traj->target.cart.y += y;
427 traj->state = RUNNING_XY_START;
428 trajectory_manager_event(traj);
429 schedule_event(traj);
432 /** go forward to a x,y relative point, using a trajectory event */
433 void trajectory_goto_xy_rel(struct trajectory *traj, double x_rel_mm, double y_rel_mm)
437 double x = position_get_x_double(traj->position);
438 double y = position_get_y_double(traj->position);
440 DEBUG(E_TRAJECTORY, "Goto XY rel");
446 vect2_cart2pol(&c, &p);
447 p.theta += position_get_a_rad_double(traj->position);;
448 vect2_pol2cart(&p, &traj->target.cart);
450 traj->target.cart.x += x;
451 traj->target.cart.y += y;
453 traj->state = RUNNING_XY_START;
454 trajectory_manager_event(traj);
455 schedule_event(traj);
458 /************ FUNCS FOR GETTING TRAJ STATE */
460 /** return true if the position consign is equal to the filtered
461 * position consign (after quadramp filter), for angle and
463 uint8_t trajectory_finished(struct trajectory *traj)
465 return cs_get_consign(traj->csm_angle) == cs_get_filtered_consign(traj->csm_angle) &&
466 cs_get_consign(traj->csm_distance) == cs_get_filtered_consign(traj->csm_distance) ;
469 /** return true if traj is nearly finished */
470 uint8_t trajectory_in_window(struct trajectory *traj, double d_win, double a_win_rad)
472 switch(traj->state) {
474 case RUNNING_XY_ANGLE_OK:
475 case RUNNING_XY_F_ANGLE_OK:
476 case RUNNING_XY_B_ANGLE_OK:
477 /* if robot coordinates are near the x,y target */
478 return is_robot_in_xy_window(traj, d_win);
481 return is_robot_in_angle_window(traj, a_win_rad);
484 return is_robot_in_dist_window(traj, d_win);
487 return is_robot_in_dist_window(traj, d_win) &&
488 is_robot_in_angle_window(traj, a_win_rad);
490 case RUNNING_XY_START:
491 case RUNNING_XY_F_START:
492 case RUNNING_XY_B_START:
493 case RUNNING_XY_ANGLE:
494 case RUNNING_XY_F_ANGLE:
495 case RUNNING_XY_B_ANGLE:
501 /*********** *TRAJECTORY EVENT FUNC */
503 /** event called for xy trajectories */
504 static void trajectory_manager_event(void * param)
506 struct trajectory *traj = (struct trajectory *)param;
508 double x = position_get_x_double(traj->position);
509 double y = position_get_y_double(traj->position);
510 double a = position_get_a_rad_double(traj->position);
511 int32_t d_consign=0, a_consign=0;
513 /* These vectors contain target position of the robot in
514 * its own coordinates */
515 vect2_cart v2cart_pos;
516 vect2_pol v2pol_target;
518 /* step 1 : process new commands to quadramps */
520 switch (traj->state) {
521 case RUNNING_XY_START:
522 case RUNNING_XY_ANGLE:
523 case RUNNING_XY_ANGLE_OK:
524 case RUNNING_XY_F_START:
525 case RUNNING_XY_F_ANGLE:
526 case RUNNING_XY_F_ANGLE_OK:
527 case RUNNING_XY_B_START:
528 case RUNNING_XY_B_ANGLE:
529 case RUNNING_XY_B_ANGLE_OK:
531 /* process the command vector from current position to
532 * absolute target, or to the center of the circle. */
533 v2cart_pos.x = traj->target.cart.x - x;
534 v2cart_pos.y = traj->target.cart.y - y;
535 vect2_cart2pol(&v2cart_pos, &v2pol_target);
536 v2pol_target.theta = simple_modulo_2pi(v2pol_target.theta - a);
538 /* asked to go backwards */
539 if (traj->state >= RUNNING_XY_B_START &&
540 traj->state <= RUNNING_XY_B_ANGLE_OK ) {
541 v2pol_target.r = -v2pol_target.r;
542 v2pol_target.theta = simple_modulo_2pi(v2pol_target.theta + M_PI);
545 /* if we don't need to go forward */
546 if (traj->state >= RUNNING_XY_START &&
547 traj->state <= RUNNING_XY_ANGLE_OK ) {
548 /* If the target is behind the robot, we need to go
549 * backwards. 0.52 instead of 0.5 because we prefer to
551 if ((v2pol_target.theta > 0.52*M_PI) ||
552 (v2pol_target.theta < -0.52*M_PI ) ) {
553 v2pol_target.r = -v2pol_target.r;
554 v2pol_target.theta = simple_modulo_2pi(v2pol_target.theta + M_PI);
560 /* If the robot is correctly oriented to start moving in distance */
561 /* here limit dist speed depending on v2pol_target.theta */
562 if (ABS(v2pol_target.theta) > traj->a_start_rad) // || ABS(v2pol_target.r) < traj->d_win)
563 set_quadramp_speed(traj, 0, traj->a_speed);
565 coef = (traj->a_start_rad - ABS(v2pol_target.theta)) / traj->a_start_rad;
566 set_quadramp_speed(traj, traj->d_speed * coef, traj->a_speed);
569 d_consign = (int32_t)(v2pol_target.r * (traj->position->phys.distance_imp_per_mm));
570 d_consign += rs_get_distance(traj->robot);
573 /* XXX here we specify 2.2 instead of 2.0 to avoid oscillations */
574 a_consign = (int32_t)(v2pol_target.theta *
575 (traj->position->phys.distance_imp_per_mm) *
576 (traj->position->phys.track_mm) / 2.2);
577 a_consign += rs_get_angle(traj->robot);
582 /* hmmm quite odd, delete the event */
583 DEBUG(E_TRAJECTORY, "GNI ???");
589 /* step 2 : update state, or delete event if we reached the
592 /* XXX if target is our pos !! */
594 switch (traj->state) {
595 case RUNNING_XY_START:
596 case RUNNING_XY_F_START:
597 case RUNNING_XY_B_START:
599 DEBUG(E_TRAJECTORY, "-> ANGLE");
603 case RUNNING_XY_ANGLE:
604 case RUNNING_XY_F_ANGLE:
605 case RUNNING_XY_B_ANGLE: {
606 struct quadramp_filter *q_a;
607 q_a = traj->csm_angle->consign_filter_params;
608 /* if d_speed is not 0, we are in start_angle_win */
609 if (get_quadramp_distance_speed(traj)) {
610 if(is_robot_in_xy_window(traj, traj->d_win)) {
613 /* ANGLE -> ANGLE_OK */
615 DEBUG(E_TRAJECTORY, "-> ANGLE_OK");
620 case RUNNING_XY_ANGLE_OK:
621 case RUNNING_XY_F_ANGLE_OK:
622 case RUNNING_XY_B_ANGLE_OK:
623 /* If we reached the destination */
624 if(is_robot_in_xy_window(traj, traj->d_win)) {
633 /* step 3 : send the processed commands to cs */
635 DEBUG(E_TRAJECTORY, "EVENT XY cur=(%f,%f,%f) cart=(%f,%f) pol=(%f,%f)",
636 x, y, a, v2cart_pos.x, v2cart_pos.y, v2pol_target.r, v2pol_target.theta);
638 DEBUG(E_TRAJECTORY,"d_cur=%" PRIi32 ", d_consign=%" PRIi32 ", d_speed=%" PRIi32 ", "
639 "a_cur=%" PRIi32 ", a_consign=%" PRIi32 ", a_speed=%" PRIi32,
640 rs_get_distance(traj->robot), d_consign, get_quadramp_distance_speed(traj),
641 rs_get_angle(traj->robot), a_consign, get_quadramp_angle_speed(traj));
643 cs_set_consign(traj->csm_angle, a_consign);
644 cs_set_consign(traj->csm_distance, d_consign);
647 /*********** *CIRCLE */
650 * Compute the fastest distance and angle speeds matching the radius
651 * from current traj_speed
653 /* static */void circle_get_da_speed_from_radius(struct trajectory *traj,
658 /* speed_d = coef * speed_a */
660 double speed_d2, speed_a2;
662 coef = 2. * radius_mm / traj->position->phys.track_mm;
664 speed_d2 = traj->a_speed * coef;
665 if (speed_d2 < traj->d_speed) {
667 *speed_a = traj->a_speed;
670 speed_a2 = traj->d_speed / coef;
671 *speed_d = traj->d_speed;
676 /* return the distance in millimeters that corresponds to an angle in
677 * degree and a radius in mm */
678 /* static */double circle_get_dist_from_degrees(double radius_mm, double a_deg)
680 double a_rad = RAD(a_deg);
681 return a_rad * radius_mm;
685 * Start a circle of specified radius around the specified center
686 * (relative with d,a). The distance is specified in mm.
688 void trajectory_circle(struct trajectory *traj,
689 double center_d_mm, double center_a_rad,
690 double radius_mm, double dist_mm)
694 /* DEBUG(E_TRAJECTORY, "CIRCLE to d=%f a_rad=%f", center_d_mm, */
696 /* delete_event(traj); */
697 /* traj->state = RUNNING_CIRCLE; */
703 * Start a circle of specified radius around the specified center
704 * (absolute). The distance is specified in mm.
706 void trajectory_circle_abs_dist_mm(struct trajectory *traj,
707 double x_rel_mm, double y_rel_mm,
708 double radius_mm, double dist_mm)
713 * Start a circle of specified radius around the specified center
714 * (absolute). The distance is specified in degrees.
716 void trajectory_circle_abs_dist_deg(struct trajectory *traj,
717 double x_rel_mm, double y_rel_mm,
718 double radius_mm, double dist_degrees)