er.h
00001 /*
00002  *  Player - One Hell of a Robot Server
00003  *  Copyright (C) 2000-2003
00004  *     David Feil-Seifer
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 /*
00024  *
00025  * Relevant constants for the "ER" robots, made by Evolution Robotics.
00026  *
00027  * Some of this code is borrowed and/or adapted from the player
00028  * module of trogdor; thanks to the author of that module.
00029  */
00030 
00031 #ifndef _ER1_DRIVER_H_
00032 #define _ER1_DRIVER_H_
00033 
00034 #define ER_DEFAULT_PORT "/dev/usb/tts/0"
00035 #define ER_DEFAULT_LEFT_MOTOR 0
00036 #define ER_DELAY_US 10000
00037 
00038 
00039 /* ER1 Commands */
00040 
00041 #define NOOP                0x00
00042 #define SETVEL              0x11
00043 #define UPDATE              0x1A
00044 #define GETCMDPOS           0x1D
00045 #define GETEVENTSTATUS      0x31
00046 #define RESETEVENTSTATUS    0x34
00047 #define RESET               0x39
00048 #define SETMOTORCMD         0x77
00049 #define SETLIMITSWITCHMODE  0x80
00050 #define GETVERSION          0x8F
00051 #define SETACCEL            0x90
00052 #define SETDECEL            0x91
00053 #define SETPROFILEMODE      0xA0
00054 #define SETSTOPMODE         0xD0
00055 #define READANALOG          0xEF
00056 
00057 #define MOTOR_0             0x00
00058 #define MOTOR_1             0x01
00059 
00060 /* robot-specific info */
00061 #define ER_DEFAULT_MOTOR_0_DIR          -1
00062 #define ER_DEFAULT_MOTOR_1_DIR          1
00063 #define ER_DEFAULT_AXLE_LENGTH          .38
00064 
00065 #define ER_MAX_TICKS 3000
00066 #define ER_WHEEL_RADIUS         .055
00067 #define ER_WHEEL_CIRC           .345575197
00068 #define ER_WHEEL_STEP           .45
00069 #define ER_M_PER_TICK           .0000058351 //JM 12/11/06
00070 //#define ER_M_PER_TICK         .00000602836879 // NdT 1/4/06
00071 //#define ER_M_PER_TICK           .000005700      // THC 1/23/06
00072 
00073 /* for safety */
00074 #define ER_MAX_WHEELSPEED       .500
00075 #define ER_MPS_PER_TICK         1
00076 
00077 #define FULL_STOP       0
00078 #define STOP            1
00079 
00080 
00081 #include <libplayerinterface/player.h>
00082 #include <libplayercore/driver.h>
00083 #include <libplayercore/drivertable.h>
00084 
00085 
00086 typedef struct
00087 {
00088   player_position2d_data_t position;
00089 } __attribute__ ((packed)) player_er1_data_t;
00090 
00091 
00092 
00093 class ER : public ThreadedDriver
00094 {
00095   private:
00096     player_er1_data_t er1_data;
00097 
00098     player_devaddr_t position_id;
00099     int position_subscriptions;
00100 
00101   public:
00102     ER(ConfigFile* cf, int section);
00103 
00104     // public, so that it can be called from pthread cleanup function
00105     int SetVelocity(double lvel, double rvel);
00106     void Stop( int StopMode );
00107     virtual void Main();
00108     virtual int MainSetup();
00109     virtual void MainQuit();
00110 
00111     // MessageHandler
00112     virtual int ProcessMessage(QueuePointer & resp_queue,
00113                                player_msghdr * hdr,
00114                                void * data);
00115     void Test();
00116 
00117   private:
00118     // this function will be run in a separate thread
00119     int InitOdom();
00120     int InitRobot();
00121 
00122     //serial connection
00123     int *_tc_num;
00124     int _fd; // device file descriptor
00125     const char* _serial_port; // name of dev file
00126     bool _fd_blocking;
00127 
00128     // methods for internal use
00129     int WriteBuf(unsigned char* s, size_t len);
00130     int ReadBuf(unsigned char* s, size_t len);
00131     int SendCommand(unsigned char * cmd, int cmd_len, unsigned char * ret_val, int ret_len);
00132     int checksum_ok (unsigned char *buf, int len);
00133 
00134     int ComputeTickDiff(int from, int to);
00135     int ChangeMotorState(int state);
00136     int BytesToInt32(unsigned char *ptr);
00137     float BytesToFloat(unsigned char *ptr);
00138     void UpdateOdom(int ltics, int rtics);
00139     void SpeedCommand( unsigned char address, double speed, int dir );
00140     void SetOdometry (long,long,long);
00141     void ResetOdometry();
00142     //periodic functions
00143     int GetOdom(int *ltics, int *rtics);
00144     int GetBatteryVoltage(int* voltage);
00145     int GetRangeSensor( int s, float * val );
00146     void MotorSpeed();
00147 
00148     //er values
00149     double _axle_length;
00150     int _motor_0_dir;
00151     int _motor_1_dir;
00152 
00153     //internal info
00154     bool _debug;
00155     bool _need_to_set_speed;
00156     bool _odom_initialized;
00157     bool _stopped;
00158     int _last_ltics, _last_rtics;
00159     double _px, _py, _pa;  // integrated odometric pose (m,m,rad)
00160     int _powered, _resting; // If _powered is false, no constant updates. _resting means the last update was a 0,0 one.
00161 
00162     int send_command( unsigned char address, unsigned char c, int ret_num, unsigned char * ret );
00163     int send_command_2_arg( unsigned char address, unsigned char c, int arg, int ret_num, unsigned char * ret );
00164     int send_command_4_arg( unsigned char address, unsigned char c, int arg, int ret_num, unsigned char * ret );
00165 
00166 };
00167 #endif
00168