sip.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: sip.h 4356 2008-02-15 08:53:55Z thjc $
00025  *
00026  * part of the P2OS parser.  methods for filling and parsing server
00027  * information packets (SIPs)
00028  */
00029 #ifndef _SIP_H
00030 #define _SIP_H
00031 
00032 #include <limits.h>
00033 
00034 #include <p2os.h>
00035 
00036 typedef struct ArmJoint
00037 {
00038         char speed;
00039         unsigned char home;
00040         unsigned char min;
00041         unsigned char centre;
00042         unsigned char max;
00043         unsigned char ticksPer90;
00044 } ArmJoint;
00045 
00046 class SIP
00047 {
00048  private:
00049   int PositionChange( unsigned short, unsigned short );
00050   int param_idx; // index of our robot's data in the parameter table
00051 
00052  public:
00053   // these values are returned in every standard SIP
00054   bool lwstall, rwstall;
00055   unsigned char status, battery, sonarreadings, analog, digin, digout;
00056   unsigned short ptu, compass, timer, rawxpos;
00057   unsigned short rawypos, frontbumpers, rearbumpers;
00058   short angle, lvel, rvel, control;
00059   unsigned short *sonars;
00060   int xpos, ypos;
00061   int x_offset,y_offset,angle_offset;
00062 
00063   // these values are returned in a CMUcam serial string extended SIP
00064   // (in host byte-order)
00065   unsigned short blobmx, blobmy;        // Centroid
00066   unsigned short blobx1, blobx2, bloby1, bloby2;        // Bounding box
00067   unsigned short blobarea, blobconf;    // Area and confidence
00068   unsigned int   blobcolor;
00069 
00070   // This value is filled by ParseGyro()
00071   int32_t gyro_rate;
00072 
00073   // This information comes from the ARMpac and ARMINFOpac packets
00074   bool armPowerOn, armConnected;
00075   bool armJointMoving[6];
00076   unsigned char armJointPos[6];
00077   double armJointPosRads[6];
00078   unsigned char armJointTargetPos[6];
00079   char *armVersionString;
00080   unsigned char armNumJoints;
00081   ArmJoint *armJoints;
00082 
00083   // Need this value to calculate approx position of lift when in between up and down
00084   double lastLiftPos;
00085   
00086   //Timestamping SIP packets
00087   //double timeStandardSIP, timeGyro, timeSERAUX, timeArm;
00088 
00089   /* returns 0 if Parsed correctly otherwise 1 */
00090   void ParseStandard( unsigned char *buffer );
00091   void ParseSERAUX( unsigned char *buffer );
00092   void ParseGyro(unsigned char* buffer);
00093   void ParseArm (unsigned char *buffer);
00094   void ParseArmInfo (unsigned char *buffer);
00095   void Print();
00096   void PrintSonars();
00097   void PrintArm ();
00098   void PrintArmInfo ();
00099   void FillStandard(player_p2os_data_t* data);
00100   void FillSERAUX(player_p2os_data_t* data);
00101   void FillGyro(player_p2os_data_t* data);
00102   void FillArm(player_p2os_data_t* data);
00103 
00104   SIP(int idx)
00105   {
00106     param_idx = idx;
00107     sonarreadings = 0;
00108     sonars = NULL;
00109 
00110     xpos = INT_MAX;
00111     ypos = INT_MAX;
00112 
00113     // intialise some of the internal values
00114     blobmx = blobmy = blobx1 = blobx2 = bloby1 = bloby2 = blobarea = blobconf = blobcolor = 0;
00115     armPowerOn = armConnected = false;
00116     armVersionString = NULL;
00117     armJoints = NULL;
00118     armNumJoints = 0;
00119     for (int i = 0; i < 6; ++i)
00120     {
00121       armJointMoving[i] = false;
00122       armJointPos[i] = 0;
00123       armJointPosRads[i] = 0;
00124       armJointTargetPos[i] = 0;
00125     }
00126   }
00127 
00128   ~SIP(void)
00129   {
00130     delete[] sonars;
00131   }
00132 };
00133 
00134 #endif
Last updated 12 September 2005 21:38:45