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>
37 #include "angle_distance.h"
38 #include "robot_system.h"
41 /** Call a pwm() pointer :
42 * - lock the interrupts
43 * - read the pointer to the pwm function
44 * - unlock the interrupts
45 * - if pointer is null, don't do anything
46 * - else call the pwm with the parameters
49 safe_setpwm(void (*f)(void *, int32_t), void * param, int32_t value)
51 void (*f_tmp)(void *, int32_t);
59 f_tmp(param_tmp, value);
63 /** Call a encoder() pointer :
64 * - lock the interrupts
65 * - read the pointer to the encoder function
66 * - unlock the interrupts
67 * - if pointer is null, return 0
68 * - else return the value processed by the function
70 static inline uint32_t
71 safe_getencoder(int32_t (*f)(void *), void * param)
73 int32_t (*f_tmp)(void *);
81 return f_tmp(param_tmp);
86 /** Set the structure to 0 */
87 void rs_init( struct robot_system * rs)
92 memset(rs, 0, sizeof(struct robot_system));
93 #ifdef CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT
94 rs_set_ratio(rs, 1.0);
99 #ifdef CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT
100 /** define ratio between mot and ext track. (track_mot / track_ext) */
101 void rs_set_ratio(struct robot_system * rs, double ratio)
106 rs->ratio_mot_ext = f64_from_double(ratio);
111 /** define left PWM function and param */
112 void rs_set_left_pwm(struct robot_system * rs, void (*left_pwm)(void *, int32_t), void *left_pwm_param)
117 rs->left_pwm = left_pwm;
118 rs->left_pwm_param = left_pwm_param;
122 /** define right PWM function and param */
123 void rs_set_right_pwm(struct robot_system * rs, void (*right_pwm)(void *, int32_t), void *right_pwm_param)
128 rs->right_pwm = right_pwm;
129 rs->right_pwm_param = right_pwm_param;
133 #ifdef CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT
134 /** define left motor encoder function and param */
135 void rs_set_left_mot_encoder(struct robot_system * rs, int32_t (*left_mot_encoder)(void *),
136 void *left_mot_encoder_param, double gain)
141 rs->left_mot_encoder = left_mot_encoder;
142 rs->left_mot_encoder_param = left_mot_encoder_param;
143 rs->left_mot_gain = f64_from_double(gain);
147 /** define right motor encoder function and param */
148 void rs_set_right_mot_encoder(struct robot_system * rs, int32_t (*right_mot_encoder)(void *),
149 void *right_mot_encoder_param, double gain)
154 rs->right_mot_encoder = right_mot_encoder;
155 rs->right_mot_encoder_param = right_mot_encoder_param;
156 rs->right_mot_gain = f64_from_double(gain);
161 /** define left external encoder function and param */
162 void rs_set_left_ext_encoder(struct robot_system * rs, int32_t (*left_ext_encoder)(void *),
163 void *left_ext_encoder_param, double gain)
168 rs->left_ext_encoder = left_ext_encoder;
169 rs->left_ext_encoder_param = left_ext_encoder_param;
170 rs->left_ext_gain = f64_from_double(gain);
174 /** define right external encoder function and param */
175 void rs_set_right_ext_encoder(struct robot_system * rs, int32_t (*right_ext_encoder)(void *),
176 void *right_ext_encoder_param, double gain)
181 rs->right_ext_encoder = right_ext_encoder;
182 rs->right_ext_encoder_param = right_ext_encoder_param;
183 rs->right_ext_gain = f64_from_double(gain);
187 /**** Virtual encoders and PWM */
190 * set the real pwms according to the specified angle (it also
191 * depends on the last distance command sent)
193 void rs_set_angle(void * data, int32_t angle)
197 struct robot_system * rs = data;
201 p.distance = rs->virtual_pwm.distance ;
202 rs->virtual_pwm.angle = angle;
206 rs_get_wheels_from_polar(&w, &p);
208 safe_setpwm(rs->left_pwm, rs->left_pwm_param, w.left);
209 safe_setpwm(rs->right_pwm, rs->right_pwm_param, w.right);
213 * set the real pwms according to the specified distance (it also
214 * depends on the last angle command sent)
216 void rs_set_distance(void * data, int32_t distance)
218 struct robot_system * rs = data;
224 p.angle = rs->virtual_pwm.angle ;
225 rs->virtual_pwm.distance = distance;
228 p.distance = distance;
229 rs_get_wheels_from_polar(&w, &p);
231 safe_setpwm(rs->left_pwm, rs->left_pwm_param, w.left);
232 safe_setpwm(rs->right_pwm, rs->right_pwm_param, w.right);
236 * get the virtual angle according to real encoders value.
238 int32_t rs_get_angle(void * data)
240 struct robot_system * rs = data;
245 angle = rs->virtual_encoders.angle ;
251 * get the virtual distance according to real encoders value.
253 int32_t rs_get_distance(void * data)
255 struct robot_system * rs = data;
260 distance = rs->virtual_encoders.distance ;
265 int32_t rs_get_ext_angle(void * data)
267 struct robot_system * rs = data;
272 angle = rs->pext_prev.angle ;
277 int32_t rs_get_ext_distance(void * data)
279 struct robot_system * rs = data;
284 distance = rs->pext_prev.distance ;
289 #ifdef CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT
290 int32_t rs_get_mot_angle(void * data)
292 struct robot_system * rs = data;
297 angle = rs->pmot_prev.angle ;
302 int32_t rs_get_mot_distance(void * data)
304 struct robot_system * rs = data;
309 distance = rs->pmot_prev.distance ;
315 int32_t rs_get_ext_left(void * data)
317 struct robot_system * rs = data;
322 left = rs->wext_prev.left ;
327 int32_t rs_get_ext_right(void * data)
329 struct robot_system * rs = data;
334 right = rs->wext_prev.right ;
339 #ifdef CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT
340 int32_t rs_get_mot_left(void * data)
342 struct robot_system * rs = data;
347 left = rs->wmot_prev.left ;
352 int32_t rs_get_mot_right(void * data)
354 struct robot_system * rs = data;
359 right = rs->wmot_prev.right ;
365 void rs_set_flags(struct robot_system * rs, uint8_t flags)
375 * Read the encoders, and update internal virtual counters. Call this
376 * function is needed before reading the virtual encoders.The program
377 * will decide if it the external encoders or the motor encoders are
378 * taken in account (depending on flags, but not yet)
380 void rs_update(void * data)
382 struct robot_system * rs = data;
383 struct rs_wheels wext;
384 struct rs_polar pext;
385 #ifdef CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT
386 struct rs_wheels wmot;
387 struct rs_polar pmot;
389 int32_t delta_angle, delta_distance;
393 wext.left = safe_getencoder(rs->left_ext_encoder, rs->left_ext_encoder_param);
394 wext.right = safe_getencoder(rs->right_ext_encoder, rs->right_ext_encoder_param);
396 #ifdef CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT
397 wmot.left = safe_getencoder(rs->left_mot_encoder, rs->left_mot_encoder_param);
398 wmot.right = safe_getencoder(rs->right_mot_encoder, rs->right_mot_encoder_param);
401 /* apply gains to each wheel */
402 if (! (rs->flags & RS_IGNORE_EXT_GAIN )) {
403 wext.left = f64_msb_mul(f64_from_lsb(wext.left), rs->left_ext_gain);
404 wext.right = f64_msb_mul(f64_from_lsb(wext.right), rs->right_ext_gain);
407 #ifdef CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT
408 if (! (rs->flags & RS_IGNORE_MOT_GAIN )) {
409 wmot.left = f64_msb_mul(f64_from_lsb(wmot.left), rs->left_mot_gain);
410 wmot.right = f64_msb_mul(f64_from_lsb(wmot.right), rs->right_mot_gain);
414 rs_get_polar_from_wheels(&pext, &wext);
415 #ifdef CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT
416 rs_get_polar_from_wheels(&pmot, &wmot);
418 /* apply ratio to polar and reupdate wheels for ext coders */
419 pext.angle = f64_msb_mul(f64_from_lsb(pext.angle), rs->ratio_mot_ext);
420 rs_get_wheels_from_polar(&wext, &pext);
423 #ifdef CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT
424 /* update from external encoders */
425 if (rs->flags & RS_USE_EXT) {
426 delta_angle = pext.angle - rs->pext_prev.angle;
427 delta_distance = pext.distance - rs->pext_prev.distance;
430 /* update from motor encoders */
432 delta_angle = pmot.angle - rs->pmot_prev.angle;
433 delta_distance = pmot.distance - rs->pmot_prev.distance;
436 delta_angle = pext.angle - rs->pext_prev.angle;
437 delta_distance = pext.distance - rs->pext_prev.distance;
441 rs->virtual_encoders.angle += delta_angle;
442 rs->virtual_encoders.distance += delta_distance;
445 /* don't lock too much time */
448 rs->pext_prev = pext;
449 rs->wext_prev = wext;
452 #ifdef CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT
454 rs->pmot_prev = pmot;
455 rs->wmot_prev = wmot;