31 #ifndef _ER1_DRIVER_H_
32 #define _ER1_DRIVER_H_
34 #define ER_DEFAULT_PORT "/dev/usb/tts/0"
35 #define ER_DEFAULT_LEFT_MOTOR 0
36 #define ER_DELAY_US 10000
44 #define GETCMDPOS 0x1D
45 #define GETEVENTSTATUS 0x31
46 #define RESETEVENTSTATUS 0x34
48 #define SETMOTORCMD 0x77
49 #define SETLIMITSWITCHMODE 0x80
50 #define GETVERSION 0x8F
53 #define SETPROFILEMODE 0xA0
54 #define SETSTOPMODE 0xD0
55 #define READANALOG 0xEF
61 #define ER_DEFAULT_MOTOR_0_DIR -1
62 #define ER_DEFAULT_MOTOR_1_DIR 1
63 #define ER_DEFAULT_AXLE_LENGTH .38
65 #define ER_MAX_TICKS 3000
66 #define ER_WHEEL_RADIUS .055
67 #define ER_WHEEL_CIRC .345575197
68 #define ER_WHEEL_STEP .45
69 #define ER_M_PER_TICK .0000058351 //JM 12/11/06
74 #define ER_MAX_WHEELSPEED .500
75 #define ER_MPS_PER_TICK 1
81 #include <libplayerinterface/player.h>
82 #include <libplayercore/driver.h>
83 #include <libplayercore/drivertable.h>
88 player_position2d_data_t position;
96 player_er1_data_t er1_data;
99 int position_subscriptions;
105 int SetVelocity(
double lvel,
double rvel);
106 void Stop(
int StopMode );
125 const char* _serial_port;
129 int WriteBuf(
unsigned char* s,
size_t len);
130 int ReadBuf(
unsigned char* s,
size_t len);
131 int SendCommand(
unsigned char * cmd,
int cmd_len,
unsigned char * ret_val,
int ret_len);
132 int checksum_ok (
unsigned char *buf,
int len);
134 int ComputeTickDiff(
int from,
int to);
135 int ChangeMotorState(
int state);
136 int BytesToInt32(
unsigned char *ptr);
137 float BytesToFloat(
unsigned char *ptr);
138 void UpdateOdom(
int ltics,
int rtics);
139 void SpeedCommand(
unsigned char address,
double speed,
int dir );
140 void SetOdometry (
long,
long,
long);
141 void ResetOdometry();
143 int GetOdom(
int *ltics,
int *rtics);
144 int GetBatteryVoltage(
int* voltage);
145 int GetRangeSensor(
int s,
float * val );
155 bool _need_to_set_speed;
156 bool _odom_initialized;
158 int _last_ltics, _last_rtics;
159 double _px, _py, _pa;
160 int _powered, _resting;
162 int send_command(
unsigned char address,
unsigned char c,
int ret_num,
unsigned char * ret );
163 int send_command_2_arg(
unsigned char address,
unsigned char c,
int arg,
int ret_num,
unsigned char * ret );
164 int send_command_4_arg(
unsigned char address,
unsigned char c,
int arg,
int ret_num,
unsigned char * ret );
virtual void Publish(player_devaddr_t addr, QueuePointer &queue, uint8_t type, uint8_t subtype, void *src=NULL, size_t deprecated=0, double *timestamp=NULL, bool copy=true)
Publish a message via one of this driver's interfaces.
static bool MatchMessage(player_msghdr_t *hdr, int type, int subtype, player_devaddr_t addr)
Helper for message processing.
Definition: message.h:159
double ReadFloat(int section, const char *name, double value)
Read a floating point (double) value.
Generic message header.
Definition: player.h:162
virtual void MainQuit()
Cleanup method for driver thread (called when main exits)
Definition: er.cc:495
virtual int MainSetup()
Sets up the resources needed by the driver thread.
Definition: er.cc:409
const char * ReadString(int section, const char *name, const char *value)
Read a string value.
virtual void Main()
Main method for driver thread.
Definition: er.cc:741
uint8_t subtype
Message subtype; interface specific.
Definition: player.h:168
int ReadInt(int section, const char *name, int value)
Read an integer value.
void ProcessMessages(void)
Process pending messages.
#define PLAYER_MSGTYPE_DATA
A data message.
Definition: player.h:95
#define PLAYER_MSGTYPE_RESP_ACK
A positive response message.
Definition: player.h:112
#define PLAYER_MSGTYPE_REQ
A request message.
Definition: player.h:106
#define PLAYER_MSGTYPE_RESP_NACK
A negative response message.
Definition: player.h:125
int ReadDeviceAddr(player_devaddr_t *addr, int section, const char *name, int code, int index, const char *key)
Read a device id.
virtual int ProcessMessage(QueuePointer &resp_queue, player_msghdr *hdr, void *data)
Message handler.
Definition: er.cc:784
Class for loading configuration file information.
Definition: configfile.h:197
A device address.
Definition: player.h:146
An autopointer for the message queue.
Definition: message.h:74
Base class for drivers which oeprate with a thread.
Definition: driver.h:553
Messages between wsn and a robot.
Definition: er.h:87
#define PLAYER_MSGTYPE_CMD
A command message.
Definition: player.h:99
Base class for all drivers.
Definition: driver.h:109