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