modules/devices/robot/position_manager/position_manager.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: position_manager.c,v 1.6.4.7 2009-05-18 12:27:26 zer0 Exp $
  19  *
  20  */
  21
  22 #include <string.h>
  23 #include <math.h>
  24
  25 #include <robot_system.h>
  26 #include <position_manager.h>
  27
  28 /** initialization of the robot_position pos, everthing is set to 0 */
  29 void position_init(struct robot_position *pos)
  30 {
  31         uint8_t flags;
  32         IRQ_LOCK(flags);
  33         memset(pos, 0, sizeof(struct robot_position));
  34         IRQ_UNLOCK(flags);
  35 }
  36
  37 /** Set a new robot position */
  38 void position_set(struct robot_position *pos, int16_t x, int16_t y, int16_t a)
  39 {
  40         uint8_t flags;
  41         IRQ_LOCK(flags);
  42         pos->pos_d.a = ((double)a * M_PI)/ 180.0;
  43         pos->pos_d.x = x;
  44         pos->pos_d.y = y;
  45         pos->pos_s16.x = x;
  46         pos->pos_s16.y = y;
  47         pos->pos_s16.a = a;
  48         IRQ_UNLOCK(flags);
  49 }
  50
  51 #ifdef CONFIG_MODULE_COMPENSATE_CENTRIFUGAL_FORCE
  52 void position_set_centrifugal_coef(struct robot_position *pos, double coef)
  53 {
  54         pos->centrifugal_coef = coef;
  55 }
  56 #endif
  57
  58 /**
  59  * Save in pos structure the pointer to the associated robot_system.
  60  * The robot_system structure is used to get values from virtual encoders
  61  * that return angle and distance.
  62  */
  63 void position_set_related_robot_system(struct robot_position *pos, struct robot_system *rs)
  64 {
  65         uint8_t flags;
  66         IRQ_LOCK(flags);
  67         pos->rs = rs;
  68         IRQ_UNLOCK(flags);
  69 }
  70
  71 /**
  72  * Set the physical parameters of the robot :
  73  *  - number of impulsions for 1 mm (distance)
  74  *  - number of impulsions for 1 degree (angle)
  75  */
  76 void position_set_physical_params(struct robot_position *pos, double track_mm,
  77                                   double distance_imp_per_mm)
  78 {
  79         uint8_t flags;
  80         IRQ_LOCK(flags);
  81         pos->phys.track_mm = track_mm;
  82         pos->phys.distance_imp_per_mm = distance_imp_per_mm;
  83         IRQ_UNLOCK(flags);
  84 }
  85
  86 void position_use_ext(struct robot_position *pos)
  87 {
  88         struct rs_polar encoders;
  89         uint8_t flags;
  90
  91         IRQ_LOCK(flags);
  92         encoders.distance = rs_get_ext_distance(pos->rs);
  93         encoders.angle = rs_get_ext_angle(pos->rs);
  94         pos->prev_encoders = encoders;
  95         pos->use_ext = 1;
  96         IRQ_UNLOCK(flags);
  97 }
  98
  99 #ifdef CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT
 100 void position_use_mot(struct robot_position *pos)
 101 {
 102         struct rs_polar encoders;
 103         uint8_t flags;
 104
 105         IRQ_LOCK(flags);
 106         encoders.distance = rs_get_mot_distance(pos->rs);
 107         encoders.angle = rs_get_mot_angle(pos->rs);
 108         pos->prev_encoders = encoders;
 109         pos->use_ext = 0;
 110         IRQ_UNLOCK(flags);
 111 }
 112 #endif
 113
 114 /**
 115  * Process the absolute position (x,y,a) depending on the delta on
 116  * virtual encoders since last read, and depending on physical
 117  * parameters. The processed position is in mm.
 118  */
 119 void position_manage(struct robot_position *pos)
 120 {
 121         double x, y, a, r, arc_angle;
 122         double dx, dy;
 123         s16 x_s16, y_s16, a_s16;
 124         struct rs_polar encoders;
 125         struct rs_polar delta;
 126         struct robot_system * rs;
 127         uint8_t flags;
 128
 129         IRQ_LOCK(flags);
 130         rs = pos->rs;
 131         IRQ_UNLOCK(flags);
 132         /* here we could raise an error */
 133         if (rs == NULL)
 134                 return;
 135
 136 #ifdef CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT
 137         if (pos->use_ext) {
 138                 encoders.distance = rs_get_ext_distance(rs);
 139                 encoders.angle = rs_get_ext_angle(rs);
 140         }
 141         else {
 142                 encoders.distance = rs_get_mot_distance(rs);
 143                 encoders.angle = rs_get_mot_angle(rs);
 144         }
 145 #else
 146         encoders.distance = rs_get_ext_distance(rs);
 147         encoders.angle = rs_get_ext_angle(rs);
 148 #endif
 149
 150         /* process difference between 2 measures.
 151          * No lock for prev_encoders since we are the only one to use
 152          * this var XXX that's wrong now, perhaps we should lock */
 153         delta.distance = encoders.distance - pos->prev_encoders.distance;
 154         delta.angle = encoders.angle - pos->prev_encoders.angle;
 155
 156         pos->prev_encoders = encoders;
 157
 158         /* update double position */
 159         IRQ_LOCK(flags);
 160         a = pos->pos_d.a;
 161         x = pos->pos_d.x;
 162         y = pos->pos_d.y;
 163         IRQ_UNLOCK(flags);
 164
 165         if (delta.angle == 0) {
 166                 /* we go straight */
 167                 dx = cos(a) * ((double) delta.distance / (pos->phys.distance_imp_per_mm)) ;
 168                 dy = sin(a) * ((double) delta.distance / (pos->phys.distance_imp_per_mm)) ;
 169                 x += dx;
 170                 y += dy;
 171         }
 172         else {
 173                 /* r the radius of the circle arc */
 174                 r = (double)delta.distance * pos->phys.track_mm / ((double) delta.angle * 2);
 175                 arc_angle = 2 * (double) delta.angle / (pos->phys.track_mm * pos->phys.distance_imp_per_mm);
 176
 177                 dx = r * (-sin(a) + sin(a+arc_angle));
 178                 dy = r * (cos(a) - cos(a+arc_angle));
 179
 180                 x += dx;
 181                 y += dy;
 182                 a += arc_angle;
 183
 184                 if (a < -M_PI)
 185                         a += (M_PI*2);
 186                 else if (a > (M_PI))
 187                         a -= (M_PI*2);
 188
 189 #ifdef CONFIG_MODULE_COMPENSATE_CENTRIFUGAL_FORCE
 190                 /* This part compensate the centrifugal force when we
 191                  * turn very quickly. Idea is from Gargamel (RCVA). */
 192                 if (pos->centrifugal_coef && r != 0) {
 193                         double k;
 194
 195                         /*
 196                          * centrifugal force is F = (m.v^2 / R)
 197                          * with v: angular speed
 198                          *      R: radius of the circle
 199                          */
 200
 201                         k = ((double) delta.distance);
 202                         k = k * k;
 203                         k /= r;
 204                         k *= pos->centrifugal_coef;
 205
 206                         /*
 207                          * F acts perpendicularly to the vector
 208                          */
 209                         x += k * sin(a);
 210                         y -= k * cos(a);
 211                 }
 212 #endif
 213         }
 214
 215         /* update int position */
 216         x_s16 = (int16_t)x;
 217         y_s16 = (int16_t)y;
 218         a_s16 = (int16_t)(a * (360.0/(M_PI*2)));
 219
 220         IRQ_LOCK(flags);
 221         pos->pos_d.a = a;
 222         pos->pos_d.x = x;
 223         pos->pos_d.y = y;
 224         pos->pos_s16.x = x_s16;
 225         pos->pos_s16.y = y_s16;
 226         pos->pos_s16.a = a_s16;
 227         IRQ_UNLOCK(flags);
 228 }
 229
 230
 231 /**
 232  * returns current x
 233  */
 234 int16_t position_get_x_s16(struct robot_position *pos)
 235 {
 236         return pos->pos_s16.x;
 237 }
 238
 239 /**
 240  * returns current y
 241  */
 242 int16_t position_get_y_s16(struct robot_position *pos)
 243 {
 244         return pos->pos_s16.y;
 245 }
 246
 247 /**
 248  * returns current alpha
 249  */
 250 int16_t position_get_a_deg_s16(struct robot_position *pos)
 251 {
 252         return pos->pos_s16.a;
 253 }
 254
 255 /********* double */
 256
 257 /**
 258  * returns current x
 259  */
 260 double position_get_x_double(struct robot_position *pos)
 261 {
 262         return pos->pos_d.x;
 263 }
 264
 265 /**
 266  * returns current y
 267  */
 268 double position_get_y_double(struct robot_position *pos)
 269 {
 270         return pos->pos_d.y;
 271 }
 272
 273 /**
 274  * returns current alpha
 275  */
 276 double position_get_a_rad_double(struct robot_position *pos)
 277 {
 278         return pos->pos_d.a;
 279 }
 280