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