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