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