34 #ifndef _KHEPERADEVICE_H
35 #define _KHEPERADEVICE_H
41 #include <libplayercore/playercore.h>
43 #include "khepera_serial.h"
45 #define KHEPERA_CONFIG_BUFFER_SIZE 1024
46 #define KHEPERA_BAUDRATE B38400
47 #define KHEPERA_DEFAULT_SERIAL_PORT "/dev/ttyUSB0"
48 #define KHEPERA_DEFAULT_SCALE 10
49 #define KHEPERA_DEFAULT_ENCODER_RES (1.0/12.0)
50 #define KHEPERA_DEFAULT_IR_CALIB_A (64.158)
51 #define KHEPERA_DEFAULT_IR_CALIB_B (-0.1238)
53 #define KHEPERA_MOTOR_LEFT 0
54 #define KHEPERA_MOTOR_RIGHT 1
56 #define KHEPERA_FIXED_FACTOR 10000
59 #define KHEPERA_COMMAND_PROMPT "\r\n"
62 #define ABS(x) ((x) < 0 ? -(x) : (x))
66 #define SGN(x) ((x) < 0 ? -1 : 1)
76 player_position2d_geom_t position;
100 void SetIRState(
int);
102 void UpdateData(
void);
104 void UpdateIRData(player_ir_data_t *);
105 void UpdatePosData(player_position2d_data_t *);
110 unsigned short ReadAD(
int);
111 int ReadAllIR(player_ir_data_t *);
114 int SetSpeed(
int,
int);
115 int ReadSpeed(
int*,
int*);
117 int SetPos(
int,
int);
119 int SetPosCounter(
int,
int);
120 int ReadPos(
int *,
int*);
130 int position_subscriptions;
131 int ir_subscriptions;
135 player_khepera_geom_t* geometry;
140 struct timeval last_position;
141 bool refresh_last_position;
142 int last_lpos, last_rpos;
144 int last_x_f, last_y_f;
147 struct timeval last_pos_update;
148 struct timeval last_ir_update;
150 int pos_update_period;
152 short desired_heading;
155 struct timeval last_ir;
159 bool direct_velocity_control;
162 char khepera_serial_port[MAX_FILENAME_SIZE];
virtual int Subscribe(player_devaddr_t addr)
Subscribe to this driver.
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.
uint32_t host
The "host" on which the device resides.
Definition: player.h:148
static bool MatchMessage(player_msghdr_t *hdr, int type, int subtype, player_devaddr_t addr)
Helper for message processing.
Definition: message.h:159
virtual void Main()
Main method for driver thread.
Definition: khepera.cc:442
virtual void MainQuit()
Cleanup method for driver thread (called when main exits)
Definition: khepera.cc:426
double ReadFloat(int section, const char *name, double value)
Read a floating point (double) value.
int AddInterface(player_devaddr_t addr)
Add an interface.
A pose in the plane.
Definition: player.h:218
Generic message header.
Definition: player.h:162
virtual int MainSetup(void)
Sets up the resources needed by the driver thread.
Definition: driver.h:658
virtual void MainQuit(void)
Cleanup method for driver thread (called when main exits)
Definition: driver.h:664
uint8_t type
Message type; must be one of PLAYER_MSGTYPE_*.
Definition: player.h:166
Encapsulates a device (i.e., a driver bound to an interface)
Definition: device.h:75
const char * ReadString(int section, const char *name, const char *value)
Read a string value.
virtual int Subscribe(player_devaddr_t addr)
Subscribe to this driver.
Definition: khepera.cc:352
Definition: khepera_serial.h:34
uint8_t subtype
Message subtype; interface specific.
Definition: player.h:168
uint32_t robot
The "robot" or device collection in which the device resides.
Definition: player.h:151
double ReadTupleFloat(int section, const char *name, int index, double value)
Read a float (double) from a tuple field.
virtual void Main(void)=0
Main method for driver thread.
const char * ReadTupleString(int section, const char *name, int index, const char *value)
Read a string from a tuple field.
int ReadInt(int section, const char *name, int value)
Read an integer value.
void * GetPayload()
Get pointer to payload.
Definition: message.h:188
void ProcessMessages(void)
Process pending messages.
Available interfaces are stored in an array of these, defined in interface_util.c.
Definition: interface_util.h:72
#define PLAYER_MSGTYPE_DATA
A data message.
Definition: player.h:95
#define PLAYER_ERROR2(msg, a, b)
Definition: error.h:83
virtual int MainSetup()
Sets up the resources needed by the driver thread.
Definition: khepera.cc:402
#define PLAYER_MSGTYPE_RESP_ACK
A positive response message.
Definition: player.h:112
virtual int ProcessMessage(QueuePointer &resp_queue, player_msghdr *hdr, void *data)
Message handler.
int ProcessMessage(QueuePointer &resp_queue, player_msghdr *hdr, void *data)
Message handler.
Definition: khepera.cc:162
#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.
int GetTupleCount(int section, const char *name)
Get the number of values in a tuple.
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
void SetError(int code)
Set/reset error code.
Definition: driver.h:145
#define PLAYER_ERROR1(msg, a)
Definition: error.h:82
A pose in space.
Definition: player.h:229
virtual int Unsubscribe(player_devaddr_t addr)
Unsubscribe from this driver.
Definition: khepera.cc:375
#define PLAYER_ERROR(msg)
Definition: error.h:81
Base class for drivers which oeprate with a thread.
Definition: driver.h:553
Messages between wsn and a robot.
Definition: er.h:87
double timestamp
Time associated with message contents (seconds since epoch)
Definition: player.h:170
uint32_t size
Size in bytes of the payload to follow.
Definition: player.h:174
Reference-counted message objects.
Definition: message.h:133
#define PLAYER_MSGTYPE_CMD
A command message.
Definition: player.h:99
Base class for all drivers.
Definition: driver.h:109
int Unsubscribe(QueuePointer &sub_queue)
Unsubscribe the given queue from this device.
virtual int Unsubscribe(player_devaddr_t addr)
Unsubscribe from this driver.
uint16_t index
Which device of that interface.
Definition: player.h:155
player_devaddr_t addr
Device to which this message pertains.
Definition: player.h:164
uint16_t interf
The interface provided by the device; must be one of PLAYER_*_CODE.
Definition: player.h:153
#define PLAYER_MSGQUEUE_DEFAULT_MAXLEN
Default maximum length for a message queue.
Definition: player.h:76