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