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           .00000602836879
00070 
00071 /* for safety */
00072 #define ER_MAX_WHEELSPEED   500
00073 #define ER_MPS_PER_TICK         1
00074 
00075 #define FULL_STOP       0
00076 #define STOP            1
00077 
00078 
00079 #include <libplayercore/playercore.h>
00080 
00081 typedef struct
00082 {
00083   player_position2d_data_t position;
00084 } __attribute__ ((packed)) player_er1_data_t;
00085 
00086 
00087 
00088 class ER : public Driver 
00089 {
00090   private:
00091     player_er1_data_t er1_data;
00092     
00093     player_devaddr_t position_id;
00094     int position_subscriptions;
00095 
00096   public:
00097     ER(ConfigFile* cf, int section);
00098 
00099     // public, so that it can be called from pthread cleanup function
00100     int SetVelocity(double lvel, double rvel);
00101     void Stop( int StopMode );
00102     virtual void Main();
00103     virtual int Setup();
00104     virtual int Shutdown();
00105     //void HandleConfig(void);
00106     //void GetCommand(void);
00107     //void PutData(void);
00108     
00109     // MessageHandler
00110     virtual int ProcessMessage(MessageQueue * resp_queue,
00111                                player_msghdr * hdr,
00112                                void * data);
00113     //void HandlePositionCommand(player_position2d_cmd_t position_cmd);
00114 
00115         
00116   private:
00117     // this function will be run in a separate thread
00118         int InitOdom();
00119     int InitRobot();
00120     
00121     //serial connection
00122     int *_tc_num;
00123     int _fd; // device file descriptor
00124     const char* _serial_port; // name of dev file
00125     bool _fd_blocking;
00126     
00127     // methods for internal use
00128     int WriteBuf(unsigned char* s, size_t len);
00129     int ReadBuf(unsigned char* s, size_t len);
00130     int SendCommand(unsigned char * cmd, int cmd_len, unsigned char * ret_val, int ret_len);
00131     int ComputeTickDiff(int from, int to);
00132     int ChangeMotorState(int state);
00133     int BytesToInt32(unsigned char *ptr);
00134     float BytesToFloat(unsigned char *ptr);
00135     void UpdateOdom(int ltics, int rtics);
00136     void SpeedCommand( unsigned char address, double speed, int dir );
00137     //periodic functions
00138     int GetOdom(int *ltics, int *rtics, int *lvel, int *rvel);
00139     int GetBatteryVoltage(int* voltage);
00140         int GetRangeSensor( int s, float * val );
00141         void MotorSpeed();
00142 
00143     //er values
00144     double _axle_length;
00145     int _motor_0_dir;
00146     int _motor_1_dir;
00147 
00148     //internal info
00149         bool _debug;
00150         bool _need_to_set_speed;
00151     bool _odom_initialized;
00152         bool _stopped;
00153     int _last_ltics, _last_rtics;
00154     double _px, _py, _pa;  // integrated odometric pose (m,m,rad)
00155     
00156     int send_command( unsigned char address, unsigned char c, int ret_num, unsigned char * ret );
00157     int send_command_2_arg( unsigned char address, unsigned char c, int arg, int ret_num, unsigned char * ret );
00158     int send_command_4_arg( unsigned char address, unsigned char c, int arg, int ret_num, unsigned char * ret );
00159 
00160 };
00161 #endif

Last updated 12 September 2005 21:38:45