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","ChinBut","BackBut","HeadFrBut","HeadBkBut"
00171 };
00172
00173
00174
00175 enum SensorOffset_t {
00176 IRDistOffset = 0,
00177 BAccelOffset,
00178 LAccelOffset,
00179 DAccelOffset,
00180 ThermoOffset,
00181 PowerRemainOffset,
00182 PowerThermoOffset,
00183 PowerCapacityOffset,
00184 PowerVoltageOffset,
00185 PowerCurrentOffset
00186 };
00187
00188
00189
00190
00191
00192 const unsigned outputNameLen = 9;
00193
00194 const char* const outputNames[NumOutputs] = {
00195 "LFr:rotor",
00196 "LFr:elvtr",
00197 "LFr:knee~",
00198 "RFr:rotor",
00199 "RFr:elvtr",
00200 "RFr:knee~",
00201 "LBk:rotor",
00202 "LBk:elvtr",
00203 "LBk:knee~",
00204 "RBk:rotor",
00205 "RBk:elvtr",
00206 "RBk:knee~",
00207
00208 "NECK:tilt",
00209 "NECK:pan~",
00210 "NECK:roll",
00211
00212 "TAIL:tilt",
00213 "TAIL:pan~",
00214
00215 "MOUTH~~~~",
00216
00217 "LED:botL~",
00218 "LED:botR~",
00219 "LED:midL~",
00220 "LED:midR~",
00221 "LED:topL~",
00222 "LED:topR~",
00223 "LED:topBr",
00224
00225 "LED:tlRed",
00226 "LED:tlBlu",
00227
00228 "EAR:left~",
00229 "EAR:right"
00230 };
00231
00232
00233
00234
00235
00236
00237
00238
00239
00240
00241
00242
00243 const char* const PrimitiveName [NumOutputs] = {
00244 "PRM:/r2/c1-Joint2:j1",
00245 "PRM:/r2/c1/c2-Joint2:j2",
00246 "PRM:/r2/c1/c2/c3-Joint2:j3",
00247 "PRM:/r4/c1-Joint2:j1",
00248 "PRM:/r4/c1/c2-Joint2:j2",
00249 "PRM:/r4/c1/c2/c3-Joint2:j3",
00250
00251 "PRM:/r3/c1-Joint2:j1",
00252 "PRM:/r3/c1/c2-Joint2:j2",
00253 "PRM:/r3/c1/c2/c3-Joint2:j3",
00254 "PRM:/r5/c1-Joint2:j1",
00255 "PRM:/r5/c1/c2-Joint2:j2",
00256 "PRM:/r5/c1/c2/c3-Joint2:j3",
00257
00258 "PRM:/r1/c1-Joint2:j1",
00259 "PRM:/r1/c1/c2-Joint2:j2",
00260 "PRM:/r1/c1/c2/c3-Joint2:j3",
00261
00262 "PRM:/r6/c2-Joint2:j2",
00263 "PRM:/r6/c1-Joint2:j1",
00264
00265 "PRM:/r1/c1/c2/c3/c4-Joint2:j4",
00266
00267 "PRM:/r1/c1/c2/c3/l1-LED2:l1",
00268 "PRM:/r1/c1/c2/c3/l4-LED2:l4",
00269 "PRM:/r1/c1/c2/c3/l2-LED2:l2",
00270 "PRM:/r1/c1/c2/c3/l5-LED2:l5",
00271 "PRM:/r1/c1/c2/c3/l3-LED2:l3",
00272 "PRM:/r1/c1/c2/c3/l6-LED2:l6",
00273 "PRM:/r1/c1/c2/c3/l7-LED2:l7",
00274
00275 "PRM:/r6/l2-LED2:l2",
00276 "PRM:/r6/l1-LED2:l1",
00277
00278 "PRM:/r1/c1/c2/c3/e1-Joint3:j5",
00279 "PRM:/r1/c1/c2/c3/e2-Joint3:j6"
00280 };
00281
00282
00283 const char* const SpeakerLocator="PRM:/r1/c1/c2/c3/s1-Speaker:S1";
00284
00285
00286 const char* const CameraLocator="PRM:/r1/c1/c2/c3/i1-FbkImageSensor:F1";
00287
00288
00289
00290
00291
00292
00293
00294
00295
00296
00297
00298
00299
00300
00301
00302
00303
00304
00305
00306
00307
00308
00309
00310
00311
00312
00313
00314 const float DefaultPIDs[NumPIDJoints][3] =
00315 {
00316 { 0x16/(double)(1<<(16-0xE)), 0x04/(double)(1<<(16-0x2)), 0x08/(double)(1<<(16-0xF)) },
00317 { 0x14/(double)(1<<(16-0xE)), 0x04/(double)(1<<(16-0x2)), 0x06/(double)(1<<(16-0xF)) },
00318 { 0x23/(double)(1<<(16-0xE)), 0x04/(double)(1<<(16-0x2)), 0x05/(double)(1<<(16-0xF)) },
00319 { 0x16/(double)(1<<(16-0xE)), 0x04/(double)(1<<(16-0x2)), 0x08/(double)(1<<(16-0xF)) },
00320 { 0x14/(double)(1<<(16-0xE)), 0x04/(double)(1<<(16-0x2)), 0x06/(double)(1<<(16-0xF)) },
00321 { 0x23/(double)(1<<(16-0xE)), 0x04/(double)(1<<(16-0x2)), 0x05/(double)(1<<(16-0xF)) },
00322 { 0x16/(double)(1<<(16-0xE)), 0x04/(double)(1<<(16-0x2)), 0x08/(double)(1<<(16-0xF)) },
00323 { 0x14/(double)(1<<(16-0xE)), 0x04/(double)(1<<(16-0x2)), 0x06/(double)(1<<(16-0xF)) },
00324 { 0x23/(double)(1<<(16-0xE)), 0x04/(double)(1<<(16-0x2)), 0x05/(double)(1<<(16-0xF)) },
00325 { 0x16/(double)(1<<(16-0xE)), 0x04/(double)(1<<(16-0x2)), 0x08/(double)(1<<(16-0xF)) },
00326 { 0x14/(double)(1<<(16-0xE)), 0x04/(double)(1<<(16-0x2)), 0x06/(double)(1<<(16-0xF)) },
00327 { 0x23/(double)(1<<(16-0xE)), 0x04/(double)(1<<(16-0x2)), 0x05/(double)(1<<(16-0xF)) },
00328
00329 { 0x0A/(double)(1<<(16-0xE)), 0x08/(double)(1<<(16-0x2)), 0x0C/(double)(1<<(16-0xF)) },
00330 { 0x0D/(double)(1<<(16-0xE)), 0x08/(double)(1<<(16-0x2)), 0x0B/(double)(1<<(16-0xF)) },
00331 { 0x0A/(double)(1<<(16-0xE)), 0x08/(double)(1<<(16-0x2)), 0x0C/(double)(1<<(16-0xF)) },
00332
00333 { 0x0A/(double)(1<<(16-0xE)), 0x00/(double)(1<<(16-0x2)), 0x18/(double)(1<<(16-0xF)) },
00334 { 0x07/(double)(1<<(16-0xE)), 0x00/(double)(1<<(16-0x2)), 0x11/(double)(1<<(16-0xF)) },
00335
00336 { 0x0E/(double)(1<<(16-0xE)), 0x08/(double)(1<<(16-0x2)), 0x10/(double)(1<<(16-0xF)) }
00337 };
00338
00339
00340 const unsigned char DefaultPIDShifts[3] = {0x0E, 0x02, 0x0F};
00341
00342
00343
00344
00345
00346
00347 const float MaxOutputSpeed[NumOutputs] = {
00348 2.8143434e-03,
00349 2.4980025e-03,
00350 2.8361600e-03,
00351 2.8143434e-03,
00352 2.4980025e-03,
00353 2.8361600e-03,
00354 2.8143434e-03,
00355 2.4980025e-03,
00356 2.8361600e-03,
00357 2.8143434e-03,
00358 2.4980025e-03,
00359 2.8361600e-03,
00360
00361 2.1053034e-03,
00362 3.0106930e-03,
00363 3.0106930e-03,
00364
00365 4.4724062e-03,
00366 4.4724062e-03,
00367
00368 4.3742314e-03,
00369
00370 0,0,0,0,0,0,0,0,0,
00371
00372 0,0
00373 };
00374
00375 #ifndef RAD
00376
00377 #define RAD(deg) (((deg) * M_PI ) / 180.0)
00378
00379 #define __RI_RAD_FLAG
00380 #endif
00381
00382
00383 enum MinMaxRange_t { MinRange,MaxRange };
00384
00385
00386 const double outputRanges[NumOutputs][2] =
00387 {
00388 { RAD(-117),RAD(117) },{ RAD(-11),RAD(89) },{ RAD(-27),RAD(147) },
00389 { RAD(-117),RAD(117) },{ RAD(-11),RAD(89) },{ RAD(-27),RAD(147) },
00390 { RAD(-117),RAD(117) },{ RAD(-11),RAD(89) },{ RAD(-27),RAD(147) },
00391 { RAD(-117),RAD(117) },{ RAD(-11),RAD(89) },{ RAD(-27),RAD(147) },
00392
00393 { RAD(-82),RAD(43) },{ RAD(-89.6),RAD(89.6) },{ RAD(-29),RAD(29) },
00394
00395 { RAD(-22),RAD(22) },{ RAD(-22),RAD(22) },
00396
00397 { RAD(-47),RAD(-3) },
00398
00399 {0,1},{0,1},{0,1},{0,1},{0,1},{0,1},{0,1},{0,1},{0,1},
00400
00401 {0,1},{0,1}
00402 };
00403
00404
00405 const double mechanicalLimits[NumOutputs][2] =
00406 {
00407 { RAD(-120),RAD(120) },{ RAD(-14),RAD(92) },{ RAD(-30),RAD(150) },
00408 { RAD(-120),RAD(120) },{ RAD(-14),RAD(92) },{ RAD(-30),RAD(150) },
00409 { RAD(-120),RAD(120) },{ RAD(-14),RAD(92) },{ RAD(-30),RAD(150) },
00410 { RAD(-120),RAD(120) },{ RAD(-14),RAD(92) },{ RAD(-30),RAD(150) },
00411
00412 { RAD(-85),RAD(46) },{ RAD(-92.6),RAD(92.6) },{ RAD(-32),RAD(32) },
00413
00414 { RAD(-25),RAD(25) },{ RAD(-25),RAD(25) },
00415
00416 { RAD(-50),RAD(0) },
00417
00418 {0,1},{0,1},{0,1},{0,1},{0,1},{0,1},{0,1},{0,1},{0,1},
00419
00420 {0,1},{0,1}
00421 };
00422
00423 #ifdef __RI_RAD_FLAG
00424 #undef RAD
00425 #undef __RI_RAD_FLAG
00426 #endif
00427
00428 #endif //TGT_ERS2xx check
00429
00430
00431
00432 static const int CPCJointNeckTilt = 0;
00433 static const int CPCJointNeckPan = 1;
00434 static const int CPCJointNeckRoll = 2;
00435 static const int CPCSensorHeadBackPressure = 3;
00436 static const int CPCSensorHeadFrontPressure = 4;
00437 static const int CPCSensorPSD = 5;
00438 static const int CPCJointMouth = 6;
00439 static const int CPCSensorChinSwitch = 7;
00440 static const int CPCJointLFRotator = 8;
00441 static const int CPCJointLFElevator = 9;
00442 static const int CPCJointLFKnee = 10;
00443 static const int CPCSensorLFPaw = 11;
00444 static const int CPCJointLHRotator = 12;
00445 static const int CPCJointLHElevator = 13;
00446 static const int CPCJointLHKnee = 14;
00447 static const int CPCSensorLHPaw = 15;
00448 static const int CPCJointRFRotator = 16;
00449 static const int CPCJointRFElevator = 17;
00450 static const int CPCJointRFKnee = 18;
00451 static const int CPCSensorRFPaw = 19;
00452 static const int CPCJointRHRotator = 20;
00453 static const int CPCJointRHElevator = 21;
00454 static const int CPCJointRHKnee = 22;
00455 static const int CPCSensorRHPaw = 23;
00456 static const int CPCJointTailPan = 24;
00457 static const int CPCJointTailTilt = 25;
00458 static const int CPCSensorThermoSensor = 26;
00459 static const int CPCSensorBackSwitch = 27;
00460 static const int CPCSensorAccelFB = 28;
00461 static const int CPCSensorAccelLR = 29;
00462 static const int CPCSensorAccelUD = 30;
00463
00464
00465 }
00466
00467
00468
00469
00470
00471
00472
00473
00474
00475
00476
00477
00478 #endif