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