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) { \
60 * update angle and/or distance
61 * this function is not called directly by the user
62 * traj : pointer to the trajectory structure
63 * d_mm : distance in mm
64 * a_rad : angle in radian
65 * flags : what to update (UPDATE_A, UPDATE_D)
67 void __trajectory_goto_d_a_rel(struct trajectory *traj, double d_mm,
68 double a_rad, uint8_t state, uint8_t flags)
70 int32_t a_consign, d_consign;
72 DEBUG(E_TRAJECTORY, "Goto DA/RS rel to d=%f a_rad=%f", d_mm, a_rad);
75 if (flags & UPDATE_A) {
76 if (flags & RESET_A) {
80 a_consign = (int32_t)(a_rad * (traj->position->phys.distance_imp_per_mm) *
81 (traj->position->phys.track_mm) / 2);
83 a_consign += rs_get_angle(traj->robot);
84 traj->target.pol.angle = a_consign;
85 cs_set_consign(traj->csm_angle, a_consign);
87 if (flags & UPDATE_D) {
88 if (flags & RESET_D) {
92 d_consign = (int32_t)((d_mm) * (traj->position->phys.distance_imp_per_mm));
94 d_consign += rs_get_distance(traj->robot);
95 traj->target.pol.distance = d_consign;
96 cs_set_consign(traj->csm_distance, d_consign);
100 /** go straight forward (d is in mm) */
101 void trajectory_d_rel(struct trajectory *traj, double d_mm)
103 __trajectory_goto_d_a_rel(traj, d_mm, 0, RUNNING_D,
104 UPDATE_D | UPDATE_A | RESET_A);
107 /** update distance consign without changing angle consign */
108 void trajectory_only_d_rel(struct trajectory *traj, double d_mm)
110 __trajectory_goto_d_a_rel(traj, d_mm, 0, RUNNING_D, UPDATE_D);
113 /** turn by 'a' degrees */
114 void trajectory_a_rel(struct trajectory *traj, double a_deg_rel)
116 __trajectory_goto_d_a_rel(traj, 0, RAD(a_deg_rel), RUNNING_A,
117 UPDATE_A | UPDATE_D | RESET_D);
120 /** turn by 'a' degrees */
121 void trajectory_a_abs(struct trajectory *traj, double a_deg_abs)
123 double posa = position_get_a_rad_double(traj->position);
126 a = RAD(a_deg_abs) - posa;
128 __trajectory_goto_d_a_rel(traj, 0, a, RUNNING_A,
129 UPDATE_A | UPDATE_D | RESET_D);
132 /** turn the robot until the point x,y is in front of us */
133 void trajectory_turnto_xy(struct trajectory *traj, double x_abs_mm, double y_abs_mm)
135 double posx = position_get_x_double(traj->position);
136 double posy = position_get_y_double(traj->position);
137 double posa = position_get_a_rad_double(traj->position);
139 DEBUG(E_TRAJECTORY, "Goto Turn To xy %f %f", x_abs_mm, y_abs_mm);
140 __trajectory_goto_d_a_rel(traj, 0,
141 simple_modulo_2pi(atan2(y_abs_mm - posy, x_abs_mm - posx) - posa),
143 UPDATE_A | UPDATE_D | RESET_D);
146 /** turn the robot until the point x,y is behind us */
147 void trajectory_turnto_xy_behind(struct trajectory *traj, double x_abs_mm, double y_abs_mm)
149 double posx = position_get_x_double(traj->position);
150 double posy = position_get_y_double(traj->position);
151 double posa = position_get_a_rad_double(traj->position);
153 DEBUG(E_TRAJECTORY, "Goto Turn To xy %f %f", x_abs_mm, y_abs_mm);
154 __trajectory_goto_d_a_rel(traj, 0,
155 modulo_2pi(atan2(y_abs_mm - posy, x_abs_mm - posx) - posa + M_PI),
157 UPDATE_A | UPDATE_D | RESET_D);
160 /** update angle consign without changing distance consign */
161 void trajectory_only_a_rel(struct trajectory *traj, double a_deg)
163 __trajectory_goto_d_a_rel(traj, 0, RAD(a_deg), RUNNING_A,
167 /** update angle consign without changing distance consign */
168 void trajectory_only_a_abs(struct trajectory *traj, double a_deg_abs)
170 double posa = position_get_a_rad_double(traj->position);
173 a = RAD(a_deg_abs) - posa;
175 __trajectory_goto_d_a_rel(traj, 0, a, RUNNING_A, UPDATE_A);
178 /** turn by 'a' degrees */
179 void trajectory_d_a_rel(struct trajectory *traj, double d_mm, double a_deg)
181 __trajectory_goto_d_a_rel(traj, d_mm, RAD(a_deg),
182 RUNNING_AD, UPDATE_A | UPDATE_D);
185 /** set relative angle and distance consign to 0 */
186 void trajectory_stop(struct trajectory *traj)
188 __trajectory_goto_d_a_rel(traj, 0, 0, READY,
189 UPDATE_A | UPDATE_D | RESET_D | RESET_A);
192 /** set relative angle and distance consign to 0, and break any
193 * deceleration ramp in quadramp filter */
194 void trajectory_hardstop(struct trajectory *traj)
196 struct quadramp_filter *q_d, *q_a;
198 q_d = traj->csm_distance->consign_filter_params;
199 q_a = traj->csm_angle->consign_filter_params;
200 __trajectory_goto_d_a_rel(traj, 0, 0, READY,
201 UPDATE_A | UPDATE_D | RESET_D | RESET_A);
203 q_d->previous_var = 0;
204 q_d->previous_out = rs_get_distance(traj->robot);
205 q_a->previous_var = 0;
206 q_a->previous_out = rs_get_angle(traj->robot);
210 /************ GOTO XY, USE EVENTS */
212 /** goto a x,y point, using a trajectory event */
213 void trajectory_goto_xy_abs(struct trajectory *traj, double x, double y)
215 DEBUG(E_TRAJECTORY, "Goto XY");
217 traj->target.cart.x = x;
218 traj->target.cart.y = y;
219 traj->state = RUNNING_XY_START;
220 trajectory_manager_event(traj);
221 schedule_event(traj);
224 /** go forward to a x,y point, using a trajectory event */
225 void trajectory_goto_forward_xy_abs(struct trajectory *traj, double x, double y)
227 DEBUG(E_TRAJECTORY, "Goto XY_F");
229 traj->target.cart.x = x;
230 traj->target.cart.y = y;
231 traj->state = RUNNING_XY_F_START;
232 trajectory_manager_event(traj);
233 schedule_event(traj);
236 /** go backward to a x,y point, using a trajectory event */
237 void trajectory_goto_backward_xy_abs(struct trajectory *traj, double x, double y)
239 DEBUG(E_TRAJECTORY, "Goto XY_B");
241 traj->target.cart.x = x;
242 traj->target.cart.y = y;
243 traj->state = RUNNING_XY_B_START;
244 trajectory_manager_event(traj);
245 schedule_event(traj);
248 /** go forward to a d,a point, using a trajectory event */
249 void trajectory_goto_d_a_rel(struct trajectory *traj, double d, double a)
252 double x = position_get_x_double(traj->position);
253 double y = position_get_y_double(traj->position);
255 DEBUG(E_TRAJECTORY, "Goto DA rel");
259 p.theta = RAD(a) + position_get_a_rad_double(traj->position);
260 vect2_pol2cart(&p, &traj->target.cart);
261 traj->target.cart.x += x;
262 traj->target.cart.y += y;
264 traj->state = RUNNING_XY_START;
265 trajectory_manager_event(traj);
266 schedule_event(traj);
269 /** go forward to a x,y relative point, using a trajectory event */
270 void trajectory_goto_xy_rel(struct trajectory *traj, double x_rel_mm, double y_rel_mm)
274 double x = position_get_x_double(traj->position);
275 double y = position_get_y_double(traj->position);
277 DEBUG(E_TRAJECTORY, "Goto XY rel");
283 vect2_cart2pol(&c, &p);
284 p.theta += position_get_a_rad_double(traj->position);;
285 vect2_pol2cart(&p, &traj->target.cart);
287 traj->target.cart.x += x;
288 traj->target.cart.y += y;
290 traj->state = RUNNING_XY_START;
291 trajectory_manager_event(traj);
292 schedule_event(traj);
295 /************ FUNCS FOR GETTING TRAJ STATE */
297 /** return true if the position consign is equal to the filtered
298 * position consign (after quadramp filter), for angle and
300 uint8_t trajectory_finished(struct trajectory *traj)
302 return cs_get_consign(traj->csm_angle) == cs_get_filtered_consign(traj->csm_angle) &&
303 cs_get_consign(traj->csm_distance) == cs_get_filtered_consign(traj->csm_distance) ;
306 /** return true if traj is nearly finished */
307 uint8_t trajectory_in_window(struct trajectory *traj, double d_win, double a_win_rad)
309 switch(traj->state) {
311 case RUNNING_XY_ANGLE_OK:
312 case RUNNING_XY_F_ANGLE_OK:
313 case RUNNING_XY_B_ANGLE_OK:
314 /* if robot coordinates are near the x,y target */
315 return is_robot_in_xy_window(traj, d_win);
318 return is_robot_in_angle_window(traj, a_win_rad);
321 return is_robot_in_dist_window(traj, d_win);
324 return is_robot_in_dist_window(traj, d_win) &&
325 is_robot_in_angle_window(traj, a_win_rad);
327 case RUNNING_XY_START:
328 case RUNNING_XY_F_START:
329 case RUNNING_XY_B_START:
330 case RUNNING_XY_ANGLE:
331 case RUNNING_XY_F_ANGLE:
332 case RUNNING_XY_B_ANGLE:
338 /*********** *TRAJECTORY EVENT FUNC */
340 /** event called for xy trajectories */
341 void trajectory_manager_xy_event(struct trajectory *traj)
344 double x = position_get_x_double(traj->position);
345 double y = position_get_y_double(traj->position);
346 double a = position_get_a_rad_double(traj->position);
347 int32_t d_consign=0, a_consign=0;
349 /* These vectors contain target position of the robot in
350 * its own coordinates */
351 vect2_cart v2cart_pos;
352 vect2_pol v2pol_target;
354 /* step 1 : process new commands to quadramps */
356 switch (traj->state) {
357 case RUNNING_XY_START:
358 case RUNNING_XY_ANGLE:
359 case RUNNING_XY_ANGLE_OK:
360 case RUNNING_XY_F_START:
361 case RUNNING_XY_F_ANGLE:
362 case RUNNING_XY_F_ANGLE_OK:
363 case RUNNING_XY_B_START:
364 case RUNNING_XY_B_ANGLE:
365 case RUNNING_XY_B_ANGLE_OK:
367 /* process the command vector from current position to
368 * absolute target. */
369 v2cart_pos.x = traj->target.cart.x - x;
370 v2cart_pos.y = traj->target.cart.y - y;
371 vect2_cart2pol(&v2cart_pos, &v2pol_target);
372 v2pol_target.theta = simple_modulo_2pi(v2pol_target.theta - a);
374 /* asked to go backwards */
375 if (traj->state >= RUNNING_XY_B_START &&
376 traj->state <= RUNNING_XY_B_ANGLE_OK ) {
377 v2pol_target.r = -v2pol_target.r;
378 v2pol_target.theta = simple_modulo_2pi(v2pol_target.theta + M_PI);
381 /* if we don't need to go forward */
382 if (traj->state >= RUNNING_XY_START &&
383 traj->state <= RUNNING_XY_ANGLE_OK ) {
384 /* If the target is behind the robot, we need to go
385 * backwards. 0.52 instead of 0.5 because we prefer to
387 if ((v2pol_target.theta > 0.52*M_PI) ||
388 (v2pol_target.theta < -0.52*M_PI ) ) {
389 v2pol_target.r = -v2pol_target.r;
390 v2pol_target.theta = simple_modulo_2pi(v2pol_target.theta + M_PI);
394 /* If the robot is correctly oriented to start moving in distance */
395 /* here limit dist speed depending on v2pol_target.theta */
396 if (ABS(v2pol_target.theta) > traj->a_start_rad) // || ABS(v2pol_target.r) < traj->d_win)
397 set_quadramp_speed(traj, 0, traj->a_speed);
399 coef = (traj->a_start_rad - ABS(v2pol_target.theta)) / traj->a_start_rad;
400 set_quadramp_speed(traj, traj->d_speed * coef, traj->a_speed);
403 d_consign = (int32_t)(v2pol_target.r * (traj->position->phys.distance_imp_per_mm));
404 d_consign += rs_get_distance(traj->robot);
407 /* XXX here we specify 2.2 instead of 2.0 to avoid oscillations */
408 a_consign = (int32_t)(v2pol_target.theta *
409 (traj->position->phys.distance_imp_per_mm) *
410 (traj->position->phys.track_mm) / 2.2);
411 a_consign += rs_get_angle(traj->robot);
416 /* hmmm quite odd, delete the event */
417 DEBUG(E_TRAJECTORY, "GNI ???");
423 /* step 2 : update state, or delete event if we reached the
426 /* XXX if target is our pos !! */
428 switch (traj->state) {
429 case RUNNING_XY_START:
430 case RUNNING_XY_F_START:
431 case RUNNING_XY_B_START:
433 DEBUG(E_TRAJECTORY, "-> ANGLE");
437 case RUNNING_XY_ANGLE:
438 case RUNNING_XY_F_ANGLE:
439 case RUNNING_XY_B_ANGLE: {
440 struct quadramp_filter *q_a;
441 q_a = traj->csm_angle->consign_filter_params;
442 /* if d_speed is not 0, we are in start_angle_win */
443 if (get_quadramp_distance_speed(traj)) {
444 if (is_robot_in_xy_window(traj, traj->d_win)) {
447 /* ANGLE -> ANGLE_OK */
449 DEBUG(E_TRAJECTORY, "-> ANGLE_OK");
454 case RUNNING_XY_ANGLE_OK:
455 case RUNNING_XY_F_ANGLE_OK:
456 case RUNNING_XY_B_ANGLE_OK:
457 /* If we reached the destination */
458 if (is_robot_in_xy_window(traj, traj->d_win)) {
467 /* step 3 : send the processed commands to cs */
469 EVT_DEBUG(E_TRAJECTORY,"EVENT XY d_cur=%" PRIi32 ", d_consign=%" PRIi32 ", d_speed=%" PRIi32 ", "
470 "a_cur=%" PRIi32 ", a_consign=%" PRIi32 ", a_speed=%" PRIi32,
471 rs_get_distance(traj->robot), d_consign, get_quadramp_distance_speed(traj),
472 rs_get_angle(traj->robot), a_consign, get_quadramp_angle_speed(traj));
474 cs_set_consign(traj->csm_angle, a_consign);
475 cs_set_consign(traj->csm_distance, d_consign);
479 * Compute the fastest distance and angle speeds matching the radius
480 * from current traj_speed
482 void circle_get_da_speed_from_radius(struct trajectory *traj,
487 /* speed_d = coef * speed_a */
489 double speed_d2, speed_a2;
491 coef = 2. * radius_mm / traj->position->phys.track_mm;
493 speed_d2 = traj->a_speed * coef;
494 if (speed_d2 < traj->d_speed) {
496 *speed_a = traj->a_speed;
499 speed_a2 = traj->d_speed / coef;
500 *speed_d = traj->d_speed;
505 /* trajectory event for circles */
507 void trajectory_manager_circle_event(struct trajectory *traj)
510 double x = position_get_x_double(traj->position);
511 double y = position_get_y_double(traj->position);
512 double a = position_get_a_rad_double(traj->position);
513 int32_t d_consign = 0, a_consign = 0;
514 double angle_to_center_rad;
515 double coef_p, coef_d;
516 double d_speed, a_speed;
518 /* These vectors contain target position of the robot in
519 * its own coordinates */
520 vect2_cart v2cart_pos;
521 vect2_pol v2pol_target;
523 /* step 1 : process new commands to quadramps */
525 /* process the command vector from current position to the
526 * center of the circle. */
527 v2cart_pos.x = traj->target.circle.center.x - x;
528 v2cart_pos.y = traj->target.circle.center.y - y;
529 vect2_cart2pol(&v2cart_pos, &v2pol_target);
530 v2pol_target.theta = simple_modulo_2pi(v2pol_target.theta - a);
533 radius = traj->target.circle.radius;
535 coef_p = v2pol_target.r / radius;
536 coef_p = 1. * coef_p;
538 angle_to_center_rad = v2pol_target.theta - (M_PI / 2.);
539 angle_to_center_rad = simple_modulo_2pi(angle_to_center_rad);
540 if (angle_to_center_rad > 0.5)
541 angle_to_center_rad = 0.5;
542 if (angle_to_center_rad < -0.5)
543 angle_to_center_rad = -0.5;
544 coef_d = exp(5*angle_to_center_rad);
547 circle_get_da_speed_from_radius(traj, radius / (coef_p * coef_d),
549 set_quadramp_speed(traj, d_speed, a_speed);
551 EVT_DEBUG(E_TRAJECTORY, "angle=%2.2f radius=%2.2f r=%2.2f coef_p=%2.2f coef_d=%2.2f "
552 "d_speed=%2.2f a_speed=%2.2f",
553 angle_to_center_rad, radius, v2pol_target.r,
554 coef_p, coef_d, d_speed, a_speed);
556 /* XXX check flags */
557 d_consign = 400000 + rs_get_distance(traj->robot);
558 a_consign = 400000 + rs_get_angle(traj->robot);
561 /* a_consign = (int32_t)(v2pol_target.theta * */
562 /* (traj->position->phys.distance_imp_per_mm) * */
563 /* (traj->position->phys.track_mm) / 2.0); */
564 /* a_consign += rs_get_angle(traj->robot); */
566 /* step 2 : update state, or delete event if we reached the
569 /* /\* output angle -> delete event *\/ */
570 /* if (a_consign >= traj->target.circle.dest_angle) { */
571 /* a_consign = traj->target.circle.dest_angle; */
572 /* delete_event(traj); */
575 /* step 3 : send the processed commands to cs */
577 /* EVT_DEBUG(E_TRAJECTORY,"EVENT CIRCLE d_cur=%" PRIi32 ", d_consign=%" PRIi32 */
578 /* ", d_speed=%" PRIi32 ", a_cur=%" PRIi32 ", a_consign=%" PRIi32 */
579 /* ", a_speed=%" PRIi32 ", radius = %f", */
580 /* rs_get_distance(traj->robot), d_consign, get_quadramp_distance_speed(traj), */
581 /* rs_get_angle(traj->robot), a_consign, get_quadramp_angle_speed(traj), */
584 cs_set_consign(traj->csm_angle, a_consign);
585 cs_set_consign(traj->csm_distance, d_consign);
588 /* trajectory event for lines */
589 static void trajectory_manager_line_event(struct trajectory *traj)
591 double x = position_get_x_double(traj->position);
592 double y = position_get_y_double(traj->position);
593 double a = position_get_a_rad_double(traj->position);
594 double advance, dist_to_line;
595 point_t robot, proj, target_pt;
596 int32_t d_consign = 0, a_consign = 0;
597 vect2_cart v2cart_pos;
598 vect2_pol v2pol_target;
603 /* target point on the line is further on the line */
604 proj_pt_line(&robot, &traj->target.line.line, &proj);
605 dist_to_line = pt_norm(&robot, &proj);
606 if (dist_to_line > traj->target.line.advance)
609 advance = traj->target.line.advance - dist_to_line;
610 target_pt.x = proj.x + advance * cos(traj->target.line.angle);
611 target_pt.y = proj.y + advance * sin(traj->target.line.angle);
614 v2cart_pos.x = target_pt.x - x;
615 v2cart_pos.y = target_pt.y - y;
616 vect2_cart2pol(&v2cart_pos, &v2pol_target);
617 v2pol_target.theta = simple_modulo_2pi(v2pol_target.theta - a);
619 /* If the robot is correctly oriented to start moving in distance */
620 /* here limit dist speed depending on v2pol_target.theta */
621 if (ABS(v2pol_target.theta) > traj->a_start_rad) // || ABS(v2pol_target.r) < traj->d_win)
622 set_quadramp_speed(traj, 0, traj->a_speed);
625 coef = (traj->a_start_rad - ABS(v2pol_target.theta)) / traj->a_start_rad;
626 set_quadramp_speed(traj, traj->d_speed * coef, traj->a_speed);
629 /* position consign is infinite */
630 d_consign = (int32_t)(v2pol_target.r * (traj->position->phys.distance_imp_per_mm));
631 d_consign += rs_get_distance(traj->robot);
634 a_consign = (int32_t)(v2pol_target.theta *
635 (traj->position->phys.distance_imp_per_mm) *
636 (traj->position->phys.track_mm) / 2.2);
637 a_consign += rs_get_angle(traj->robot);
639 EVT_DEBUG(E_TRAJECTORY, "target.x=%2.2f target.y=%2.2f "
640 "a_consign=%"PRIi32" d_consign=%"PRIi32,
641 target_pt.x, target_pt.y, a_consign, d_consign);
643 cs_set_consign(traj->csm_angle, a_consign);
644 cs_set_consign(traj->csm_distance, d_consign);
648 /* trajectory event */
649 void trajectory_manager_event(void * param)
651 struct trajectory *traj = (struct trajectory *)param;
653 switch (traj->state) {
654 case RUNNING_XY_START:
655 case RUNNING_XY_ANGLE:
656 case RUNNING_XY_ANGLE_OK:
657 case RUNNING_XY_F_START:
658 case RUNNING_XY_F_ANGLE:
659 case RUNNING_XY_F_ANGLE_OK:
660 case RUNNING_XY_B_START:
661 case RUNNING_XY_B_ANGLE:
662 case RUNNING_XY_B_ANGLE_OK:
663 trajectory_manager_xy_event(traj);
667 trajectory_manager_circle_event(traj);
671 trajectory_manager_line_event(traj);
679 /*********** *CIRCLE */
681 /* make the robot orbiting around (x,y) on a circle whose radius is
682 * radius_mm, and exit when relative destination angle is reached. The
683 * flags set if we go forward or backwards, and CW/CCW. */
684 void trajectory_circle_rel(struct trajectory *traj,
694 traj->target.circle.center.x = x;
695 traj->target.circle.center.y = y;
696 traj->target.circle.radius = radius_mm;
697 traj->target.circle.flags = flags;
699 /* convert in steps */
700 dst_angle = RAD(rel_a_deg) *
701 (traj->position->phys.distance_imp_per_mm) *
702 (traj->position->phys.track_mm) / 2.0;
704 traj->target.circle.dest_angle = rs_get_angle(traj->robot);
705 traj->target.circle.dest_angle += dst_angle;
707 DEBUG(E_TRAJECTORY, "Circle rel (x,y)=%2.2f,%2.2f r=%2.2f flags=%x dst_angle=%"PRIi32"",
708 x, y, radius_mm, flags, traj->target.circle.dest_angle);
710 traj->state = RUNNING_CIRCLE;
711 trajectory_manager_event(traj);
712 schedule_event(traj);
715 /* return the distance in millimeters that corresponds to an angle in
716 * degree and a radius in mm */
717 /* static */double circle_get_dist_from_degrees(double radius_mm, double a_deg)
719 double a_rad = RAD(a_deg);
720 return a_rad * radius_mm;
724 * Start a circle of specified radius around the specified center
725 * (relative with d,a). The distance is specified in mm.
727 void trajectory_circle(struct trajectory *traj,
728 double center_d_mm, double center_a_rad,
729 double radius_mm, double dist_mm)
733 /* DEBUG(E_TRAJECTORY, "CIRCLE to d=%f a_rad=%f", center_d_mm, */
735 /* delete_event(traj); */
736 /* traj->state = RUNNING_CIRCLE; */
742 * Start a circle of specified radius around the specified center
743 * (absolute). The distance is specified in mm.
745 void trajectory_circle_abs_dist_mm(struct trajectory *traj,
746 double x_rel_mm, double y_rel_mm,
747 double radius_mm, double dist_mm)
752 * Start a circle of specified radius around the specified center
753 * (absolute). The distance is specified in degrees.
755 void trajectory_circle_abs_dist_deg(struct trajectory *traj,
756 double x_rel_mm, double y_rel_mm,
757 double radius_mm, double dist_degrees)
762 /*********** *LINE */
765 void trajectory_line_abs(struct trajectory *traj,
766 double x1, double y1,
767 double x2, double y2,
774 /* find the line EQ */
779 pts2line(&p1, &p2, &traj->target.line.line);
781 /* find the line angle */
782 traj->target.line.angle = atan2(y2-y1, x2-x1);
783 traj->target.line.advance = advance;
785 DEBUG(E_TRAJECTORY, "Line rel (a,b,c)=%2.2f,%2.2f,%2.2f",
786 traj->target.line.line.a,
787 traj->target.line.line.b,
788 traj->target.line.line.c,
789 traj->target.line.angle);
791 traj->state = RUNNING_LINE;
792 trajectory_manager_event(traj);
793 schedule_event(traj);