00001
00002 #ifndef INCLUDED_ERS7Info_h
00003 #define INCLUDED_ERS7Info_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
00016 namespace ERS7Info {
00017
00018
00019
00020
00021
00022
00023 const unsigned int FrameTime=8;
00024 const unsigned int NumFrames=4;
00025 const unsigned int SlowFrameTime=8;
00026 const unsigned int NumSlowFrames=4;
00027 const unsigned int SoundBufferTime=32;
00028
00029
00030
00031
00032 const unsigned JointsPerLeg = 3;
00033 const unsigned NumLegs = 4;
00034 const unsigned NumLegJoints = JointsPerLeg*NumLegs;
00035 const unsigned NumHeadJoints = 3;
00036 const unsigned NumTailJoints = 2;
00037 const unsigned NumMouthJoints = 1;
00038 const unsigned NumEarJoints = 2;
00039 const unsigned NumButtons = 2+4+3+1;
00040 const unsigned NumSensors = 3+3+5;
00041 const unsigned NumLEDs = 27;
00042
00043 const unsigned NumPIDJoints = NumLegJoints+NumHeadJoints+NumTailJoints+NumMouthJoints;
00044 const unsigned NumBinJoints = NumEarJoints;
00045 const unsigned NumOutputs = NumPIDJoints + NumBinJoints + NumLEDs;
00046 const unsigned NumReferenceFrames = NumOutputs + 1 + NumLegs + 1 + 3;
00047
00048
00049 const bool IsFastOutput[NumOutputs] = {
00050 true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,
00051 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,
00052 true,true
00053 };
00054
00055 const bool IsRealERS7[NumOutputs] = {
00056 true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,
00057 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,
00058 true,true
00059 };
00060
00061
00062
00063
00064
00065
00066
00067
00068
00069
00070 const unsigned PIDJointOffset = 0;
00071 const unsigned LegOffset = PIDJointOffset;
00072 const unsigned HeadOffset = LegOffset+NumLegJoints;
00073 const unsigned TailOffset = HeadOffset+NumHeadJoints;
00074 const unsigned MouthOffset = TailOffset+NumTailJoints;
00075
00076 const unsigned LEDOffset = PIDJointOffset + NumPIDJoints;
00077
00078 const unsigned BinJointOffset = LEDOffset + NumLEDs;
00079 const unsigned EarOffset = BinJointOffset;
00080
00081 const unsigned BaseFrameOffset = NumOutputs;
00082 const unsigned PawFrameOffset = BaseFrameOffset+1;
00083 const unsigned CameraFrameOffset = PawFrameOffset+NumLegs;
00084 const unsigned NearIRFrameOffset = CameraFrameOffset+1;
00085 const unsigned FarIRFrameOffset = NearIRFrameOffset+1;
00086 const unsigned ChestIRFrameOffset = FarIRFrameOffset+1;
00087
00088
00089
00090 enum LegOffset_t {
00091 LFrLegOffset = LegOffset+LFrLegOrder*JointsPerLeg,
00092 RFrLegOffset = LegOffset+RFrLegOrder*JointsPerLeg,
00093 LBkLegOffset = LegOffset+LBkLegOrder*JointsPerLeg,
00094 RBkLegOffset = LegOffset+RBkLegOrder*JointsPerLeg
00095 };
00096
00097
00098
00099
00100 enum LEDOffset_t {
00101 HeadColorLEDOffset = LEDOffset,
00102 HeadWhiteLEDOffset,
00103 ModeRedLEDOffset,
00104 ModeGreenLEDOffset,
00105 ModeBlueLEDOffset,
00106 WirelessLEDOffset,
00107 FaceLEDPanelOffset,
00108 FrBackColorLEDOffset = FaceLEDPanelOffset+14,
00109 FrBackWhiteLEDOffset,
00110 MdBackColorLEDOffset,
00111 MdBackWhiteLEDOffset,
00112 RrBackColorLEDOffset,
00113 RrBackWhiteLEDOffset,
00114 LEDABModeOffset,
00115
00116
00117 BotLLEDOffset = FaceLEDPanelOffset+0,
00118 BotRLEDOffset = FaceLEDPanelOffset+1,
00119 MidLLEDOffset = FaceLEDPanelOffset+2,
00120 MidRLEDOffset = FaceLEDPanelOffset+3,
00121 TopLLEDOffset = FaceLEDPanelOffset+6,
00122 TopRLEDOffset = FaceLEDPanelOffset+7,
00123 TopBrLEDOffset= HeadColorLEDOffset,
00124 TlRedLEDOffset= RrBackColorLEDOffset,
00125 TlBluLEDOffset= FrBackColorLEDOffset
00126 };
00127
00128
00129
00130 typedef unsigned int LEDBitMask_t;
00131
00132 const LEDBitMask_t HeadColorLEDMask = 1<<(HeadColorLEDOffset-LEDOffset);
00133 const LEDBitMask_t HeadWhiteLEDMask = 1<<(HeadWhiteLEDOffset-LEDOffset);
00134 const LEDBitMask_t ModeRedLEDMask = 1<<(ModeRedLEDOffset-LEDOffset);
00135 const LEDBitMask_t ModeGreenLEDMask = 1<<(ModeGreenLEDOffset-LEDOffset);
00136 const LEDBitMask_t ModeBlueLEDMask = 1<<(ModeBlueLEDOffset-LEDOffset);
00137 const LEDBitMask_t WirelessLEDMask = 1<<(WirelessLEDOffset-LEDOffset);
00138 const LEDBitMask_t FaceLEDPanelMask = 1<<(FaceLEDPanelOffset-LEDOffset);
00139 const LEDBitMask_t FrBackColorLEDMask = 1<<(FrBackColorLEDOffset-LEDOffset);
00140 const LEDBitMask_t FrBackWhiteLEDMask = 1<<(FrBackWhiteLEDOffset-LEDOffset);
00141 const LEDBitMask_t MdBackColorLEDMask = 1<<(MdBackColorLEDOffset-LEDOffset);
00142 const LEDBitMask_t MdBackWhiteLEDMask = 1<<(MdBackWhiteLEDOffset-LEDOffset);
00143 const LEDBitMask_t RrBackColorLEDMask = 1<<(RrBackColorLEDOffset-LEDOffset);
00144 const LEDBitMask_t RrBackWhiteLEDMask = 1<<(RrBackWhiteLEDOffset-LEDOffset);
00145 const LEDBitMask_t LEDABModeMask = 1<<(LEDABModeOffset-LEDOffset);
00146
00147 const LEDBitMask_t BotLLEDMask = 1<<(BotLLEDOffset-LEDOffset);
00148 const LEDBitMask_t BotRLEDMask = 1<<(BotRLEDOffset-LEDOffset);
00149 const LEDBitMask_t MidLLEDMask = 1<<(MidLLEDOffset-LEDOffset);
00150 const LEDBitMask_t MidRLEDMask = 1<<(MidRLEDOffset-LEDOffset);
00151 const LEDBitMask_t TopLLEDMask = 1<<(TopLLEDOffset-LEDOffset);
00152 const LEDBitMask_t TopRLEDMask = 1<<(TopRLEDOffset-LEDOffset);
00153 const LEDBitMask_t TopBrLEDMask= 1<<(TopBrLEDOffset-LEDOffset);
00154 const LEDBitMask_t TlRedLEDMask= 1<<(TlRedLEDOffset-LEDOffset);
00155 const LEDBitMask_t TlBluLEDMask= 1<<(TlBluLEDOffset-LEDOffset);
00156
00157 const LEDBitMask_t FaceLEDMask = (FaceLEDPanelMask<<0)|(FaceLEDPanelMask<<1)|(FaceLEDPanelMask<<2)|(FaceLEDPanelMask<<3)|(FaceLEDPanelMask<<4)|(FaceLEDPanelMask<<5)|(FaceLEDPanelMask<<6)|(FaceLEDPanelMask<<7)|(FaceLEDPanelMask<<8)|(FaceLEDPanelMask<<9)|(FaceLEDPanelMask<<10)|(FaceLEDPanelMask<<11)|(FaceLEDPanelMask<<12)|(FaceLEDPanelMask<<13);
00158 const LEDBitMask_t HeadLEDMask = FaceLEDMask|HeadColorLEDMask|HeadWhiteLEDMask|ModeRedLEDMask|ModeGreenLEDMask|ModeBlueLEDMask|WirelessLEDMask;
00159 const LEDBitMask_t BackLEDMask = FrBackColorLEDMask|FrBackWhiteLEDMask|MdBackColorLEDMask|MdBackWhiteLEDMask|RrBackColorLEDMask|RrBackWhiteLEDMask;
00160 const LEDBitMask_t TailLEDMask = 0;
00161 const LEDBitMask_t AllLEDMask = ~0;
00162
00163
00164
00165
00166
00167
00168
00169
00170
00171
00172
00173
00174
00175
00176
00177
00178
00179
00180
00181
00182 enum ButtonOffset_t {
00183 LFrPawOffset = LFrLegOrder,
00184 RFrPawOffset = RFrLegOrder,
00185 LBkPawOffset = LBkLegOrder,
00186 RBkPawOffset = RBkLegOrder,
00187 ChinButOffset= 4,
00188 HeadButOffset,
00189 HeadFrButOffset=HeadButOffset,
00190 FrontBackButOffset,
00191 MiddleBackButOffset,
00192 RearBackButOffset,
00193 WirelessSwOffset
00194 };
00195
00196
00197 const char* const ButtonNames[NumButtons] = {
00198 "LFrPaw","RFrPaw","LBkPaw","ChinBut","HeadBut","FrontBackBut","MiddleBackBut","RearBackBut","WirelessSw"
00199 };
00200
00201
00202
00203 enum SensorOffset_t {
00204 NearIRDistOffset = 0,
00205 FarIRDistOffset,
00206 ChestIRDistOffset,
00207 BAccelOffset,
00208 LAccelOffset,
00209 DAccelOffset,
00210 PowerRemainOffset,
00211 PowerThermoOffset,
00212 PowerCapacityOffset,
00213 PowerVoltageOffset,
00214 PowerCurrentOffset
00215 };
00216
00217
00218
00219
00220
00221 const unsigned outputNameLen = 9;
00222
00223 const char* const outputNames[NumOutputs] = {
00224 "LFr:rotor",
00225 "LFr:elvtr",
00226 "LFr:knee~",
00227 "RFr:rotor",
00228 "RFr:elvtr",
00229 "RFr:knee~",
00230 "LBk:rotor",
00231 "LBk:elvtr",
00232 "LBk:knee~",
00233 "RBk:rotor",
00234 "RBk:elvtr",
00235 "RBk:knee~",
00236
00237 "NECK:tilt",
00238 "NECK:pan~",
00239 "NECK:nod~",
00240
00241 "TAIL:tilt",
00242 "TAIL:pan~",
00243
00244 "MOUTH~~~~",
00245
00246 "LED:headC",
00247 "LED:headW",
00248 "LED:modeR",
00249 "LED:modeG",
00250 "LED:modeB",
00251 "LED:wless",
00252 "LED:faceA",
00253 "LED:faceB",
00254 "LED:faceC",
00255 "LED:faceD",
00256 "LED:faceE",
00257 "LED:faceF",
00258 "LED:faceG",
00259 "LED:faceH",
00260 "LED:faceI",
00261 "LED:faceJ",
00262 "LED:faceK",
00263 "LED:faceL",
00264 "LED:faceM",
00265 "LED:faceN",
00266 "LED:bkFrC",
00267 "LED:bkFrW",
00268 "LED:bkMdC",
00269 "LED:bkMdW",
00270 "LED:bkRrC",
00271 "LED:bkRrW",
00272 "LED:ABmod",
00273
00274 "EAR:left~",
00275 "EAR:right"
00276 };
00277
00278
00279
00280
00281
00282
00283
00284
00285
00286
00287
00288
00289 const char* const PrimitiveName [NumOutputs] = {
00290 "PRM:/r2/c1-Joint2:21",
00291 "PRM:/r2/c1/c2-Joint2:22",
00292 "PRM:/r2/c1/c2/c3-Joint2:23",
00293 "PRM:/r4/c1-Joint2:41",
00294 "PRM:/r4/c1/c2-Joint2:42",
00295 "PRM:/r4/c1/c2/c3-Joint2:43",
00296
00297 "PRM:/r3/c1-Joint2:31",
00298 "PRM:/r3/c1/c2-Joint2:32",
00299 "PRM:/r3/c1/c2/c3-Joint2:33",
00300 "PRM:/r5/c1-Joint2:51",
00301 "PRM:/r5/c1/c2-Joint2:52",
00302 "PRM:/r5/c1/c2/c3-Joint2:53",
00303
00304 "PRM:/r1/c1-Joint2:11",
00305 "PRM:/r1/c1/c2-Joint2:12",
00306 "PRM:/r1/c1/c2/c3-Joint2:13",
00307
00308 "PRM:/r6/c1-Joint2:61",
00309 "PRM:/r6/c2-Joint2:62",
00310
00311 "PRM:/r1/c1/c2/c3/c4-Joint2:14",
00312
00313 "PRM:/r1/c1/c2/c3/l1-LED2:l1",
00314 "PRM:/r1/c1/c2/c3/l2-LED2:l2",
00315 "PRM:/r1/c1/c2/c3/l3-LED2:l3",
00316 "PRM:/r1/c1/c2/c3/l4-LED2:l4",
00317 "PRM:/r1/c1/c2/c3/l5-LED2:l5",
00318 "PRM:/r1/c1/c2/c3/l6-LED2:l6",
00319
00320 "PRM:/r1/c1/c2/c3/la-LED3:la",
00321 "PRM:/r1/c1/c2/c3/lb-LED3:lb",
00322 "PRM:/r1/c1/c2/c3/lc-LED3:lc",
00323 "PRM:/r1/c1/c2/c3/ld-LED3:ld",
00324 "PRM:/r1/c1/c2/c3/le-LED3:le",
00325 "PRM:/r1/c1/c2/c3/lf-LED3:lf",
00326 "PRM:/r1/c1/c2/c3/lg-LED3:lg",
00327 "PRM:/r1/c1/c2/c3/lh-LED3:lh",
00328 "PRM:/r1/c1/c2/c3/li-LED3:li",
00329 "PRM:/r1/c1/c2/c3/lj-LED3:lj",
00330 "PRM:/r1/c1/c2/c3/lk-LED3:lk",
00331 "PRM:/r1/c1/c2/c3/ll-LED3:ll",
00332 "PRM:/r1/c1/c2/c3/lm-LED3:lm",
00333 "PRM:/r1/c1/c2/c3/ln-LED3:ln",
00334
00335 "PRM:/lu-LED3:lu",
00336 "PRM:/lv-LED3:lv",
00337 "PRM:/lw-LED3:lw",
00338 "PRM:/lx-LED3:lx",
00339 "PRM:/ly-LED3:ly",
00340 "PRM:/lz-LED3:lz",
00341 "",
00342
00343 "PRM:/r1/c1/c2/c3/e5-Joint4:15",
00344 "PRM:/r1/c1/c2/c3/e6-Joint4:16"
00345 };
00346
00347
00348 const char* const SpeakerLocator="PRM:/s1-Speaker:S1";
00349
00350
00351 const char* const CameraLocator="PRM:/r1/c1/c2/c3/i1-FbkImageSensor:F1";
00352
00353
00354 const float DefaultPIDs[NumPIDJoints][3] =
00355 {
00356 { 0x1C/(double)(1<<(16-0xE)), 0x08/(double)(1<<(16-0x2)), 0x01/(double)(1<<(16-0xF)) },
00357 { 0x14/(double)(1<<(16-0xE)), 0x04/(double)(1<<(16-0x2)), 0x01/(double)(1<<(16-0xF)) },
00358 { 0x1C/(double)(1<<(16-0xE)), 0x08/(double)(1<<(16-0x2)), 0x01/(double)(1<<(16-0xF)) },
00359 { 0x1C/(double)(1<<(16-0xE)), 0x08/(double)(1<<(16-0x2)), 0x01/(double)(1<<(16-0xF)) },
00360 { 0x14/(double)(1<<(16-0xE)), 0x04/(double)(1<<(16-0x2)), 0x01/(double)(1<<(16-0xF)) },
00361 { 0x1C/(double)(1<<(16-0xE)), 0x08/(double)(1<<(16-0x2)), 0x01/(double)(1<<(16-0xF)) },
00362 { 0x1C/(double)(1<<(16-0xE)), 0x08/(double)(1<<(16-0x2)), 0x01/(double)(1<<(16-0xF)) },
00363 { 0x14/(double)(1<<(16-0xE)), 0x04/(double)(1<<(16-0x2)), 0x01/(double)(1<<(16-0xF)) },
00364 { 0x1C/(double)(1<<(16-0xE)), 0x08/(double)(1<<(16-0x2)), 0x01/(double)(1<<(16-0xF)) },
00365 { 0x1C/(double)(1<<(16-0xE)), 0x08/(double)(1<<(16-0x2)), 0x01/(double)(1<<(16-0xF)) },
00366 { 0x14/(double)(1<<(16-0xE)), 0x04/(double)(1<<(16-0x2)), 0x01/(double)(1<<(16-0xF)) },
00367 { 0x1C/(double)(1<<(16-0xE)), 0x08/(double)(1<<(16-0x2)), 0x01/(double)(1<<(16-0xF)) },
00368
00369 { 0x0A/(double)(1<<(16-0xE)), 0x04/(double)(1<<(16-0x2)), 0x02/(double)(1<<(16-0xF)) },
00370 { 0x08/(double)(1<<(16-0xE)), 0x02/(double)(1<<(16-0x2)), 0x04/(double)(1<<(16-0xF)) },
00371 { 0x08/(double)(1<<(16-0xE)), 0x08/(double)(1<<(16-0x2)), 0x02/(double)(1<<(16-0xF)) },
00372
00373 { 0x0A/(double)(1<<(16-0xE)), 0x04/(double)(1<<(16-0x2)), 0x04/(double)(1<<(16-0xF)) },
00374 { 0x0A/(double)(1<<(16-0xE)), 0x04/(double)(1<<(16-0x2)), 0x04/(double)(1<<(16-0xF)) },
00375
00376 { 0x08/(double)(1<<(16-0xE)), 0x00/(double)(1<<(16-0x2)), 0x04/(double)(1<<(16-0xF)) }
00377 };
00378
00379
00380
00381 const unsigned char DefaultPIDShifts[3] = {0x0E, 0x02-1, 0x0F-3};
00382
00383
00384
00385
00386
00387
00388
00389
00390 const float MaxOutputSpeed[NumOutputs] = {
00391 4.86510529e-3,
00392 5.27962099e-3,
00393 5.27962099e-3,
00394 4.86510529e-3,
00395 5.27962099e-3,
00396 5.27962099e-3,
00397 4.86510529e-3,
00398 5.27962099e-3,
00399 5.27962099e-3,
00400 4.86510529e-3,
00401 5.27962099e-3,
00402 5.27962099e-3,
00403
00404 3.18522588e-3,
00405 1.00574598e-2,
00406 5.78140315e-3,
00407
00408 1.51625479e-2,
00409 1.51625479e-2,
00410
00411 1.01447263e-2,
00412
00413 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
00414
00415 0,0
00416 };
00417
00418 #ifndef RAD
00419
00420 #define RAD(deg) (((deg) * M_PI ) / 180.0)
00421
00422 #define __RI_RAD_FLAG
00423 #endif
00424
00425
00426 enum MinMaxRange_t { MinRange,MaxRange };
00427
00428
00429 const double outputRanges[NumOutputs][2] =
00430 {
00431 { RAD(-115),RAD(130) },{ RAD(-10),RAD(88) },{ RAD(-25),RAD(122) },
00432 { RAD(-115),RAD(130) },{ RAD(-10),RAD(88) },{ RAD(-25),RAD(122) },
00433 { RAD(-130),RAD(115) },{ RAD(-10),RAD(88) },{ RAD(-25),RAD(122) },
00434 { RAD(-130),RAD(115) },{ RAD(-10),RAD(88) },{ RAD(-25),RAD(122) },
00435
00436 { RAD(-75),RAD(0) },{ RAD(-88),RAD(88) },{ RAD(-15),RAD(45) },
00437
00438 { RAD(5),RAD(60) },{ RAD(-45),RAD(45) },
00439
00440 { RAD(-55),RAD(-3) },
00441
00442 {0,1},{0,1},{0,1},{0,1},{0,1},{0,1},
00443 {0,1},{0,1},{0,1},{0,1},{0,1},{0,1},{0,1},{0,1},{0,1},{0,1},{0,1},{0,1},{0,1},{0,1},
00444 {0,1},{0,1},{0,1},{0,1},{0,1},{0,1},
00445 {0,1},
00446
00447 {0,1},{0,1}
00448 };
00449
00450
00451 const double mechanicalLimits[NumOutputs][2] =
00452 {
00453 { RAD(-115),RAD(130) },{ RAD(-10),RAD(88) },{ RAD(-25),RAD(122) },
00454 { RAD(-115),RAD(130) },{ RAD(-10),RAD(88) },{ RAD(-25),RAD(122) },
00455 { RAD(-115),RAD(130) },{ RAD(-10),RAD(88) },{ RAD(-25),RAD(122) },
00456 { RAD(-115),RAD(130) },{ RAD(-10),RAD(88) },{ RAD(-25),RAD(122) },
00457
00458 { RAD(-75),RAD(0) },{ RAD(-88),RAD(88) },{ RAD(-15),RAD(45) },
00459
00460 { RAD(5),RAD(60) },{ RAD(-45),RAD(45) },
00461
00462 { RAD(-55),RAD(-3) },
00463
00464 {0,1},{0,1},{0,1},{0,1},{0,1},{0,1},{0,1},{0,1},{0,1},{0,1},{0,1},{0,1},{0,1},{0,1},{0,1},{0,1},{0,1},{0,1},{0,1},{0,1},{0,1},{0,1},{0,1},{0,1},{0,1},{0,1},{0,1},
00465
00466 {0,1},{0,1}
00467 };
00468
00469 #ifdef __RI_RAD_FLAG
00470 #undef RAD
00471 #undef __RI_RAD_FLAG
00472 #endif
00473
00474
00475
00476
00477 static const int CPCJointMouth = 0;
00478 static const int CPCSwitchChin = 1;
00479 static const int CPCJointNeckNod = 2;
00480 static const int CPCSensorHead = 3;
00481 static const int CPCSensorNearPSD = 4;
00482 static const int CPCSensorFarPSD = 5;
00483 static const int CPCJointNeckPan = 6;
00484 static const int CPCJointNeckTilt = 7;
00485 static const int CPCSwitchLFPaw = 8;
00486 static const int CPCJointLFKnee = 9;
00487 static const int CPCJointLFElevator = 10;
00488 static const int CPCJointLFRotator = 11;
00489 static const int CPCSwitchLHPaw = 12;
00490 static const int CPCJointLHKnee = 13;
00491 static const int CPCJointLHElevator = 14;
00492 static const int CPCJointLHRotator = 15;
00493 static const int CPCSwitchRFPaw = 16;
00494 static const int CPCJointRFKnee = 17;
00495 static const int CPCJointRFElevator = 18;
00496 static const int CPCJointRFRotator = 19;
00497 static const int CPCSwitchRHPaw = 20;
00498 static const int CPCJointRHKnee = 21;
00499 static const int CPCJointRHElevator = 22;
00500 static const int CPCJointRHRotator = 23;
00501 static const int CPCJointTailTilt = 24;
00502 static const int CPCJointTailPan = 25;
00503 static const int CPCSensorAccelFB = 26;
00504 static const int CPCSensorAccelLR = 27;
00505 static const int CPCSensorAccelUD = 28;
00506 static const int CPCSensorChestPSD = 29;
00507 static const int CPCSwitchWireless = 30;
00508 static const int CPCSensorBackRear = 31;
00509 static const int CPCSensorBackMiddle= 32;
00510 static const int CPCSensorBackFront = 33;
00511
00512
00513 }
00514
00515
00516
00517
00518
00519
00520
00521
00522
00523
00524
00525
00526 #endif