clodbuster.h
00001 /*
00002  *  Player - One Hell of a Robot Server
00003  *  Copyright (C) 2000
00004  *     Brian Gerkey, Kasper Stoy, Richard Vaughan, & Andrew Howard
00005  *
00006  *
00007  *  This program is free software; you can redistribute it and/or modify
00008  *  it under the terms of the GNU General Public License as published by
00009  *  the Free Software Foundation; either version 2 of the License, or
00010  *  (at your option) any later version.
00011  *
00012  *  This program is distributed in the hope that it will be useful,
00013  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00014  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00015  *  GNU General Public License for more details.
00016  *
00017  *  You should have received a copy of the GNU General Public License
00018  *  along with this program; if not, write to the Free Software
00019  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00020  *
00021  */
00022 
00023 /*
00024  * $Id: clodbuster.h 7208 2008-12-19 02:10:56Z gbiggs $
00025  *
00026  *   the clodbuster device.   there's a thread here that
00027  *   actually interacts with grasp board via the serial line.  the other
00028  *   "devices" communicate with this thread by putting into and getting
00029  *   data out of shared buffers.
00030  */
00031 #ifndef _CLODBUSTERDEVICE_H
00032 #define _CLODBUSTERDEVICE_H
00033 
00034 #include <pthread.h>
00035 #include <sys/time.h>
00036 
00037 #include <libplayercore/playercore.h>
00038 
00039 /* data for the clodbuster */
00040 #define CLODBUSTER_CYCLETIME_USEC 50000
00041 
00042 /* Grasp Board Command numbers */
00043 #define SYNC 255
00044 
00045 #define SET_SERVO_THROTTLE 0
00046 #define SET_SERVO_FRONTSTEER 1
00047 #define SET_SERVO_BACKSTEER 2
00048 #define SET_SERVO_PAN 3
00049 
00050 #define ECHO_SERVO_VALUES 64 // 0x40
00051 #define ECHO_MAX_SERVO_LIMITS 65 //0x41
00052 #define ECHO_MIN_SERVO_LIMITS 66 //0x42
00053 #define ECHO_CEN_SERVO_LIMITS 67 //0x43
00054 
00055 #define ECHO_ENCODER_COUNTS 112 // 0x70
00056 #define ECHO_ENCODER_COUNTS_TS 113 // 0x71
00057 #define ECHO_ADC 128 //0x80
00058 #define READ_ID 97
00059 
00060 #define SET_SLEEP_MODE 144 // 0x90
00061 #define ECHO_SLEEP_MODE 145 // 0x91
00062 
00063 #define SLEEP_MODE_ON 1
00064 #define SLEEP_MODE_OFF 0
00065 
00066 #define SERVO_CHANNELS 8
00067 
00068 // I think this might be useful.. so leaving it in for the time being
00069 /* Argument types */
00070 #define ARGINT          0x3B    // Positive int (LSB, MSB)
00071 #define ARGNINT         0x1B    // Negative int (LSB, MSB)
00072 #define ARGSTR          0x2B    // String (Note: 1st byte is length!!)
00073 
00074 #define CLODBUSTER_CONFIG_BUFFER_SIZE 256
00075 
00076 #define DEFAULT_CLODBUSTER_PORT "/dev/ttyUSB0" // This has to be USB - serial port.
00077 
00078 typedef struct clodbuster_encoder_data
00079 {
00080   uint32_t time_count;
00081   int32_t left,right;
00082 } __attribute__ ((packed)) clodbuster_encoder_data_t;
00083 
00084 class PIDGains
00085 {
00086  private:
00087      float kp, ki, kd, freq, k1, k2, k3;
00088      void findK()
00089           {
00090                float T=1.0/freq;
00091                k1 = kp + .5*T*ki + kd/T;
00092                k2 = -kp - 2.0*kd/T + .5*ki*T;
00093                k3 = kd/T;
00094                printf("Gain constants set to K1 = %f, K2 = %f, K3 = %f\n",k1,k2,k3);
00095           };
00096  public:
00097      PIDGains(float kp_, float ki_, float kd_, float freq_)
00098           :kp(kp_), ki(ki_), kd(kd_),freq(freq_)
00099           {
00100                findK();
00101           };
00102      //~PIDGains();
00103      void SetKp(float k)
00104           {
00105                kp=k;
00106                findK();
00107           };
00108      void SetKi(float k)
00109           {
00110                ki=k;
00111                findK();
00112           };
00113      void SetKd(float k)
00114           {
00115                kd=k;
00116                findK();
00117           };
00118      void SetFreq(float f)
00119           {
00120                freq=f;
00121                findK();
00122           };
00123      float K1(){return(k1);};
00124      float K2(){return(k2);};
00125      float K3(){return(k3);};
00126 };
00127 
00128 class ClodBuster:public ThreadedDriver
00129 {
00130   private:
00131     player_position2d_data_t position_data;
00132     void ResetRawPositions();
00133     clodbuster_encoder_data_t ReadEncoders();
00134 
00135     int clodbuster_fd;               // clodbuster device file descriptor
00136 
00137     // device used to communicate with GRASP IO Board
00138     char clodbuster_serial_port[MAX_FILENAME_SIZE];
00139 
00140     int kp,ki,kd;
00141 
00142     // did we initialize the common data segments yet?
00143     bool initdone;
00144     clodbuster_encoder_data_t encoder_offset;
00145     clodbuster_encoder_data_t encoder_measurement;
00146     clodbuster_encoder_data_t old_encoder_measurement;
00147     float EncV, EncOmega, EncVleft, EncVright;
00148 
00149     bool direct_command_control;
00150     unsigned char max_limits[SERVO_CHANNELS],center_limits[SERVO_CHANNELS],min_limits[SERVO_CHANNELS];
00151     void GetGraspBoardParams();
00152 
00153     // CB geometry parameters
00154     float WheelRadius;
00155     float WheelBase;
00156     float WheelSeparation;
00157     unsigned int CountsPerRev;
00158     float Kenc; // counts --> distance traveled
00159 
00160     // control parameters
00161     float LoopFreq;
00162     PIDGains *Kv, *Kw;
00163 
00164     void IntegrateEncoders();
00165     void DifferenceEncoders();
00166 
00167  protected:
00168 
00169     // Max motor speeds
00170     int motor_max_speed;
00171     int motor_max_turnspeed;
00172 
00173     // Bound the command velocities
00174     bool use_vel_band;
00175 
00176   short speedDemand, turnRateDemand;
00177   bool newmotorspeed, newmotorturn;
00178 
00179   public:
00180 
00181     ClodBuster( ConfigFile* cf, int section);
00182     virtual ~ClodBuster();
00183 
00184     /* the main thread */
00185     virtual void Main();
00186 
00187     // Process incoming messages from clients
00188     int ProcessMessage (QueuePointer &resp_queue, player_msghdr * hdr, void * data);
00189 
00190     virtual int MainSetup();
00191     virtual void MainQuit();
00192 
00193     unsigned char SetServo(unsigned char chan, int value);
00194     void SetServo(unsigned char chan, unsigned char cmd);
00195     /*
00196     void CMUcamReset();
00197     void CMUcamTrack(int rmin=0, int rmax=0, int gmin=0,
00198                           int gmax=0, int bmin=0, int bmax=0);
00199     void CMUcamStopTracking();
00200 
00201     */ // don't want to have this right now.. but maybe eventually.
00202 };
00203 
00204 
00205 #endif