X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=modules%2Fdevices%2Frobot%2Frobot_system%2Frobot_system.c.~1.6.4.7.~;fp=modules%2Fdevices%2Frobot%2Frobot_system%2Frobot_system.c.~1.6.4.7.~;h=0000000000000000000000000000000000000000;hp=be158e6bd1a47137c1230ee493c683f63c9b3fed;hb=bd8acd4f11e6720c96946a7537df5538c0a9a5d4;hpb=085db3d9933da5bfaa582472420063ab4a2c047d diff --git a/modules/devices/robot/robot_system/robot_system.c.~1.6.4.7.~ b/modules/devices/robot/robot_system/robot_system.c.~1.6.4.7.~ deleted file mode 100755 index be158e6..0000000 --- a/modules/devices/robot/robot_system/robot_system.c.~1.6.4.7.~ +++ /dev/null @@ -1,458 +0,0 @@ -/* - * Copyright Droids Corporation, Microb Technology, Eirbot (2005) - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program; if not, write to the Free Software - * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - * - * Revision : $Id: robot_system.c,v 1.6.4.7 2009-03-29 18:48:23 zer0 Exp $ - * - */ - -/** - * The goal of this module is to provide an interface to motor and - * encoders of the robot. This module provide a function that returns - * the value of virtual encoders containing distance and angle. It - * also allow the user to command virtual angle and distance PWMs. - */ - -#include -#include - -#include - -#include -#include - -#include "angle_distance.h" -#include "robot_system.h" - - -/** Call a pwm() pointer : - * - lock the interrupts - * - read the pointer to the pwm function - * - unlock the interrupts - * - if pointer is null, don't do anything - * - else call the pwm with the parameters - */ -static inline void -safe_setpwm(void (*f)(void *, int32_t), void * param, int32_t value) -{ - void (*f_tmp)(void *, int32_t); - void * param_tmp; - uint8_t flags; - IRQ_LOCK(flags); - f_tmp = f; - param_tmp = param; - IRQ_UNLOCK(flags); - if (f_tmp) { - f_tmp(param_tmp, value); - } -} - -/** Call a encoder() pointer : - * - lock the interrupts - * - read the pointer to the encoder function - * - unlock the interrupts - * - if pointer is null, return 0 - * - else return the value processed by the function - */ -static inline uint32_t -safe_getencoder(int32_t (*f)(void *), void * param) -{ - int32_t (*f_tmp)(void *); - void * param_tmp; - uint8_t flags; - IRQ_LOCK(flags); - f_tmp = f; - param_tmp = param; - IRQ_UNLOCK(flags); - if (f_tmp) { - return f_tmp(param_tmp); - } - return 0; -} - -/** Set the structure to 0 */ -void rs_init( struct robot_system * rs) -{ - uint8_t flags; - - IRQ_LOCK(flags); - memset(rs, 0, sizeof(struct robot_system)); -#ifdef CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT - rs_set_ratio(rs, 1.0); -#endif - IRQ_UNLOCK(flags); -} - -#ifdef CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT -/** define ratio between mot and ext track. (track_mot / track_ext) */ -void rs_set_ratio(struct robot_system * rs, double ratio) -{ - uint8_t flags; - - IRQ_LOCK(flags); - rs->ratio_mot_ext = f64_from_double(ratio); - IRQ_UNLOCK(flags); -} -#endif - -/** define left PWM function and param */ -void rs_set_left_pwm(struct robot_system * rs, void (*left_pwm)(void *, int32_t), void *left_pwm_param) -{ - uint8_t flags; - - IRQ_LOCK(flags); - rs->left_pwm = left_pwm; - rs->left_pwm_param = left_pwm_param; - IRQ_UNLOCK(flags); -} - -/** define right PWM function and param */ -void rs_set_right_pwm(struct robot_system * rs, void (*right_pwm)(void *, int32_t), void *right_pwm_param) -{ - uint8_t flags; - - IRQ_LOCK(flags); - rs->right_pwm = right_pwm; - rs->right_pwm_param = right_pwm_param; - IRQ_UNLOCK(flags); -} - -#ifdef CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT -/** define left motor encoder function and param */ -void rs_set_left_mot_encoder(struct robot_system * rs, int32_t (*left_mot_encoder)(void *), - void *left_mot_encoder_param, double gain) -{ - uint8_t flags; - - IRQ_LOCK(flags); - rs->left_mot_encoder = left_mot_encoder; - rs->left_mot_encoder_param = left_mot_encoder_param; - rs->left_mot_gain = f64_from_double(gain); - IRQ_UNLOCK(flags); -} - -/** define right motor encoder function and param */ -void rs_set_right_mot_encoder(struct robot_system * rs, int32_t (*right_mot_encoder)(void *), - void *right_mot_encoder_param, double gain) -{ - uint8_t flags; - - IRQ_LOCK(flags); - rs->right_mot_encoder = right_mot_encoder; - rs->right_mot_encoder_param = right_mot_encoder_param; - rs->right_mot_gain = f64_from_double(gain); - IRQ_UNLOCK(flags); -} -#endif - -/** define left external encoder function and param */ -void rs_set_left_ext_encoder(struct robot_system * rs, int32_t (*left_ext_encoder)(void *), - void *left_ext_encoder_param, double gain) -{ - uint8_t flags; - - IRQ_LOCK(flags); - rs->left_ext_encoder = left_ext_encoder; - rs->left_ext_encoder_param = left_ext_encoder_param; - rs->left_ext_gain = f64_from_double(gain); - IRQ_UNLOCK(flags); -} - -/** define right external encoder function and param */ -void rs_set_right_ext_encoder(struct robot_system * rs, int32_t (*right_ext_encoder)(void *), - void *right_ext_encoder_param, double gain) -{ - uint8_t flags; - - IRQ_LOCK(flags); - rs->right_ext_encoder = right_ext_encoder; - rs->right_ext_encoder_param = right_ext_encoder_param; - rs->right_ext_gain = f64_from_double(gain); - IRQ_UNLOCK(flags); -} - -/**** Virtual encoders and PWM */ - -/** - * set the real pwms according to the specified angle (it also - * depends on the last distance command sent) - */ -void rs_set_angle(void * data, int32_t angle) -{ - struct rs_polar p; - struct rs_wheels w; - struct robot_system * rs = data; - uint8_t flags; - - IRQ_LOCK(flags); - p.distance = rs->virtual_pwm.distance ; - rs->virtual_pwm.angle = angle; - IRQ_UNLOCK(flags); - - p.angle = angle; - rs_get_wheels_from_polar(&w, &p); - - safe_setpwm(rs->left_pwm, rs->left_pwm_param, w.left); - safe_setpwm(rs->right_pwm, rs->right_pwm_param, w.right); -} - -/** - * set the real pwms according to the specified distance (it also - * depends on the last angle command sent) - */ -void rs_set_distance(void * data, int32_t distance) -{ - struct robot_system * rs = data; - struct rs_polar p; - struct rs_wheels w; - uint8_t flags; - - IRQ_LOCK(flags); - p.angle = rs->virtual_pwm.angle ; - rs->virtual_pwm.distance = distance; - IRQ_UNLOCK(flags); - - p.distance = distance; - rs_get_wheels_from_polar(&w, &p); - - safe_setpwm(rs->left_pwm, rs->left_pwm_param, w.left); - safe_setpwm(rs->right_pwm, rs->right_pwm_param, w.right); -} - -/** - * get the virtual angle according to real encoders value. - */ -int32_t rs_get_angle(void * data) -{ - struct robot_system * rs = data; - int32_t angle; - uint8_t flags; - - IRQ_LOCK(flags); - angle = rs->virtual_encoders.angle ; - IRQ_UNLOCK(flags); - return angle; -} - -/** - * get the virtual distance according to real encoders value. - */ -int32_t rs_get_distance(void * data) -{ - struct robot_system * rs = data; - int32_t distance; - uint8_t flags; - - IRQ_LOCK(flags); - distance = rs->virtual_encoders.distance ; - IRQ_UNLOCK(flags); - return distance; -} - -int32_t rs_get_ext_angle(void * data) -{ - struct robot_system * rs = data; - int32_t angle; - uint8_t flags; - - IRQ_LOCK(flags); - angle = rs->pext_prev.angle ; - IRQ_UNLOCK(flags); - return angle; -} - -int32_t rs_get_ext_distance(void * data) -{ - struct robot_system * rs = data; - int32_t distance; - uint8_t flags; - - IRQ_LOCK(flags); - distance = rs->pext_prev.distance ; - IRQ_UNLOCK(flags); - return distance; -} - -#ifdef CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT -int32_t rs_get_mot_angle(void * data) -{ - struct robot_system * rs = data; - int32_t angle; - uint8_t flags; - - IRQ_LOCK(flags); - angle = rs->pmot_prev.angle ; - IRQ_UNLOCK(flags); - return angle; -} - -int32_t rs_get_mot_distance(void * data) -{ - struct robot_system * rs = data; - int32_t distance; - uint8_t flags; - - IRQ_LOCK(flags); - distance = rs->pmot_prev.distance ; - IRQ_UNLOCK(flags); - return distance; -} -#endif - -int32_t rs_get_ext_left(void * data) -{ - struct robot_system * rs = data; - int32_t left; - uint8_t flags; - - IRQ_LOCK(flags); - left = rs->wext_prev.left ; - IRQ_UNLOCK(flags); - return left; -} - -int32_t rs_get_ext_right(void * data) -{ - struct robot_system * rs = data; - int32_t right; - uint8_t flags; - - IRQ_LOCK(flags); - right = rs->wext_prev.right ; - IRQ_UNLOCK(flags); - return right; -} - -#ifdef CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT -int32_t rs_get_mot_left(void * data) -{ - struct robot_system * rs = data; - int32_t left; - uint8_t flags; - - IRQ_LOCK(flags); - left = rs->wmot_prev.left ; - IRQ_UNLOCK(flags); - return left; -} - -int32_t rs_get_mot_right(void * data) -{ - struct robot_system * rs = data; - int32_t right; - uint8_t flags; - - IRQ_LOCK(flags); - right = rs->wmot_prev.right ; - IRQ_UNLOCK(flags); - return right; -} -#endif - -void rs_set_flags(struct robot_system * rs, uint8_t flags) -{ - uint8_t i_flags; - - IRQ_LOCK(i_flags); - rs->flags = flags; - IRQ_UNLOCK(i_flags); -} - -/** - * Read the encoders, and update internal virtual counters. Call this - * function is needed before reading the virtual encoders.The program - * will decide if it the external encoders or the motor encoders are - * taken in account (depending on flags, but not yet) - */ -void rs_update(void * data) -{ - struct robot_system * rs = data; - struct rs_wheels wext; - struct rs_polar pext; -#ifdef CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT - struct rs_wheels wmot; - struct rs_polar pmot; -#endif - int32_t delta_angle, delta_distance; - uint8_t flags; - - /* read encoders */ - wext.left = safe_getencoder(rs->left_ext_encoder, rs->left_ext_encoder_param); - wext.right = safe_getencoder(rs->right_ext_encoder, rs->right_ext_encoder_param); - -#ifdef CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT - wmot.left = safe_getencoder(rs->left_mot_encoder, rs->left_mot_encoder_param); - wmot.right = safe_getencoder(rs->right_mot_encoder, rs->right_mot_encoder_param); -#endif - - /* apply gains to each wheel */ - if (! (rs->flags & RS_IGNORE_EXT_GAIN )) { - wext.left = f64_msb_mul(f64_from_lsb(wext.left), rs->left_ext_gain); - wext.right = f64_msb_mul(f64_from_lsb(wext.right), rs->right_ext_gain); - } - -#ifdef CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT - if (! (rs->flags & RS_IGNORE_MOT_GAIN )) { - wmot.left = f64_msb_mul(f64_from_lsb(wmot.left), rs->left_mot_gain); - wmot.right = f64_msb_mul(f64_from_lsb(wmot.right), rs->right_mot_gain); - } -#endif - - rs_get_polar_from_wheels(&pext, &wext); -#ifdef CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT - rs_get_polar_from_wheels(&pmot, &wmot); - - /* apply ratio to polar and reupdate wheels for ext coders */ - pext.angle = f64_msb_mul(f64_from_lsb(pext.angle), rs->ratio_mot_ext); - rs_get_wheels_from_polar(&wext, &pext); -#endif - -#ifdef CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT - /* update from external encoders */ - if (rs->flags & RS_USE_EXT) { - delta_angle = pext.angle - rs->pext_prev.angle; - delta_distance = pext.distance - rs->pext_prev.distance; - } - - /* update from motor encoders */ - else { - delta_angle = pmot.angle - rs->pmot_prev.angle; - delta_distance = pmot.distance - rs->pmot_prev.distance; - } -#else - delta_angle = pext.angle - rs->pext_prev.angle; - delta_distance = pext.distance - rs->pext_prev.distance; -#endif - - IRQ_LOCK(flags); - rs->virtual_encoders.angle += delta_angle; - rs->virtual_encoders.distance += delta_distance; - IRQ_UNLOCK(flags); - - /* don't lock too much time */ - - IRQ_LOCK(flags); - rs->pext_prev = pext; - rs->wext_prev = wext; - IRQ_UNLOCK(flags); - -#ifdef CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT - IRQ_LOCK(flags); - rs->pmot_prev = pmot; - rs->wmot_prev = wmot; - IRQ_UNLOCK(flags); -#endif -}