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: robot_system.c,v 1.6.4.7 2009-03-29 18:48:23 zer0 Exp $
23 * The goal of this module is to provide an interface to motor and
24 * encoders of the robot. This module provide a function that returns
25 * the value of virtual encoders containing distance and angle. It
26 * also allow the user to command virtual angle and distance PWMs.
32 #include <aversive/error.h>
35 #ifdef CONFIG_MODULE_ROBOT_SYSTEM_USE_F64
39 #include "angle_distance.h"
40 #include "robot_system.h"
43 /** Call a pwm() pointer :
44 * - lock the interrupts
45 * - read the pointer to the pwm function
46 * - unlock the interrupts
47 * - if pointer is null, don't do anything
48 * - else call the pwm with the parameters
51 safe_setpwm(void (*f)(void *, int32_t), void * param, int32_t value)
53 void (*f_tmp)(void *, int32_t);
61 f_tmp(param_tmp, value);
65 /** Call a encoder() pointer :
66 * - lock the interrupts
67 * - read the pointer to the encoder function
68 * - unlock the interrupts
69 * - if pointer is null, return 0
70 * - else return the value processed by the function
72 static inline uint32_t
73 safe_getencoder(int32_t (*f)(void *), void * param)
75 int32_t (*f_tmp)(void *);
83 return f_tmp(param_tmp);
88 /** Set the structure to 0 */
89 void rs_init( struct robot_system * rs)
94 memset(rs, 0, sizeof(struct robot_system));
95 #ifdef CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT
96 rs_set_ratio(rs, 1.0);
101 #ifdef CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT
102 /** define ratio between mot and ext track. (track_mot / track_ext) */
103 void rs_set_ratio(struct robot_system * rs, double ratio)
108 rs->ratio_mot_ext = f64_from_double(ratio);
113 /** define left PWM function and param */
114 void rs_set_left_pwm(struct robot_system * rs, void (*left_pwm)(void *, int32_t), void *left_pwm_param)
119 rs->left_pwm = left_pwm;
120 rs->left_pwm_param = left_pwm_param;
124 /** define right PWM function and param */
125 void rs_set_right_pwm(struct robot_system * rs, void (*right_pwm)(void *, int32_t), void *right_pwm_param)
130 rs->right_pwm = right_pwm;
131 rs->right_pwm_param = right_pwm_param;
135 #ifdef CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT
136 /** define left motor encoder function and param */
137 void rs_set_left_mot_encoder(struct robot_system * rs, int32_t (*left_mot_encoder)(void *),
138 void *left_mot_encoder_param, double gain)
143 rs->left_mot_encoder = left_mot_encoder;
144 rs->left_mot_encoder_param = left_mot_encoder_param;
145 rs->left_mot_gain = f64_from_double(gain);
149 /** define right motor encoder function and param */
150 void rs_set_right_mot_encoder(struct robot_system * rs, int32_t (*right_mot_encoder)(void *),
151 void *right_mot_encoder_param, double gain)
156 rs->right_mot_encoder = right_mot_encoder;
157 rs->right_mot_encoder_param = right_mot_encoder_param;
158 rs->right_mot_gain = f64_from_double(gain);
163 /** define left external encoder function and param */
164 void rs_set_left_ext_encoder(struct robot_system * rs, int32_t (*left_ext_encoder)(void *),
165 void *left_ext_encoder_param, double gain)
170 rs->left_ext_encoder = left_ext_encoder;
171 rs->left_ext_encoder_param = left_ext_encoder_param;
172 #ifdef CONFIG_MODULE_ROBOT_SYSTEM_USE_F64
173 rs->left_ext_gain = f64_from_double(gain);
175 rs->left_ext_gain = gain;
180 /** define right external encoder function and param */
181 void rs_set_right_ext_encoder(struct robot_system * rs, int32_t (*right_ext_encoder)(void *),
182 void *right_ext_encoder_param, double gain)
187 rs->right_ext_encoder = right_ext_encoder;
188 rs->right_ext_encoder_param = right_ext_encoder_param;
189 #ifdef CONFIG_MODULE_ROBOT_SYSTEM_USE_F64
190 rs->right_ext_gain = f64_from_double(gain);
192 rs->right_ext_gain = gain;
197 /**** Virtual encoders and PWM */
200 * set the real pwms according to the specified angle (it also
201 * depends on the last distance command sent)
203 void rs_set_angle(void * data, int32_t angle)
207 struct robot_system * rs = data;
211 p.distance = rs->virtual_pwm.distance ;
212 rs->virtual_pwm.angle = angle;
216 rs_get_wheels_from_polar(&w, &p);
218 safe_setpwm(rs->left_pwm, rs->left_pwm_param, w.left);
219 safe_setpwm(rs->right_pwm, rs->right_pwm_param, w.right);
223 * set the real pwms according to the specified distance (it also
224 * depends on the last angle command sent)
226 void rs_set_distance(void * data, int32_t distance)
228 struct robot_system * rs = data;
234 p.angle = rs->virtual_pwm.angle ;
235 rs->virtual_pwm.distance = distance;
238 p.distance = distance;
239 rs_get_wheels_from_polar(&w, &p);
241 safe_setpwm(rs->left_pwm, rs->left_pwm_param, w.left);
242 safe_setpwm(rs->right_pwm, rs->right_pwm_param, w.right);
246 * get the virtual angle according to real encoders value.
248 int32_t rs_get_angle(void * data)
250 struct robot_system * rs = data;
255 angle = rs->virtual_encoders.angle ;
261 * get the virtual distance according to real encoders value.
263 int32_t rs_get_distance(void * data)
265 struct robot_system * rs = data;
270 distance = rs->virtual_encoders.distance ;
275 int32_t rs_get_ext_angle(void * data)
277 struct robot_system * rs = data;
282 angle = rs->pext_prev.angle ;
287 int32_t rs_get_ext_distance(void * data)
289 struct robot_system * rs = data;
294 distance = rs->pext_prev.distance ;
299 #ifdef CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT
300 int32_t rs_get_mot_angle(void * data)
302 struct robot_system * rs = data;
307 angle = rs->pmot_prev.angle ;
312 int32_t rs_get_mot_distance(void * data)
314 struct robot_system * rs = data;
319 distance = rs->pmot_prev.distance ;
325 int32_t rs_get_ext_left(void * data)
327 struct robot_system * rs = data;
332 left = rs->wext_prev.left ;
337 int32_t rs_get_ext_right(void * data)
339 struct robot_system * rs = data;
344 right = rs->wext_prev.right ;
349 #ifdef CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT
350 int32_t rs_get_mot_left(void * data)
352 struct robot_system * rs = data;
357 left = rs->wmot_prev.left ;
362 int32_t rs_get_mot_right(void * data)
364 struct robot_system * rs = data;
369 right = rs->wmot_prev.right ;
375 void rs_set_flags(struct robot_system * rs, uint8_t flags)
385 * Read the encoders, and update internal virtual counters. Call this
386 * function is needed before reading the virtual encoders.The program
387 * will decide if it the external encoders or the motor encoders are
388 * taken in account (depending on flags, but not yet)
390 void rs_update(void * data)
392 struct robot_system * rs = data;
393 struct rs_wheels wext;
394 struct rs_polar pext;
395 #ifdef CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT
396 struct rs_wheels wmot;
397 struct rs_polar pmot;
399 int32_t delta_angle, delta_distance;
403 wext.left = safe_getencoder(rs->left_ext_encoder, rs->left_ext_encoder_param);
404 wext.right = safe_getencoder(rs->right_ext_encoder, rs->right_ext_encoder_param);
406 #ifdef CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT
407 wmot.left = safe_getencoder(rs->left_mot_encoder, rs->left_mot_encoder_param);
408 wmot.right = safe_getencoder(rs->right_mot_encoder, rs->right_mot_encoder_param);
411 /* apply gains to each wheel */
412 if (! (rs->flags & RS_IGNORE_EXT_GAIN )) {
413 #ifdef CONFIG_MODULE_ROBOT_SYSTEM_USE_F64
414 wext.left = f64_msb_mul(f64_from_lsb(wext.left), rs->left_ext_gain);
415 wext.right = f64_msb_mul(f64_from_lsb(wext.right), rs->right_ext_gain);
419 tmp *= rs->left_ext_gain;
422 tmp *= rs->right_ext_gain;
427 #ifdef CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT
428 if (! (rs->flags & RS_IGNORE_MOT_GAIN )) {
429 wmot.left = f64_msb_mul(f64_from_lsb(wmot.left), rs->left_mot_gain);
430 wmot.right = f64_msb_mul(f64_from_lsb(wmot.right), rs->right_mot_gain);
434 rs_get_polar_from_wheels(&pext, &wext);
435 #ifdef CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT
436 rs_get_polar_from_wheels(&pmot, &wmot);
438 /* apply ratio to polar and reupdate wheels for ext coders */
439 pext.angle = f64_msb_mul(f64_from_lsb(pext.angle), rs->ratio_mot_ext);
440 rs_get_wheels_from_polar(&wext, &pext);
443 #ifdef CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT
444 /* update from external encoders */
445 if (rs->flags & RS_USE_EXT) {
446 delta_angle = pext.angle - rs->pext_prev.angle;
447 delta_distance = pext.distance - rs->pext_prev.distance;
450 /* update from motor encoders */
452 delta_angle = pmot.angle - rs->pmot_prev.angle;
453 delta_distance = pmot.distance - rs->pmot_prev.distance;
456 delta_angle = pext.angle - rs->pext_prev.angle;
457 delta_distance = pext.distance - rs->pext_prev.distance;
461 rs->virtual_encoders.angle += delta_angle;
462 rs->virtual_encoders.distance += delta_distance;
465 /* don't lock too much time */
468 rs->pext_prev = pext;
469 rs->wext_prev = wext;
472 #ifdef CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT
474 rs->pmot_prev = pmot;
475 rs->wmot_prev = wmot;