AVR: aversive_10-03-12/modules/devices/robot/robot_system/robot

00001 /*  
00002  *  Copyright Droids Corporation, Microb Technology, Eirbot (2005)
00003  * 
00004  *  This program is free software; you can redistribute it and/or modify
00005  *  it under the terms of the GNU General Public License as published by
00006  *  the Free Software Foundation; either version 2 of the License, or
00007  *  (at your option) any later version.
00008  *
00009  *  This program is distributed in the hope that it will be useful,
00010  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00011  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00012  *  GNU General Public License for more details.
00013  *
00014  *  You should have received a copy of the GNU General Public License
00015  *  along with this program; if not, write to the Free Software
00016  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00017  *
00018  *  Revision : $Id: robot_system.c,v 1.9 2008-04-13 16:55:31 zer0 Exp $
00019  *
00020  */
00021 
00029 #include <string.h>
00030 #include <stdio.h>
00031 
00032 #include <aversive/error.h>
00033 
00034 #include <aversive.h>
00035 #include <f64.h>
00036 
00037 #include "angle_distance.h"
00038 #include "robot_system.h"
00039 
00040 
00048 static inline void
00049 safe_setpwm(void (*f)(void *, int32_t), void * param, int32_t value)
00050 {
00051         void (*f_tmp)(void *, int32_t);
00052         void * param_tmp;
00053         uint8_t flags;
00054         IRQ_LOCK(flags);
00055         f_tmp = f;
00056         param_tmp = param;
00057         IRQ_UNLOCK(flags);
00058         if (f_tmp) {
00059                 f_tmp(param_tmp, value);
00060         }
00061 }
00062 
00070 static inline uint32_t
00071 safe_getencoder(int32_t (*f)(void *), void * param)
00072 {
00073         int32_t (*f_tmp)(void *);
00074         void * param_tmp;
00075         uint8_t flags;
00076         IRQ_LOCK(flags);
00077         f_tmp = f;
00078         param_tmp = param;
00079         IRQ_UNLOCK(flags);
00080         if (f_tmp) {
00081                 return f_tmp(param_tmp);
00082         }
00083         return 0;
00084 }
00085 
00087 void rs_init( struct robot_system * rs)
00088 {
00089         uint8_t flags;
00090 
00091         IRQ_LOCK(flags);
00092         memset(rs, 0, sizeof(struct robot_system));
00093 #ifdef CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT
00094         rs_set_ratio(rs, 1.0);
00095 #endif
00096         IRQ_UNLOCK(flags);
00097 }
00098 
00099 #ifdef CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT
00100 
00101 void rs_set_ratio(struct robot_system * rs, double ratio)
00102 {
00103         uint8_t flags;
00104 
00105         IRQ_LOCK(flags);
00106         rs->ratio_mot_ext = f64_from_double(ratio);
00107         IRQ_UNLOCK(flags);
00108 }
00109 #endif
00110 
00112 void rs_set_left_pwm(struct robot_system * rs, void (*left_pwm)(void *, int32_t), void *left_pwm_param)
00113 {
00114         uint8_t flags;
00115 
00116         IRQ_LOCK(flags);
00117         rs->left_pwm = left_pwm;
00118         rs->left_pwm_param = left_pwm_param;
00119         IRQ_UNLOCK(flags);
00120 }
00121 
00123 void rs_set_right_pwm(struct robot_system * rs, void (*right_pwm)(void *, int32_t), void *right_pwm_param)
00124 {
00125         uint8_t flags;
00126 
00127         IRQ_LOCK(flags);
00128         rs->right_pwm = right_pwm;
00129         rs->right_pwm_param = right_pwm_param;
00130         IRQ_UNLOCK(flags);
00131 }
00132 
00133 #ifdef CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT
00134 
00135 void rs_set_left_mot_encoder(struct robot_system * rs, int32_t (*left_mot_encoder)(void *), 
00136                              void *left_mot_encoder_param, double gain)
00137 {
00138         uint8_t flags;
00139 
00140         IRQ_LOCK(flags);
00141         rs->left_mot_encoder = left_mot_encoder;
00142         rs->left_mot_encoder_param = left_mot_encoder_param;
00143         rs->left_mot_gain = f64_from_double(gain);
00144         IRQ_UNLOCK(flags);
00145 }
00146 
00148 void rs_set_right_mot_encoder(struct robot_system * rs, int32_t (*right_mot_encoder)(void *), 
00149                               void *right_mot_encoder_param, double gain)
00150 {
00151         uint8_t flags;
00152 
00153         IRQ_LOCK(flags);
00154         rs->right_mot_encoder = right_mot_encoder;
00155         rs->right_mot_encoder_param = right_mot_encoder_param;
00156         rs->right_mot_gain = f64_from_double(gain);
00157         IRQ_UNLOCK(flags);
00158 }
00159 #endif
00160 
00162 void rs_set_left_ext_encoder(struct robot_system * rs, int32_t (*left_ext_encoder)(void *), 
00163                              void *left_ext_encoder_param, double gain)
00164 {
00165         uint8_t flags;
00166 
00167         IRQ_LOCK(flags);
00168         rs->left_ext_encoder = left_ext_encoder;
00169         rs->left_ext_encoder_param = left_ext_encoder_param;
00170         rs->left_ext_gain = f64_from_double(gain);
00171         IRQ_UNLOCK(flags);
00172 }
00173 
00175 void rs_set_right_ext_encoder(struct robot_system * rs, int32_t (*right_ext_encoder)(void *), 
00176                               void *right_ext_encoder_param, double gain)
00177 {
00178         uint8_t flags;
00179 
00180         IRQ_LOCK(flags);
00181         rs->right_ext_encoder = right_ext_encoder;
00182         rs->right_ext_encoder_param = right_ext_encoder_param;
00183         rs->right_ext_gain = f64_from_double(gain);
00184         IRQ_UNLOCK(flags);
00185 }
00186 
00187 /**** Virtual encoders and PWM */
00188 
00193 void rs_set_angle(void * data, int32_t angle)
00194 {
00195         struct rs_polar p;
00196         struct rs_wheels w;
00197         struct robot_system * rs = data;
00198         uint8_t flags;
00199 
00200         IRQ_LOCK(flags);
00201         p.distance = rs->virtual_pwm.distance ;
00202         rs->virtual_pwm.angle = angle;
00203         IRQ_UNLOCK(flags);
00204 
00205         p.angle = angle;
00206         rs_get_wheels_from_polar(&w, &p);
00207         
00208         safe_setpwm(rs->left_pwm, rs->left_pwm_param, w.left);
00209         safe_setpwm(rs->right_pwm, rs->right_pwm_param, w.right);
00210 }
00211 
00216 void rs_set_distance(void * data, int32_t distance)
00217 {
00218         struct robot_system * rs = data;
00219         struct rs_polar p;
00220         struct rs_wheels w;
00221         uint8_t flags;
00222 
00223         IRQ_LOCK(flags);
00224         p.angle = rs->virtual_pwm.angle ;
00225         rs->virtual_pwm.distance = distance;
00226         IRQ_UNLOCK(flags);
00227 
00228         p.distance = distance;
00229         rs_get_wheels_from_polar(&w, &p);
00230         
00231         safe_setpwm(rs->left_pwm, rs->left_pwm_param, w.left);
00232         safe_setpwm(rs->right_pwm, rs->right_pwm_param, w.right);
00233 }
00234 
00238 int32_t rs_get_angle(void * data)
00239 {
00240         struct robot_system * rs = data;
00241         int32_t angle;
00242         uint8_t flags;
00243         
00244         IRQ_LOCK(flags);
00245         angle = rs->virtual_encoders.angle ;    
00246         IRQ_UNLOCK(flags);
00247         return angle;
00248 }
00249 
00253 int32_t rs_get_distance(void * data)
00254 {
00255         struct robot_system * rs = data;
00256         int32_t distance;
00257         uint8_t flags;
00258         
00259         IRQ_LOCK(flags);
00260         distance = rs->virtual_encoders.distance ;      
00261         IRQ_UNLOCK(flags);
00262         return distance;
00263 }
00264 
00265 int32_t rs_get_ext_angle(void * data)
00266 {
00267         struct robot_system * rs = data;
00268         int32_t angle;
00269         uint8_t flags;
00270         
00271         IRQ_LOCK(flags);
00272         angle = rs->pext_prev.angle ;   
00273         IRQ_UNLOCK(flags);
00274         return angle;
00275 }
00276 
00277 int32_t rs_get_ext_distance(void * data)
00278 {
00279         struct robot_system * rs = data;
00280         int32_t distance;
00281         uint8_t flags;
00282         
00283         IRQ_LOCK(flags);
00284         distance = rs->pext_prev.distance ;     
00285         IRQ_UNLOCK(flags);
00286         return distance;
00287 }
00288 
00289 #ifdef CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT
00290 int32_t rs_get_mot_angle(void * data)
00291 {
00292         struct robot_system * rs = data;
00293         int32_t angle;
00294         uint8_t flags;  
00295 
00296         IRQ_LOCK(flags);
00297         angle = rs->pmot_prev.angle ;   
00298         IRQ_UNLOCK(flags);
00299         return angle;
00300 }
00301 
00302 int32_t rs_get_mot_distance(void * data)
00303 {
00304         struct robot_system * rs = data;
00305         int32_t distance;
00306         uint8_t flags;
00307         
00308         IRQ_LOCK(flags);
00309         distance = rs->pmot_prev.distance ;     
00310         IRQ_UNLOCK(flags);
00311         return distance;
00312 }
00313 #endif
00314 
00315 int32_t rs_get_ext_left(void * data)
00316 {
00317         struct robot_system * rs = data;
00318         int32_t left;
00319         uint8_t flags;
00320         
00321         IRQ_LOCK(flags);
00322         left = rs->wext_prev.left ;     
00323         IRQ_UNLOCK(flags);
00324         return left;
00325 }
00326 
00327 int32_t rs_get_ext_right(void * data)
00328 {
00329         struct robot_system * rs = data;
00330         int32_t right;
00331         uint8_t flags;
00332         
00333         IRQ_LOCK(flags);
00334         right = rs->wext_prev.right ;   
00335         IRQ_UNLOCK(flags);
00336         return right;
00337 }
00338 
00339 #ifdef CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT
00340 int32_t rs_get_mot_left(void * data)
00341 {
00342         struct robot_system * rs = data;
00343         int32_t left;
00344         uint8_t flags;  
00345 
00346         IRQ_LOCK(flags);
00347         left = rs->wmot_prev.left ;     
00348         IRQ_UNLOCK(flags);
00349         return left;
00350 }
00351 
00352 int32_t rs_get_mot_right(void * data)
00353 {
00354         struct robot_system * rs = data;
00355         int32_t right;
00356         uint8_t flags;
00357         
00358         IRQ_LOCK(flags);
00359         right = rs->wmot_prev.right ;   
00360         IRQ_UNLOCK(flags);
00361         return right;
00362 }
00363 #endif
00364 
00365 void rs_set_flags(struct robot_system * rs, uint8_t flags)
00366 {
00367         uint8_t i_flags;
00368         
00369         IRQ_LOCK(i_flags);
00370         rs->flags = flags;
00371         IRQ_UNLOCK(i_flags);
00372 }
00373 
00380 void rs_update(void * data)
00381 {
00382         struct robot_system * rs = data;
00383         struct rs_wheels wext;
00384         struct rs_polar pext;
00385 #ifdef CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT
00386         struct rs_wheels wmot;
00387         struct rs_polar pmot;
00388 #endif
00389         int32_t delta_angle, delta_distance;
00390         uint8_t flags;
00391         
00392         /* read encoders */
00393         wext.left = safe_getencoder(rs->left_ext_encoder, rs->left_ext_encoder_param);
00394         wext.right = safe_getencoder(rs->right_ext_encoder, rs->right_ext_encoder_param);
00395 
00396 #ifdef CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT
00397         wmot.left = safe_getencoder(rs->left_mot_encoder, rs->left_mot_encoder_param);
00398         wmot.right = safe_getencoder(rs->right_mot_encoder, rs->right_mot_encoder_param);
00399 #endif
00400         
00401         /* apply gains to each wheel */
00402         if( ! (rs->flags & RS_IGNORE_EXT_GAIN ) ) {
00403                 wext.left = f64_msb_mul(f64_from_lsb(wext.left), rs->left_ext_gain);
00404                 wext.right = f64_msb_mul(f64_from_lsb(wext.right), rs->right_ext_gain);
00405         }
00406         
00407 #ifdef CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT
00408         if( ! (rs->flags & RS_IGNORE_MOT_GAIN ) ) {
00409                 wmot.left = f64_msb_mul(f64_from_lsb(wmot.left), rs->left_mot_gain);
00410                 wmot.right = f64_msb_mul(f64_from_lsb(wmot.right), rs->right_mot_gain);
00411         }
00412 #endif
00413 
00414         rs_get_polar_from_wheels(&pext, &wext);
00415 #ifdef CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT
00416         rs_get_polar_from_wheels(&pmot, &wmot);
00417 
00418         /* apply ratio to polar and reupdate wheels for ext coders */
00419         pext.angle = f64_msb_mul(f64_from_lsb(pext.angle), rs->ratio_mot_ext);
00420 #endif
00421         rs_get_wheels_from_polar(&wext, &pext);
00422 
00423 #ifdef CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT
00424         /* update from external encoders */
00425         if( rs->flags & RS_USE_EXT ) {
00426                 delta_angle = pext.angle - rs->pext_prev.angle;
00427                 delta_distance = pext.distance - rs->pext_prev.distance;
00428         }
00429 
00430         /* update from motor encoders */
00431         else {
00432                 delta_angle = pmot.angle - rs->pmot_prev.angle;
00433                 delta_distance = pmot.distance - rs->pmot_prev.distance;
00434         }
00435 #else
00436         delta_angle = pext.angle - rs->pext_prev.angle;
00437         delta_distance = pext.distance - rs->pext_prev.distance;
00438 #endif
00439 
00440         IRQ_LOCK(flags);
00441         rs->virtual_encoders.angle += delta_angle;
00442         rs->virtual_encoders.distance += delta_distance;
00443         IRQ_UNLOCK(flags);
00444 
00445         /* don't lock too much time */
00446 
00447         IRQ_LOCK(flags);
00448         rs->pext_prev = pext;
00449         rs->wext_prev = wext;
00450         IRQ_UNLOCK(flags);
00451 
00452 #ifdef CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT
00453         IRQ_LOCK(flags);
00454         rs->pmot_prev = pmot;
00455         rs->wmot_prev = wmot;
00456         IRQ_UNLOCK(flags);
00457 #endif
00458 }
aversive_10-03-12/modules/devices/robot/robot_system/robot_system.c
