khepera.h
1/*
2 * Player - One Hell of a Robot Server
3 * Copyright (C) 2000
4 * Brian Gerkey, Kasper Stoy, Richard Vaughan, & Andrew Howard
5 *
6 *
7 * This program is free software; you can redistribute it and/or modify
8 * it under the terms of the GNU General Public License as published by
9 * the Free Software Foundation; either version 2 of the License, or
10 * (at your option) any later version.
11 *
12 * This program is distributed in the hope that it will be useful,
13 * but WITHOUT ANY WARRANTY; without even the implied warranty of
14 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15 * GNU General Public License for more details.
16 *
17 * You should have received a copy of the GNU General Public License
18 * along with this program; if not, write to the Free Software
19 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
20 *
21 */
22
23/* Copyright (C) 2004
24 * Toby Collett, University of Auckland Robotics Group
25 *
26 * Header for the khepera robot. The
27 * architecture is similar to the P2OS device, in that the position, IR and
28 * power services all need to go through a single serial port and
29 * base device class. So this code was copied from p2osdevice and
30 * modified to taste.
31 *
32 */
33
34#ifndef _KHEPERADEVICE_H
35#define _KHEPERADEVICE_H
36
37#include <pthread.h>
38#include <sys/time.h>
39#include <errno.h>
40
41#include <libplayercore/playercore.h>
42//#include <khepera_params.h>
43#include "khepera_serial.h"
44
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)
52
53#define KHEPERA_MOTOR_LEFT 0
54#define KHEPERA_MOTOR_RIGHT 1
55
56#define KHEPERA_FIXED_FACTOR 10000
57
58#define CRLF "\r\n"
59#define KHEPERA_COMMAND_PROMPT "\r\n"
60
61#ifndef ABS
62#define ABS(x) ((x) < 0 ? -(x) : (x))
63#endif
64
65#ifndef SGN
66#define SGN(x) ((x) < 0 ? -1 : 1)
67#endif
68
69typedef struct player_khepera_geom
70{
71 char * PortName;
72 double scale;
73 player_ir_pose_t ir;
74 double * ir_calib_a;
75 double * ir_calib_b;
76 player_position2d_geom_t position;
77 double encoder_res;
78} __attribute__ ((packed)) player_khepera_geom_t;
79
80
81class Khepera : public ThreadedDriver
82{
83public:
84
85 Khepera(ConfigFile *cf, int section);
86 ~Khepera();
87
88 /* the main thread */
89 virtual void Main();
90
91 virtual int Subscribe(player_devaddr_t addr);
92 virtual int Unsubscribe(player_devaddr_t addr);
93
94 virtual int MainSetup();
95 virtual void MainQuit();
96
97 int ResetOdometry();
98
99 // handle IR
100 void SetIRState(int);
101
102 void UpdateData(void);
103
104 void UpdateIRData(player_ir_data_t *);
105 void UpdatePosData(player_position2d_data_t *);
106
107 // the following are all interface functions to the REB
108 // this handles the A/D device which deals with IR for us
109 //void ConfigAD(int, int);
110 unsigned short ReadAD(int);
111 int ReadAllIR(player_ir_data_t *);
112
113 // this handles motor control
114 int SetSpeed(int, int);
115 int ReadSpeed(int*, int*);
116
117 int SetPos(int, int);
118
119 int SetPosCounter(int, int);
120 int ReadPos(int *, int*);
121
122 //unsigned char ReadStatus(int, int *, int *);
123
124 // MessageHandler
125 int ProcessMessage(QueuePointer & resp_queue, player_msghdr * hdr, void * data);
126
127private:
128 player_devaddr_t ir_addr;
129 player_devaddr_t position_addr;
130 int position_subscriptions;
131 int ir_subscriptions;
132
133 KheperaSerial * Serial;
134
135 player_khepera_geom_t* geometry;
136
137 int param_index; // index in the RobotParams table for this robot
138 int khepera_fd; // khepera device file descriptor
139
140 struct timeval last_position; // last position update
141 bool refresh_last_position;
142 int last_lpos, last_rpos;
143 double x,y,yaw;
144 int last_x_f, last_y_f;
145 double last_theta;
146
147 struct timeval last_pos_update; // time of last pos update
148 struct timeval last_ir_update;
149
150 int pos_update_period;
151
152 short desired_heading;
153
154 int ir_sequence;
155 struct timeval last_ir;
156
157 bool motors_enabled;
158 bool velocity_mode;
159 bool direct_velocity_control;
160
161 // device used to communicate with reb
162 char khepera_serial_port[MAX_FILENAME_SIZE];
163
164 //struct pollfd write_pfd, read_pfd;
165
166
167};
168
169
170#endif
Class for loading configuration file information.
Definition configfile.h:197
Definition khepera_serial.h:34
Definition khepera.h:82
virtual int Subscribe(player_devaddr_t addr)
Subscribe to this driver.
Definition khepera.cc:352
virtual int MainSetup()
Sets up the resources needed by the driver thread.
Definition khepera.cc:402
int ProcessMessage(QueuePointer &resp_queue, player_msghdr *hdr, void *data)
Message handler.
Definition khepera.cc:162
virtual int Unsubscribe(player_devaddr_t addr)
Unsubscribe from this driver.
Definition khepera.cc:375
virtual void MainQuit()
Cleanup method for driver thread (called when main exits)
Definition khepera.cc:426
virtual void Main()
Main method for driver thread.
Definition khepera.cc:442
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
A device address.
Definition player.h:146
Definition khepera.h:70
Generic message header.
Definition player.h:162