khepera.h

00001 /*
00002  *  Player - One Hell of a Robot Server
00003  *  Copyright (C) 2000  
00004  *     Brian Gerkey, Kasper Stoy, Richard Vaughan, & Andrew Howard
00005  *                      
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., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00020  *
00021  */
00022 
00023 /* Copyright (C) 2004
00024  *   Toby Collett, University of Auckland Robotics Group
00025  *
00026  * Header for the khepera robot.  The
00027  * architecture is similar to the P2OS device, in that the position, IR and
00028  * power services all need to go through a single serial port and
00029  * base device class.  So this code was copied from p2osdevice and
00030  * modified to taste.
00031  * 
00032  */
00033 
00034 #ifndef _KHEPERADEVICE_H
00035 #define _KHEPERADEVICE_H
00036 
00037 #include <pthread.h>
00038 #include <sys/time.h>
00039 #include <errno.h>
00040 
00041 #include <libplayercore/playercore.h>
00042 //#include <khepera_params.h>
00043 #include <khepera_serial.h>
00044 
00045 #define KHEPERA_CONFIG_BUFFER_SIZE 1024
00046 #define KHEPERA_BAUDRATE B38400
00047 #define KHEPERA_DEFAULT_SERIAL_PORT "/dev/ttyUSB0"
00048 #define KHEPERA_DEFAULT_SCALE 10
00049 #define KHEPERA_DEFAULT_ENCODER_RES (1.0/12.0) 
00050 #define KHEPERA_DEFAULT_IR_CALIB_A (64.158)
00051 #define KHEPERA_DEFAULT_IR_CALIB_B (-0.1238)
00052 
00053 #define KHEPERA_MOTOR_LEFT 0
00054 #define KHEPERA_MOTOR_RIGHT 1
00055 
00056 #define KHEPERA_FIXED_FACTOR 10000
00057 
00058 #define CRLF "\r\n"
00059 #define KHEPERA_COMMAND_PROMPT "\r\n"
00060 
00061 #ifndef ABS
00062 #define ABS(x) ((x) < 0 ? -(x) : (x))
00063 #endif
00064 
00065 #ifndef SGN
00066 #define SGN(x) ((x) < 0 ? -1 : 1)
00067 #endif
00068 
00069 typedef struct player_khepera_geom
00070 {
00071         char * PortName;
00072         double scale;
00073         player_ir_pose_t ir;
00074         double * ir_calib_a;
00075         double * ir_calib_b;
00076         player_position2d_geom_t position;
00077         double encoder_res;
00078 } __attribute__ ((packed)) player_khepera_geom_t;
00079         
00080 
00081 class Khepera : public Driver 
00082 {
00083 public:
00084   
00085   Khepera(ConfigFile *cf, int section);
00086 
00087   /* the main thread */
00088   virtual void Main();
00089 
00090   virtual int Subscribe(player_devaddr_t addr);
00091   virtual int Unsubscribe(player_devaddr_t addr);
00092   
00093   virtual int Setup();
00094   virtual int Shutdown();
00095   
00096   int ResetOdometry();
00097   
00098   // handle IR
00099   void SetIRState(int);
00100 
00101   void UpdateData(void);
00102 
00103   void UpdateIRData(player_ir_data_t *);
00104   void UpdatePosData(player_position2d_data_t *);
00105 
00106   // the following are all interface functions to the REB
00107   // this handles the A/D device which deals with IR for us
00108   //void ConfigAD(int, int);
00109   unsigned short ReadAD(int);
00110   int ReadAllIR(player_ir_data_t *);
00111 
00112   // this handles motor control
00113   int SetSpeed(int, int);
00114   int ReadSpeed(int*, int*);
00115 
00116   int SetPos(int, int);
00117   
00118   int SetPosCounter(int, int);
00119   int ReadPos(int *, int*);
00120   
00121   //unsigned char ReadStatus(int, int *, int *);
00122 
00123                 // MessageHandler
00124                 int ProcessMessage(MessageQueue* resp_queue, player_msghdr * hdr, void * data);
00125 
00126 private:
00127   player_devaddr_t ir_addr;
00128   player_devaddr_t position_addr;
00129   int position_subscriptions;
00130   int ir_subscriptions;
00131 
00132   KheperaSerial * Serial;
00133 
00134   player_khepera_geom_t* geometry;
00135 
00136   int param_index;  // index in the RobotParams table for this robot
00137   int khepera_fd;               // khepera device file descriptor
00138   
00139   struct timeval last_position; // last position update
00140   bool refresh_last_position;
00141   int last_lpos, last_rpos;
00142   double x,y,yaw;
00143   int last_x_f, last_y_f;
00144   double last_theta;
00145 
00146   struct timeval last_pos_update; // time of last pos update
00147   struct timeval last_ir_update;
00148 
00149   int pos_update_period;
00150 
00151   short desired_heading;
00152 
00153   int ir_sequence;
00154   struct timeval last_ir;
00155 
00156   bool motors_enabled;
00157   bool velocity_mode;
00158   bool direct_velocity_control;
00159 
00160   // device used to communicate with reb
00161   char khepera_serial_port[MAX_FILENAME_SIZE]; 
00162 
00163   //struct pollfd write_pfd, read_pfd;
00164 
00165   
00166 };
00167 
00168 
00169 #endif

Last updated 12 September 2005 21:38:45