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