er.h
1/*
2 * Player - One Hell of a Robot Server
3 * Copyright (C) 2000-2003
4 * David Feil-Seifer
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/*
24 *
25 * Relevant constants for the "ER" robots, made by Evolution Robotics.
26 *
27 * Some of this code is borrowed and/or adapted from the player
28 * module of trogdor; thanks to the author of that module.
29 */
30
31#ifndef _ER1_DRIVER_H_
32#define _ER1_DRIVER_H_
33
34#define ER_DEFAULT_PORT "/dev/usb/tts/0"
35#define ER_DEFAULT_LEFT_MOTOR 0
36#define ER_DELAY_US 10000
37
38
39/* ER1 Commands */
40
41#define NOOP 0x00
42#define SETVEL 0x11
43#define UPDATE 0x1A
44#define GETCMDPOS 0x1D
45#define GETEVENTSTATUS 0x31
46#define RESETEVENTSTATUS 0x34
47#define RESET 0x39
48#define SETMOTORCMD 0x77
49#define SETLIMITSWITCHMODE 0x80
50#define GETVERSION 0x8F
51#define SETACCEL 0x90
52#define SETDECEL 0x91
53#define SETPROFILEMODE 0xA0
54#define SETSTOPMODE 0xD0
55#define READANALOG 0xEF
56
57#define MOTOR_0 0x00
58#define MOTOR_1 0x01
59
60/* robot-specific info */
61#define ER_DEFAULT_MOTOR_0_DIR -1
62#define ER_DEFAULT_MOTOR_1_DIR 1
63#define ER_DEFAULT_AXLE_LENGTH .38
64
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
70//#define ER_M_PER_TICK .00000602836879 // NdT 1/4/06
71//#define ER_M_PER_TICK .000005700 // THC 1/23/06
72
73/* for safety */
74#define ER_MAX_WHEELSPEED .500
75#define ER_MPS_PER_TICK 1
76
77#define FULL_STOP 0
78#define STOP 1
79
80
81#include <libplayerinterface/player.h>
82#include <libplayercore/driver.h>
83#include <libplayercore/drivertable.h>
84
85
86typedef struct
87{
88 player_position2d_data_t position;
89} __attribute__ ((packed)) player_er1_data_t;
90
91
92
93class ER : public ThreadedDriver
94{
95 private:
96 player_er1_data_t er1_data;
97
98 player_devaddr_t position_id;
99 int position_subscriptions;
100
101 public:
102 ER(ConfigFile* cf, int section);
103
104 // public, so that it can be called from pthread cleanup function
105 int SetVelocity(double lvel, double rvel);
106 void Stop( int StopMode );
107 virtual void Main();
108 virtual int MainSetup();
109 virtual void MainQuit();
110
111 // MessageHandler
112 virtual int ProcessMessage(QueuePointer & resp_queue,
113 player_msghdr * hdr,
114 void * data);
115 void Test();
116
117 private:
118 // this function will be run in a separate thread
119 int InitOdom();
120 int InitRobot();
121
122 //serial connection
123 int *_tc_num;
124 int _fd; // device file descriptor
125 const char* _serial_port; // name of dev file
126 bool _fd_blocking;
127
128 // methods for internal use
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);
133
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();
142 //periodic functions
143 int GetOdom(int *ltics, int *rtics);
144 int GetBatteryVoltage(int* voltage);
145 int GetRangeSensor( int s, float * val );
146 void MotorSpeed();
147
148 //er values
149 double _axle_length;
150 int _motor_0_dir;
151 int _motor_1_dir;
152
153 //internal info
154 bool _debug;
155 bool _need_to_set_speed;
156 bool _odom_initialized;
157 bool _stopped;
158 int _last_ltics, _last_rtics;
159 double _px, _py, _pa; // integrated odometric pose (m,m,rad)
160 int _powered, _resting; // If _powered is false, no constant updates. _resting means the last update was a 0,0 one.
161
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 );
165
166};
167#endif
168
Class for loading configuration file information.
Definition configfile.h:197
Definition er.h:94
virtual int ProcessMessage(QueuePointer &resp_queue, player_msghdr *hdr, void *data)
Message handler.
Definition er.cc:784
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
virtual void Main()
Main method for driver thread.
Definition er.cc:741
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
Generic message header.
Definition player.h:162