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
00016
00017
00018
00019 namespace ERS2xxInfo {
00020
00021
00022
00023
00024
00025
00026 const unsigned int FrameTime=8;
00027 const unsigned int NumFrames=4;
00028 const unsigned int SlowFrameTime=128;
00029 const unsigned int NumSlowFrames=1;
00030 const unsigned int SoundBufferTime=32;
00031
00032
00033
00034
00035 const unsigned NumPIDJoints = 18;
00036 const unsigned NumLEDs = 22;
00037 const unsigned NumBinJoints = 2;
00038 const unsigned NumOutputs = NumPIDJoints + NumBinJoints + NumLEDs;
00039
00040 const bool IsFastOutput[NumOutputs] = {
00041
00042 true, true, true,
00043 true, true, true,
00044 true, true, true,
00045 true, true, true,
00046 true, true, true,
00047 true, true,
00048 true,
00049
00050 true, true, true,
00051 true, true, true,
00052 true,
00053 true, true, true,
00054 true, true, true,
00055 true, true, true,
00056 true, true, true,
00057 true,
00058 true, true,
00059
00060 false, false
00061 };
00062
00063
00064 const bool IsRealERS210[NumOutputs] = {
00065
00066 true, true, true,
00067 true, true, true,
00068 true, true, true,
00069 true, true, true,
00070 true, true, true,
00071 true, true,
00072 true,
00073
00074 true, true, true,
00075 true, true, true,
00076 true,
00077 false, false, false,
00078 false, false, false,
00079 false, false, false,
00080 false, false, false,
00081 false,
00082 true, true,
00083
00084 true, true
00085 };
00086
00087
00088 const bool IsRealERS220[NumOutputs] = {
00089
00090 true, true, true,
00091 true, true, true,
00092 true, true, true,
00093 true, true, true,
00094 true, true, true,
00095 false, false,
00096 false,
00097
00098 true, true, true,
00099 true, true, true,
00100 true,
00101 true, true, true,
00102 true, true, true,
00103 true, true, true,
00104 true, true, true,
00105 true,
00106 false, false,
00107
00108 false, false
00109 };
00110
00111 const unsigned JointsPerLeg = 3;
00112 const unsigned NumLegs = 4;
00113 const unsigned NumLegJoints = JointsPerLeg*NumLegs;
00114 const unsigned NumHeadJoints = 3;
00115 const unsigned NumTailJoints = 2;
00116 const unsigned NumMouthJoints = 1;
00117 const unsigned NumEarJoints = 2;
00118 const unsigned NumButtons = 11;
00119 const unsigned NumSensors = 1+3+1+5;
00120
00121
00122
00123
00124
00125
00126
00127
00128
00129 const unsigned PIDJointOffset = 0;
00130 const unsigned LegOffset = PIDJointOffset;
00131 const unsigned HeadOffset = LegOffset+NumLegJoints;
00132 const unsigned TailOffset = HeadOffset+NumHeadJoints;
00133 const unsigned MouthOffset = TailOffset+NumTailJoints;
00134
00135 const unsigned LEDOffset = PIDJointOffset + NumPIDJoints;
00136
00137 const unsigned BinJointOffset = LEDOffset+NumLEDs;
00138 const unsigned EarOffset = BinJointOffset;
00139
00140
00141
00142 enum LegOrder_t {
00143 LFrLegOrder = 0,
00144 RFrLegOrder,
00145 LBkLegOrder,
00146 RBkLegOrder
00147 };
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 enum REKOffset_t {
00159 RotatorOffset=0,
00160 ElevatorOffset,
00161 KneeOffset
00162 };
00163
00164
00165 enum TPROffset_t {
00166 TiltOffset = 0,
00167 PanOffset,
00168 RollOffset
00169 };
00170
00171
00172 enum LEDOffset_t {
00173 FaceFrontLeftLEDOffset = LEDOffset,
00174 FaceFrontRightLEDOffset,
00175 FaceCenterLeftLEDOffset,
00176 FaceCenterRightLEDOffset,
00177 FaceBackLeftLEDOffset,
00178 FaceBackRightLEDOffset,
00179 ModeLEDOffset,
00180 BackLeft1LEDOffset,
00181 BackLeft2LEDOffset,
00182 BackLeft3LEDOffset,
00183 BackRight3LEDOffset,
00184 BackRight2LEDOffset,
00185 BackRight1LEDOffset,
00186 TailLeftLEDOffset,
00187 TailCenterLEDOffset,
00188 TailRightLEDOffset,
00189 FaceFrontBLEDOffset,
00190 FaceFrontALEDOffset,
00191 FaceFrontCLEDOffset,
00192 RetractableHeadLEDOffset,
00193
00194 TlBluLEDOffset,
00195 TlRedLEDOffset,
00196
00197
00198 BotLLEDOffset = FaceFrontLeftLEDOffset,
00199 BotRLEDOffset = FaceFrontRightLEDOffset,
00200 MidLLEDOffset = FaceCenterLeftLEDOffset,
00201 MidRLEDOffset = FaceCenterRightLEDOffset,
00202 TopLLEDOffset = FaceBackLeftLEDOffset,
00203 TopRLEDOffset = FaceBackRightLEDOffset,
00204 TopBrLEDOffset = ModeLEDOffset,
00205 };
00206
00207
00208
00209 typedef unsigned int LEDBitMask_t;
00210 const LEDBitMask_t FaceFrontLeftLEDMask = 1<<(FaceFrontLeftLEDOffset-LEDOffset);
00211 const LEDBitMask_t FaceFrontRightLEDMask = 1<<(FaceFrontRightLEDOffset-LEDOffset);
00212 const LEDBitMask_t FaceCenterLeftLEDMask = 1<<(FaceCenterLeftLEDOffset-LEDOffset);
00213 const LEDBitMask_t FaceCenterRightLEDMask = 1<<(FaceCenterRightLEDOffset-LEDOffset);
00214 const LEDBitMask_t FaceBackLeftLEDMask = 1<<(FaceBackLeftLEDOffset-LEDOffset);
00215 const LEDBitMask_t FaceBackRightLEDMask = 1<<(FaceBackRightLEDOffset-LEDOffset);
00216 const LEDBitMask_t ModeLEDMask = 1<<(ModeLEDOffset-LEDOffset);
00217 const LEDBitMask_t BackLeft1LEDMask = 1<<(BackLeft1LEDOffset-LEDOffset);
00218 const LEDBitMask_t BackLeft2LEDMask = 1<<(BackLeft2LEDOffset-LEDOffset);
00219 const LEDBitMask_t BackLeft3LEDMask = 1<<(BackLeft3LEDOffset-LEDOffset);
00220 const LEDBitMask_t BackRight3LEDMask = 1<<(BackRight3LEDOffset-LEDOffset);
00221 const LEDBitMask_t BackRight2LEDMask = 1<<(BackRight2LEDOffset-LEDOffset);
00222 const LEDBitMask_t BackRight1LEDMask = 1<<(BackRight1LEDOffset-LEDOffset);
00223 const LEDBitMask_t TailLeftLEDMask = 1<<(TailLeftLEDOffset-LEDOffset);
00224 const LEDBitMask_t TailCenterLEDMask = 1<<(TailCenterLEDOffset-LEDOffset);
00225 const LEDBitMask_t TailRightLEDMask = 1<<(TailRightLEDOffset-LEDOffset);
00226 const LEDBitMask_t FaceFrontBLEDMask = 1<<(FaceFrontBLEDOffset-LEDOffset);
00227 const LEDBitMask_t FaceFrontALEDMask = 1<<(FaceFrontALEDOffset-LEDOffset);
00228 const LEDBitMask_t FaceFrontCLEDMask = 1<<(FaceFrontCLEDOffset-LEDOffset);
00229 const LEDBitMask_t RetractableHeadLEDMask = 1<<(RetractableHeadLEDOffset-LEDOffset);
00230
00231 const LEDBitMask_t TlRedLEDMask= 1<<(TlRedLEDOffset-LEDOffset);
00232 const LEDBitMask_t TlBluLEDMask= 1<<(TlBluLEDOffset-LEDOffset);
00233
00234
00235 const LEDBitMask_t BotLLEDMask = 1<<(BotLLEDOffset-LEDOffset);
00236 const LEDBitMask_t BotRLEDMask = 1<<(BotRLEDOffset-LEDOffset);
00237 const LEDBitMask_t MidLLEDMask = 1<<(MidLLEDOffset-LEDOffset);
00238 const LEDBitMask_t MidRLEDMask = 1<<(MidRLEDOffset-LEDOffset);
00239 const LEDBitMask_t TopLLEDMask = 1<<(TopLLEDOffset-LEDOffset);
00240 const LEDBitMask_t TopRLEDMask = 1<<(TopRLEDOffset-LEDOffset);
00241 const LEDBitMask_t TopBrLEDMask= 1<<(TopBrLEDOffset-LEDOffset);
00242
00243 const LEDBitMask_t FaceLEDMask
00244 = FaceFrontLeftLEDMask
00245 | FaceFrontRightLEDMask
00246 | FaceCenterLeftLEDMask
00247 | FaceCenterRightLEDMask
00248 | FaceBackLeftLEDMask
00249 | FaceBackRightLEDMask
00250 | FaceFrontALEDMask
00251 | FaceFrontBLEDMask
00252 | FaceFrontCLEDMask
00253 | ModeLEDMask;
00254
00255 const LEDBitMask_t HeadLEDMask
00256 = FaceLEDMask
00257 | RetractableHeadLEDMask;
00258
00259 const LEDBitMask_t BackLEDMask
00260 = BackLeft1LEDMask
00261 | BackLeft2LEDMask
00262 | BackLeft3LEDMask
00263 | BackRight1LEDMask
00264 | BackRight2LEDMask
00265 | BackRight3LEDMask;
00266
00267 const LEDBitMask_t TailLEDMask
00268 = TailLeftLEDMask
00269 | TailCenterLEDMask
00270 | TailRightLEDMask
00271 | TlRedLEDMask
00272 | TlBluLEDMask;
00273
00274 const LEDBitMask_t AllLEDMask = ~0;
00275
00276
00277
00278
00279
00280
00281
00282
00283
00284
00285
00286
00287
00288
00289
00290
00291
00292
00293
00294
00295 enum ButtonOffset_t {
00296 LFrPawOffset = LFrLegOrder,
00297 RFrPawOffset = RFrLegOrder,
00298 LBkPawOffset = LBkLegOrder,
00299 RBkPawOffset = RBkLegOrder,
00300 ChinButOffset= 4,
00301 BackButOffset,
00302 HeadFrButOffset,
00303 HeadBkButOffset,
00304 TailLeftButOffset,
00305 TailCenterButOffset,
00306 TailRightButOffset,
00307 };
00308
00309
00310
00311 enum SensorOffset_t {
00312 IRDistOffset = 0,
00313 BAccelOffset,
00314 LAccelOffset,
00315 DAccelOffset,
00316 ThermoOffset,
00317 PowerRemainOffset,
00318 PowerThermoOffset,
00319 PowerCapacityOffset,
00320 PowerVoltageOffset,
00321 PowerCurrentOffset
00322 };
00323
00324
00325
00326
00327
00328 const unsigned outputNameLen = 9;
00329
00330 const char* const outputNames[NumOutputs] = {
00331 "LFr:rotor",
00332 "LFr:elvtr",
00333 "LFr:knee~",
00334 "RFr:rotor",
00335 "RFr:elvtr",
00336 "RFr:knee~",
00337 "LBk:rotor",
00338 "LBk:elvtr",
00339 "LBk:knee~",
00340 "RBk:rotor",
00341 "RBk:elvtr",
00342 "RBk:knee~",
00343
00344 "NECK:tilt",
00345 "NECK:pan~",
00346 "NECK:roll",
00347
00348 "TAIL:tilt",
00349 "TAIL:pan~",
00350
00351 "MOUTH~~~~",
00352
00353 "LED:botL~",
00354 "LED:botR~",
00355 "LED:midL~",
00356 "LED:midR~",
00357 "LED:topL~",
00358 "LED:topR~",
00359 "LED:topBr",
00360
00361 "LED:bkL1~",
00362 "LED:bkL2~",
00363 "LED:bkL3~",
00364 "LED:bkR3~",
00365 "LED:bkR2~",
00366 "LED:bkR1~",
00367 "LED:tailL",
00368 "LED:tailC",
00369 "LED:tailR",
00370 "LED:faceB",
00371 "LED:faceA",
00372 "LED:faceC",
00373 "LED:light",
00374
00375 "LED:tlRed",
00376 "LED:tlBlu",
00377
00378 "EAR:left~",
00379 "EAR:right"
00380 };
00381
00382
00383
00384
00385
00386
00387
00388
00389
00390
00391
00392
00393 const char* const PrimitiveName [NumOutputs] = {
00394 "PRM:/r2/c1-Joint2:j1",
00395 "PRM:/r2/c1/c2-Joint2:j2",
00396 "PRM:/r2/c1/c2/c3-Joint2:j3",
00397 "PRM:/r4/c1-Joint2:j1",
00398 "PRM:/r4/c1/c2-Joint2:j2",
00399 "PRM:/r4/c1/c2/c3-Joint2:j3",
00400
00401 "PRM:/r3/c1-Joint2:j1",
00402 "PRM:/r3/c1/c2-Joint2:j2",
00403 "PRM:/r3/c1/c2/c3-Joint2:j3",
00404 "PRM:/r5/c1-Joint2:j1",
00405 "PRM:/r5/c1/c2-Joint2:j2",
00406 "PRM:/r5/c1/c2/c3-Joint2:j3",
00407
00408 "PRM:/r1/c1-Joint2:j1",
00409 "PRM:/r1/c1/c2-Joint2:j2",
00410 "PRM:/r1/c1/c2/c3-Joint2:j3",
00411
00412 "PRM:/r6/c2-Joint2:j2",
00413 "PRM:/r6/c1-Joint2:j1",
00414
00415 "PRM:/r1/c1/c2/c3/c4-Joint2:j4",
00416
00417 "PRM:/r1/c1/c2/c3/l1-LED2:l1",
00418 "PRM:/r1/c1/c2/c3/l4-LED2:l4",
00419 "PRM:/r1/c1/c2/c3/l2-LED2:l2",
00420 "PRM:/r1/c1/c2/c3/l5-LED2:l5",
00421 "PRM:/r1/c1/c2/c3/l3-LED2:l3",
00422 "PRM:/r1/c1/c2/c3/l6-LED2:l6",
00423 "PRM:/r1/c1/c2/c3/l7-LED2:l7",
00424
00425 "PRM:/r6/l1-LED2:l1",
00426 "PRM:/r6/l2-LED2:l2",
00427 "PRM:/r6/l3-LED2:l3",
00428 "PRM:/r6/l4-LED2:l4",
00429 "PRM:/r6/l5-LED2:l5",
00430 "PRM:/r6/l6-LED2:l6",
00431
00432 "PRM:/r6/l9-LED2:l9",
00433 "PRM:/r6/l7-LED2:l7",
00434 "PRM:/r6/l8-LED2:l8",
00435
00436 "PRM:/r1/c1/c2/c3/l8-LED2:l8",
00437 "PRM:/r1/c1/c2/c3/l9-LED2:l9",
00438 "PRM:/r1/c1/c2/c3/la-LED2:la",
00439 "PRM:/r1/c1/c2/c3/lb-LED2:lb",
00440
00441 "PRM:/r6/l2-LED2:l2",
00442 "PRM:/r6/l1-LED2:l1",
00443
00444 "PRM:/r1/c1/c2/c3/e1-Joint3:j5",
00445 "PRM:/r1/c1/c2/c3/e2-Joint3:j6"
00446 };
00447
00448
00449
00450
00451
00452
00453
00454
00455
00456
00457
00458
00459
00460
00461
00462
00463
00464
00465
00466
00467
00468
00469
00470
00471
00472
00473
00474 const float DefaultPIDs[NumPIDJoints][3] =
00475 {
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 { 0x16/(double)(1<<(16-0xE)), 0x04/(double)(1<<(16-0x2)), 0x08/(double)(1<<(16-0xF)) },
00483 { 0x14/(double)(1<<(16-0xE)), 0x04/(double)(1<<(16-0x2)), 0x06/(double)(1<<(16-0xF)) },
00484 { 0x23/(double)(1<<(16-0xE)), 0x04/(double)(1<<(16-0x2)), 0x05/(double)(1<<(16-0xF)) },
00485 { 0x16/(double)(1<<(16-0xE)), 0x04/(double)(1<<(16-0x2)), 0x08/(double)(1<<(16-0xF)) },
00486 { 0x14/(double)(1<<(16-0xE)), 0x04/(double)(1<<(16-0x2)), 0x06/(double)(1<<(16-0xF)) },
00487 { 0x23/(double)(1<<(16-0xE)), 0x04/(double)(1<<(16-0x2)), 0x05/(double)(1<<(16-0xF)) },
00488
00489 { 0x0A/(double)(1<<(16-0xE)), 0x08/(double)(1<<(16-0x2)), 0x0C/(double)(1<<(16-0xF)) },
00490 { 0x0D/(double)(1<<(16-0xE)), 0x08/(double)(1<<(16-0x2)), 0x0B/(double)(1<<(16-0xF)) },
00491 { 0x0A/(double)(1<<(16-0xE)), 0x08/(double)(1<<(16-0x2)), 0x0C/(double)(1<<(16-0xF)) },
00492
00493 { 0x0A/(double)(1<<(16-0xE)), 0x00/(double)(1<<(16-0x2)), 0x18/(double)(1<<(16-0xF)) },
00494 { 0x07/(double)(1<<(16-0xE)), 0x00/(double)(1<<(16-0x2)), 0x11/(double)(1<<(16-0xF)) },
00495
00496 { 0x0E/(double)(1<<(16-0xE)), 0x08/(double)(1<<(16-0x2)), 0x10/(double)(1<<(16-0xF)) }
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