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