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