00001
00002 #ifndef INCLUDED_ERS210Info_h
00003 #define INCLUDED_ERS210Info_h
00004
00005 #include <math.h>
00006 #ifndef PLATFORM_APERIOS
00007 typedef unsigned short word;
00008 #else
00009 #include <Types.h>
00010 #endif
00011
00012 #include "CommonInfo.h"
00013 using namespace RobotInfo;
00014
00015 #if TGT_ERS2xx
00016 #include "ERS2xxInfo.h"
00017 #endif
00018
00019
00020 namespace ERS210Info {
00021
00022 #if TGT_ERS2xx
00023 using namespace ERS2xxInfo;
00024 #else
00025
00026
00027
00028
00029
00030 const unsigned int FrameTime=8;
00031 const unsigned int NumFrames=4;
00032 const unsigned int SlowFrameTime=128;
00033 const unsigned int NumSlowFrames=1;
00034 const unsigned int SoundBufferTime=32;
00035
00036
00037
00038
00039 const unsigned JointsPerLeg = 3;
00040 const unsigned NumLegs = 4;
00041 const unsigned NumLegJoints = JointsPerLeg*NumLegs;
00042 const unsigned NumHeadJoints = 3;
00043 const unsigned NumTailJoints = 2;
00044 const unsigned NumMouthJoints = 1;
00045 const unsigned NumEarJoints = 2;
00046 const unsigned NumButtons = 8;
00047 const unsigned NumSensors = 1+3+1+5;
00048 const unsigned NumLEDs = 9;
00049
00050 const unsigned NumPIDJoints = NumLegJoints+NumHeadJoints+NumTailJoints+NumMouthJoints;
00051 const unsigned NumBinJoints = NumEarJoints;
00052 const unsigned NumOutputs = NumPIDJoints + NumBinJoints + NumLEDs;
00053 const unsigned NumReferenceFrames = NumOutputs + 1 + NumLegs + 1 + 1;
00054
00055 const float CameraHorizFOV=57.6/180*M_PI;
00056 const float CameraVertFOV=47.8/180*M_PI;
00057 const float CameraFOV=CameraHorizFOV;
00058 const unsigned int CameraResolutionX=176;
00059 const unsigned int CameraResolutionY=144;
00060
00061 const bool IsFastOutput[NumOutputs] = { true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false };
00062
00063 const bool IsRealERS210[NumOutputs] = { true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true };
00064
00065
00066
00067
00068
00069
00070
00071
00072
00073
00074
00075 const unsigned PIDJointOffset = 0;
00076 const unsigned LegOffset = PIDJointOffset;
00077 const unsigned HeadOffset = LegOffset+NumLegJoints;
00078 const unsigned TailOffset = HeadOffset+NumHeadJoints;
00079 const unsigned MouthOffset = TailOffset+NumTailJoints;
00080
00081 const unsigned LEDOffset = PIDJointOffset + NumPIDJoints;
00082
00083 const unsigned BinJointOffset = LEDOffset + NumLEDs;
00084 const unsigned EarOffset = BinJointOffset;
00085
00086 const unsigned BaseFrameOffset = NumOutputs;
00087 const unsigned PawFrameOffset = BaseFrameOffset+1;
00088 const unsigned CameraFrameOffset = PawFrameOffset+NumLegs;
00089 const unsigned IRFrameOffset = CameraFrameOffset+1;
00090
00091
00092 enum LegOffset_t {
00093 LFrLegOffset = LegOffset+LFrLegOrder*JointsPerLeg,
00094 RFrLegOffset = LegOffset+RFrLegOrder*JointsPerLeg,
00095 LBkLegOffset = LegOffset+LBkLegOrder*JointsPerLeg,
00096 RBkLegOffset = LegOffset+RBkLegOrder*JointsPerLeg
00097 };
00098
00099
00100
00101
00102 enum LEDOffset_t {
00103 BotLLEDOffset = LEDOffset,
00104 BotRLEDOffset,
00105 MidLLEDOffset,
00106 MidRLEDOffset,
00107 TopLLEDOffset,
00108 TopRLEDOffset,
00109 TopBrLEDOffset,
00110 TlRedLEDOffset,
00111 TlBluLEDOffset,
00112
00113
00114 FaceFrontLeftLEDOffset = BotLLEDOffset,
00115 FaceFrontRightLEDOffset = BotRLEDOffset,
00116 FaceCenterLeftLEDOffset = MidLLEDOffset,
00117 FaceCenterRightLEDOffset = MidRLEDOffset,
00118 FaceBackLeftLEDOffset = TopLLEDOffset,
00119 FaceBackRightLEDOffset = TopRLEDOffset,
00120 ModeLEDOffset = TopBrLEDOffset,
00121 TailRightLEDOffset = TlRedLEDOffset,
00122 TailLeftLEDOffset = TlBluLEDOffset
00123 };
00124
00125
00126
00127 typedef unsigned int LEDBitMask_t;
00128 const LEDBitMask_t BotLLEDMask = 1<<(BotLLEDOffset-LEDOffset);
00129 const LEDBitMask_t BotRLEDMask = 1<<(BotRLEDOffset-LEDOffset);
00130 const LEDBitMask_t MidLLEDMask = 1<<(MidLLEDOffset-LEDOffset);
00131 const LEDBitMask_t MidRLEDMask = 1<<(MidRLEDOffset-LEDOffset);
00132 const LEDBitMask_t TopLLEDMask = 1<<(TopLLEDOffset-LEDOffset);
00133 const LEDBitMask_t TopRLEDMask = 1<<(TopRLEDOffset-LEDOffset);
00134 const LEDBitMask_t TopBrLEDMask= 1<<(TopBrLEDOffset-LEDOffset);
00135 const LEDBitMask_t TlRedLEDMask= 1<<(TlRedLEDOffset-LEDOffset);
00136 const LEDBitMask_t TlBluLEDMask= 1<<(TlBluLEDOffset-LEDOffset);
00137
00138 const LEDBitMask_t FaceLEDMask = BotLLEDMask|BotRLEDMask|MidLLEDMask|MidRLEDMask|TopLLEDMask|TopRLEDMask|TopBrLEDMask;
00139 const LEDBitMask_t HeadLEDMask = BotLLEDMask|BotRLEDMask|MidLLEDMask|MidRLEDMask|TopLLEDMask|TopRLEDMask|TopBrLEDMask;
00140 const LEDBitMask_t BackLEDMask = 0;
00141 const LEDBitMask_t TailLEDMask = TlRedLEDMask|TlBluLEDMask;
00142 const LEDBitMask_t AllLEDMask = (LEDBitMask_t)~0;
00143
00144
00145
00146
00147
00148
00149
00150
00151
00152
00153
00154
00155
00156
00157
00158
00159
00160
00161
00162
00163 enum ButtonOffset_t {
00164 LFrPawOffset = LFrLegOrder,
00165 RFrPawOffset = RFrLegOrder,
00166 LBkPawOffset = LBkLegOrder,
00167 RBkPawOffset = RBkLegOrder,
00168 ChinButOffset= 4,
00169 BackButOffset,
00170 HeadFrButOffset,
00171 HeadBkButOffset
00172 };
00173
00174
00175 const char* const buttonNames[NumButtons] = {
00176 "LFrPaw","RFrPaw","LBkPaw","RBkPaw",
00177 "ChinBut","BackBut",
00178 "HeadFrBut","HeadBkBut"
00179 };
00180
00181
00182
00183 enum SensorOffset_t {
00184 IRDistOffset = 0,
00185 BAccelOffset,
00186 LAccelOffset,
00187 DAccelOffset,
00188 ThermoOffset,
00189 PowerRemainOffset,
00190 PowerThermoOffset,
00191 PowerCapacityOffset,
00192 PowerVoltageOffset,
00193 PowerCurrentOffset
00194 };
00195
00196
00197 const char* const sensorNames[NumSensors] = {
00198 "IRDist",
00199 "BAccel","LAccel","DAccel",
00200 "Thermo",
00201 "PowerRemain","PowerThermo","PowerCapacity","PowerVoltage","PowerCurrent"
00202 };
00203
00204
00205
00206
00207
00208 const unsigned outputNameLen = 9;
00209
00210 const char* const outputNames[NumOutputs] = {
00211 "LFr:rotor",
00212 "LFr:elvtr",
00213 "LFr:knee~",
00214 "RFr:rotor",
00215 "RFr:elvtr",
00216 "RFr:knee~",
00217 "LBk:rotor",
00218 "LBk:elvtr",
00219 "LBk:knee~",
00220 "RBk:rotor",
00221 "RBk:elvtr",
00222 "RBk:knee~",
00223
00224 "NECK:tilt",
00225 "NECK:pan~",
00226 "NECK:roll",
00227
00228 "TAIL:tilt",
00229 "TAIL:pan~",
00230
00231 "MOUTH~~~~",
00232
00233 "LED:botL~",
00234 "LED:botR~",
00235 "LED:midL~",
00236 "LED:midR~",
00237 "LED:topL~",
00238 "LED:topR~",
00239 "LED:topBr",
00240
00241 "LED:tlRed",
00242 "LED:tlBlu",
00243
00244 "EAR:left~",
00245 "EAR:right"
00246 };
00247
00248
00249
00250
00251
00252
00253
00254
00255
00256
00257
00258
00259 const char* const PrimitiveName [NumOutputs] = {
00260 "PRM:/r2/c1-Joint2:j1",
00261 "PRM:/r2/c1/c2-Joint2:j2",
00262 "PRM:/r2/c1/c2/c3-Joint2:j3",
00263 "PRM:/r4/c1-Joint2:j1",
00264 "PRM:/r4/c1/c2-Joint2:j2",
00265 "PRM:/r4/c1/c2/c3-Joint2:j3",
00266
00267 "PRM:/r3/c1-Joint2:j1",
00268 "PRM:/r3/c1/c2-Joint2:j2",
00269 "PRM:/r3/c1/c2/c3-Joint2:j3",
00270 "PRM:/r5/c1-Joint2:j1",
00271 "PRM:/r5/c1/c2-Joint2:j2",
00272 "PRM:/r5/c1/c2/c3-Joint2:j3",
00273
00274 "PRM:/r1/c1-Joint2:j1",
00275 "PRM:/r1/c1/c2-Joint2:j2",
00276 "PRM:/r1/c1/c2/c3-Joint2:j3",
00277
00278 "PRM:/r6/c2-Joint2:j2",
00279 "PRM:/r6/c1-Joint2:j1",
00280
00281 "PRM:/r1/c1/c2/c3/c4-Joint2:j4",
00282
00283 "PRM:/r1/c1/c2/c3/l1-LED2:l1",
00284 "PRM:/r1/c1/c2/c3/l4-LED2:l4",
00285 "PRM:/r1/c1/c2/c3/l2-LED2:l2",
00286 "PRM:/r1/c1/c2/c3/l5-LED2:l5",
00287 "PRM:/r1/c1/c2/c3/l3-LED2:l3",
00288 "PRM:/r1/c1/c2/c3/l6-LED2:l6",
00289 "PRM:/r1/c1/c2/c3/l7-LED2:l7",
00290
00291 "PRM:/r6/l2-LED2:l2",
00292 "PRM:/r6/l1-LED2:l1",
00293
00294 "PRM:/r1/c1/c2/c3/e1-Joint3:j5",
00295 "PRM:/r1/c1/c2/c3/e2-Joint3:j6"
00296 };
00297
00298
00299 const char* const SpeakerLocator="PRM:/r1/c1/c2/c3/s1-Speaker:S1";
00300
00301
00302 const char* const CameraLocator="PRM:/r1/c1/c2/c3/i1-FbkImageSensor:F1";
00303
00304
00305
00306
00307
00308
00309
00310
00311
00312
00313
00314
00315
00316
00317
00318
00319
00320
00321
00322
00323
00324
00325
00326
00327
00328
00329
00330 const float DefaultPIDs[NumPIDJoints][3] =
00331 {
00332 { 0x16/(double)(1<<(16-0xE)), 0x04/(double)(1<<(16-0x2)), 0x08/(double)(1<<(16-0xF)) },
00333 { 0x14/(double)(1<<(16-0xE)), 0x04/(double)(1<<(16-0x2)), 0x06/(double)(1<<(16-0xF)) },
00334 { 0x23/(double)(1<<(16-0xE)), 0x04/(double)(1<<(16-0x2)), 0x05/(double)(1<<(16-0xF)) },
00335 { 0x16/(double)(1<<(16-0xE)), 0x04/(double)(1<<(16-0x2)), 0x08/(double)(1<<(16-0xF)) },
00336 { 0x14/(double)(1<<(16-0xE)), 0x04/(double)(1<<(16-0x2)), 0x06/(double)(1<<(16-0xF)) },
00337 { 0x23/(double)(1<<(16-0xE)), 0x04/(double)(1<<(16-0x2)), 0x05/(double)(1<<(16-0xF)) },
00338 { 0x16/(double)(1<<(16-0xE)), 0x04/(double)(1<<(16-0x2)), 0x08/(double)(1<<(16-0xF)) },
00339 { 0x14/(double)(1<<(16-0xE)), 0x04/(double)(1<<(16-0x2)), 0x06/(double)(1<<(16-0xF)) },
00340 { 0x23/(double)(1<<(16-0xE)), 0x04/(double)(1<<(16-0x2)), 0x05/(double)(1<<(16-0xF)) },
00341 { 0x16/(double)(1<<(16-0xE)), 0x04/(double)(1<<(16-0x2)), 0x08/(double)(1<<(16-0xF)) },
00342 { 0x14/(double)(1<<(16-0xE)), 0x04/(double)(1<<(16-0x2)), 0x06/(double)(1<<(16-0xF)) },
00343 { 0x23/(double)(1<<(16-0xE)), 0x04/(double)(1<<(16-0x2)), 0x05/(double)(1<<(16-0xF)) },
00344
00345 { 0x0A/(double)(1<<(16-0xE)), 0x08/(double)(1<<(16-0x2)), 0x0C/(double)(1<<(16-0xF)) },
00346 { 0x0D/(double)(1<<(16-0xE)), 0x08/(double)(1<<(16-0x2)), 0x0B/(double)(1<<(16-0xF)) },
00347 { 0x0A/(double)(1<<(16-0xE)), 0x08/(double)(1<<(16-0x2)), 0x0C/(double)(1<<(16-0xF)) },
00348
00349 { 0x0A/(double)(1<<(16-0xE)), 0x00/(double)(1<<(16-0x2)), 0x18/(double)(1<<(16-0xF)) },
00350 { 0x07/(double)(1<<(16-0xE)), 0x00/(double)(1<<(16-0x2)), 0x11/(double)(1<<(16-0xF)) },
00351
00352 { 0x0E/(double)(1<<(16-0xE)), 0x08/(double)(1<<(16-0x2)), 0x10/(double)(1<<(16-0xF)) }
00353 };
00354
00355
00356 const unsigned char DefaultPIDShifts[3] = {0x0E, 0x02, 0x0F};
00357
00358
00359
00360
00361
00362
00363 const float MaxOutputSpeed[NumOutputs] = {
00364 2.8143434e-03,
00365 2.4980025e-03,
00366 2.8361600e-03,
00367 2.8143434e-03,
00368 2.4980025e-03,
00369 2.8361600e-03,
00370 2.8143434e-03,
00371 2.4980025e-03,
00372 2.8361600e-03,
00373 2.8143434e-03,
00374 2.4980025e-03,
00375 2.8361600e-03,
00376
00377 2.1053034e-03,
00378 3.0106930e-03,
00379 3.0106930e-03,
00380
00381 4.4724062e-03,
00382 4.4724062e-03,
00383
00384 4.3742314e-03,
00385
00386 0,0,0,0,0,0,0,0,0,
00387
00388 0,0
00389 };
00390
00391 #ifndef RAD
00392
00393 #define RAD(deg) (((deg) * M_PI ) / 180.0)
00394
00395 #define __RI_RAD_FLAG
00396 #endif
00397
00398
00399 enum MinMaxRange_t { MinRange,MaxRange };
00400
00401
00402 const double outputRanges[NumOutputs][2] =
00403 {
00404 { RAD(-117),RAD(117) },{ RAD(-11),RAD(89) },{ RAD(-27),RAD(147) },
00405 { RAD(-117),RAD(117) },{ RAD(-11),RAD(89) },{ RAD(-27),RAD(147) },
00406 { RAD(-117),RAD(117) },{ RAD(-11),RAD(89) },{ RAD(-27),RAD(147) },
00407 { RAD(-117),RAD(117) },{ RAD(-11),RAD(89) },{ RAD(-27),RAD(147) },
00408
00409 { RAD(-82),RAD(43) },{ RAD(-89.6),RAD(89.6) },{ RAD(-29),RAD(29) },
00410
00411 { RAD(-22),RAD(22) },{ RAD(-22),RAD(22) },
00412
00413 { RAD(-47),RAD(-3) },
00414
00415 {0,1},{0,1},{0,1},{0,1},{0,1},{0,1},{0,1},{0,1},{0,1},
00416
00417 {0,1},{0,1}
00418 };
00419
00420
00421 const double mechanicalLimits[NumOutputs][2] =
00422 {
00423 { RAD(-120),RAD(120) },{ RAD(-14),RAD(92) },{ RAD(-30),RAD(150) },
00424 { RAD(-120),RAD(120) },{ RAD(-14),RAD(92) },{ RAD(-30),RAD(150) },
00425 { RAD(-120),RAD(120) },{ RAD(-14),RAD(92) },{ RAD(-30),RAD(150) },
00426 { RAD(-120),RAD(120) },{ RAD(-14),RAD(92) },{ RAD(-30),RAD(150) },
00427
00428 { RAD(-85),RAD(46) },{ RAD(-92.6),RAD(92.6) },{ RAD(-32),RAD(32) },
00429
00430 { RAD(-25),RAD(25) },{ RAD(-25),RAD(25) },
00431
00432 { RAD(-50),RAD(0) },
00433
00434 {0,1},{0,1},{0,1},{0,1},{0,1},{0,1},{0,1},{0,1},{0,1},
00435
00436 {0,1},{0,1}
00437 };
00438
00439 #ifdef __RI_RAD_FLAG
00440 #undef RAD
00441 #undef __RI_RAD_FLAG
00442 #endif
00443
00444 #endif //TGT_ERS2xx check
00445
00446
00447
00448 static const int CPCJointNeckTilt = 0;
00449 static const int CPCJointNeckPan = 1;
00450 static const int CPCJointNeckRoll = 2;
00451 static const int CPCSensorHeadBackPressure = 3;
00452 static const int CPCSensorHeadFrontPressure = 4;
00453 static const int CPCSensorPSD = 5;
00454 static const int CPCJointMouth = 6;
00455 static const int CPCSensorChinSwitch = 7;
00456 static const int CPCJointLFRotator = 8;
00457 static const int CPCJointLFElevator = 9;
00458 static const int CPCJointLFKnee = 10;
00459 static const int CPCSensorLFPaw = 11;
00460 static const int CPCJointLHRotator = 12;
00461 static const int CPCJointLHElevator = 13;
00462 static const int CPCJointLHKnee = 14;
00463 static const int CPCSensorLHPaw = 15;
00464 static const int CPCJointRFRotator = 16;
00465 static const int CPCJointRFElevator = 17;
00466 static const int CPCJointRFKnee = 18;
00467 static const int CPCSensorRFPaw = 19;
00468 static const int CPCJointRHRotator = 20;
00469 static const int CPCJointRHElevator = 21;
00470 static const int CPCJointRHKnee = 22;
00471 static const int CPCSensorRHPaw = 23;
00472 static const int CPCJointTailPan = 24;
00473 static const int CPCJointTailTilt = 25;
00474 static const int CPCSensorThermoSensor = 26;
00475 static const int CPCSensorBackSwitch = 27;
00476 static const int CPCSensorAccelFB = 28;
00477 static const int CPCSensorAccelLR = 29;
00478 static const int CPCSensorAccelUD = 30;
00479
00480
00481 }
00482
00483
00484
00485
00486
00487
00488
00489
00490
00491
00492
00493
00494 #endif