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 ThreadedDriver 
00082 {
00083 public:
00084 
00085   Khepera(ConfigFile *cf, int section);
00086   ~Khepera();
00087 
00088   /* the main thread */
00089   virtual void Main();
00090 
00091   virtual int Subscribe(player_devaddr_t addr);
00092   virtual int Unsubscribe(player_devaddr_t addr);
00093 
00094   virtual int MainSetup();
00095   virtual void MainQuit();
00096 
00097   int ResetOdometry();
00098 
00099   // handle IR
00100   void SetIRState(int);
00101 
00102   void UpdateData(void);
00103 
00104   void UpdateIRData(player_ir_data_t *);
00105   void UpdatePosData(player_position2d_data_t *);
00106 
00107   // the following are all interface functions to the REB
00108   // this handles the A/D device which deals with IR for us
00109   //void ConfigAD(int, int);
00110   unsigned short ReadAD(int);
00111   int ReadAllIR(player_ir_data_t *);
00112 
00113   // this handles motor control
00114   int SetSpeed(int, int);
00115   int ReadSpeed(int*, int*);
00116 
00117   int SetPos(int, int);
00118 
00119   int SetPosCounter(int, int);
00120   int ReadPos(int *, int*);
00121 
00122   //unsigned char ReadStatus(int, int *, int *);
00123 
00124                 // MessageHandler
00125                 int ProcessMessage(QueuePointer & resp_queue, player_msghdr * hdr, void * data);
00126 
00127 private:
00128   player_devaddr_t ir_addr;
00129   player_devaddr_t position_addr;
00130   int position_subscriptions;
00131   int ir_subscriptions;
00132 
00133   KheperaSerial * Serial;
00134 
00135   player_khepera_geom_t* geometry;
00136 
00137   int param_index;  // index in the RobotParams table for this robot
00138   int khepera_fd;               // khepera device file descriptor
00139 
00140   struct timeval last_position; // last position update
00141   bool refresh_last_position;
00142   int last_lpos, last_rpos;
00143   double x,y,yaw;
00144   int last_x_f, last_y_f;
00145   double last_theta;
00146 
00147   struct timeval last_pos_update; // time of last pos update
00148   struct timeval last_ir_update;
00149 
00150   int pos_update_period;
00151 
00152   short desired_heading;
00153 
00154   int ir_sequence;
00155   struct timeval last_ir;
00156 
00157   bool motors_enabled;
00158   bool velocity_mode;
00159   bool direct_velocity_control;
00160 
00161   // device used to communicate with reb
00162   char khepera_serial_port[MAX_FILENAME_SIZE];
00163 
00164   //struct pollfd write_pfd, read_pfd;
00165 
00166 
00167 };
00168 
00169 
00170 #endif