sip.h
00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
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;
00051
00052 public:
00053
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[PLAYER_SONAR_MAX_SAMPLES];
00060 int xpos, ypos;
00061 int x_offset,y_offset,angle_offset;
00062
00063
00064
00065 unsigned short blobmx, blobmy;
00066 unsigned short blobx1, blobx2, bloby1, bloby2;
00067 unsigned short blobarea, blobconf;
00068 unsigned int blobcolor;
00069
00070
00071 int32_t gyro_rate;
00072
00073
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
00084 double lastLiftPos;
00085
00086
00087 void Parse( unsigned char *buffer );
00088 void ParseSERAUX( unsigned char *buffer );
00089 void ParseGyro(unsigned char* buffer);
00090 void ParseArm (unsigned char *buffer);
00091 void ParseArmInfo (unsigned char *buffer);
00092 void Print();
00093 void PrintSonars();
00094 void PrintArm ();
00095 void PrintArmInfo ();
00096 void Fill(player_p2os_data_t* data);
00097
00098 SIP(int idx)
00099 {
00100 param_idx = idx;
00101 for(int i=0;i<ARRAYSIZE(sonars);i++)
00102 sonars[i] = 0;
00103
00104 xpos = INT_MAX;
00105 ypos = INT_MAX;
00106
00107
00108 blobmx = blobmy = blobx1 = blobx2 = bloby1 = bloby2 = blobarea = blobconf = blobcolor = 0;
00109 armPowerOn = armConnected = false;
00110 armVersionString = NULL;
00111 armJoints = NULL;
00112 armNumJoints = 0;
00113 for (int i = 0; i < 6; ++i)
00114 {
00115 armJointMoving[i] = false;
00116 armJointPos[i] = 0;
00117 armJointPosRads[i] = 0;
00118 armJointTargetPos[i] = 0;
00119 }
00120 }
00121 };
00122
00123 #endif