save
[aversive.git] / modules / devices / robot / trajectory_manager / trajectory_manager_utils.c
1 /*
2  *  Copyright Droids Corporation, Microb Technology, Eirbot (2005)
3  *
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.
8  *
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.
13  *
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
17  *
18  *  Revision : $Id: trajectory_manager.c,v 1.4.4.17 2009-05-18 12:28:36 zer0 Exp $
19  *
20  */
21
22 /* Trajectory Manager v3 - zer0 - for Eurobot 2010 */
23
24
25 #include <string.h>
26 #include <stdlib.h>
27 #include <math.h>
28
29 #include <aversive.h>
30 #include <aversive/error.h>
31 #include <scheduler.h>
32 #include <vect2.h>
33
34 #include <position_manager.h>
35 #include <robot_system.h>
36 #include <control_system_manager.h>
37 #include <quadramp.h>
38
39 #include <trajectory_manager.h>
40 #include "trajectory_manager_utils.h"
41 #include "trajectory_manager_core.h"
42
43 /** set speed consign in quadramp filter */
44 void set_quadramp_speed(struct trajectory *traj, double d_speed, double a_speed)
45 {
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));
51 }
52
53 /** get angle speed consign in quadramp filter */
54 double get_quadramp_angle_speed(struct trajectory *traj)
55 {
56         struct quadramp_filter *q_a;
57         q_a = traj->csm_angle->consign_filter_params;
58         return q_a->var_1st_ord_pos;
59 }
60
61 /** get distance speed consign in quadramp filter */
62 double get_quadramp_distance_speed(struct trajectory *traj)
63 {
64         struct quadramp_filter *q_d;
65         q_d = traj->csm_distance->consign_filter_params;
66         return q_d->var_1st_ord_pos;
67 }
68
69 /** set speed consign in quadramp filter */
70 void set_quadramp_acc(struct trajectory *traj, double d_acc, double a_acc)
71 {
72         struct quadramp_filter * q_d, * q_a;
73         q_d = traj->csm_distance->consign_filter_params;
74         q_a = traj->csm_angle->consign_filter_params;
75         quadramp_set_2nd_order_vars(q_d, ABS(d_acc), ABS(d_acc));
76         quadramp_set_2nd_order_vars(q_a, ABS(a_acc), ABS(a_acc));
77 }
78
79 /** remove event if any */
80 void delete_event(struct trajectory *traj)
81 {
82         set_quadramp_speed(traj, traj->d_speed, traj->a_speed);
83         set_quadramp_acc(traj, traj->d_acc, traj->a_acc);
84         if ( traj->scheduler_task != -1) {
85                 DEBUG(E_TRAJECTORY, "Delete event");
86                 scheduler_del_event(traj->scheduler_task);
87                 traj->scheduler_task = -1;
88         }
89 }
90
91 /** schedule the trajectory event */
92 void schedule_event(struct trajectory *traj)
93 {
94         if ( traj->scheduler_task != -1) {
95                 DEBUG(E_TRAJECTORY, "Schedule event, already scheduled");
96         }
97         else {
98                 traj->scheduler_task =
99                         scheduler_add_periodical_event_priority(&trajectory_manager_event,
100                                                                 (void*)traj,
101                                                                 TRAJ_EVT_PERIOD, 30);
102         }
103 }
104
105 /** do a modulo 2.pi -> [-Pi,+Pi], knowing that 'a' is in [-3Pi,+3Pi] */
106 double simple_modulo_2pi(double a)
107 {
108         if (a < -M_PI) {
109                 a += M_2PI;
110         }
111         else if (a > M_PI) {
112                 a -= M_2PI;
113         }
114         return a;
115 }
116
117 /** do a modulo 2.pi -> [-Pi,+Pi] */
118 double modulo_2pi(double a)
119 {
120         double res = a - (((int32_t) (a/M_2PI)) * M_2PI);
121         return simple_modulo_2pi(res);
122 }
123
124 /** near the target (dist) ? */
125 uint8_t is_robot_in_dist_window(struct trajectory *traj, double d_win)
126 {
127         double d = traj->target.pol.distance - rs_get_distance(traj->robot);
128         d = ABS(d);
129         d = d / traj->position->phys.distance_imp_per_mm;
130         return (d < d_win);
131 }
132
133 /** near the target (dist in x,y) ? */
134 uint8_t is_robot_in_xy_window(struct trajectory *traj, double d_win)
135 {
136         double x1 = traj->target.cart.x;
137         double y1 = traj->target.cart.y;
138         double x2 = position_get_x_double(traj->position);
139         double y2 = position_get_y_double(traj->position);
140         return ( sqrt ((x2-x1) * (x2-x1) + (y2-y1) * (y2-y1)) < d_win );
141 }
142
143 /** near the angle target in radian ? Only valid if
144  *  traj->target.pol.angle is set (i.e. an angle command, not an xy
145  *  command) */
146 uint8_t is_robot_in_angle_window(struct trajectory *traj, double a_win_rad)
147 {
148         double a;
149
150         /* convert relative angle from imp to rad */
151         a = traj->target.pol.angle - rs_get_angle(traj->robot);
152         a /= traj->position->phys.distance_imp_per_mm;
153         a /= traj->position->phys.track_mm;
154         a *= 2.;
155         return ABS(a) < a_win_rad;
156 }
157
158 /* distance unit conversions */
159
160 double pos_mm2imp(struct trajectory *traj, double pos)
161 {
162         return pos * traj->position->phys.distance_imp_per_mm;
163 }
164
165 double pos_imp2mm(struct trajectory *traj, double pos)
166 {
167         return pos / traj->position->phys.distance_imp_per_mm;
168 }
169
170 double speed_mm2imp(struct trajectory *traj, double speed)
171 {
172         return speed *
173                 traj->position->phys.distance_imp_per_mm /
174                 traj->cs_hz;
175 }
176
177 double speed_imp2mm(struct trajectory *traj, double speed)
178 {
179         return speed * traj->cs_hz /
180                 traj->position->phys.distance_imp_per_mm;
181 }
182
183 double acc_mm2imp(struct trajectory *traj, double acc)
184 {
185         return acc * traj->position->phys.distance_imp_per_mm /
186                 (traj->cs_hz * traj->cs_hz);
187 }
188
189 double acc_imp2mm(struct trajectory *traj, double acc)
190 {
191         return acc * traj->cs_hz * traj->cs_hz /
192                 traj->position->phys.distance_imp_per_mm;
193 }
194
195 /* angle unit conversions */
196
197 double pos_rd2imp(struct trajectory *traj, double pos)
198 {
199         return pos *
200                 (traj->position->phys.distance_imp_per_mm *
201                  traj->position->phys.track_mm / 2.);
202 }
203
204 double pos_imp2rd(struct trajectory *traj, double pos)
205 {
206         return pos /
207                 (traj->position->phys.distance_imp_per_mm *
208                  traj->position->phys.track_mm / 2.);
209 }
210
211 double speed_rd2imp(struct trajectory *traj, double speed)
212 {
213         return speed *
214                 (traj->position->phys.distance_imp_per_mm *
215                  traj->position->phys.track_mm /
216                  (2. * traj->cs_hz));
217 }
218
219 double speed_imp2rd(struct trajectory *traj, double speed)
220 {
221         return speed /
222                 (traj->position->phys.distance_imp_per_mm *
223                  traj->position->phys.track_mm /
224                  (2. * traj->cs_hz));
225 }
226
227 double acc_rd2imp(struct trajectory *traj, double acc)
228 {
229         return acc *
230                 (traj->position->phys.distance_imp_per_mm *
231                  traj->position->phys.track_mm /
232                  (2. * traj->cs_hz * traj->cs_hz));
233 }
234
235 double acc_imp2rd(struct trajectory *traj, double acc)
236 {
237         return acc /
238                 (traj->position->phys.distance_imp_per_mm *
239                  traj->position->phys.track_mm /
240                  (2. * traj->cs_hz * traj->cs_hz));
241 }
242