modules/devices/robot/robot_system/robot_system.c.~1.6.4.7.~

   1 /*
   2  *  Copyright Droids Corporation, Microb Technology, Eirbot (2005)
   3  *
   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.
   8  *
   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.
  13  *
  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
  17  *
  18  *  Revision : $Id: robot_system.c,v 1.6.4.7 2009-03-29 18:48:23 zer0 Exp $
  19  *
  20  */
  21
  22 /**
  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.
  27  */
  28
  29 #include <string.h>
  30 #include <stdio.h>
  31
  32 #include <aversive/error.h>
  33
  34 #include <aversive.h>
  35 #include <f64.h>
  36
  37 #include "angle_distance.h"
  38 #include "robot_system.h"
  39
  40
  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
  47  */
  48 static inline void
  49 safe_setpwm(void (*f)(void *, int32_t), void * param, int32_t value)
  50 {
  51         void (*f_tmp)(void *, int32_t);
  52         void * param_tmp;
  53         uint8_t flags;
  54         IRQ_LOCK(flags);
  55         f_tmp = f;
  56         param_tmp = param;
  57         IRQ_UNLOCK(flags);
  58         if (f_tmp) {
  59                 f_tmp(param_tmp, value);
  60         }
  61 }
  62
  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
  69  */
  70 static inline uint32_t
  71 safe_getencoder(int32_t (*f)(void *), void * param)
  72 {
  73         int32_t (*f_tmp)(void *);
  74         void * param_tmp;
  75         uint8_t flags;
  76         IRQ_LOCK(flags);
  77         f_tmp = f;
  78         param_tmp = param;
  79         IRQ_UNLOCK(flags);
  80         if (f_tmp) {
  81                 return f_tmp(param_tmp);
  82         }
  83         return 0;
  84 }
  85
  86 /** Set the structure to 0 */
  87 void rs_init( struct robot_system * rs)
  88 {
  89         uint8_t flags;
  90
  91         IRQ_LOCK(flags);
  92         memset(rs, 0, sizeof(struct robot_system));
  93 #ifdef CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT
  94         rs_set_ratio(rs, 1.0);
  95 #endif
  96         IRQ_UNLOCK(flags);
  97 }
  98
  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)
 102 {
 103         uint8_t flags;
 104
 105         IRQ_LOCK(flags);
 106         rs->ratio_mot_ext = f64_from_double(ratio);
 107         IRQ_UNLOCK(flags);
 108 }
 109 #endif
 110
 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)
 113 {
 114         uint8_t flags;
 115
 116         IRQ_LOCK(flags);
 117         rs->left_pwm = left_pwm;
 118         rs->left_pwm_param = left_pwm_param;
 119         IRQ_UNLOCK(flags);
 120 }
 121
 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)
 124 {
 125         uint8_t flags;
 126
 127         IRQ_LOCK(flags);
 128         rs->right_pwm = right_pwm;
 129         rs->right_pwm_param = right_pwm_param;
 130         IRQ_UNLOCK(flags);
 131 }
 132
 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)
 137 {
 138         uint8_t flags;
 139
 140         IRQ_LOCK(flags);
 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);
 144         IRQ_UNLOCK(flags);
 145 }
 146
 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)
 150 {
 151         uint8_t flags;
 152
 153         IRQ_LOCK(flags);
 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);
 157         IRQ_UNLOCK(flags);
 158 }
 159 #endif
 160
 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)
 164 {
 165         uint8_t flags;
 166
 167         IRQ_LOCK(flags);
 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);
 171         IRQ_UNLOCK(flags);
 172 }
 173
 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)
 177 {
 178         uint8_t flags;
 179
 180         IRQ_LOCK(flags);
 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);
 184         IRQ_UNLOCK(flags);
 185 }
 186
 187 /**** Virtual encoders and PWM */
 188
 189 /**
 190  * set the real pwms according to the specified angle (it also
 191  * depends on the last distance command sent)
 192  */
 193 void rs_set_angle(void * data, int32_t angle)
 194 {
 195         struct rs_polar p;
 196         struct rs_wheels w;
 197         struct robot_system * rs = data;
 198         uint8_t flags;
 199
 200         IRQ_LOCK(flags);
 201         p.distance = rs->virtual_pwm.distance ;
 202         rs->virtual_pwm.angle = angle;
 203         IRQ_UNLOCK(flags);
 204
 205         p.angle = angle;
 206         rs_get_wheels_from_polar(&w, &p);
 207
 208         safe_setpwm(rs->left_pwm, rs->left_pwm_param, w.left);
 209         safe_setpwm(rs->right_pwm, rs->right_pwm_param, w.right);
 210 }
 211
 212 /**
 213  * set the real pwms according to the specified distance (it also
 214  * depends on the last angle command sent)
 215  */
 216 void rs_set_distance(void * data, int32_t distance)
 217 {
 218         struct robot_system * rs = data;
 219         struct rs_polar p;
 220         struct rs_wheels w;
 221         uint8_t flags;
 222
 223         IRQ_LOCK(flags);
 224         p.angle = rs->virtual_pwm.angle ;
 225         rs->virtual_pwm.distance = distance;
 226         IRQ_UNLOCK(flags);
 227
 228         p.distance = distance;
 229         rs_get_wheels_from_polar(&w, &p);
 230
 231         safe_setpwm(rs->left_pwm, rs->left_pwm_param, w.left);
 232         safe_setpwm(rs->right_pwm, rs->right_pwm_param, w.right);
 233 }
 234
 235 /**
 236  * get the virtual angle according to real encoders value.
 237  */
 238 int32_t rs_get_angle(void * data)
 239 {
 240         struct robot_system * rs = data;
 241         int32_t angle;
 242         uint8_t flags;
 243
 244         IRQ_LOCK(flags);
 245         angle = rs->virtual_encoders.angle ;
 246         IRQ_UNLOCK(flags);
 247         return angle;
 248 }
 249
 250 /**
 251  * get the virtual distance according to real encoders value.
 252  */
 253 int32_t rs_get_distance(void * data)
 254 {
 255         struct robot_system * rs = data;
 256         int32_t distance;
 257         uint8_t flags;
 258
 259         IRQ_LOCK(flags);
 260         distance = rs->virtual_encoders.distance ;
 261         IRQ_UNLOCK(flags);
 262         return distance;
 263 }
 264
 265 int32_t rs_get_ext_angle(void * data)
 266 {
 267         struct robot_system * rs = data;
 268         int32_t angle;
 269         uint8_t flags;
 270
 271         IRQ_LOCK(flags);
 272         angle = rs->pext_prev.angle ;
 273         IRQ_UNLOCK(flags);
 274         return angle;
 275 }
 276
 277 int32_t rs_get_ext_distance(void * data)
 278 {
 279         struct robot_system * rs = data;
 280         int32_t distance;
 281         uint8_t flags;
 282
 283         IRQ_LOCK(flags);
 284         distance = rs->pext_prev.distance ;
 285         IRQ_UNLOCK(flags);
 286         return distance;
 287 }
 288
 289 #ifdef CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT
 290 int32_t rs_get_mot_angle(void * data)
 291 {
 292         struct robot_system * rs = data;
 293         int32_t angle;
 294         uint8_t flags;
 295
 296         IRQ_LOCK(flags);
 297         angle = rs->pmot_prev.angle ;
 298         IRQ_UNLOCK(flags);
 299         return angle;
 300 }
 301
 302 int32_t rs_get_mot_distance(void * data)
 303 {
 304         struct robot_system * rs = data;
 305         int32_t distance;
 306         uint8_t flags;
 307
 308         IRQ_LOCK(flags);
 309         distance = rs->pmot_prev.distance ;
 310         IRQ_UNLOCK(flags);
 311         return distance;
 312 }
 313 #endif
 314
 315 int32_t rs_get_ext_left(void * data)
 316 {
 317         struct robot_system * rs = data;
 318         int32_t left;
 319         uint8_t flags;
 320
 321         IRQ_LOCK(flags);
 322         left = rs->wext_prev.left ;
 323         IRQ_UNLOCK(flags);
 324         return left;
 325 }
 326
 327 int32_t rs_get_ext_right(void * data)
 328 {
 329         struct robot_system * rs = data;
 330         int32_t right;
 331         uint8_t flags;
 332
 333         IRQ_LOCK(flags);
 334         right = rs->wext_prev.right ;
 335         IRQ_UNLOCK(flags);
 336         return right;
 337 }
 338
 339 #ifdef CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT
 340 int32_t rs_get_mot_left(void * data)
 341 {
 342         struct robot_system * rs = data;
 343         int32_t left;
 344         uint8_t flags;
 345
 346         IRQ_LOCK(flags);
 347         left = rs->wmot_prev.left ;
 348         IRQ_UNLOCK(flags);
 349         return left;
 350 }
 351
 352 int32_t rs_get_mot_right(void * data)
 353 {
 354         struct robot_system * rs = data;
 355         int32_t right;
 356         uint8_t flags;
 357
 358         IRQ_LOCK(flags);
 359         right = rs->wmot_prev.right ;
 360         IRQ_UNLOCK(flags);
 361         return right;
 362 }
 363 #endif
 364
 365 void rs_set_flags(struct robot_system * rs, uint8_t flags)
 366 {
 367         uint8_t i_flags;
 368
 369         IRQ_LOCK(i_flags);
 370         rs->flags = flags;
 371         IRQ_UNLOCK(i_flags);
 372 }
 373
 374 /**
 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)
 379  */
 380 void rs_update(void * data)
 381 {
 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;
 388 #endif
 389         int32_t delta_angle, delta_distance;
 390         uint8_t flags;
 391
 392         /* read encoders */
 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);
 395
 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);
 399 #endif
 400
 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);
 405         }
 406
 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);
 411         }
 412 #endif
 413
 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);
 417
 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);
 421 #endif
 422
 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;
 428         }
 429
 430         /* update from motor encoders */
 431         else {
 432                 delta_angle = pmot.angle - rs->pmot_prev.angle;
 433                 delta_distance = pmot.distance - rs->pmot_prev.distance;
 434         }
 435 #else
 436         delta_angle = pext.angle - rs->pext_prev.angle;
 437         delta_distance = pext.distance - rs->pext_prev.distance;
 438 #endif
 439
 440         IRQ_LOCK(flags);
 441         rs->virtual_encoders.angle += delta_angle;
 442         rs->virtual_encoders.distance += delta_distance;
 443         IRQ_UNLOCK(flags);
 444
 445         /* don't lock too much time */
 446
 447         IRQ_LOCK(flags);
 448         rs->pext_prev = pext;
 449         rs->wext_prev = wext;
 450         IRQ_UNLOCK(flags);
 451
 452 #ifdef CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT
 453         IRQ_LOCK(flags);
 454         rs->pmot_prev = pmot;
 455         rs->wmot_prev = wmot;
 456         IRQ_UNLOCK(flags);
 457 #endif
 458 }