er.h
00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
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
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
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
00071
00072
00073
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
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
00112 virtual int ProcessMessage(QueuePointer & resp_queue,
00113 player_msghdr * hdr,
00114 void * data);
00115 void Test();
00116
00117 private:
00118
00119 int InitOdom();
00120 int InitRobot();
00121
00122
00123 int *_tc_num;
00124 int _fd;
00125 const char* _serial_port;
00126 bool _fd_blocking;
00127
00128
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
00143 int GetOdom(int *ltics, int *rtics);
00144 int GetBatteryVoltage(int* voltage);
00145 int GetRangeSensor( int s, float * val );
00146 void MotorSpeed();
00147
00148
00149 double _axle_length;
00150 int _motor_0_dir;
00151 int _motor_1_dir;
00152
00153
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;
00160 int _powered, _resting;
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