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>
32 #include <vect_base.h>
35 #include <position_manager.h>
36 #include <robot_system.h>
37 #include <control_system_manager.h>
40 #include <trajectory_manager.h>
41 #include "trajectory_manager_utils.h"
42 #include "trajectory_manager_core.h"
44 /************ SIMPLE TRAJS, NO EVENT */
51 static uint8_t evt_debug_cpt = 0;
52 #define EVT_DEBUG(args...) do { \
53 if (((evt_debug_cpt ++) & 0x07) == 0) { \
58 static void start_clitoid(struct trajectory *traj);
61 * update angle and/or distance
62 * this function is not called directly by the user
63 * traj : pointer to the trajectory structure
64 * d_mm : distance in mm
65 * a_rad : angle in radian
66 * flags : what to update (UPDATE_A, UPDATE_D)
68 void __trajectory_goto_d_a_rel(struct trajectory *traj, double d_mm,
69 double a_rad, uint8_t state, uint8_t flags)
71 int32_t a_consign, d_consign;
73 DEBUG(E_TRAJECTORY, "Goto DA/RS rel to d=%f a_rad=%f", d_mm, a_rad);
76 if (flags & UPDATE_A) {
77 if (flags & RESET_A) {
81 a_consign = (int32_t)(a_rad * (traj->position->phys.distance_imp_per_mm) *
82 (traj->position->phys.track_mm) / 2);
84 a_consign += rs_get_angle(traj->robot);
85 traj->target.pol.angle = a_consign;
86 cs_set_consign(traj->csm_angle, a_consign);
88 if (flags & UPDATE_D) {
89 if (flags & RESET_D) {
93 d_consign = (int32_t)((d_mm) * (traj->position->phys.distance_imp_per_mm));
95 d_consign += rs_get_distance(traj->robot);
96 traj->target.pol.distance = d_consign;
97 cs_set_consign(traj->csm_distance, d_consign);
101 /** go straight forward (d is in mm) */
102 void trajectory_d_rel(struct trajectory *traj, double d_mm)
104 __trajectory_goto_d_a_rel(traj, d_mm, 0, RUNNING_D,
105 UPDATE_D | UPDATE_A | RESET_A);
108 /** update distance consign without changing angle consign */
109 void trajectory_only_d_rel(struct trajectory *traj, double d_mm)
111 __trajectory_goto_d_a_rel(traj, d_mm, 0, RUNNING_D, UPDATE_D);
114 /** turn by 'a' degrees */
115 void trajectory_a_rel(struct trajectory *traj, double a_deg_rel)
117 __trajectory_goto_d_a_rel(traj, 0, RAD(a_deg_rel), RUNNING_A,
118 UPDATE_A | UPDATE_D | RESET_D);
121 /** turn by 'a' degrees */
122 void trajectory_a_abs(struct trajectory *traj, double a_deg_abs)
124 double posa = position_get_a_rad_double(traj->position);
127 a = RAD(a_deg_abs) - posa;
129 __trajectory_goto_d_a_rel(traj, 0, a, RUNNING_A,
130 UPDATE_A | UPDATE_D | RESET_D);
133 /** turn the robot until the point x,y is in front of us */
134 void trajectory_turnto_xy(struct trajectory *traj, double x_abs_mm, double y_abs_mm)
136 double posx = position_get_x_double(traj->position);
137 double posy = position_get_y_double(traj->position);
138 double posa = position_get_a_rad_double(traj->position);
140 DEBUG(E_TRAJECTORY, "Goto Turn To xy %f %f", x_abs_mm, y_abs_mm);
141 __trajectory_goto_d_a_rel(traj, 0,
142 simple_modulo_2pi(atan2(y_abs_mm - posy, x_abs_mm - posx) - posa),
144 UPDATE_A | UPDATE_D | RESET_D);
147 /** turn the robot until the point x,y is behind us */
148 void trajectory_turnto_xy_behind(struct trajectory *traj, double x_abs_mm, double y_abs_mm)
150 double posx = position_get_x_double(traj->position);
151 double posy = position_get_y_double(traj->position);
152 double posa = position_get_a_rad_double(traj->position);
154 DEBUG(E_TRAJECTORY, "Goto Turn To xy %f %f", x_abs_mm, y_abs_mm);
155 __trajectory_goto_d_a_rel(traj, 0,
156 modulo_2pi(atan2(y_abs_mm - posy, x_abs_mm - posx) - posa + M_PI),
158 UPDATE_A | UPDATE_D | RESET_D);
161 /** update angle consign without changing distance consign */
162 void trajectory_only_a_rel(struct trajectory *traj, double a_deg)
164 __trajectory_goto_d_a_rel(traj, 0, RAD(a_deg), RUNNING_A,
168 /** update angle consign without changing distance consign */
169 void trajectory_only_a_abs(struct trajectory *traj, double a_deg_abs)
171 double posa = position_get_a_rad_double(traj->position);
174 a = RAD(a_deg_abs) - posa;
176 __trajectory_goto_d_a_rel(traj, 0, a, RUNNING_A, UPDATE_A);
179 /** turn by 'a' degrees */
180 void trajectory_d_a_rel(struct trajectory *traj, double d_mm, double a_deg)
182 __trajectory_goto_d_a_rel(traj, d_mm, RAD(a_deg),
183 RUNNING_AD, UPDATE_A | UPDATE_D);
186 /** set relative angle and distance consign to 0 */
187 void trajectory_stop(struct trajectory *traj)
189 DEBUG(E_TRAJECTORY, "stop");
190 __trajectory_goto_d_a_rel(traj, 0, 0, READY,
191 UPDATE_A | UPDATE_D | RESET_D | RESET_A);
194 /** set relative angle and distance consign to 0, and break any
195 * deceleration ramp in quadramp filter */
196 void trajectory_hardstop(struct trajectory *traj)
198 struct quadramp_filter *q_d, *q_a;
200 DEBUG(E_TRAJECTORY, "hardstop");
202 q_d = traj->csm_distance->consign_filter_params;
203 q_a = traj->csm_angle->consign_filter_params;
204 __trajectory_goto_d_a_rel(traj, 0, 0, READY,
205 UPDATE_A | UPDATE_D | RESET_D | RESET_A);
207 q_d->previous_var = 0;
208 q_d->previous_out = rs_get_distance(traj->robot);
209 q_a->previous_var = 0;
210 q_a->previous_out = rs_get_angle(traj->robot);
214 /************ GOTO XY, USE EVENTS */
216 /** goto a x,y point, using a trajectory event */
217 void trajectory_goto_xy_abs(struct trajectory *traj, double x, double y)
219 DEBUG(E_TRAJECTORY, "Goto XY");
221 traj->target.cart.x = x;
222 traj->target.cart.y = y;
223 traj->state = RUNNING_XY_START;
224 trajectory_manager_event(traj);
225 schedule_event(traj);
228 /** go forward to a x,y point, using a trajectory event */
229 void trajectory_goto_forward_xy_abs(struct trajectory *traj, double x, double y)
231 DEBUG(E_TRAJECTORY, "Goto XY_F");
233 traj->target.cart.x = x;
234 traj->target.cart.y = y;
235 traj->state = RUNNING_XY_F_START;
236 trajectory_manager_event(traj);
237 schedule_event(traj);
240 /** go backward to a x,y point, using a trajectory event */
241 void trajectory_goto_backward_xy_abs(struct trajectory *traj, double x, double y)
243 DEBUG(E_TRAJECTORY, "Goto XY_B");
245 traj->target.cart.x = x;
246 traj->target.cart.y = y;
247 traj->state = RUNNING_XY_B_START;
248 trajectory_manager_event(traj);
249 schedule_event(traj);
252 /** go forward to a d,a point, using a trajectory event */
253 void trajectory_goto_d_a_rel(struct trajectory *traj, double d, double a)
256 double x = position_get_x_double(traj->position);
257 double y = position_get_y_double(traj->position);
259 DEBUG(E_TRAJECTORY, "Goto DA rel");
263 p.theta = RAD(a) + position_get_a_rad_double(traj->position);
264 vect2_pol2cart(&p, &traj->target.cart);
265 traj->target.cart.x += x;
266 traj->target.cart.y += y;
268 traj->state = RUNNING_XY_START;
269 trajectory_manager_event(traj);
270 schedule_event(traj);
273 /** go forward to a x,y relative point, using a trajectory event */
274 void trajectory_goto_xy_rel(struct trajectory *traj, double x_rel_mm, double y_rel_mm)
278 double x = position_get_x_double(traj->position);
279 double y = position_get_y_double(traj->position);
281 DEBUG(E_TRAJECTORY, "Goto XY rel");
287 vect2_cart2pol(&c, &p);
288 p.theta += position_get_a_rad_double(traj->position);;
289 vect2_pol2cart(&p, &traj->target.cart);
291 traj->target.cart.x += x;
292 traj->target.cart.y += y;
294 traj->state = RUNNING_XY_START;
295 trajectory_manager_event(traj);
296 schedule_event(traj);
299 /************ FUNCS FOR GETTING TRAJ STATE */
301 uint8_t trajectory_angle_finished(struct trajectory *traj)
303 return cs_get_consign(traj->csm_angle) ==
304 cs_get_filtered_consign(traj->csm_angle);
307 uint8_t trajectory_distance_finished(struct trajectory *traj)
309 if (traj->state == RUNNING_CLITOID_CURVE)
312 return cs_get_consign(traj->csm_distance) ==
313 cs_get_filtered_consign(traj->csm_distance) ;
316 /** return true if the position consign is equal to the filtered
317 * position consign (after quadramp filter), for angle and
319 uint8_t trajectory_finished(struct trajectory *traj)
325 ret = trajectory_distance_finished(traj) &&
326 trajectory_angle_finished(traj);
330 /* XXX THIS IS A VERY BAD WORKAROUND (fix a race) */
332 ret2 = trajectory_distance_finished(traj) &&
333 trajectory_angle_finished(traj);
342 /** return true if traj is nearly finished */
343 uint8_t trajectory_in_window(struct trajectory *traj, double d_win, double a_win_rad)
345 switch(traj->state) {
347 case RUNNING_XY_ANGLE_OK:
348 case RUNNING_XY_F_ANGLE_OK:
349 case RUNNING_XY_B_ANGLE_OK:
350 /* if robot coordinates are near the x,y target */
351 return is_robot_in_xy_window(traj, d_win);
354 return is_robot_in_angle_window(traj, a_win_rad);
357 return is_robot_in_dist_window(traj, d_win);
360 return is_robot_in_dist_window(traj, d_win) &&
361 is_robot_in_angle_window(traj, a_win_rad);
363 case RUNNING_XY_START:
364 case RUNNING_XY_F_START:
365 case RUNNING_XY_B_START:
366 case RUNNING_XY_ANGLE:
367 case RUNNING_XY_F_ANGLE:
368 case RUNNING_XY_B_ANGLE:
374 /*********** *TRAJECTORY EVENT FUNC */
376 /** event called for xy trajectories */
377 void trajectory_manager_xy_event(struct trajectory *traj)
380 double x = position_get_x_double(traj->position);
381 double y = position_get_y_double(traj->position);
382 double a = position_get_a_rad_double(traj->position);
383 int32_t d_consign=0, a_consign=0;
385 /* These vectors contain target position of the robot in
386 * its own coordinates */
387 vect2_cart v2cart_pos;
388 vect2_pol v2pol_target;
390 /* step 1 : process new commands to quadramps */
392 switch (traj->state) {
393 case RUNNING_XY_START:
394 case RUNNING_XY_ANGLE:
395 case RUNNING_XY_ANGLE_OK:
396 case RUNNING_XY_F_START:
397 case RUNNING_XY_F_ANGLE:
398 case RUNNING_XY_F_ANGLE_OK:
399 case RUNNING_XY_B_START:
400 case RUNNING_XY_B_ANGLE:
401 case RUNNING_XY_B_ANGLE_OK:
403 /* process the command vector from current position to
404 * absolute target. */
405 v2cart_pos.x = traj->target.cart.x - x;
406 v2cart_pos.y = traj->target.cart.y - y;
407 vect2_cart2pol(&v2cart_pos, &v2pol_target);
408 v2pol_target.theta = simple_modulo_2pi(v2pol_target.theta - a);
410 /* asked to go backwards */
411 if (traj->state >= RUNNING_XY_B_START &&
412 traj->state <= RUNNING_XY_B_ANGLE_OK ) {
413 v2pol_target.r = -v2pol_target.r;
414 v2pol_target.theta = simple_modulo_2pi(v2pol_target.theta + M_PI);
417 /* if we don't need to go forward */
418 if (traj->state >= RUNNING_XY_START &&
419 traj->state <= RUNNING_XY_ANGLE_OK ) {
420 /* If the target is behind the robot, we need to go
421 * backwards. 0.52 instead of 0.5 because we prefer to
423 if ((v2pol_target.theta > 0.52*M_PI) ||
424 (v2pol_target.theta < -0.52*M_PI ) ) {
425 v2pol_target.r = -v2pol_target.r;
426 v2pol_target.theta = simple_modulo_2pi(v2pol_target.theta + M_PI);
430 /* If the robot is correctly oriented to start moving in distance */
431 /* here limit dist speed depending on v2pol_target.theta */
432 if (ABS(v2pol_target.theta) > traj->a_start_rad) // || ABS(v2pol_target.r) < traj->d_win)
433 set_quadramp_speed(traj, 0, traj->a_speed);
435 coef = (traj->a_start_rad - ABS(v2pol_target.theta)) / traj->a_start_rad;
436 set_quadramp_speed(traj, traj->d_speed * coef, traj->a_speed);
439 d_consign = (int32_t)(v2pol_target.r * (traj->position->phys.distance_imp_per_mm));
440 d_consign += rs_get_distance(traj->robot);
443 /* XXX here we specify 2.2 instead of 2.0 to avoid oscillations */
444 a_consign = (int32_t)(v2pol_target.theta *
445 (traj->position->phys.distance_imp_per_mm) *
446 (traj->position->phys.track_mm) / 2.2);
447 a_consign += rs_get_angle(traj->robot);
452 /* hmmm quite odd, delete the event */
453 DEBUG(E_TRAJECTORY, "GNI ???");
459 /* step 2 : update state, or delete event if we reached the
462 /* XXX if target is our pos !! */
464 switch (traj->state) {
465 case RUNNING_XY_START:
466 case RUNNING_XY_F_START:
467 case RUNNING_XY_B_START:
469 DEBUG(E_TRAJECTORY, "-> ANGLE");
473 case RUNNING_XY_ANGLE:
474 case RUNNING_XY_F_ANGLE:
475 case RUNNING_XY_B_ANGLE: {
476 struct quadramp_filter *q_a;
477 q_a = traj->csm_angle->consign_filter_params;
478 /* if d_speed is not 0, we are in start_angle_win */
479 if (get_quadramp_distance_speed(traj)) {
480 if (is_robot_in_xy_window(traj, traj->d_win)) {
483 /* ANGLE -> ANGLE_OK */
485 DEBUG(E_TRAJECTORY, "-> ANGLE_OK");
490 case RUNNING_XY_ANGLE_OK:
491 case RUNNING_XY_F_ANGLE_OK:
492 case RUNNING_XY_B_ANGLE_OK:
493 /* If we reached the destination */
494 if (is_robot_in_xy_window(traj, traj->d_win)) {
503 /* step 3 : send the processed commands to cs */
505 EVT_DEBUG(E_TRAJECTORY,"EVENT XY d_cur=%" PRIi32 ", d_consign=%" PRIi32 ", d_speed=%" PRIi32 ", "
506 "a_cur=%" PRIi32 ", a_consign=%" PRIi32 ", a_speed=%" PRIi32,
507 rs_get_distance(traj->robot), d_consign, get_quadramp_distance_speed(traj),
508 rs_get_angle(traj->robot), a_consign, get_quadramp_angle_speed(traj));
510 cs_set_consign(traj->csm_angle, a_consign);
511 cs_set_consign(traj->csm_distance, d_consign);
515 * Compute the fastest distance and angle speeds matching the radius
516 * from current traj_speed
518 void circle_get_da_speed_from_radius(struct trajectory *traj,
523 /* speed_d = coef * speed_a */
525 double speed_d2, speed_a2;
527 coef = 2. * radius_mm / traj->position->phys.track_mm;
529 speed_d2 = traj->a_speed * coef;
530 if (speed_d2 < traj->d_speed) {
532 *speed_a = traj->a_speed;
535 speed_a2 = traj->d_speed / coef;
536 *speed_d = traj->d_speed;
541 /* trajectory event for circles */
543 void trajectory_manager_circle_event(struct trajectory *traj)
546 double x = position_get_x_double(traj->position);
547 double y = position_get_y_double(traj->position);
548 double a = position_get_a_rad_double(traj->position);
549 int32_t d_consign = 0, a_consign = 0;
550 double angle_to_center_rad;
551 double coef_p, coef_d;
552 double d_speed, a_speed;
554 /* These vectors contain target position of the robot in
555 * its own coordinates */
556 vect2_cart v2cart_pos;
557 vect2_pol v2pol_target;
559 /* step 1 : process new commands to quadramps */
561 /* process the command vector from current position to the
562 * center of the circle. */
563 v2cart_pos.x = traj->target.circle.center.x - x;
564 v2cart_pos.y = traj->target.circle.center.y - y;
565 vect2_cart2pol(&v2cart_pos, &v2pol_target);
566 v2pol_target.theta = simple_modulo_2pi(v2pol_target.theta - a);
569 radius = traj->target.circle.radius;
571 coef_p = v2pol_target.r / radius;
572 coef_p = 1. * coef_p;
574 angle_to_center_rad = v2pol_target.theta - (M_PI / 2.);
575 angle_to_center_rad = simple_modulo_2pi(angle_to_center_rad);
576 if (angle_to_center_rad > 0.5)
577 angle_to_center_rad = 0.5;
578 if (angle_to_center_rad < -0.5)
579 angle_to_center_rad = -0.5;
580 coef_d = exp(5*angle_to_center_rad);
583 circle_get_da_speed_from_radius(traj, radius / (coef_p * coef_d),
585 set_quadramp_speed(traj, d_speed, a_speed);
587 EVT_DEBUG(E_TRAJECTORY, "angle=%2.2f radius=%2.2f r=%2.2f coef_p=%2.2f coef_d=%2.2f "
588 "d_speed=%2.2f a_speed=%2.2f",
589 angle_to_center_rad, radius, v2pol_target.r,
590 coef_p, coef_d, d_speed, a_speed);
592 /* XXX check flags */
593 d_consign = 400000 + rs_get_distance(traj->robot);
594 a_consign = 400000 + rs_get_angle(traj->robot);
597 /* a_consign = (int32_t)(v2pol_target.theta * */
598 /* (traj->position->phys.distance_imp_per_mm) * */
599 /* (traj->position->phys.track_mm) / 2.0); */
600 /* a_consign += rs_get_angle(traj->robot); */
602 /* step 2 : update state, or delete event if we reached the
605 /* /\* output angle -> delete event *\/ */
606 /* if (a_consign >= traj->target.circle.dest_angle) { */
607 /* a_consign = traj->target.circle.dest_angle; */
608 /* delete_event(traj); */
611 /* step 3 : send the processed commands to cs */
613 /* EVT_DEBUG(E_TRAJECTORY,"EVENT CIRCLE d_cur=%" PRIi32 ", d_consign=%" PRIi32 */
614 /* ", d_speed=%" PRIi32 ", a_cur=%" PRIi32 ", a_consign=%" PRIi32 */
615 /* ", a_speed=%" PRIi32 ", radius = %f", */
616 /* rs_get_distance(traj->robot), d_consign, get_quadramp_distance_speed(traj), */
617 /* rs_get_angle(traj->robot), a_consign, get_quadramp_angle_speed(traj), */
620 cs_set_consign(traj->csm_angle, a_consign);
621 cs_set_consign(traj->csm_distance, d_consign);
624 /* trajectory event for lines */
625 static void trajectory_manager_line_event(struct trajectory *traj)
627 double x = position_get_x_double(traj->position);
628 double y = position_get_y_double(traj->position);
629 double a = position_get_a_rad_double(traj->position);
630 double advance, dist_to_line;
631 point_t robot, proj, target_pt;
632 int32_t d_consign = 0, a_consign = 0;
633 vect2_cart v2cart_pos;
634 vect2_pol v2pol_target;
639 /* target point on the line is further on the line */
640 proj_pt_line(&robot, &traj->target.line.line, &proj);
641 dist_to_line = pt_norm(&robot, &proj);
642 if (dist_to_line > traj->target.line.advance)
645 advance = traj->target.line.advance - dist_to_line;
646 target_pt.x = proj.x + advance * cos(traj->target.line.angle);
647 target_pt.y = proj.y + advance * sin(traj->target.line.angle);
650 v2cart_pos.x = target_pt.x - x;
651 v2cart_pos.y = target_pt.y - y;
652 vect2_cart2pol(&v2cart_pos, &v2pol_target);
653 v2pol_target.theta = simple_modulo_2pi(v2pol_target.theta - a);
655 /* If the robot is correctly oriented to start moving in distance */
656 /* here limit dist speed depending on v2pol_target.theta */
657 if (ABS(v2pol_target.theta) > traj->a_start_rad) // || ABS(v2pol_target.r) < traj->d_win)
658 set_quadramp_speed(traj, 0, traj->a_speed);
661 coef = (traj->a_start_rad - ABS(v2pol_target.theta)) / traj->a_start_rad;
662 set_quadramp_speed(traj, traj->d_speed * coef, traj->a_speed);
665 /* position consign is infinite */
666 d_consign = pos_mm2imp(traj, v2pol_target.r);
667 d_consign += rs_get_distance(traj->robot);
669 /* angle consign (1.1 to avoid oscillations) */
670 a_consign = pos_rd2imp(traj, v2pol_target.theta) / 1.1;
671 a_consign += rs_get_angle(traj->robot);
673 EVT_DEBUG(E_TRAJECTORY, "target.x=%2.2f target.y=%2.2f "
674 "a_consign=%"PRIi32" d_consign=%"PRIi32,
675 target_pt.x, target_pt.y, a_consign, d_consign);
677 cs_set_consign(traj->csm_angle, a_consign);
678 cs_set_consign(traj->csm_distance, d_consign);
680 /* we reached dest, start clitoid */
681 if (traj->state == RUNNING_CLITOID_LINE &&
684 traj->target.line.turn_pt.x,
685 traj->target.line.turn_pt.y) <
686 xy_norm(proj.x + cos(traj->target.line.angle),
687 proj.y + sin(traj->target.line.angle),
688 traj->target.line.turn_pt.x,
689 traj->target.line.turn_pt.y)) {
695 /* trajectory event */
696 void trajectory_manager_event(void * param)
698 struct trajectory *traj = (struct trajectory *)param;
700 switch (traj->state) {
701 case RUNNING_XY_START:
702 case RUNNING_XY_ANGLE:
703 case RUNNING_XY_ANGLE_OK:
704 case RUNNING_XY_F_START:
705 case RUNNING_XY_F_ANGLE:
706 case RUNNING_XY_F_ANGLE_OK:
707 case RUNNING_XY_B_START:
708 case RUNNING_XY_B_ANGLE:
709 case RUNNING_XY_B_ANGLE_OK:
710 trajectory_manager_xy_event(traj);
714 trajectory_manager_circle_event(traj);
718 case RUNNING_CLITOID_LINE:
719 trajectory_manager_line_event(traj);
727 /*********** *CIRCLE */
729 /* make the robot orbiting around (x,y) on a circle whose radius is
730 * radius_mm, and exit when relative destination angle is reached. The
731 * flags set if we go forward or backwards, and CW/CCW. */
732 void trajectory_circle_rel(struct trajectory *traj,
742 traj->target.circle.center.x = x;
743 traj->target.circle.center.y = y;
744 traj->target.circle.radius = radius_mm;
745 traj->target.circle.flags = flags;
747 /* convert in steps */
748 dst_angle = RAD(rel_a_deg) *
749 (traj->position->phys.distance_imp_per_mm) *
750 (traj->position->phys.track_mm) / 2.0;
752 traj->target.circle.dest_angle = rs_get_angle(traj->robot);
753 traj->target.circle.dest_angle += dst_angle;
755 DEBUG(E_TRAJECTORY, "Circle rel (x,y)=%2.2f,%2.2f r=%2.2f flags=%x dst_angle=%"PRIi32"",
756 x, y, radius_mm, flags, traj->target.circle.dest_angle);
758 traj->state = RUNNING_CIRCLE;
759 trajectory_manager_event(traj);
760 schedule_event(traj);
763 /* return the distance in millimeters that corresponds to an angle in
764 * degree and a radius in mm */
765 /* static */double circle_get_dist_from_degrees(double radius_mm, double a_deg)
767 double a_rad = RAD(a_deg);
768 return a_rad * radius_mm;
772 * Start a circle of specified radius around the specified center
773 * (relative with d,a). The distance is specified in mm.
775 void trajectory_circle(struct trajectory *traj,
776 double center_d_mm, double center_a_rad,
777 double radius_mm, double dist_mm)
781 /* DEBUG(E_TRAJECTORY, "CIRCLE to d=%f a_rad=%f", center_d_mm, */
783 /* delete_event(traj); */
784 /* traj->state = RUNNING_CIRCLE; */
790 * Start a circle of specified radius around the specified center
791 * (absolute). The distance is specified in mm.
793 void trajectory_circle_abs_dist_mm(struct trajectory *traj,
794 double x_rel_mm, double y_rel_mm,
795 double radius_mm, double dist_mm)
800 * Start a circle of specified radius around the specified center
801 * (absolute). The distance is specified in degrees.
803 void trajectory_circle_abs_dist_deg(struct trajectory *traj,
804 double x_rel_mm, double y_rel_mm,
805 double radius_mm, double dist_degrees)
810 /*********** *LINE */
813 static void __trajectory_line_abs(struct trajectory *traj,
814 double x1, double y1,
815 double x2, double y2,
820 /* find the line EQ */
825 pts2line(&p1, &p2, &traj->target.line.line);
827 /* find the line angle */
828 traj->target.line.angle = atan2(y2-y1, x2-x1);
829 traj->target.line.advance = advance;
831 DEBUG(E_TRAJECTORY, "Line rel (a,b,c)=%2.2f,%2.2f,%2.2f",
832 traj->target.line.line.a,
833 traj->target.line.line.b,
834 traj->target.line.line.c,
835 traj->target.line.angle);
840 void trajectory_line_abs(struct trajectory *traj,
841 double x1, double y1,
842 double x2, double y2,
846 __trajectory_line_abs(traj, x1, y1, x2, y2, advance);
847 traj->state = RUNNING_LINE;
848 trajectory_manager_event(traj);
849 schedule_event(traj);
855 * process clitoid parameters
857 * - alpha: total angle
858 * - beta: circular part of angle (lower than alpha)
859 * - R: the radius of the circle (must be != 0)
860 * - Vd: linear speed to use (in imp per cs period)
861 * - Amax: maximum angular acceleration
862 * - d_inter: distance in mm until the intersection of the
865 * return 0 on success: in this case these parameters are filled:
866 * - Aa_out: the angular acceleration to configure in quadramp
867 * - Va_out: the angular speed to configure in quadramp
868 * - remain_d_mm_out: remaining distance before start to turn
870 static int8_t calc_clitoid(struct trajectory *traj,
871 double x, double y, double a_rad,
872 double alpha_deg, double beta_deg, double R_mm,
873 double Vd, double Amax, double d_inter_mm,
874 double *Aa_out, double *Va_out, double *remain_d_mm_out)
878 double t, tau, d_mm, alpha_rad, beta_rad;
882 line_t line1_int, line2_int;
883 point_t robot, intersect, pt2, center, proj, M;
888 if (fabs(alpha_deg) <= fabs(beta_deg)) {
889 DEBUG(E_TRAJECTORY, "alpha is smaller than beta");
893 /* get angular speed Va */
894 Vd_mm_s = speed_imp2mm(traj, Vd);
895 DEBUG(E_TRAJECTORY, "Vd_mm_s=%2.2f", Vd_mm_s);
896 Va_rd_s = Vd_mm_s / R_mm;
897 Va = speed_rd2imp(traj, Va_rd_s);
898 DEBUG(E_TRAJECTORY, "Va_rd_s=%2.2f Va=%2.2f", Va_rd_s, Va);
900 /* process 't', the time in seconds that we will take to do
901 * the first clothoid */
902 alpha_rad = RAD(alpha_deg);
903 beta_rad = RAD(beta_deg);
904 t = fabs(((alpha_rad - beta_rad) * R_mm) / Vd_mm_s);
905 DEBUG(E_TRAJECTORY, "R_mm=%2.2f a_rad=%2.2f alpha_rad=%2.2f beta_rad=%2.2f t=%2.2f",
906 R_mm, a_rad, alpha_rad, beta_rad, t);
908 /* process the angular acceleration */
909 Aa_rd_s2 = Va_rd_s / t;
910 Aa = acc_rd2imp(traj, Aa_rd_s2);
911 DEBUG(E_TRAJECTORY, "Aa_rd_s2=%2.2f Aa=%2.2f", Aa_rd_s2, Aa);
913 /* exit if the robot cannot physically do it */
915 DEBUG(E_TRAJECTORY, "greater than max acceleration");
919 /* define line1 and line2 */
922 intersect.x = x + cos(a_rad) * d_inter_mm;
923 intersect.y = y + sin(a_rad) * d_inter_mm;
924 pts2line(&robot, &intersect, &line1);
925 pt2.x = intersect.x + cos(a_rad + alpha_rad);
926 pt2.y = intersect.y + sin(a_rad + alpha_rad);
927 pts2line(&intersect, &pt2, &line2);
928 DEBUG(E_TRAJECTORY, "intersect=(%2.2f, %2.2f)",
929 intersect.x, intersect.y);
931 /* L and A are the parameters of the clothoid, xm and ym are
932 * the relative coords (starting from the beginning of
933 * clothoid) of the crossing point between the clothoid and
936 A = R_mm * sqrt(fabs(alpha_rad - beta_rad));
939 - (pow(L, 5) / (40. * pow(A, 4)))
940 + (pow(L, 9) / (3456. * pow(A, 8)))
941 - (pow(L, 13) / (599040. * pow(A, 12)));
943 (pow(L, 3) / (6. * pow(A, 2)))
944 - (pow(L, 7) / (336. * pow(A, 6)))
945 + (pow(L, 11) / (42240. * pow(A, 10)))
946 - (pow(L, 15) / (9676800. * pow(A, 14)));
947 DEBUG(E_TRAJECTORY, "relative xm,ym = (%2.2f, %2.2f)",
950 /* the center of the circle is at d_mm when we have to start
952 tau = (alpha_rad - beta_rad) / 2.;
953 d_mm = ym + (R_mm * cos(tau));
954 DEBUG(E_TRAJECTORY, "d_mm=%2.2f", d_mm);
956 /* translate line1 */
957 memcpy(&line1_int, &line1, sizeof(line1_int));
958 memcpy(&line2_int, &line2, sizeof(line2_int));
959 v.x = intersect.x - robot.x;
960 v.y = intersect.y - robot.y;
965 vect_resize(&v, d_mm);
966 line_translate(&line1_int, &v);
967 DEBUG(E_TRAJECTORY, "translate line1 by %2.2f,%2.2f", v.x, v.y);
969 /* translate line2_int */
970 v.x = intersect.x - pt2.x;
971 v.y = intersect.y - pt2.y;
976 vect_resize(&v, d_mm);
977 line_translate(&line2_int, &v);
978 DEBUG(E_TRAJECTORY, "translate line2 by %2.2f,%2.2f", v.x, v.y);
980 /* find the center of the circle, at the intersection of the
981 * new translated lines */
982 if (intersect_line(&line1_int, &line2_int, ¢er) != 1) {
983 DEBUG(E_TRAJECTORY, "cannot find circle center");
986 DEBUG(E_TRAJECTORY, "center=(%2.2f,%2.2f)", center.x, center.y);
988 /* M is the same point than xm, ym but in absolute coords */
990 M.x = center.x + cos(a_rad + M_PI/2 + tau) * R_mm;
991 M.y = center.y + sin(a_rad + M_PI/2 + tau) * R_mm;
994 M.x = center.x + cos(a_rad - M_PI/2 + tau) * R_mm;
995 M.y = center.y + sin(a_rad - M_PI/2 + tau) * R_mm;
997 DEBUG(E_TRAJECTORY, "absolute M = (%2.2f, %2.2f)", M.x, M.y);
999 /* project M on line 1 */
1000 proj_pt_line(&M, &line1, &proj);
1001 DEBUG(E_TRAJECTORY, "proj M = (%2.2f, %2.2f)", proj.x, proj.y);
1003 /* process remaining distance before start turning */
1004 remain_d_mm = d_inter_mm - (pt_norm(&proj, &intersect) + xm);
1005 DEBUG(E_TRAJECTORY, "remain_d=%2.2f", remain_d_mm);
1006 if (remain_d_mm < 0) {
1007 DEBUG(E_TRAJECTORY, "too late, cannot turn");
1014 *remain_d_mm_out = remain_d_mm;
1018 /* after the line, start the clothoid */
1019 static void start_clitoid(struct trajectory *traj)
1021 double Aa = traj->target.line.Aa;
1022 double Va = traj->target.line.Va;
1023 double a_rad = traj->target.line.alpha;
1024 double R_mm = traj->target.line.R;
1027 DEBUG(E_TRAJECTORY, "%s() Va=%2.2f Aa=%2.2f",
1028 __FUNCTION__, Va, Aa);
1030 d = fabs(R_mm * a_rad);
1031 d *= 3.; /* margin to avoid deceleration */
1032 trajectory_d_a_rel(traj, d, DEG(a_rad));
1033 set_quadramp_acc(traj, traj->d_acc, Aa);
1034 set_quadramp_speed(traj, traj->d_speed, Va);
1035 traj->state = RUNNING_CLITOID_CURVE;
1040 * do a superb curve joining line1 to line2 which is composed of:
1041 * - a clothoid starting from line1
1043 * - another clothoid up to line2
1044 * this curve is called a clitoid (hehe)
1046 * the function assumes that the initial linear speed is Vd and
1047 * angular speed is 0.
1049 * - x,y,a_deg: starting position
1050 * - advance: parameter for line following
1051 * - alpha: total angle
1052 * - beta: circular part of angle (lower than alpha)
1053 * - R: the radius of the circle (must be != 0)
1054 * - Vd: linear speed to use (in imp per cs period)
1055 * - Amax: maximum angular acceleration
1056 * - d_inter: distance in mm until the intersection of the
1059 * return 0 if trajectory can be loaded, then it is processed in
1062 int8_t trajectory_clitoid(struct trajectory *traj,
1063 double x, double y, double a_deg, double advance,
1064 double alpha_deg, double beta_deg, double R_mm,
1067 double remain = 0, Aa = 0, Va = 0, Vd;
1068 double turnx, turny;
1069 double a_rad = RAD(a_deg);
1072 if (calc_clitoid(traj, x, y, a_rad, alpha_deg, beta_deg, R_mm,
1073 Vd, traj->a_acc, d_inter_mm,
1074 &Aa, &Va, &remain) < 0) {
1075 DEBUG(E_TRAJECTORY, "%s() calc_clitoid returned an error",
1081 turnx = x + cos(a_rad) * remain;
1082 turny = y + sin(a_rad) * remain;
1083 traj->target.line.Aa = Aa;
1084 traj->target.line.Va = Va;
1085 traj->target.line.alpha = RAD(alpha_deg);
1086 traj->target.line.R = R_mm;
1087 traj->target.line.turn_pt.x = turnx;
1088 traj->target.line.turn_pt.y = turny;
1089 DEBUG(E_TRAJECTORY, "%s() turn_pt=%2.2f,%2.2f",
1090 __FUNCTION__, turnx, turny);
1092 __trajectory_line_abs(traj, x, y, turnx, turny,
1094 traj->state = RUNNING_CLITOID_LINE;
1095 trajectory_manager_event(traj);
1096 schedule_event(traj);