clodbuster.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/*
24 * $Id$
25 *
26 * the clodbuster device. there's a thread here that
27 * actually interacts with grasp board via the serial line. the other
28 * "devices" communicate with this thread by putting into and getting
29 * data out of shared buffers.
30 */
31#ifndef _CLODBUSTERDEVICE_H
32#define _CLODBUSTERDEVICE_H
33
34#include <pthread.h>
35#include <sys/time.h>
36
37#include <libplayercore/playercore.h>
38
39/* data for the clodbuster */
40#define CLODBUSTER_CYCLETIME_USEC 50000
41
42/* Grasp Board Command numbers */
43#define SYNC 255
44
45#define SET_SERVO_THROTTLE 0
46#define SET_SERVO_FRONTSTEER 1
47#define SET_SERVO_BACKSTEER 2
48#define SET_SERVO_PAN 3
49
50#define ECHO_SERVO_VALUES 64 // 0x40
51#define ECHO_MAX_SERVO_LIMITS 65 //0x41
52#define ECHO_MIN_SERVO_LIMITS 66 //0x42
53#define ECHO_CEN_SERVO_LIMITS 67 //0x43
54
55#define ECHO_ENCODER_COUNTS 112 // 0x70
56#define ECHO_ENCODER_COUNTS_TS 113 // 0x71
57#define ECHO_ADC 128 //0x80
58#define READ_ID 97
59
60#define SET_SLEEP_MODE 144 // 0x90
61#define ECHO_SLEEP_MODE 145 // 0x91
62
63#define SLEEP_MODE_ON 1
64#define SLEEP_MODE_OFF 0
65
66#define SERVO_CHANNELS 8
67
68// I think this might be useful.. so leaving it in for the time being
69/* Argument types */
70#define ARGINT 0x3B // Positive int (LSB, MSB)
71#define ARGNINT 0x1B // Negative int (LSB, MSB)
72#define ARGSTR 0x2B // String (Note: 1st byte is length!!)
73
74#define CLODBUSTER_CONFIG_BUFFER_SIZE 256
75
76#define DEFAULT_CLODBUSTER_PORT "/dev/ttyUSB0" // This has to be USB - serial port.
77
79{
80 uint32_t time_count;
81 int32_t left,right;
82} __attribute__ ((packed)) clodbuster_encoder_data_t;
83
85{
86 private:
87 float kp, ki, kd, freq, k1, k2, k3;
88 void findK()
89 {
90 float T=1.0/freq;
91 k1 = kp + .5*T*ki + kd/T;
92 k2 = -kp - 2.0*kd/T + .5*ki*T;
93 k3 = kd/T;
94 printf("Gain constants set to K1 = %f, K2 = %f, K3 = %f\n",k1,k2,k3);
95 };
96 public:
97 PIDGains(float kp_, float ki_, float kd_, float freq_)
98 :kp(kp_), ki(ki_), kd(kd_),freq(freq_)
99 {
100 findK();
101 };
102 //~PIDGains();
103 void SetKp(float k)
104 {
105 kp=k;
106 findK();
107 };
108 void SetKi(float k)
109 {
110 ki=k;
111 findK();
112 };
113 void SetKd(float k)
114 {
115 kd=k;
116 findK();
117 };
118 void SetFreq(float f)
119 {
120 freq=f;
121 findK();
122 };
123 float K1(){return(k1);};
124 float K2(){return(k2);};
125 float K3(){return(k3);};
126};
127
129{
130 private:
131 player_position2d_data_t position_data;
132 void ResetRawPositions();
133 clodbuster_encoder_data_t ReadEncoders();
134
135 int clodbuster_fd; // clodbuster device file descriptor
136
137 // device used to communicate with GRASP IO Board
138 char clodbuster_serial_port[MAX_FILENAME_SIZE];
139
140 int kp,ki,kd;
141
142 // did we initialize the common data segments yet?
143 bool initdone;
144 clodbuster_encoder_data_t encoder_offset;
145 clodbuster_encoder_data_t encoder_measurement;
146 clodbuster_encoder_data_t old_encoder_measurement;
147 float EncV, EncOmega, EncVleft, EncVright;
148
149 bool direct_command_control;
150 unsigned char max_limits[SERVO_CHANNELS],center_limits[SERVO_CHANNELS],min_limits[SERVO_CHANNELS];
151 void GetGraspBoardParams();
152
153 // CB geometry parameters
154 float WheelRadius;
155 float WheelBase;
156 float WheelSeparation;
157 unsigned int CountsPerRev;
158 float Kenc; // counts --> distance traveled
159
160 // control parameters
161 float LoopFreq;
162 PIDGains *Kv, *Kw;
163
164 void IntegrateEncoders();
165 void DifferenceEncoders();
166
167 protected:
168
169 // Max motor speeds
170 int motor_max_speed;
171 int motor_max_turnspeed;
172
173 // Bound the command velocities
174 bool use_vel_band;
175
176 short speedDemand, turnRateDemand;
177 bool newmotorspeed, newmotorturn;
178
179 public:
180
181 ClodBuster( ConfigFile* cf, int section);
182 virtual ~ClodBuster();
183
184 /* the main thread */
185 virtual void Main();
186
187 // Process incoming messages from clients
188 int ProcessMessage (QueuePointer &resp_queue, player_msghdr * hdr, void * data);
189
190 virtual int MainSetup();
191 virtual void MainQuit();
192
193 unsigned char SetServo(unsigned char chan, int value);
194 void SetServo(unsigned char chan, unsigned char cmd);
195 /*
196 void CMUcamReset();
197 void CMUcamTrack(int rmin=0, int rmax=0, int gmin=0,
198 int gmax=0, int bmin=0, int bmax=0);
199 void CMUcamStopTracking();
200
201 */ // don't want to have this right now.. but maybe eventually.
202};
203
204
205#endif
Definition clodbuster.h:129
virtual void Main()
Main method for driver thread.
Definition clodbuster.cc:352
virtual int MainSetup()
Sets up the resources needed by the driver thread.
Definition clodbuster.cc:144
int ProcessMessage(QueuePointer &resp_queue, player_msghdr *hdr, void *data)
Message handler.
Definition clodbuster.cc:243
virtual void MainQuit()
Cleanup method for driver thread (called when main exits)
Definition clodbuster.cc:227
Class for loading configuration file information.
Definition configfile.h:197
Definition clodbuster.h:85
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
Definition clodbuster.h:79
Generic message header.
Definition player.h:162