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 /** near the target (dist) ? */
180 static uint8_t is_robot_in_dist_window(struct trajectory *traj, double d_win)
182 double d = traj->target.pol.distance - rs_get_distance(traj->robot);
184 d = d / traj->position->phys.distance_imp_per_mm;
188 /** near the target (dist in x,y) ? */
189 static uint8_t is_robot_in_xy_window(struct trajectory *traj, double d_win)
191 double x1 = traj->target.cart.x;
192 double y1 = traj->target.cart.y;
193 double x2 = position_get_x_double(traj->position);
194 double y2 = position_get_y_double(traj->position);
195 return ( sqrt ((x2-x1) * (x2-x1) + (y2-y1) * (y2-y1)) < d_win );
198 /** near the angle target in radian ? Only valid if
199 * traj->target.pol.angle is set (i.e. an angle command, not an xy
201 static uint8_t is_robot_in_angle_window(struct trajectory *traj, double a_win_rad)
205 /* convert relative angle from imp to rad */
206 a = traj->target.pol.angle - rs_get_angle(traj->robot);
207 a /= traj->position->phys.distance_imp_per_mm;
208 a /= traj->position->phys.track_mm;
210 return ABS(a) < a_win_rad;
214 /************ SIMPLE TRAJS, NO EVENT */
222 * update angle and/or distance
223 * this function is not called directly by the user
224 * traj : pointer to the trajectory structure
225 * d_mm : distance in mm
226 * a_rad : angle in radian
227 * flags : what to update (UPDATE_A, UPDATE_D)
229 void __trajectory_goto_d_a_rel(struct trajectory *traj, double d_mm,
230 double a_rad, uint8_t state, uint8_t flags)
232 int32_t a_consign, d_consign;
234 DEBUG(E_TRAJECTORY, "Goto DA/RS rel to d=%f a_rad=%f", d_mm, a_rad);
237 if (flags & UPDATE_A) {
238 if (flags & RESET_A) {
242 a_consign = (int32_t)(a_rad * (traj->position->phys.distance_imp_per_mm) *
243 (traj->position->phys.track_mm) / 2);
245 a_consign += rs_get_angle(traj->robot);
246 traj->target.pol.angle = a_consign;
247 cs_set_consign(traj->csm_angle, a_consign);
249 if (flags & UPDATE_D) {
250 if (flags & RESET_D) {
254 d_consign = (int32_t)((d_mm) * (traj->position->phys.distance_imp_per_mm));
256 d_consign += rs_get_distance(traj->robot);
257 traj->target.pol.distance = d_consign;
258 cs_set_consign(traj->csm_distance, d_consign);
262 /** go straight forward (d is in mm) */
263 void trajectory_d_rel(struct trajectory *traj, double d_mm)
265 __trajectory_goto_d_a_rel(traj, d_mm, 0, RUNNING_D,
266 UPDATE_D | UPDATE_A | RESET_A);
269 /** update distance consign without changing angle consign */
270 void trajectory_only_d_rel(struct trajectory *traj, double d_mm)
272 __trajectory_goto_d_a_rel(traj, d_mm, 0, RUNNING_D, UPDATE_D);
275 /** turn by 'a' degrees */
276 void trajectory_a_rel(struct trajectory *traj, double a_deg_rel)
278 __trajectory_goto_d_a_rel(traj, 0, RAD(a_deg_rel), RUNNING_A,
279 UPDATE_A | UPDATE_D | RESET_D);
282 /** turn by 'a' degrees */
283 void trajectory_a_abs(struct trajectory *traj, double a_deg_abs)
285 double posa = position_get_a_rad_double(traj->position);
288 a = RAD(a_deg_abs) - posa;
290 __trajectory_goto_d_a_rel(traj, 0, a, RUNNING_A,
291 UPDATE_A | UPDATE_D | RESET_D);
294 /** turn the robot until the point x,y is in front of us */
295 void trajectory_turnto_xy(struct trajectory *traj, double x_abs_mm, double y_abs_mm)
297 double posx = position_get_x_double(traj->position);
298 double posy = position_get_y_double(traj->position);
299 double posa = position_get_a_rad_double(traj->position);
301 DEBUG(E_TRAJECTORY, "Goto Turn To xy %f %f", x_abs_mm, y_abs_mm);
302 __trajectory_goto_d_a_rel(traj, 0,
303 simple_modulo_2pi(atan2(y_abs_mm - posy, x_abs_mm - posx) - posa),
305 UPDATE_A | UPDATE_D | RESET_D);
308 /** turn the robot until the point x,y is behind us */
309 void trajectory_turnto_xy_behind(struct trajectory *traj, double x_abs_mm, double y_abs_mm)
311 double posx = position_get_x_double(traj->position);
312 double posy = position_get_y_double(traj->position);
313 double posa = position_get_a_rad_double(traj->position);
315 DEBUG(E_TRAJECTORY, "Goto Turn To xy %f %f", x_abs_mm, y_abs_mm);
316 __trajectory_goto_d_a_rel(traj, 0,
317 modulo_2pi(atan2(y_abs_mm - posy, x_abs_mm - posx) - posa + M_PI),
319 UPDATE_A | UPDATE_D | RESET_D);
322 /** update angle consign without changing distance consign */
323 void trajectory_only_a_rel(struct trajectory *traj, double a_deg)
325 __trajectory_goto_d_a_rel(traj, 0, RAD(a_deg), RUNNING_A,
329 /** update angle consign without changing distance consign */
330 void trajectory_only_a_abs(struct trajectory *traj, double a_deg_abs)
332 double posa = position_get_a_rad_double(traj->position);
335 a = RAD(a_deg_abs) - posa;
337 __trajectory_goto_d_a_rel(traj, 0, a, RUNNING_A, UPDATE_A);
340 /** turn by 'a' degrees */
341 void trajectory_d_a_rel(struct trajectory *traj, double d_mm, double a_deg)
343 __trajectory_goto_d_a_rel(traj, d_mm, RAD(a_deg),
344 RUNNING_AD, UPDATE_A | UPDATE_D);
347 /** set relative angle and distance consign to 0 */
348 void trajectory_stop(struct trajectory *traj)
350 __trajectory_goto_d_a_rel(traj, 0, 0, READY,
351 UPDATE_A | UPDATE_D | RESET_D | RESET_A);
354 /** set relative angle and distance consign to 0, and break any
355 * deceleration ramp in quadramp filter */
356 void trajectory_hardstop(struct trajectory *traj)
358 struct quadramp_filter *q_d, *q_a;
360 q_d = traj->csm_distance->consign_filter_params;
361 q_a = traj->csm_angle->consign_filter_params;
362 __trajectory_goto_d_a_rel(traj, 0, 0, READY,
363 UPDATE_A | UPDATE_D | RESET_D | RESET_A);
365 q_d->previous_var = 0;
366 q_d->previous_out = rs_get_distance(traj->robot);
367 q_a->previous_var = 0;
368 q_a->previous_out = rs_get_angle(traj->robot);
372 /************ GOTO XY, USE EVENTS */
374 /** goto a x,y point, using a trajectory event */
375 void trajectory_goto_xy_abs(struct trajectory *traj, double x, double y)
377 DEBUG(E_TRAJECTORY, "Goto XY");
379 traj->target.cart.x = x;
380 traj->target.cart.y = y;
381 traj->state = RUNNING_XY_START;
382 trajectory_manager_event(traj);
383 schedule_event(traj);
386 /** go forward to a x,y point, using a trajectory event */
387 void trajectory_goto_forward_xy_abs(struct trajectory *traj, double x, double y)
389 DEBUG(E_TRAJECTORY, "Goto XY_F");
391 traj->target.cart.x = x;
392 traj->target.cart.y = y;
393 traj->state = RUNNING_XY_F_START;
394 trajectory_manager_event(traj);
395 schedule_event(traj);
398 /** go backward to a x,y point, using a trajectory event */
399 void trajectory_goto_backward_xy_abs(struct trajectory *traj, double x, double y)
401 DEBUG(E_TRAJECTORY, "Goto XY_B");
403 traj->target.cart.x = x;
404 traj->target.cart.y = y;
405 traj->state = RUNNING_XY_B_START;
406 trajectory_manager_event(traj);
407 schedule_event(traj);
410 /** go forward to a d,a point, using a trajectory event */
411 void trajectory_goto_d_a_rel(struct trajectory *traj, double d, double a)
414 double x = position_get_x_double(traj->position);
415 double y = position_get_y_double(traj->position);
417 DEBUG(E_TRAJECTORY, "Goto DA rel");
421 p.theta = RAD(a) + position_get_a_rad_double(traj->position);
422 vect2_pol2cart(&p, &traj->target.cart);
423 traj->target.cart.x += x;
424 traj->target.cart.y += y;
426 traj->state = RUNNING_XY_START;
427 trajectory_manager_event(traj);
428 schedule_event(traj);
431 /** go forward to a x,y relative point, using a trajectory event */
432 void trajectory_goto_xy_rel(struct trajectory *traj, double x_rel_mm, double y_rel_mm)
436 double x = position_get_x_double(traj->position);
437 double y = position_get_y_double(traj->position);
439 DEBUG(E_TRAJECTORY, "Goto XY rel");
445 vect2_cart2pol(&c, &p);
446 p.theta += position_get_a_rad_double(traj->position);;
447 vect2_pol2cart(&p, &traj->target.cart);
449 traj->target.cart.x += x;
450 traj->target.cart.y += y;
452 traj->state = RUNNING_XY_START;
453 trajectory_manager_event(traj);
454 schedule_event(traj);
457 /************ FUNCS FOR GETTING TRAJ STATE */
459 /** return true if the position consign is equal to the filtered
460 * position consign (after quadramp filter), for angle and
462 uint8_t trajectory_finished(struct trajectory *traj)
464 return cs_get_consign(traj->csm_angle) == cs_get_filtered_consign(traj->csm_angle) &&
465 cs_get_consign(traj->csm_distance) == cs_get_filtered_consign(traj->csm_distance) ;
468 /** return true if traj is nearly finished */
469 uint8_t trajectory_in_window(struct trajectory *traj, double d_win, double a_win_rad)
471 switch(traj->state) {
473 case RUNNING_XY_ANGLE_OK:
474 case RUNNING_XY_F_ANGLE_OK:
475 case RUNNING_XY_B_ANGLE_OK:
476 /* if robot coordinates are near the x,y target */
477 return is_robot_in_xy_window(traj, d_win);
480 return is_robot_in_angle_window(traj, a_win_rad);
483 return is_robot_in_dist_window(traj, d_win);
486 return is_robot_in_dist_window(traj, d_win) &&
487 is_robot_in_angle_window(traj, a_win_rad);
489 case RUNNING_XY_START:
490 case RUNNING_XY_F_START:
491 case RUNNING_XY_B_START:
492 case RUNNING_XY_ANGLE:
493 case RUNNING_XY_F_ANGLE:
494 case RUNNING_XY_B_ANGLE:
500 /*********** *TRAJECTORY EVENT FUNC */
502 /** event called for xy trajectories */
503 static void trajectory_manager_event(void * param)
505 struct trajectory *traj = (struct trajectory *)param;
507 double x = position_get_x_double(traj->position);
508 double y = position_get_y_double(traj->position);
509 double a = position_get_a_rad_double(traj->position);
510 int32_t d_consign=0, a_consign=0;
512 /* These vectors contain target position of the robot in
513 * its own coordinates */
514 vect2_cart v2cart_pos;
515 vect2_pol v2pol_target;
517 /* step 1 : process new commands to quadramps */
519 switch (traj->state) {
520 case RUNNING_XY_START:
521 case RUNNING_XY_ANGLE:
522 case RUNNING_XY_ANGLE_OK:
523 case RUNNING_XY_F_START:
524 case RUNNING_XY_F_ANGLE:
525 case RUNNING_XY_F_ANGLE_OK:
526 case RUNNING_XY_B_START:
527 case RUNNING_XY_B_ANGLE:
528 case RUNNING_XY_B_ANGLE_OK:
530 /* process the command vector from absolute target and
531 * current position */
532 v2cart_pos.x = traj->target.cart.x - x;
533 v2cart_pos.y = traj->target.cart.y - y;
534 vect2_cart2pol(&v2cart_pos, &v2pol_target);
535 v2pol_target.theta = simple_modulo_2pi(v2pol_target.theta - a);
537 /* asked to go backwards */
538 if (traj->state >= RUNNING_XY_B_START &&
539 traj->state <= RUNNING_XY_B_ANGLE_OK ) {
540 v2pol_target.r = -v2pol_target.r;
541 v2pol_target.theta = simple_modulo_2pi(v2pol_target.theta + M_PI);
544 /* if we don't need to go forward */
545 if (traj->state >= RUNNING_XY_START &&
546 traj->state <= RUNNING_XY_ANGLE_OK ) {
547 /* If the target is behind the robot, we need to go
548 * backwards. 0.52 instead of 0.5 because we prefer to
550 if ((v2pol_target.theta > 0.52*M_PI) ||
551 (v2pol_target.theta < -0.52*M_PI ) ) {
552 v2pol_target.r = -v2pol_target.r;
553 v2pol_target.theta = simple_modulo_2pi(v2pol_target.theta + M_PI);
557 /* If the robot is correctly oriented to start moving in distance */
558 /* here limit dist speed depending on v2pol_target.theta */
559 if (ABS(v2pol_target.theta) > traj->a_start_rad) // || ABS(v2pol_target.r) < traj->d_win)
560 set_quadramp_speed(traj, 0, traj->a_speed);
562 coef = (traj->a_start_rad - ABS(v2pol_target.theta)) / traj->a_start_rad;
563 set_quadramp_speed(traj, traj->d_speed * coef, traj->a_speed);
566 d_consign = (int32_t)(v2pol_target.r * (traj->position->phys.distance_imp_per_mm));
567 d_consign += rs_get_distance(traj->robot);
570 /* XXX here we specify 2.2 instead of 2.0 to avoid oscillations */
571 a_consign = (int32_t)(v2pol_target.theta *
572 (traj->position->phys.distance_imp_per_mm) *
573 (traj->position->phys.track_mm) / 2.2);
574 a_consign += rs_get_angle(traj->robot);
579 /* hmmm quite odd, delete the event */
580 DEBUG(E_TRAJECTORY, "GNI ???");
586 /* step 2 : update state, or delete event if we reached the
589 /* XXX if target is our pos !! */
591 switch (traj->state) {
592 case RUNNING_XY_START:
593 case RUNNING_XY_F_START:
594 case RUNNING_XY_B_START:
596 DEBUG(E_TRAJECTORY, "-> ANGLE");
600 case RUNNING_XY_ANGLE:
601 case RUNNING_XY_F_ANGLE:
602 case RUNNING_XY_B_ANGLE: {
603 struct quadramp_filter *q_a;
604 q_a = traj->csm_angle->consign_filter_params;
605 /* if d_speed is not 0, we are in start_angle_win */
606 if (get_quadramp_distance_speed(traj)) {
607 if(is_robot_in_xy_window(traj, traj->d_win)) {
610 /* ANGLE -> ANGLE_OK */
612 DEBUG(E_TRAJECTORY, "-> ANGLE_OK");
617 case RUNNING_XY_ANGLE_OK:
618 case RUNNING_XY_F_ANGLE_OK:
619 case RUNNING_XY_B_ANGLE_OK:
620 /* If we reached the destination */
621 if(is_robot_in_xy_window(traj, traj->d_win)) {
630 /* step 3 : send the processed commands to cs */
632 DEBUG(E_TRAJECTORY, "EVENT XY cur=(%f,%f,%f) cart=(%f,%f) pol=(%f,%f)",
633 x, y, a, v2cart_pos.x, v2cart_pos.y, v2pol_target.r, v2pol_target.theta);
635 DEBUG(E_TRAJECTORY,"d_cur=%" PRIi32 ", d_consign=%" PRIi32 ", d_speed=%" PRIi32 ", "
636 "a_cur=%" PRIi32 ", a_consign=%" PRIi32 ", a_speed=%" PRIi32,
637 rs_get_distance(traj->robot), d_consign, get_quadramp_distance_speed(traj),
638 rs_get_angle(traj->robot), a_consign, get_quadramp_angle_speed(traj));
640 cs_set_consign(traj->csm_angle, a_consign);
641 cs_set_consign(traj->csm_distance, d_consign);