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 #if TGT_ERS2xx
00013 #include "ERS2xxInfo.h"
00014 #endif
00015
00016
00017 namespace ERS210Info {
00018
00019 #if TGT_ERS2xx
00020 using namespace ERS2xxInfo;
00021 #else
00022
00023
00024
00025
00026
00027 const unsigned int FrameTime=8;
00028 const unsigned int NumFrames=4;
00029 const unsigned int SlowFrameTime=128;
00030 const unsigned int NumSlowFrames=1;
00031 const unsigned int SoundBufferTime=32;
00032
00033
00034
00035
00036 const unsigned NumPIDJoints = 18;
00037 const unsigned NumLEDs = 9;
00038 const unsigned NumBinJoints = 2;
00039 const unsigned NumOutputs = NumPIDJoints + NumBinJoints + NumLEDs;
00040
00041 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 };
00042
00043 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 };
00044
00045
00046 const unsigned JointsPerLeg = 3;
00047 const unsigned NumLegs = 4;
00048 const unsigned NumLegJoints = JointsPerLeg*NumLegs;
00049 const unsigned NumHeadJoints = 3;
00050 const unsigned NumTailJoints = 2;
00051 const unsigned NumMouthJoints = 1;
00052 const unsigned NumEarJoints = 2;
00053 const unsigned NumButtons = 8;
00054 const unsigned NumSensors = 1+3+1+5;
00055
00056
00057
00058
00059
00060
00061
00062
00063
00064
00065 const unsigned PIDJointOffset = 0;
00066 const unsigned LegOffset = PIDJointOffset;
00067 const unsigned HeadOffset = LegOffset+NumLegJoints;
00068 const unsigned TailOffset = HeadOffset+NumHeadJoints;
00069 const unsigned MouthOffset = TailOffset+NumTailJoints;
00070
00071 const unsigned LEDOffset = PIDJointOffset + NumPIDJoints;
00072
00073 const unsigned BinJointOffset = LEDOffset + NumLEDs;
00074 const unsigned EarOffset = BinJointOffset;
00075
00076
00077
00078 enum LegOrder_t {
00079 LFrLegOrder = 0,
00080 RFrLegOrder,
00081 LBkLegOrder,
00082 RBkLegOrder
00083 };
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 enum REKOffset_t {
00095 RotatorOffset=0,
00096 ElevatorOffset,
00097 KneeOffset
00098 };
00099
00100
00101 enum TPROffset_t {
00102 TiltOffset = 0,
00103 PanOffset,
00104 RollOffset
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 const LEDBitMask_t FaceLEDMask = BotLLEDMask|BotRLEDMask|MidLLEDMask|MidRLEDMask|TopLLEDMask|TopRLEDMask|TopBrLEDMask;
00145 const LEDBitMask_t HeadLEDMask = BotLLEDMask|BotRLEDMask|MidLLEDMask|MidRLEDMask|TopLLEDMask|TopRLEDMask|TopBrLEDMask;
00146 const LEDBitMask_t BackLEDMask = 0;
00147 const LEDBitMask_t TailLEDMask = TlRedLEDMask|TlBluLEDMask;
00148 const LEDBitMask_t AllLEDMask = ~0;
00149
00150
00151
00152
00153
00154
00155
00156
00157
00158
00159
00160
00161
00162
00163
00164
00165
00166
00167
00168
00169 enum ButtonOffset_t {
00170 LFrPawOffset = LFrLegOrder,
00171 RFrPawOffset = RFrLegOrder,
00172 LBkPawOffset = LBkLegOrder,
00173 RBkPawOffset = RBkLegOrder,
00174 ChinButOffset= 4,
00175 BackButOffset,
00176 HeadFrButOffset,
00177 HeadBkButOffset
00178 };
00179
00180
00181
00182 enum SensorOffset_t {
00183 IRDistOffset = 0,
00184 BAccelOffset,
00185 LAccelOffset,
00186 DAccelOffset,
00187 ThermoOffset,
00188 PowerRemainOffset,
00189 PowerThermoOffset,
00190 PowerCapacityOffset,
00191 PowerVoltageOffset,
00192 PowerCurrentOffset
00193 };
00194
00195
00196
00197
00198
00199 const unsigned outputNameLen = 9;
00200
00201 const char* const outputNames[NumOutputs] = {
00202 "LFr:rotor",
00203 "LFr:elvtr",
00204 "LFr:knee~",
00205 "RFr:rotor",
00206 "RFr:elvtr",
00207 "RFr:knee~",
00208 "LBk:rotor",
00209 "LBk:elvtr",
00210 "LBk:knee~",
00211 "RBk:rotor",
00212 "RBk:elvtr",
00213 "RBk:knee~",
00214
00215 "NECK:tilt",
00216 "NECK:pan~",
00217 "NECK:roll",
00218
00219 "TAIL:tilt",
00220 "TAIL:pan~",
00221
00222 "MOUTH~~~~",
00223
00224 "LED:botL~",
00225 "LED:botR~",
00226 "LED:midL~",
00227 "LED:midR~",
00228 "LED:topL~",
00229 "LED:topR~",
00230 "LED:topBr",
00231
00232 "LED:tlRed",
00233 "LED:tlBlu",
00234
00235 "EAR:left~",
00236 "EAR:right"
00237 };
00238
00239
00240
00241
00242
00243
00244
00245
00246
00247
00248
00249
00250 const char* const PrimitiveName [NumOutputs] = {
00251 "PRM:/r2/c1-Joint2:j1",
00252 "PRM:/r2/c1/c2-Joint2:j2",
00253 "PRM:/r2/c1/c2/c3-Joint2:j3",
00254 "PRM:/r4/c1-Joint2:j1",
00255 "PRM:/r4/c1/c2-Joint2:j2",
00256 "PRM:/r4/c1/c2/c3-Joint2:j3",
00257
00258 "PRM:/r3/c1-Joint2:j1",
00259 "PRM:/r3/c1/c2-Joint2:j2",
00260 "PRM:/r3/c1/c2/c3-Joint2:j3",
00261 "PRM:/r5/c1-Joint2:j1",
00262 "PRM:/r5/c1/c2-Joint2:j2",
00263 "PRM:/r5/c1/c2/c3-Joint2:j3",
00264
00265 "PRM:/r1/c1-Joint2:j1",
00266 "PRM:/r1/c1/c2-Joint2:j2",
00267 "PRM:/r1/c1/c2/c3-Joint2:j3",
00268
00269 "PRM:/r6/c2-Joint2:j2",
00270 "PRM:/r6/c1-Joint2:j1",
00271
00272 "PRM:/r1/c1/c2/c3/c4-Joint2:j4",
00273
00274 "PRM:/r1/c1/c2/c3/l1-LED2:l1",
00275 "PRM:/r1/c1/c2/c3/l4-LED2:l4",
00276 "PRM:/r1/c1/c2/c3/l2-LED2:l2",
00277 "PRM:/r1/c1/c2/c3/l5-LED2:l5",
00278 "PRM:/r1/c1/c2/c3/l3-LED2:l3",
00279 "PRM:/r1/c1/c2/c3/l6-LED2:l6",
00280 "PRM:/r1/c1/c2/c3/l7-LED2:l7",
00281
00282 "PRM:/r6/l2-LED2:l2",
00283 "PRM:/r6/l1-LED2:l1",
00284
00285 "PRM:/r1/c1/c2/c3/e1-Joint3:j5",
00286 "PRM:/r1/c1/c2/c3/e2-Joint3:j6"
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
00315 const float DefaultPIDs[NumPIDJoints][3] =
00316 {
00317 { 0x16/(double)(1<<(16-0xE)), 0x04/(double)(1<<(16-0x2)), 0x08/(double)(1<<(16-0xF)) },
00318 { 0x14/(double)(1<<(16-0xE)), 0x04/(double)(1<<(16-0x2)), 0x06/(double)(1<<(16-0xF)) },
00319 { 0x23/(double)(1<<(16-0xE)), 0x04/(double)(1<<(16-0x2)), 0x05/(double)(1<<(16-0xF)) },
00320 { 0x16/(double)(1<<(16-0xE)), 0x04/(double)(1<<(16-0x2)), 0x08/(double)(1<<(16-0xF)) },
00321 { 0x14/(double)(1<<(16-0xE)), 0x04/(double)(1<<(16-0x2)), 0x06/(double)(1<<(16-0xF)) },
00322 { 0x23/(double)(1<<(16-0xE)), 0x04/(double)(1<<(16-0x2)), 0x05/(double)(1<<(16-0xF)) },
00323 { 0x16/(double)(1<<(16-0xE)), 0x04/(double)(1<<(16-0x2)), 0x08/(double)(1<<(16-0xF)) },
00324 { 0x14/(double)(1<<(16-0xE)), 0x04/(double)(1<<(16-0x2)), 0x06/(double)(1<<(16-0xF)) },
00325 { 0x23/(double)(1<<(16-0xE)), 0x04/(double)(1<<(16-0x2)), 0x05/(double)(1<<(16-0xF)) },
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
00330 { 0x0A/(double)(1<<(16-0xE)), 0x08/(double)(1<<(16-0x2)), 0x0C/(double)(1<<(16-0xF)) },
00331 { 0x0D/(double)(1<<(16-0xE)), 0x08/(double)(1<<(16-0x2)), 0x0B/(double)(1<<(16-0xF)) },
00332 { 0x0A/(double)(1<<(16-0xE)), 0x08/(double)(1<<(16-0x2)), 0x0C/(double)(1<<(16-0xF)) },
00333
00334 { 0x0A/(double)(1<<(16-0xE)), 0x00/(double)(1<<(16-0x2)), 0x18/(double)(1<<(16-0xF)) },
00335 { 0x07/(double)(1<<(16-0xE)), 0x00/(double)(1<<(16-0x2)), 0x11/(double)(1<<(16-0xF)) },
00336
00337 { 0x0E/(double)(1<<(16-0xE)), 0x08/(double)(1<<(16-0x2)), 0x10/(double)(1<<(16-0xF)) }
00338 };
00339
00340
00341
00342 const float MaxOutputSpeed[NumOutputs] = {
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 2.8143434e-03,
00350 2.4980025e-03,
00351 2.8361600e-03,
00352 2.8143434e-03,
00353 2.4980025e-03,
00354 2.8361600e-03,
00355
00356 2.1053034e-03,
00357 3.0106930e-03,
00358 3.0106930e-03,
00359
00360 4.4724062e-03,
00361 4.4724062e-03,
00362
00363 4.3742314e-03,
00364
00365 0,0,0,0,0,0,0,0,0,
00366
00367 0,0
00368 };
00369
00370 #ifndef RAD
00371
00372 #define RAD(deg) (((deg) * M_PI ) / 180.0)
00373
00374 #define __RI_RAD_FLAG
00375 #endif
00376
00377
00378 enum MinMaxRange_t { MinRange,MaxRange };
00379
00380
00381 const double outputRanges[NumOutputs][2] =
00382 {
00383 { RAD(-117),RAD(117) },{ RAD(-11),RAD(89) },{ RAD(-27),RAD(147) },
00384 { RAD(-117),RAD(117) },{ RAD(-11),RAD(89) },{ RAD(-27),RAD(147) },
00385 { RAD(-117),RAD(117) },{ RAD(-11),RAD(89) },{ RAD(-27),RAD(147) },
00386 { RAD(-117),RAD(117) },{ RAD(-11),RAD(89) },{ RAD(-27),RAD(147) },
00387
00388 { RAD(-82),RAD(43) },{ RAD(-89.6),RAD(89.6) },{ RAD(-29),RAD(29) },
00389
00390 { RAD(-22),RAD(22) },{ RAD(-22),RAD(22) },
00391
00392 { RAD(-47),RAD(-3) },
00393
00394 {0,1},{0,1},{0,1},{0,1},{0,1},{0,1},{0,1},{0,1},{0,1},
00395
00396 {0,1},{0,1}
00397 };
00398
00399
00400 const double mechanicalLimits[NumOutputs][2] =
00401 {
00402 { RAD(-120),RAD(120) },{ RAD(-14),RAD(92) },{ RAD(-30),RAD(150) },
00403 { RAD(-120),RAD(120) },{ RAD(-14),RAD(92) },{ RAD(-30),RAD(150) },
00404 { RAD(-120),RAD(120) },{ RAD(-14),RAD(92) },{ RAD(-30),RAD(150) },
00405 { RAD(-120),RAD(120) },{ RAD(-14),RAD(92) },{ RAD(-30),RAD(150) },
00406
00407 { RAD(-85),RAD(46) },{ RAD(-92.6),RAD(92.6) },{ RAD(-32),RAD(32) },
00408
00409 { RAD(-25),RAD(25) },{ RAD(-25),RAD(25) },
00410
00411 { RAD(-50),RAD(0) },
00412
00413 {0,1},{0,1},{0,1},{0,1},{0,1},{0,1},{0,1},{0,1},{0,1},
00414
00415 {0,1},{0,1}
00416 };
00417
00418 #ifdef __RI_RAD_FLAG
00419 #undef RAD
00420 #undef __RI_RAD_FLAG
00421 #endif
00422
00423 #endif //TGT_ERS2xx check
00424
00425
00426
00427 static const int CPCJointNeckTilt = 0;
00428 static const int CPCJointNeckPan = 1;
00429 static const int CPCJointNeckRoll = 2;
00430 static const int CPCSensorHeadBackPressure = 3;
00431 static const int CPCSensorHeadFrontPressure = 4;
00432 static const int CPCSensorPSD = 5;
00433 static const int CPCJointMouth = 6;
00434 static const int CPCSensorChinSwitch = 7;
00435 static const int CPCJointLFRotator = 8;
00436 static const int CPCJointLFElevator = 9;
00437 static const int CPCJointLFKnee = 10;
00438 static const int CPCSensorLFPaw = 11;
00439 static const int CPCJointLHRotator = 12;
00440 static const int CPCJointLHElevator = 13;
00441 static const int CPCJointLHKnee = 14;
00442 static const int CPCSensorLHPaw = 15;
00443 static const int CPCJointRFRotator = 16;
00444 static const int CPCJointRFElevator = 17;
00445 static const int CPCJointRFKnee = 18;
00446 static const int CPCSensorRFPaw = 19;
00447 static const int CPCJointRHRotator = 20;
00448 static const int CPCJointRHElevator = 21;
00449 static const int CPCJointRHKnee = 22;
00450 static const int CPCSensorRHPaw = 23;
00451 static const int CPCJointTailPan = 24;
00452 static const int CPCJointTailTilt = 25;
00453 static const int CPCSensorThermoSensor = 26;
00454 static const int CPCSensorBackSwitch = 27;
00455 static const int CPCSensorAccelFB = 28;
00456 static const int CPCSensorAccelLR = 29;
00457 static const int CPCSensorAccelUD = 30;
00458
00459
00460 }
00461
00462
00463
00464
00465
00466
00467
00468
00469
00470
00471
00472
00473 #endif