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 
86 typedef struct
87 {
88  player_position2d_data_t position;
89 } __attribute__ ((packed)) player_er1_data_t;
90 
91 
92 
93 class 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:196
Generic message header.
Definition: player.h:161
virtual void MainQuit()
Cleanup method for driver thread (called when main exits)
Definition: er.cc:495
virtual void Main()
Main method for driver thread.
Definition: er.cc:741
A device address.
Definition: player.h:145
Definition: er.h:93
Base class for drivers which oeprate with a thread.
Definition: driver.h:552
virtual int MainSetup()
Sets up the resources needed by the driver thread.
Definition: er.cc:409
Messages between wsn and a robot.
Definition: er.h:86
An autopointer for the message queue.
Definition: message.h:73
virtual int ProcessMessage(QueuePointer &resp_queue, player_msghdr *hdr, void *data)
Message handler.
Definition: er.cc:784
position2d data
Definition: player_interfaces.h:606