omnilib.c
Go to the documentation of this file.
00001 /*
00002  * This file is part of the libomnidrive project.
00003  *
00004  * Copyright (C) 2009 Uwe Hermann <uwe@hermann-uwe.de>,
00005  *                    Ingo Kresse <kresse@in.tum.de>
00006  *
00007  * This program is free software; you can redistribute it and/or modify
00008  * it under the terms of the GNU General Public License as published by
00009  * the Free Software Foundation; either version 2 of the License, or
00010  * (at your option) any later version.
00011  *
00012  * This program is distributed in the hope that it will be useful,
00013  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00014  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00015  * GNU General Public License for more details.
00016  *
00017  * You should have received a copy of the GNU General Public License
00018  * along with this program; if not, write to the Free Software
00019  * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301 USA
00020  */
00021 
00022 /* DESIGN PHASE 1: 
00023  * - The program is REQUIRED to call omnidrive_odometry often to ensure
00024  *   reasonable precision.
00025  */
00026 
00027 
00028 #include <string.h>
00029 #include <stdlib.h>
00030 #include <stdio.h>
00031 #include <stdint.h>
00032 #include <unistd.h>
00033 #include <fcntl.h>
00034 #include <sys/ioctl.h>
00035 
00036 #include <math.h>
00037 
00038 #include "omnilib.h"
00039 #include "realtime.h" // defines omniread_t, omniwrite_t
00040 
00041 /*****************************************************************************/
00042 /* global variables                       */
00043 
00044 int drive_constant=3663;
00045 int odometry_constant=3998394;
00046 double odometry_correction=1.0;
00047 
00048 int odometry_initialized = 0;
00049 uint32_t last_odometry_position[4]={0, 0, 0, 0};
00050 double odometry[3] = {0, 0, 0};
00051 
00052 int status[4];
00053 commstatus_t commstatus;
00054 
00055 void omnidrive_speedcontrol();
00056 
00057 int omnidrive_init(void)
00058 {
00059   int counter=0;
00060 
00061   omnidrive_poweroff();
00062 
00063   if(!start_omni_realtime(4000))
00064     return -1;
00065 
00066   omniread_t cur = omni_read_data();
00067 
00068   for(counter = 0; counter < 200; counter ++) {
00069     if(cur.working_counter_state >= 2)
00070       break;
00071 
00072     usleep(100000);
00073     cur = omni_read_data();
00074   }
00075 
00076   if(cur.working_counter_state < 2) // failed to initialize modules...
00077     return -1;
00078 
00079   omnidrive_speedcontrol();
00080   omnidrive_recover();
00081   omnidrive_poweron();
00082 
00083   return 0;
00084 }
00085 
00086 
00087 int omnidrive_shutdown(void)
00088 {
00089   omnidrive_drive(0, 0, 0);   /* SAFETY */
00090 
00091   omnidrive_poweroff();
00092 
00093   stop_omni_realtime();
00094 
00095   return 0;
00096 }
00097 
00098 
00106 void jac_forward(double *in, double *out)
00107 {
00108   // computing:
00109   //   out = (C*J_fwd) * in
00110   // with:
00111   //   J_fwd = [1 -1 -alpha;
00112   //            1  1  alpha;
00113   //            1  1 -alpha;
00114   //            1 -1  alpha]
00115   //   C     = [0  0  0  1;
00116   //            0  0 -1  0;
00117   //            0  1  0  0;
00118   //           -1  0  0  0 ]
00119 
00120   int i,j;
00121 #define alpha (0.3425 + 0.24)
00122   double C_J_fwd[4][3] = {{ 1, -1, alpha},
00123                           {-1, -1, alpha},
00124                           { 1,  1, alpha},
00125                           {-1,  1, alpha}};
00126 
00127   // assert(in  != 0);
00128   // assert(out != 0);
00129 
00130   for(i=0; i < 4; i++) {
00131     out[i] = 0;
00132     for(j=0; j < 3; j++) {
00133       out[i] += C_J_fwd[i][j]*in[j];
00134     }
00135   }
00136 }
00137 
00138 
00139 void jac_inverse(double *in, double *out)
00140 {
00141   // computing:
00142   //   out = (J_inv*C^-1) * in
00143   // with:
00144   //   J_fwd = 1/4*[1 -1 -alpha;
00145   //                1  1  alpha;
00146   //                1  1 -alpha;
00147   //                1 -1  alpha]
00148   //   C = [0  0  0  1;
00149   //        0  0 -1  0;
00150   //        0  1  0  0;
00151   //       -1  0  0  0 ]
00152 
00153   int i,j;
00154 #define alpha (0.3425 + 0.24)
00155   double J_inv_C[3][4] = {{ 0.25,      -0.25,       0.25,      -0.25},
00156                           {-0.25,      -0.25,       0.25,       0.25},
00157                           { 0.25/alpha, 0.25/alpha, 0.25/alpha, 0.25/alpha}};
00158 
00159   // assert(in  != 0);
00160   // assert(out != 0);
00161 
00162   for(i=0; i < 3; i++) {
00163     out[i] = 0;
00164     for(j=0; j < 4; j++) {
00165       out[i] += J_inv_C[i][j]*in[j];
00166     }
00167   }
00168 }
00169 
00170 
00171 int omnidrive_drive(double x, double y, double a)
00172 {
00173   // speed limits for the robot
00174   double wheel_limit = 0.8;  // a single wheel may drive this fast (m/s)
00175   double cart_limit = 0.5;   // any point on the robot may move this fast (m/s)
00176   double radius = 0.7;       // (maximum) radius of the robot (m)
00177 
00178   // 0.5 m/s is 1831 ticks. kernel limit is 2000 ticks.
00179 
00180   double corr_wheels, corr_cart, corr;
00181 
00182   omniwrite_t tar;
00183   memset(&tar, 0, sizeof(tar));
00184 
00185   double cartesian_speeds[3] = {x, y, a}, wheel_speeds[4];
00186   int i;
00187 
00188   //TODO: check if the robot is up. if not, return immediately
00189   
00190   tar.magic_version = OMNICOM_MAGIC_VERSION;
00191 
00192   // check for limits
00193 
00194   // cartesian limit: add linear and angular parts
00195   corr_cart = cart_limit / (sqrt(x*x + y*y) + radius*fabs(a));
00196 
00197   // wheel limit: for one wheel, x,y and a always add up
00198   corr_wheels = wheel_limit / (fabs(x) + fabs(y) + fabs(a));
00199 
00200   // get limiting factor as min(1, corr_cart, corr_wheels)
00201   corr = (1 < corr_cart) ? 1 : ((corr_cart < corr_wheels) ? corr_cart : corr_wheels);
00202 
00203   jac_forward(cartesian_speeds, wheel_speeds);
00204 
00205   for(i = 0; i < 4; i++) {
00206     tar.target_velocity[i] = wheel_speeds[i] * corr * drive_constant;
00207     tar.torque_set_value[i] = 0.0;
00208   }
00209 
00210   /* Let the kernel know the velocities we want to set. */
00211   omni_write_data(tar);
00212 
00213   return 0;
00214 }
00215 
00216 void omnidrive_set_correction(double drift)
00217 {
00218   odometry_correction = drift;
00219 }
00220 
00221 int omnidrive_odometry(double *x, double *y, double *a)
00222 {
00223   omniread_t cur; /* Current velocities / torques / positions */
00224   int i;
00225   double d_wheel[4], d[3], ang;
00226 
00227   /* Read data from kernel module. */
00228   cur = omni_read_data();
00229 
00230   // copy status values
00231   for(i=0; i < 4; i++)
00232     status[i] = cur.status[i];
00233 
00234   for(i=0; i < 4; i++) {
00235     commstatus.slave_state[i] = cur.slave_state[i];
00236     commstatus.slave_online[i] = cur.slave_online[i];
00237     commstatus.slave_operational[i] = cur.slave_operational[i];
00238   }
00239   commstatus.master_link = cur.master_link;
00240   commstatus.master_al_states = cur.master_al_states;
00241   commstatus.master_slaves_responding = cur.master_slaves_responding;
00242   commstatus.working_counter = cur.working_counter;
00243   commstatus.working_counter_state = cur.working_counter_state;
00244 
00245 
00246   /* start at (0, 0, 0) */
00247   if(!odometry_initialized) {
00248     for (i = 0; i < 4; i++)
00249       last_odometry_position[i] = cur.position[i];
00250     odometry_initialized = 1;
00251   }
00252 
00253   /* compute differences of encoder readings and convert to meters */
00254   for (i = 0; i < 4; i++) {
00255     d_wheel[i] = (int) (cur.position[i] - last_odometry_position[i]) * (1.0/(odometry_constant*odometry_correction));
00256     /* remember last wheel position */
00257     last_odometry_position[i] = cur.position[i];
00258   }
00259 
00260   /* IMPORTANT: Switch order of the motor numbers! 
00261      --> moved to jacobian_inverse */
00262 
00263   jac_inverse(d_wheel, d);
00264     
00265   ang = odometry[2] + d[2]/2.0;
00266 
00267   odometry[0] += d[0]*cos(ang) - d[1]*sin(ang);
00268   odometry[1] += d[0]*sin(ang) + d[1]*cos(ang);
00269   odometry[2] += d[2];
00270 
00271   /* return current odometry values */
00272   *x = odometry[0];
00273   *y = odometry[1];
00274   *a = odometry[2];
00275 
00276   return 0;
00277 }
00278 
00279 int readSDO(int device, int objectNum)
00280 {
00281   FILE *fp;
00282   char cmd[1024], res[1024];
00283   char *redundant;
00284   sprintf(cmd, "ethercat upload -p %d --type uint16 0x%x 0 |awk '{print $2}'", device, objectNum);
00285 
00286   fp = popen(cmd, "r");
00287   redundant = fgets(res, sizeof(res), fp);
00288   pclose(fp);
00289 
00290   return atol(res);
00291 }
00292 
00293 #define INT8   0
00294 #define UINT8  1
00295 #define INT16  2
00296 #define UINT16 3
00297 
00298 int writeSDO(int device, int objectNum, int value, int type)
00299 {
00300   char cmd[1024];
00301   char *types[] = {"int8", "uint8", "int16", "uint16"};
00302   sprintf(cmd, "ethercat download -p %d --type %s -- 0x%x 0 %d", device, types[type], objectNum, value);
00303   return system(cmd);
00304 }
00305 
00306 void drive_status(char *drive, int index)
00307 {
00308   int i;
00309   char statusdisp[] = { '0',  '1',  '2',  '3',  '4',  '5',  '6',  '7',  'E',  'F'};
00310   int  statuscode[] = {0x00, 0x40, 0x21, 0x33, 0x37, 0xff, 0xff, 0x17, 0x0f, 0x08};
00311   int  statusmask[] = {0x5f, 0x4f, 0x6f, 0x7f, 0x7f, 0x00, 0x00, 0x7f, 0x4f, 0x4f};
00312   if(drive)
00313     for(i=0; i < 10; i++)
00314       if((status[index] & statusmask[i]) == statuscode[i])
00315         *drive = statusdisp[i];
00316 }
00317 
00318 commstatus_t omnidrive_commstatus()
00319 {
00320   return commstatus;
00321 }
00322 
00323 void omnidrive_status(char *drive0, char *drive1, char *drive2, char *drive3, int *estop)
00324 {
00325   drive_status(drive0, 0);
00326   drive_status(drive1, 1);
00327   drive_status(drive2, 2);
00328   drive_status(drive3, 3);
00329 
00330   if(estop)
00331     *estop = 0x80 & (status[0] & status[1] & status[2] & status[3]);
00332 }
00333 
00334 void omnidrive_recover()
00335 {
00336   writeSDO(0, 0x6040, 0x80, UINT16);
00337   writeSDO(1, 0x6040, 0x80, UINT16);
00338   writeSDO(2, 0x6040, 0x80, UINT16);
00339   writeSDO(3, 0x6040, 0x80, UINT16);
00340 }
00341 
00342 void omnidrive_poweron()
00343 {
00344   writeSDO(0, 0x6040, 0x06, UINT16);
00345   writeSDO(1, 0x6040, 0x06, UINT16);
00346   writeSDO(2, 0x6040, 0x06, UINT16);
00347   writeSDO(3, 0x6040, 0x06, UINT16);
00348 
00349   writeSDO(0, 0x6040, 0x07, UINT16);
00350   writeSDO(1, 0x6040, 0x07, UINT16);
00351   writeSDO(2, 0x6040, 0x07, UINT16);
00352   writeSDO(3, 0x6040, 0x07, UINT16);
00353 
00354   writeSDO(0, 0x6040, 0x0f, UINT16);
00355   writeSDO(1, 0x6040, 0x0f, UINT16);
00356   writeSDO(2, 0x6040, 0x0f, UINT16);
00357   writeSDO(3, 0x6040, 0x0f, UINT16);
00358 }
00359 
00360 void omnidrive_poweroff()
00361 {
00362   writeSDO(0, 0x6040, 0x00, UINT16);
00363   writeSDO(1, 0x6040, 0x00, UINT16);
00364   writeSDO(2, 0x6040, 0x00, UINT16);
00365   writeSDO(3, 0x6040, 0x00, UINT16);
00366 }
00367 
00368 void omnidrive_speedcontrol()
00369 {
00370   writeSDO(0, 0x6060, -3, INT8);
00371   writeSDO(1, 0x6060, -3, INT8);
00372   writeSDO(2, 0x6060, -3, INT8);
00373   writeSDO(3, 0x6060, -3, INT8);
00374 }


omni_ethercat
Author(s): Ingo Kresse
autogenerated on Mon Oct 6 2014 08:39:05