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