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: position_manager.c,v 1.6.4.7 2009-05-18 12:27:26 zer0 Exp $
25 #include <robot_system.h>
26 #include <position_manager.h>
28 /** initialization of the robot_position pos, everthing is set to 0 */
29 void position_init(struct robot_position *pos)
33 memset(pos, 0, sizeof(struct robot_position));
37 /** Set a new robot position */
38 void position_set(struct robot_position *pos, int16_t x, int16_t y, double a_deg)
42 pos->pos_d.a = (a_deg * M_PI)/ 180.0;
47 pos->pos_s16.a = a_deg;
51 #ifdef CONFIG_MODULE_COMPENSATE_CENTRIFUGAL_FORCE
52 void position_set_centrifugal_coef(struct robot_position *pos, double coef)
54 pos->centrifugal_coef = coef;
59 * Save in pos structure the pointer to the associated robot_system.
60 * The robot_system structure is used to get values from virtual encoders
61 * that return angle and distance.
63 void position_set_related_robot_system(struct robot_position *pos, struct robot_system *rs)
72 * Set the physical parameters of the robot :
73 * - number of impulsions for 1 mm (distance)
74 * - number of impulsions for 1 degree (angle)
76 void position_set_physical_params(struct robot_position *pos, double track_mm,
77 double distance_imp_per_mm)
81 pos->phys.track_mm = track_mm;
82 pos->phys.distance_imp_per_mm = distance_imp_per_mm;
86 void position_use_ext(struct robot_position *pos)
88 struct rs_polar encoders;
92 encoders.distance = rs_get_ext_distance(pos->rs);
93 encoders.angle = rs_get_ext_angle(pos->rs);
94 pos->prev_encoders = encoders;
99 #ifdef CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT
100 void position_use_mot(struct robot_position *pos)
102 struct rs_polar encoders;
106 encoders.distance = rs_get_mot_distance(pos->rs);
107 encoders.angle = rs_get_mot_angle(pos->rs);
108 pos->prev_encoders = encoders;
115 * Process the absolute position (x,y,a) depending on the delta on
116 * virtual encoders since last read, and depending on physical
117 * parameters. The processed position is in mm.
119 void position_manage(struct robot_position *pos)
121 double x, y, a, r, arc_angle;
123 s16 x_s16, y_s16, a_s16;
124 struct rs_polar encoders;
125 struct rs_polar delta;
126 struct robot_system * rs;
132 /* here we could raise an error */
136 #ifdef CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT
138 encoders.distance = rs_get_ext_distance(rs);
139 encoders.angle = rs_get_ext_angle(rs);
142 encoders.distance = rs_get_mot_distance(rs);
143 encoders.angle = rs_get_mot_angle(rs);
146 encoders.distance = rs_get_ext_distance(rs);
147 encoders.angle = rs_get_ext_angle(rs);
150 /* process difference between 2 measures.
151 * No lock for prev_encoders since we are the only one to use
152 * this var XXX that's wrong now, perhaps we should lock */
153 delta.distance = encoders.distance - pos->prev_encoders.distance;
154 delta.angle = encoders.angle - pos->prev_encoders.angle;
156 pos->prev_encoders = encoders;
158 /* update double position */
165 if (delta.angle == 0) {
167 dx = cos(a) * ((double) delta.distance / (pos->phys.distance_imp_per_mm)) ;
168 dy = sin(a) * ((double) delta.distance / (pos->phys.distance_imp_per_mm)) ;
173 /* r the radius of the circle arc */
174 r = (double)delta.distance * pos->phys.track_mm / ((double) delta.angle * 2);
175 arc_angle = 2 * (double) delta.angle / (pos->phys.track_mm * pos->phys.distance_imp_per_mm);
177 dx = r * (-sin(a) + sin(a+arc_angle));
178 dy = r * (cos(a) - cos(a+arc_angle));
189 #ifdef CONFIG_MODULE_COMPENSATE_CENTRIFUGAL_FORCE
190 /* This part compensate the centrifugal force when we
191 * turn very quickly. Idea is from Gargamel (RCVA). */
192 if (pos->centrifugal_coef && r != 0) {
196 * centrifugal force is F = (m.v^2 / R)
197 * with v: angular speed
198 * R: radius of the circle
201 k = ((double) delta.distance);
204 k *= pos->centrifugal_coef;
207 * F acts perpendicularly to the vector
215 /* update int position */
218 a_s16 = (int16_t)(a * (360.0/(M_PI*2)));
224 pos->pos_s16.x = x_s16;
225 pos->pos_s16.y = y_s16;
226 pos->pos_s16.a = a_s16;
234 int16_t position_get_x_s16(struct robot_position *pos)
236 return pos->pos_s16.x;
242 int16_t position_get_y_s16(struct robot_position *pos)
244 return pos->pos_s16.y;
248 * returns current alpha
250 int16_t position_get_a_deg_s16(struct robot_position *pos)
252 return pos->pos_s16.a;
260 double position_get_x_double(struct robot_position *pos)
268 double position_get_y_double(struct robot_position *pos)
274 * returns current alpha
276 double position_get_a_rad_double(struct robot_position *pos)