p2os/sip.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 * part of the P2OS parser. methods for filling and parsing server
27 * information packets (SIPs)
28 */
29#ifndef _SIP_H
30#define _SIP_H
31
32#include <limits.h>
33
34#include "p2os.h"
35
36typedef struct ArmJoint
37{
38 char speed;
39 unsigned char home;
40 unsigned char min;
41 unsigned char centre;
42 unsigned char max;
43 unsigned char ticksPer90;
44} ArmJoint;
45
46class SIP
47{
48 private:
49 int PositionChange( unsigned short, unsigned short );
50 int param_idx; // index of our robot's data in the parameter table
51
52 public:
53 // these values are returned in every standard SIP
54 bool lwstall, rwstall;
55 unsigned char status, battery, sonarreadings, analog, digin, digout;
56 unsigned short ptu, compass, timer, rawxpos;
57 unsigned short rawypos, frontbumpers, rearbumpers;
58 short angle, lvel, rvel, control;
59 unsigned short *sonars;
60 int xpos, ypos;
61 int x_offset,y_offset,angle_offset;
62
63 // these values are returned in a CMUcam serial string extended SIP
64 // (in host byte-order)
65 unsigned short blobmx, blobmy; // Centroid
66 unsigned short blobx1, blobx2, bloby1, bloby2; // Bounding box
67 unsigned short blobarea, blobconf; // Area and confidence
68 unsigned int blobcolor;
69
70 // This value is filled by ParseGyro()
71 int32_t gyro_rate;
72
73 // This information comes from the ARMpac and ARMINFOpac packets
74 bool armPowerOn, armConnected;
75 bool armJointMoving[6];
76 unsigned char armJointPos[6];
77 double armJointPosRads[6];
78 unsigned char armJointTargetPos[6];
79 char *armVersionString;
80 unsigned char armNumJoints;
81 ArmJoint *armJoints;
82
83 // Need this value to calculate approx position of lift when in between up and down
84 double lastLiftPos;
85
86 //Timestamping SIP packets
87 //double timeStandardSIP, timeGyro, timeSERAUX, timeArm;
88
89 /* returns 0 if Parsed correctly otherwise 1 */
90 void ParseStandard( unsigned char *buffer );
91 void ParseSERAUX( unsigned char *buffer );
92 void ParseGyro(unsigned char* buffer);
93 void ParseArm (unsigned char *buffer);
94 void ParseArmInfo (unsigned char *buffer);
95 void Print();
96 void PrintSonars();
97 void PrintArm ();
98 void PrintArmInfo ();
99 void FillStandard(player_p2os_data_t* data);
100 void FillSERAUX(player_p2os_data_t* data);
101 void FillGyro(player_p2os_data_t* data);
102 void FillArm(player_p2os_data_t* data);
103
104 SIP(int idx)
105 {
106 param_idx = idx;
107 sonarreadings = 0;
108 sonars = NULL;
109
110 xpos = INT_MAX;
111 ypos = INT_MAX;
112
113 // intialise some of the internal values
114 blobmx = blobmy = blobx1 = blobx2 = bloby1 = bloby2 = blobarea = blobconf = blobcolor = 0;
115 armPowerOn = armConnected = false;
116 armVersionString = NULL;
117 armJoints = NULL;
118 armNumJoints = 0;
119 for (int i = 0; i < 6; ++i)
120 {
121 armJointMoving[i] = false;
122 armJointPos[i] = 0;
123 armJointPosRads[i] = 0;
124 armJointTargetPos[i] = 0;
125 }
126 }
127
128 ~SIP(void)
129 {
130 delete[] sonars;
131 }
132};
133
134#endif
Definition p2os/sip.h:47
void ParseSERAUX(unsigned char *buffer)
Parse a SERAUX SIP packet.
Definition p2os/sip.cc:488
Definition p2os/sip.h:37