00001
00002 #ifndef INCLUDED_ERS210Info_h
00003 #define INCLUDED_ERS210Info_h
00004
00005 #include "CommonERSInfo.h"
00006
00007
00008 #if defined(TGT_ERS210)
00009 # define TGT_HAS_LEDS 9
00010 # define TGT_HAS_BUTTONS 8
00011 # define TGT_HAS_IR_DISTANCE 1
00012 #endif
00013
00014
00015 namespace ERS210Info {
00016
00017
00018
00019
00020
00021 extern const char* const TargetName;
00022
00023 const unsigned int FrameTime=8;
00024 const unsigned int NumFrames=4;
00025 const unsigned int SlowFrameTime=128;
00026 const unsigned int NumSlowFrames=1;
00027 const unsigned int SoundBufferTime=32;
00028
00029
00030
00031
00032 const unsigned NumWheels = 0;
00033
00034 const unsigned JointsPerArm = 0;
00035 const unsigned NumArms = 0;
00036 const unsigned NumArmJoints = JointsPerArm*NumArms;
00037
00038 const unsigned JointsPerLeg = 3;
00039 const unsigned NumLegs = 4;
00040 const unsigned NumLegJoints = JointsPerLeg*NumLegs;
00041 const unsigned NumHeadJoints = 3;
00042 const unsigned NumTailJoints = 2;
00043 const unsigned NumMouthJoints = 1;
00044 const unsigned NumEarJoints = 2;
00045 const unsigned NumButtons = 8;
00046 const unsigned NumSensors = 1+3+1+5;
00047 const unsigned NumLEDs = 9;
00048
00049 const unsigned NumPIDJoints = NumLegJoints+NumHeadJoints+NumTailJoints+NumMouthJoints;
00050 const unsigned NumBinJoints = NumEarJoints;
00051 const unsigned NumOutputs = NumPIDJoints + NumBinJoints + NumLEDs;
00052 const unsigned NumReferenceFrames = NumOutputs + 1 + NumLegs + 1 + 1;
00053
00054 using namespace CameraERS2xx;
00055
00056 const float BallOfFootRadius=27.922f/2;
00057 const float CylinderOfFootRadius=24.606f/2;
00058
00059
00060
00061 const bool IsFastOutput[NumOutputs] = { true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false };
00062
00063
00064
00065
00066
00067
00068
00069
00070
00071
00072
00073
00074 const unsigned PIDJointOffset = 0;
00075 const unsigned LegOffset = PIDJointOffset;
00076 const unsigned HeadOffset = LegOffset+NumLegJoints;
00077 const unsigned TailOffset = HeadOffset+NumHeadJoints;
00078 const unsigned MouthOffset = TailOffset+NumTailJoints;
00079
00080 const unsigned LEDOffset = PIDJointOffset + NumPIDJoints;
00081
00082 const unsigned BinJointOffset = LEDOffset + NumLEDs;
00083 const unsigned EarOffset = BinJointOffset;
00084
00085 const unsigned BaseFrameOffset = NumOutputs;
00086 const unsigned FootFrameOffset = BaseFrameOffset+1;
00087 const unsigned PawFrameOffset = FootFrameOffset;
00088 const unsigned CameraFrameOffset = FootFrameOffset+NumLegs;
00089 const unsigned IRFrameOffset = CameraFrameOffset+1;
00090
00091
00092 enum LegOrder_t {
00093 LFrLegOrder = 0,
00094 RFrLegOrder,
00095 LBkLegOrder,
00096 RBkLegOrder
00097 };
00098
00099
00100 enum REKOffset_t {
00101 RotatorOffset=0,
00102 ElevatorOffset,
00103 KneeOffset
00104 };
00105
00106
00107 enum TPROffset_t {
00108 TiltOffset = 0,
00109 PanOffset,
00110 RollOffset,
00111 NodOffset=RollOffset
00112 };
00113
00114
00115 enum LegOffset_t {
00116 LFrLegOffset = LegOffset+LFrLegOrder*JointsPerLeg,
00117 RFrLegOffset = LegOffset+RFrLegOrder*JointsPerLeg,
00118 LBkLegOffset = LegOffset+LBkLegOrder*JointsPerLeg,
00119 RBkLegOffset = LegOffset+RBkLegOrder*JointsPerLeg
00120 };
00121
00122
00123
00124
00125 enum LEDOffset_t {
00126 BotLLEDOffset = LEDOffset,
00127 BotRLEDOffset,
00128 MidLLEDOffset,
00129 MidRLEDOffset,
00130 TopLLEDOffset,
00131 TopRLEDOffset,
00132 TopBrLEDOffset,
00133 TlRedLEDOffset,
00134 TlBluLEDOffset,
00135
00136
00137 FaceFrontLeftLEDOffset = BotLLEDOffset,
00138 FaceFrontRightLEDOffset = BotRLEDOffset,
00139 FaceCenterLeftLEDOffset = MidLLEDOffset,
00140 FaceCenterRightLEDOffset = MidRLEDOffset,
00141 FaceBackLeftLEDOffset = TopLLEDOffset,
00142 FaceBackRightLEDOffset = TopRLEDOffset,
00143 ModeLEDOffset = TopBrLEDOffset,
00144 TailRightLEDOffset = TlRedLEDOffset,
00145 TailLeftLEDOffset = TlBluLEDOffset
00146 };
00147
00148
00149
00150 typedef unsigned int LEDBitMask_t;
00151 const LEDBitMask_t BotLLEDMask = 1<<(BotLLEDOffset-LEDOffset);
00152 const LEDBitMask_t BotRLEDMask = 1<<(BotRLEDOffset-LEDOffset);
00153 const LEDBitMask_t MidLLEDMask = 1<<(MidLLEDOffset-LEDOffset);
00154 const LEDBitMask_t MidRLEDMask = 1<<(MidRLEDOffset-LEDOffset);
00155 const LEDBitMask_t TopLLEDMask = 1<<(TopLLEDOffset-LEDOffset);
00156 const LEDBitMask_t TopRLEDMask = 1<<(TopRLEDOffset-LEDOffset);
00157 const LEDBitMask_t TopBrLEDMask= 1<<(TopBrLEDOffset-LEDOffset);
00158 const LEDBitMask_t TlRedLEDMask= 1<<(TlRedLEDOffset-LEDOffset);
00159 const LEDBitMask_t TlBluLEDMask= 1<<(TlBluLEDOffset-LEDOffset);
00160
00161
00162 const LEDBitMask_t FaceLEDMask
00163 = BotLLEDMask | BotRLEDMask
00164 | MidLLEDMask | MidRLEDMask
00165 | TopLLEDMask | TopRLEDMask
00166 | TopBrLEDMask;
00167
00168
00169 const LEDBitMask_t HeadLEDMask
00170 = BotLLEDMask | BotRLEDMask
00171 | MidLLEDMask | MidRLEDMask
00172 | TopLLEDMask | TopRLEDMask
00173 | TopBrLEDMask;
00174
00175 const LEDBitMask_t BackLEDMask = 0;
00176 const LEDBitMask_t TailLEDMask = TlRedLEDMask|TlBluLEDMask;
00177 const LEDBitMask_t AllLEDMask = (LEDBitMask_t)~0;
00178
00179
00180
00181 const fmat::Column<3> AgentBoundingBoxBaseFrameOffset = fmat::pack(0,0,0);
00182
00183
00184 const fmat::Column<3> AgentBoundingBoxHalfDims = fmat::pack(304.8/2, 304.8/2, 0);
00185
00186
00187
00188
00189
00190
00191
00192
00193
00194
00195
00196
00197
00198
00199
00200
00201
00202
00203
00204 enum ButtonOffset_t {
00205 LFrPawOffset = LFrLegOrder,
00206 RFrPawOffset = RFrLegOrder,
00207 LBkPawOffset = LBkLegOrder,
00208 RBkPawOffset = RBkLegOrder,
00209 ChinButOffset= 4,
00210 BackButOffset,
00211 HeadFrButOffset,
00212 HeadButOffset=HeadFrButOffset,
00213 HeadBkButOffset
00214 };
00215
00216
00217 const char* const buttonNames[NumButtons] = {
00218 "LFrPaw","RFrPaw","LBkPaw","RBkPaw",
00219 "ChinBut","BackBut",
00220 "HeadFrBut","HeadBkBut"
00221 };
00222
00223
00224
00225 enum SensorOffset_t {
00226 IRDistOffset = 0,
00227 BAccelOffset,
00228 LAccelOffset,
00229 DAccelOffset,
00230 ThermoOffset,
00231 PowerRemainOffset,
00232 PowerThermoOffset,
00233 PowerCapacityOffset,
00234 PowerVoltageOffset,
00235 PowerCurrentOffset
00236 };
00237
00238
00239 const char* const sensorNames[NumSensors] = {
00240 "IRDist",
00241 "BAccel","LAccel","DAccel",
00242 "Thermo",
00243 "PowerRemain","PowerThermo","PowerCapacity","PowerVoltage","PowerCurrent"
00244 };
00245
00246
00247
00248
00249
00250 const char* const outputNames[NumReferenceFrames+1] = {
00251 "LFr:rotor",
00252 "LFr:elvtr",
00253 "LFr:knee",
00254 "RFr:rotor",
00255 "RFr:elvtr",
00256 "RFr:knee",
00257 "LBk:rotor",
00258 "LBk:elvtr",
00259 "LBk:knee",
00260 "RBk:rotor",
00261 "RBk:elvtr",
00262 "RBk:knee",
00263
00264 "NECK:tilt",
00265 "NECK:pan",
00266 "NECK:roll",
00267
00268 "TAIL:tilt",
00269 "TAIL:pan",
00270
00271 "MOUTH",
00272
00273 "LED:botL",
00274 "LED:botR",
00275 "LED:midL",
00276 "LED:midR",
00277 "LED:topL",
00278 "LED:topR",
00279 "LED:topBr",
00280
00281 "LED:tlRed",
00282 "LED:tlBlu",
00283
00284 "EAR:left",
00285 "EAR:right",
00286
00287 "BaseFrame",
00288 "LFrFootFrame",
00289 "RFrFootFrame",
00290 "LBkFootFrame",
00291 "RBkFootFrame",
00292 "CameraFrame",
00293 "IRFrame",
00294 NULL
00295 };
00296
00297
00298
00299 class ERS210Capabilities : public Capabilities {
00300 public:
00301
00302 ERS210Capabilities()
00303 : Capabilities(TargetName,NumReferenceFrames,outputNames,NumButtons,buttonNames,NumSensors,sensorNames,PIDJointOffset,NumPIDJoints,LEDOffset,NumLEDs,NumOutputs)
00304 {
00305
00306 buttonToIndex["HeadBut"]=HeadButOffset;
00307
00308 frameToIndex["LED:bkL1"]=TailLeftLEDOffset;
00309 frameToIndex["LED:bkL2"]=TailRightLEDOffset;
00310 }
00311 };
00312
00313 extern const ERS210Capabilities capabilities;
00314
00315
00316
00317
00318
00319
00320
00321
00322
00323
00324
00325
00326 const char* const PrimitiveName [NumOutputs] = {
00327 "PRM:/r2/c1-Joint2:j1",
00328 "PRM:/r2/c1/c2-Joint2:j2",
00329 "PRM:/r2/c1/c2/c3-Joint2:j3",
00330 "PRM:/r4/c1-Joint2:j1",
00331 "PRM:/r4/c1/c2-Joint2:j2",
00332 "PRM:/r4/c1/c2/c3-Joint2:j3",
00333
00334 "PRM:/r3/c1-Joint2:j1",
00335 "PRM:/r3/c1/c2-Joint2:j2",
00336 "PRM:/r3/c1/c2/c3-Joint2:j3",
00337 "PRM:/r5/c1-Joint2:j1",
00338 "PRM:/r5/c1/c2-Joint2:j2",
00339 "PRM:/r5/c1/c2/c3-Joint2:j3",
00340
00341 "PRM:/r1/c1-Joint2:j1",
00342 "PRM:/r1/c1/c2-Joint2:j2",
00343 "PRM:/r1/c1/c2/c3-Joint2:j3",
00344
00345 "PRM:/r6/c2-Joint2:j2",
00346 "PRM:/r6/c1-Joint2:j1",
00347
00348 "PRM:/r1/c1/c2/c3/c4-Joint2:j4",
00349
00350 "PRM:/r1/c1/c2/c3/l1-LED2:l1",
00351 "PRM:/r1/c1/c2/c3/l4-LED2:l4",
00352 "PRM:/r1/c1/c2/c3/l2-LED2:l2",
00353 "PRM:/r1/c1/c2/c3/l5-LED2:l5",
00354 "PRM:/r1/c1/c2/c3/l3-LED2:l3",
00355 "PRM:/r1/c1/c2/c3/l6-LED2:l6",
00356 "PRM:/r1/c1/c2/c3/l7-LED2:l7",
00357
00358 "PRM:/r6/l2-LED2:l2",
00359 "PRM:/r6/l1-LED2:l1",
00360
00361 "PRM:/r1/c1/c2/c3/e1-Joint3:j5",
00362 "PRM:/r1/c1/c2/c3/e2-Joint3:j6"
00363 };
00364
00365
00366 const char* const SpeakerLocator="PRM:/r1/c1/c2/c3/s1-Speaker:S1";
00367
00368
00369 const char* const CameraLocator="PRM:/r1/c1/c2/c3/i1-FbkImageSensor:F1";
00370
00371
00372
00373
00374
00375
00376
00377
00378
00379
00380
00381
00382
00383
00384
00385
00386
00387
00388
00389
00390
00391
00392
00393
00394
00395
00396
00397 const float DefaultPIDs[NumPIDJoints][3] =
00398 {
00399 { 0x16/(float)(1<<(16-0xE)), 0x04/(float)(1<<(16-0x2)), 0x08/(float)(1<<(16-0xF)) },
00400 { 0x14/(float)(1<<(16-0xE)), 0x04/(float)(1<<(16-0x2)), 0x06/(float)(1<<(16-0xF)) },
00401 { 0x23/(float)(1<<(16-0xE)), 0x04/(float)(1<<(16-0x2)), 0x05/(float)(1<<(16-0xF)) },
00402 { 0x16/(float)(1<<(16-0xE)), 0x04/(float)(1<<(16-0x2)), 0x08/(float)(1<<(16-0xF)) },
00403 { 0x14/(float)(1<<(16-0xE)), 0x04/(float)(1<<(16-0x2)), 0x06/(float)(1<<(16-0xF)) },
00404 { 0x23/(float)(1<<(16-0xE)), 0x04/(float)(1<<(16-0x2)), 0x05/(float)(1<<(16-0xF)) },
00405 { 0x16/(float)(1<<(16-0xE)), 0x04/(float)(1<<(16-0x2)), 0x08/(float)(1<<(16-0xF)) },
00406 { 0x14/(float)(1<<(16-0xE)), 0x04/(float)(1<<(16-0x2)), 0x06/(float)(1<<(16-0xF)) },
00407 { 0x23/(float)(1<<(16-0xE)), 0x04/(float)(1<<(16-0x2)), 0x05/(float)(1<<(16-0xF)) },
00408 { 0x16/(float)(1<<(16-0xE)), 0x04/(float)(1<<(16-0x2)), 0x08/(float)(1<<(16-0xF)) },
00409 { 0x14/(float)(1<<(16-0xE)), 0x04/(float)(1<<(16-0x2)), 0x06/(float)(1<<(16-0xF)) },
00410 { 0x23/(float)(1<<(16-0xE)), 0x04/(float)(1<<(16-0x2)), 0x05/(float)(1<<(16-0xF)) },
00411
00412 { 0x0A/(float)(1<<(16-0xE)), 0x08/(float)(1<<(16-0x2)), 0x0C/(float)(1<<(16-0xF)) },
00413 { 0x0D/(float)(1<<(16-0xE)), 0x08/(float)(1<<(16-0x2)), 0x0B/(float)(1<<(16-0xF)) },
00414 { 0x0A/(float)(1<<(16-0xE)), 0x08/(float)(1<<(16-0x2)), 0x0C/(float)(1<<(16-0xF)) },
00415
00416 { 0x0A/(float)(1<<(16-0xE)), 0x00/(float)(1<<(16-0x2)), 0x18/(float)(1<<(16-0xF)) },
00417 { 0x07/(float)(1<<(16-0xE)), 0x00/(float)(1<<(16-0x2)), 0x11/(float)(1<<(16-0xF)) },
00418
00419 { 0x0E/(float)(1<<(16-0xE)), 0x08/(float)(1<<(16-0x2)), 0x10/(float)(1<<(16-0xF)) }
00420 };
00421
00422
00423 const unsigned char DefaultPIDShifts[3] = {0x0E, 0x02, 0x0F};
00424
00425
00426
00427
00428
00429
00430 const float MaxOutputSpeed[NumOutputs] = {
00431 2.8143434f,
00432 2.4980025f,
00433 2.8361600f,
00434 2.8143434f,
00435 2.4980025f,
00436 2.8361600f,
00437 2.8143434f,
00438 2.4980025f,
00439 2.8361600f,
00440 2.8143434f,
00441 2.4980025f,
00442 2.8361600f,
00443
00444 2.1053034f,
00445 3.0106930f,
00446 3.0106930f,
00447
00448 4.4724062f,
00449 4.4724062f,
00450
00451 4.3742314f,
00452
00453 0,0,0,0,0,0,0,0,0,
00454
00455 0,0
00456 };
00457
00458 #ifndef RAD
00459
00460 #define RAD(deg) (((deg) * (float)M_PI ) / 180.0f)
00461
00462 #define __RI_RAD_FLAG
00463 #endif
00464
00465
00466 const float outputRanges[NumOutputs][2] =
00467 {
00468 { RAD(-117),RAD(117) },{ RAD(-11),RAD(89) },{ RAD(-27),RAD(147) },
00469 { RAD(-117),RAD(117) },{ RAD(-11),RAD(89) },{ RAD(-27),RAD(147) },
00470 { RAD(-117),RAD(117) },{ RAD(-11),RAD(89) },{ RAD(-27),RAD(147) },
00471 { RAD(-117),RAD(117) },{ RAD(-11),RAD(89) },{ RAD(-27),RAD(147) },
00472
00473 { RAD(-82),RAD(43) },{ RAD(-89.6f),RAD(89.6f) },{ RAD(-29),RAD(29) },
00474
00475 { RAD(-22),RAD(22) },{ RAD(-22),RAD(22) },
00476
00477 { RAD(-47),RAD(-3) },
00478
00479 {0,1},{0,1},{0,1},{0,1},{0,1},{0,1},{0,1},{0,1},{0,1},
00480
00481 {0,1},{0,1}
00482 };
00483
00484
00485 const float mechanicalLimits[NumOutputs][2] =
00486 {
00487 { RAD(-120),RAD(120) },{ RAD(-14),RAD(92) },{ RAD(-30),RAD(150) },
00488 { RAD(-120),RAD(120) },{ RAD(-14),RAD(92) },{ RAD(-30),RAD(150) },
00489 { RAD(-120),RAD(120) },{ RAD(-14),RAD(92) },{ RAD(-30),RAD(150) },
00490 { RAD(-120),RAD(120) },{ RAD(-14),RAD(92) },{ RAD(-30),RAD(150) },
00491
00492 { RAD(-85),RAD(46) },{ RAD(-92.6f),RAD(92.6f) },{ RAD(-32),RAD(32) },
00493
00494 { RAD(-25),RAD(25) },{ RAD(-25),RAD(25) },
00495
00496 { RAD(-50),RAD(0) },
00497
00498 {0,1},{0,1},{0,1},{0,1},{0,1},{0,1},{0,1},{0,1},{0,1},
00499
00500 {0,1},{0,1}
00501 };
00502
00503 #ifdef __RI_RAD_FLAG
00504 #undef RAD
00505 #undef __RI_RAD_FLAG
00506 #endif
00507
00508
00509
00510 const int CPCJointNeckTilt = 0;
00511 const int CPCJointNeckPan = 1;
00512 const int CPCJointNeckRoll = 2;
00513 const int CPCSensorHeadBackPressure = 3;
00514 const int CPCSensorHeadFrontPressure = 4;
00515 const int CPCSensorPSD = 5;
00516 const int CPCJointMouth = 6;
00517 const int CPCSensorChinSwitch = 7;
00518 const int CPCJointLFRotator = 8;
00519 const int CPCJointLFElevator = 9;
00520 const int CPCJointLFKnee = 10;
00521 const int CPCSensorLFPaw = 11;
00522 const int CPCJointLHRotator = 12;
00523 const int CPCJointLHElevator = 13;
00524 const int CPCJointLHKnee = 14;
00525 const int CPCSensorLHPaw = 15;
00526 const int CPCJointRFRotator = 16;
00527 const int CPCJointRFElevator = 17;
00528 const int CPCJointRFKnee = 18;
00529 const int CPCSensorRFPaw = 19;
00530 const int CPCJointRHRotator = 20;
00531 const int CPCJointRHElevator = 21;
00532 const int CPCJointRHKnee = 22;
00533 const int CPCSensorRHPaw = 23;
00534 const int CPCJointTailPan = 24;
00535 const int CPCJointTailTilt = 25;
00536 const int CPCSensorThermoSensor = 26;
00537 const int CPCSensorBackSwitch = 27;
00538 const int CPCSensorAccelFB = 28;
00539 const int CPCSensorAccelLR = 29;
00540 const int CPCSensorAccelUD = 30;
00541
00542
00543 }
00544
00545
00546
00547
00548
00549
00550 #endif