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 .00000602836879
00070
00071
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
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
00106
00107
00108
00109
00110 virtual int ProcessMessage(MessageQueue * resp_queue,
00111 player_msghdr * hdr,
00112 void * data);
00113
00114
00115
00116 private:
00117
00118 int InitOdom();
00119 int InitRobot();
00120
00121
00122 int *_tc_num;
00123 int _fd;
00124 const char* _serial_port;
00125 bool _fd_blocking;
00126
00127
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
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
00144 double _axle_length;
00145 int _motor_0_dir;
00146 int _motor_1_dir;
00147
00148
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;
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
|