modules/devices/robot/robot_system/robot_system.c

   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 #ifdef CONFIG_MODULE_ROBOT_SYSTEM_USE_F64
  36 #include <f64.h>
  37 #endif
  38
  39 #include "angle_distance.h"
  40 #include "robot_system.h"
  41
  42
  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
  49  */
  50 static inline void
  51 safe_setpwm(void (*f)(void *, int32_t), void * param, int32_t value)
  52 {
  53         void (*f_tmp)(void *, int32_t);
  54         void * param_tmp;
  55         uint8_t flags;
  56         IRQ_LOCK(flags);
  57         f_tmp = f;
  58         param_tmp = param;
  59         IRQ_UNLOCK(flags);
  60         if (f_tmp) {
  61                 f_tmp(param_tmp, value);
  62         }
  63 }
  64
  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
  71  */
  72 static inline uint32_t
  73 safe_getencoder(int32_t (*f)(void *), void * param)
  74 {
  75         int32_t (*f_tmp)(void *);
  76         void * param_tmp;
  77         uint8_t flags;
  78         IRQ_LOCK(flags);
  79         f_tmp = f;
  80         param_tmp = param;
  81         IRQ_UNLOCK(flags);
  82         if (f_tmp) {
  83                 return f_tmp(param_tmp);
  84         }
  85         return 0;
  86 }
  87
  88 /** Set the structure to 0 */
  89 void rs_init( struct robot_system * rs)
  90 {
  91         uint8_t flags;
  92
  93         IRQ_LOCK(flags);
  94         memset(rs, 0, sizeof(struct robot_system));
  95 #ifdef CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT
  96         rs_set_ratio(rs, 1.0);
  97 #endif
  98         IRQ_UNLOCK(flags);
  99 }
 100
 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)
 104 {
 105         uint8_t flags;
 106
 107         IRQ_LOCK(flags);
 108         rs->ratio_mot_ext = f64_from_double(ratio);
 109         IRQ_UNLOCK(flags);
 110 }
 111 #endif
 112
 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)
 115 {
 116         uint8_t flags;
 117
 118         IRQ_LOCK(flags);
 119         rs->left_pwm = left_pwm;
 120         rs->left_pwm_param = left_pwm_param;
 121         IRQ_UNLOCK(flags);
 122 }
 123
 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)
 126 {
 127         uint8_t flags;
 128
 129         IRQ_LOCK(flags);
 130         rs->right_pwm = right_pwm;
 131         rs->right_pwm_param = right_pwm_param;
 132         IRQ_UNLOCK(flags);
 133 }
 134
 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)
 139 {
 140         uint8_t flags;
 141
 142         IRQ_LOCK(flags);
 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);
 146         IRQ_UNLOCK(flags);
 147 }
 148
 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)
 152 {
 153         uint8_t flags;
 154
 155         IRQ_LOCK(flags);
 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);
 159         IRQ_UNLOCK(flags);
 160 }
 161 #endif
 162
 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)
 166 {
 167         uint8_t flags;
 168
 169         IRQ_LOCK(flags);
 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);
 174 #else
 175         rs->left_ext_gain = gain;
 176 #endif
 177         IRQ_UNLOCK(flags);
 178 }
 179
 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)
 183 {
 184         uint8_t flags;
 185
 186         IRQ_LOCK(flags);
 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);
 191 #else
 192         rs->right_ext_gain = gain;
 193 #endif
 194         IRQ_UNLOCK(flags);
 195 }
 196
 197 /**** Virtual encoders and PWM */
 198
 199 /**
 200  * set the real pwms according to the specified angle (it also
 201  * depends on the last distance command sent)
 202  */
 203 void rs_set_angle(void * data, int32_t angle)
 204 {
 205         struct rs_polar p;
 206         struct rs_wheels w;
 207         struct robot_system * rs = data;
 208         uint8_t flags;
 209
 210         IRQ_LOCK(flags);
 211         p.distance = rs->virtual_pwm.distance ;
 212         rs->virtual_pwm.angle = angle;
 213         IRQ_UNLOCK(flags);
 214
 215         p.angle = angle;
 216         rs_get_wheels_from_polar(&w, &p);
 217
 218         safe_setpwm(rs->left_pwm, rs->left_pwm_param, w.left);
 219         safe_setpwm(rs->right_pwm, rs->right_pwm_param, w.right);
 220 }
 221
 222 /**
 223  * set the real pwms according to the specified distance (it also
 224  * depends on the last angle command sent)
 225  */
 226 void rs_set_distance(void * data, int32_t distance)
 227 {
 228         struct robot_system * rs = data;
 229         struct rs_polar p;
 230         struct rs_wheels w;
 231         uint8_t flags;
 232
 233         IRQ_LOCK(flags);
 234         p.angle = rs->virtual_pwm.angle ;
 235         rs->virtual_pwm.distance = distance;
 236         IRQ_UNLOCK(flags);
 237
 238         p.distance = distance;
 239         rs_get_wheels_from_polar(&w, &p);
 240
 241         safe_setpwm(rs->left_pwm, rs->left_pwm_param, w.left);
 242         safe_setpwm(rs->right_pwm, rs->right_pwm_param, w.right);
 243 }
 244
 245 /**
 246  * get the virtual angle according to real encoders value.
 247  */
 248 int32_t rs_get_angle(void * data)
 249 {
 250         struct robot_system * rs = data;
 251         int32_t angle;
 252         uint8_t flags;
 253
 254         IRQ_LOCK(flags);
 255         angle = rs->virtual_encoders.angle ;
 256         IRQ_UNLOCK(flags);
 257         return angle;
 258 }
 259
 260 /**
 261  * get the virtual distance according to real encoders value.
 262  */
 263 int32_t rs_get_distance(void * data)
 264 {
 265         struct robot_system * rs = data;
 266         int32_t distance;
 267         uint8_t flags;
 268
 269         IRQ_LOCK(flags);
 270         distance = rs->virtual_encoders.distance ;
 271         IRQ_UNLOCK(flags);
 272         return distance;
 273 }
 274
 275 int32_t rs_get_ext_angle(void * data)
 276 {
 277         struct robot_system * rs = data;
 278         int32_t angle;
 279         uint8_t flags;
 280
 281         IRQ_LOCK(flags);
 282         angle = rs->pext_prev.angle ;
 283         IRQ_UNLOCK(flags);
 284         return angle;
 285 }
 286
 287 int32_t rs_get_ext_distance(void * data)
 288 {
 289         struct robot_system * rs = data;
 290         int32_t distance;
 291         uint8_t flags;
 292
 293         IRQ_LOCK(flags);
 294         distance = rs->pext_prev.distance ;
 295         IRQ_UNLOCK(flags);
 296         return distance;
 297 }
 298
 299 #ifdef CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT
 300 int32_t rs_get_mot_angle(void * data)
 301 {
 302         struct robot_system * rs = data;
 303         int32_t angle;
 304         uint8_t flags;
 305
 306         IRQ_LOCK(flags);
 307         angle = rs->pmot_prev.angle ;
 308         IRQ_UNLOCK(flags);
 309         return angle;
 310 }
 311
 312 int32_t rs_get_mot_distance(void * data)
 313 {
 314         struct robot_system * rs = data;
 315         int32_t distance;
 316         uint8_t flags;
 317
 318         IRQ_LOCK(flags);
 319         distance = rs->pmot_prev.distance ;
 320         IRQ_UNLOCK(flags);
 321         return distance;
 322 }
 323 #endif
 324
 325 int32_t rs_get_ext_left(void * data)
 326 {
 327         struct robot_system * rs = data;
 328         int32_t left;
 329         uint8_t flags;
 330
 331         IRQ_LOCK(flags);
 332         left = rs->wext_prev.left ;
 333         IRQ_UNLOCK(flags);
 334         return left;
 335 }
 336
 337 int32_t rs_get_ext_right(void * data)
 338 {
 339         struct robot_system * rs = data;
 340         int32_t right;
 341         uint8_t flags;
 342
 343         IRQ_LOCK(flags);
 344         right = rs->wext_prev.right ;
 345         IRQ_UNLOCK(flags);
 346         return right;
 347 }
 348
 349 #ifdef CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT
 350 int32_t rs_get_mot_left(void * data)
 351 {
 352         struct robot_system * rs = data;
 353         int32_t left;
 354         uint8_t flags;
 355
 356         IRQ_LOCK(flags);
 357         left = rs->wmot_prev.left ;
 358         IRQ_UNLOCK(flags);
 359         return left;
 360 }
 361
 362 int32_t rs_get_mot_right(void * data)
 363 {
 364         struct robot_system * rs = data;
 365         int32_t right;
 366         uint8_t flags;
 367
 368         IRQ_LOCK(flags);
 369         right = rs->wmot_prev.right ;
 370         IRQ_UNLOCK(flags);
 371         return right;
 372 }
 373 #endif
 374
 375 void rs_set_flags(struct robot_system * rs, uint8_t flags)
 376 {
 377         uint8_t i_flags;
 378
 379         IRQ_LOCK(i_flags);
 380         rs->flags = flags;
 381         IRQ_UNLOCK(i_flags);
 382 }
 383
 384 /**
 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)
 389  */
 390 void rs_update(void * data)
 391 {
 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;
 398 #endif
 399         int32_t delta_angle, delta_distance;
 400         uint8_t flags;
 401
 402         /* read encoders */
 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);
 405
 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);
 409 #endif
 410
 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);
 416 #else
 417                 double tmp;
 418                 tmp = wext.left;
 419                 tmp *= rs->left_ext_gain;
 420                 wext.left = tmp;
 421                 tmp = wext.right;
 422                 tmp *= rs->right_ext_gain;
 423                 wext.right = tmp;
 424 #endif
 425         }
 426
 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);
 431         }
 432 #endif
 433
 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);
 437
 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);
 441 #endif
 442
 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;
 448         }
 449
 450         /* update from motor encoders */
 451         else {
 452                 delta_angle = pmot.angle - rs->pmot_prev.angle;
 453                 delta_distance = pmot.distance - rs->pmot_prev.distance;
 454         }
 455 #else
 456         delta_angle = pext.angle - rs->pext_prev.angle;
 457         delta_distance = pext.distance - rs->pext_prev.distance;
 458 #endif
 459
 460         IRQ_LOCK(flags);
 461         rs->virtual_encoders.angle += delta_angle;
 462         rs->virtual_encoders.distance += delta_distance;
 463         IRQ_UNLOCK(flags);
 464
 465         /* don't lock too much time */
 466
 467         IRQ_LOCK(flags);
 468         rs->pext_prev = pext;
 469         rs->wext_prev = wext;
 470         IRQ_UNLOCK(flags);
 471
 472 #ifdef CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT
 473         IRQ_LOCK(flags);
 474         rs->pmot_prev = pmot;
 475         rs->wmot_prev = wmot;
 476         IRQ_UNLOCK(flags);
 477 #endif
 478 }