00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022 #ifndef P2OS_DRIVER__ROBOT_PARAMS_HPP_
00023 #define P2OS_DRIVER__ROBOT_PARAMS_HPP_
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058
00059
00060 #include <string>
00061
00062 void initialize_robot_params(void);
00063
00064
00065 #define PLAYER_NUM_ROBOT_TYPES 30
00066
00067
00068 #define MOTOR_DEF_MAX_SPEED 0.5
00069 #define MOTOR_DEF_MAX_TURNSPEED DTOR(100)
00070
00071
00072
00073
00074
00075
00076 #define P2OS_CYCLETIME_USEC 200000
00077
00078
00079
00080 #define P2OS_NOMINAL_VOLTAGE 12.0
00081
00082
00083 #define SYNC0 0
00084 #define SYNC1 1
00085 #define SYNC2 2
00086
00087 enum P2OSCommand
00088 {
00089 PULSE = 0,
00090 OPEN = 1,
00091 CLOSE = 2,
00092 ENABLE = 4,
00093 SETA = 5,
00094 SETV = 6,
00095 SETO = 7,
00096 MOVE = 8,
00097 ROTATE = 9,
00098 SETRV = 10,
00099 VEL = 11,
00100 HEAD = 12,
00101 DHEAD = 13,
00102
00103 SAY = 15,
00104
00105 JOYINFO = 17,
00106
00107 CONFIG = 18,
00108 ENCODER = 19,
00109 RVEL = 21,
00110 DCHEAD = 22,
00111 SETRA = 23,
00112 SONAR = 28,
00113 STOP = 29,
00114 DIGOUT = 30,
00115
00116 VEL2 = 32,
00117 GRIPPER = 33,
00118
00119 ADSEL = 35,
00120 GRIPPERVAL = 36,
00121 GRIPPERPACREQUEST = 37,
00122 IOREQUEST = 40,
00123 PTUPOS = 41,
00124 TTY2 = 42,
00125 GETAUX = 43,
00126 BUMP_STALL = 44,
00127 TCM2 = 45,
00128 JOYDRIVE = 47,
00129 MOVINGBLINK = 49,
00131 HOSTBAUD = 50,
00133 AUX1BAUD = 51,
00135 AUX2BAUD = 52,
00137 ESTOP = 55,
00138 ESTALL = 56,
00139 GYRO = 58,
00140
00141
00142
00143 CALCOMP = 65,
00144
00145 TTY3 = 66,
00146 GETAUX2 = 67,
00147 ARM_INFO = 70,
00148 ARM_STATUS = 71,
00149 ARM_INIT = 72,
00150 ARM_CHECK = 73,
00151 ARM_POWER = 74,
00152 ARM_HOME = 75,
00153 ARM_PARK = 76,
00154 ARM_POS = 77,
00155 ARM_SPEED = 78,
00156 ARM_STOP = 79,
00157 ARM_AUTOPARK = 80,
00158 ARM_GRIPPARK = 81,
00159
00160 ROTKP = 82,
00161 ROTKV = 83,
00162 ROTKI = 84,
00163 TRANSKP = 85,
00164 TRANSKV = 86,
00165 TRANSKI = 87,
00166 SOUND = 90,
00167 PLAYLIST = 91,
00168 SOUNDTOG = 92,
00170
00171 POWER_PC = 95,
00172 POWER_LRF = 96,
00173 POWER_5V = 97,
00174 POWER_12V = 98,
00175 POWER_24V = 98,
00176 POWER_AUX_PC = 125,
00177 POWER_TOUCHSCREEN = 126,
00178 POWER_PTZ = 127,
00179 POWER_AUDIO = 128,
00180 POWER_LRF2 = 129,
00181
00182
00183 LATVEL = 110,
00184 LATACCEL = 113,
00186 SETLATV = 0,
00187 };
00188
00189
00190 #define STATUSSTOPPED 0x32
00191 #define STATUSMOVING 0x33
00192 #define ENCODER 0x90
00193 #define SERAUX 0xB0
00194 #define SERAUX2 0xB8 // Added in AmigOS 1.3
00195 #define GYROPAC 0x98 // Added AROS 1.8
00196 #define ARMPAC 160 // ARMpac
00197 #define ARMINFOPAC 161 // ARMINFOpac
00198
00199
00200
00201 #define ARGINT 0x3B // Positive int (LSB, MSB)
00202 #define ARGNINT 0x1B // Negative int (LSB, MSB)
00203 #define ARGSTR 0x2B // String (Note: 1st byte is length!!)
00204
00205
00206 #define GRIPopen 1
00207 #define GRIPclose 2
00208 #define GRIPstop 3
00209 #define LIFTup 4
00210 #define LIFTdown 5
00211 #define LIFTstop 6
00212 #define GRIPstore 7
00213 #define GRIPdeploy 8
00214 #define GRIPhalt 15
00215 #define GRIPpress 16
00216 #define LIFTcarry 17
00217
00218
00219 #define CMUCAM_IMAGE_WIDTH 80
00220 #define CMUCAM_IMAGE_HEIGHT 143
00221 #define CMUCAM_MESSAGE_LEN 10
00222
00223
00224 #define DEFAULT_P2OS_PORT "/dev/ttyS0"
00225 #define DEFAULT_P2OS_TCP_REMOTE_HOST "localhost"
00226 #define DEFAULT_P2OS_TCP_REMOTE_PORT 8101
00227
00228
00229 #define DTOR(a) M_PI * a / 180.0
00230 #define RTOD(a) 180.0 * a / M_PI
00231
00232 typedef struct
00233 {
00234 double x;
00235 double y;
00236 double th;
00237 double length;
00238 double radius;
00239 } bumper_def_t;
00240
00241 typedef struct
00242 {
00243 double x;
00244 double y;
00245 double th;
00246 } sonar_pose_t;
00247
00248
00249 typedef struct
00250 {
00251 double AngleConvFactor;
00252 std::string Class;
00253 double DiffConvFactor;
00254 double DistConvFactor;
00255 int FrontBumpers;
00256 double GyroScaler;
00257 int HasMoveCommand;
00258 int Holonomic;
00259 int IRNum;
00260 int IRUnit;
00261 int LaserFlipped;
00262 std::string LaserIgnore;
00263 std::string LaserPort;
00264 int LaserPossessed;
00265 int LaserPowerControlled;
00266 int LaserTh;
00267 int LaserX;
00268 int LaserY;
00269 int MaxRVelocity;
00270 int MaxVelocity;
00271 int NewTableSensingIR;
00272 int NumFrontBumpers;
00273 int NumRearBumpers;
00274 double RangeConvFactor;
00275 int RearBumpers;
00276 int RequestEncoderPackets;
00277 int RequestIOPackets;
00278 int RobotDiagonal;
00279 int RobotLength;
00280 int RobotRadius;
00281 int RobotWidth;
00282 int RotAccel;
00283 int RotDecel;
00284 int RotVelMax;
00285 int SettableAccsDecs;
00286 int SettableVelMaxes;
00287 int SonarNum;
00288 std::string Subclass;
00289 int SwitchToBaudRate;
00290 int TableSensingIR;
00291 int TransAccel;
00292 int TransDecel;
00293 int TransVelMax;
00294 int Vel2Divisor;
00295 double VelConvFactor;
00296 sonar_pose_t sonar_pose[32];
00297
00298 } RobotParams_t;
00299
00300 extern RobotParams_t PlayerRobotParams[];
00301
00302 #endif // P2OS_DRIVER__ROBOT_PARAMS_HPP_