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