00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024 #ifndef _SIP_H
00025 #define _SIP_H
00026
00027 #include <limits.h>
00028 #include <stdint.h>
00029 #include <p2os_driver/p2os.h>
00030
00031 typedef struct ArmJoint
00032 {
00033 char speed;
00034 unsigned char home;
00035 unsigned char min;
00036 unsigned char centre;
00037 unsigned char max;
00038 unsigned char ticksPer90;
00039 } ArmJoint;
00040
00041 enum PlayerGripperStates {
00042 PLAYER_GRIPPER_STATE_OPEN = 1,
00043 PLAYER_GRIPPER_STATE_CLOSED,
00044 PLAYER_GRIPPER_STATE_MOVING,
00045 PLAYER_GRIPPER_STATE_ERROR
00046 };
00047
00048 enum PlayerActArrayStates {
00049 PLAYER_ACTARRAY_ACTSTATE_IDLE = 1,
00050 PLAYER_ACTARRAY_ACTSTATE_MOVING,
00051 PLAYER_ACTARRAY_ACTSTATE_STALLED
00052 };
00053
00054 class SIP
00055 {
00056 private:
00057 int PositionChange( unsigned short, unsigned short );
00058 int param_idx;
00059
00060 public:
00061
00062 bool lwstall, rwstall;
00063 unsigned char motors_enabled, sonar_flag;
00064 unsigned char status, battery, sonarreadings, analog, digin, digout;
00065 unsigned short ptu, compass, timer, rawxpos;
00066 unsigned short rawypos, frontbumpers, rearbumpers;
00067 short angle, lvel, rvel, control;
00068 unsigned short *sonars;
00069 int xpos, ypos;
00070 int x_offset,y_offset,angle_offset;
00071
00072
00073
00074 unsigned short blobmx, blobmy;
00075 unsigned short blobx1, blobx2, bloby1, bloby2;
00076 unsigned short blobarea, blobconf;
00077 unsigned int blobcolor;
00078
00079
00080 int32_t gyro_rate;
00081
00082
00083 bool armPowerOn, armConnected;
00084 bool armJointMoving[6];
00085 unsigned char armJointPos[6];
00086 double armJointPosRads[6];
00087 unsigned char armJointTargetPos[6];
00088 char *armVersionString;
00089 unsigned char armNumJoints;
00090 ArmJoint *armJoints;
00091
00092
00093
00094 double lastLiftPos;
00095
00096
00097
00098
00099
00100 void ParseStandard( unsigned char *buffer );
00101 void ParseSERAUX( unsigned char *buffer );
00102 void ParseGyro(unsigned char* buffer);
00103 void ParseArm (unsigned char *buffer);
00104 void ParseArmInfo (unsigned char *buffer);
00105 void Print();
00106 void PrintSonars();
00107 void PrintArm ();
00108 void PrintArmInfo ();
00109 void FillStandard(ros_p2os_data_t* data);
00110
00111
00112
00113
00114 SIP(int idx) :
00115 param_idx(idx), sonarreadings(0), sonars(NULL),
00116 xpos(0), ypos(0), x_offset(0), y_offset(0), angle_offset(0),
00117 blobmx(0), blobmy(0), blobx1(0), blobx2(0), bloby1(0), bloby2(0),
00118 blobarea(0), blobconf(0), blobcolor(0),
00119 armPowerOn(false), armConnected(false), armVersionString(NULL),
00120 armNumJoints(0), armJoints(NULL),
00121 lastLiftPos(0.0f)
00122 {
00123 for (int i = 0; i < 6; ++i)
00124 {
00125 armJointMoving[i] = false;
00126 armJointPos[i] = 0;
00127 armJointPosRads[i] = 0;
00128 armJointTargetPos[i] = 0;
00129 }
00130 }
00131
00132 ~SIP(void)
00133 {
00134 delete[] sonars;
00135 }
00136 };
00137
00138 #endif