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 */
30 #include <aversive/error.h>
31 #include <scheduler.h>
34 #include <position_manager.h>
35 #include <robot_system.h>
36 #include <control_system_manager.h>
39 #include <trajectory_manager.h>
40 #include "trajectory_manager_utils.h"
41 #include "trajectory_manager_core.h"
43 /** set speed consign in quadramp filter */
44 void set_quadramp_speed(struct trajectory *traj, int16_t d_speed, int16_t a_speed)
46 struct quadramp_filter * q_d, * q_a;
47 q_d = traj->csm_distance->consign_filter_params;
48 q_a = traj->csm_angle->consign_filter_params;
49 quadramp_set_1st_order_vars(q_d, ABS(d_speed), ABS(d_speed));
50 quadramp_set_1st_order_vars(q_a, ABS(a_speed), ABS(a_speed));
53 /** get angle speed consign in quadramp filter */
54 uint32_t get_quadramp_angle_speed(struct trajectory *traj)
56 struct quadramp_filter *q_a;
57 q_a = traj->csm_angle->consign_filter_params;
58 return q_a->var_1st_ord_pos;
61 /** get distance speed consign in quadramp filter */
62 uint32_t get_quadramp_distance_speed(struct trajectory *traj)
64 struct quadramp_filter *q_d;
65 q_d = traj->csm_distance->consign_filter_params;
66 return q_d->var_1st_ord_pos;
69 /** remove event if any */
70 void delete_event(struct trajectory *traj)
72 set_quadramp_speed(traj, traj->d_speed, traj->a_speed);
73 if ( traj->scheduler_task != -1) {
74 DEBUG(E_TRAJECTORY, "Delete event");
75 scheduler_del_event(traj->scheduler_task);
76 traj->scheduler_task = -1;
80 /** schedule the trajectory event */
81 void schedule_event(struct trajectory *traj)
83 if ( traj->scheduler_task != -1) {
84 DEBUG(E_TRAJECTORY, "Schedule event, already scheduled");
87 traj->scheduler_task =
88 scheduler_add_periodical_event_priority(&trajectory_manager_event,
94 /** do a modulo 2.pi -> [-Pi,+Pi], knowing that 'a' is in [-3Pi,+3Pi] */
95 double simple_modulo_2pi(double a)
106 /** do a modulo 2.pi -> [-Pi,+Pi] */
107 double modulo_2pi(double a)
109 double res = a - (((int32_t) (a/M_2PI)) * M_2PI);
110 return simple_modulo_2pi(res);
113 /** near the target (dist) ? */
114 uint8_t is_robot_in_dist_window(struct trajectory *traj, double d_win)
116 double d = traj->target.pol.distance - rs_get_distance(traj->robot);
118 d = d / traj->position->phys.distance_imp_per_mm;
122 /** near the target (dist in x,y) ? */
123 uint8_t is_robot_in_xy_window(struct trajectory *traj, double d_win)
125 double x1 = traj->target.cart.x;
126 double y1 = traj->target.cart.y;
127 double x2 = position_get_x_double(traj->position);
128 double y2 = position_get_y_double(traj->position);
129 return ( sqrt ((x2-x1) * (x2-x1) + (y2-y1) * (y2-y1)) < d_win );
132 /** near the angle target in radian ? Only valid if
133 * traj->target.pol.angle is set (i.e. an angle command, not an xy
135 uint8_t is_robot_in_angle_window(struct trajectory *traj, double a_win_rad)
139 /* convert relative angle from imp to rad */
140 a = traj->target.pol.angle - rs_get_angle(traj->robot);
141 a /= traj->position->phys.distance_imp_per_mm;
142 a /= traj->position->phys.track_mm;
144 return ABS(a) < a_win_rad;