00001
00002 #ifndef INCLUDED_Chiara2Info_h
00003 #define INCLUDED_Chiara2Info_h
00004
00005 #include <cmath>
00006 #include <stdlib.h>
00007 #include "CommonInfo.h"
00008 using namespace RobotInfo;
00009
00010
00011 #if defined(TGT_CHIARA2)
00012 # define TGT_IS_BIOLOID
00013 # define TGT_IS_CHIARA
00014 # define TGT_IS_CHIARA2
00015 # define TGT_HAS_CAMERA 1
00016 # define TGT_HAS_LEDS 7
00017 # define TGT_HAS_BUTTONS 3
00018 # define TGT_HAS_LEGS 6
00019 # define TGT_HAS_SEK_LEGS 6
00020 # define TGT_HAS_ARMS 1
00021 # define TGT_HAS_HEAD 1
00022 # define TGT_HAS_POWER_STATUS
00023 # define TGT_HAS_IR_DISTANCE 3
00024 #endif
00025
00026
00027 namespace Chiara2Info {
00028
00029
00030
00031
00032
00033 extern const char* const TargetName;
00034
00035 const unsigned int FrameTime=32;
00036 const unsigned int NumFrames=1;
00037 const unsigned int SoundBufferTime=32;
00038
00039
00040 const unsigned NumWheels = 0;
00041
00042 const unsigned FingerJointsPerArm = 2;
00043 const unsigned JointsPerArm = 6;
00044 const unsigned NumArms = 1;
00045 const unsigned NumArmJoints = JointsPerArm*NumArms;
00046
00047 const unsigned JointsPerLeg = 3;
00048 const unsigned NumLegs = 6;
00049 const unsigned NumLegJoints = JointsPerLeg*NumLegs;
00050 const unsigned NumHeadJoints = 2;
00051 const unsigned NumTailJoints = 0;
00052 const unsigned NumMouthJoints = 0;
00053 const unsigned NumEarJoints = 0;
00054 const unsigned NumButtons = 3;
00055 const unsigned NumSensors = 11;
00056 const unsigned NumFacePanelLEDs = 0;
00057
00058 const unsigned NumLEDs = NumLegs + 1;
00059 const unsigned NumPIDJoints = NumArmJoints + 1 + NumLegJoints + NumHeadJoints+NumTailJoints+NumMouthJoints;;
00060
00061 const unsigned NumOutputs = NumWheels + NumPIDJoints + NumLEDs;
00062 const unsigned NumReferenceFrames = NumOutputs + 1 + NumLegs + NumArms + 1 + 3;
00063
00064 using namespace Camera75DOF;
00065
00066 const float BallOfFootRadius=0;
00067
00068
00069
00070
00071
00072
00073
00074
00075
00076
00077
00078 const unsigned PIDJointOffset = 0;
00079 const unsigned RFrRotatorOffset = PIDJointOffset;
00080 const unsigned LegOffset = RFrRotatorOffset+1;
00081 const unsigned ArmOffset = LegOffset+NumLegJoints;
00082 const unsigned HeadOffset = ArmOffset+NumArmJoints;
00083
00084 const unsigned LEDOffset = PIDJointOffset + NumPIDJoints;
00085
00086 const unsigned BaseFrameOffset = NumOutputs;
00087 const unsigned FootFrameOffset = BaseFrameOffset+1;
00088 const unsigned GripperFrameOffset = FootFrameOffset+NumLegs;
00089 const unsigned CameraFrameOffset = GripperFrameOffset+NumArms;
00090
00091 const unsigned LeftIRFrameOffset = CameraFrameOffset+1;
00092 const unsigned CenterIRFrameOffset = LeftIRFrameOffset+1;
00093 const unsigned IRFrameOffset = CenterIRFrameOffset;
00094 const unsigned RightIRFrameOffset = CenterIRFrameOffset+1;
00095
00096
00097 enum LegOrder_t {
00098 RFrLegOrder = 0,
00099 LFrLegOrder,
00100 RMdLegOrder,
00101 LMdLegOrder,
00102 RBkLegOrder,
00103 LBkLegOrder
00104 };
00105
00106
00107 enum SEKOffset_t {
00108 SweepOffset=0,
00109 ElevatorOffset,
00110 KneeOffset
00111 };
00112
00113
00114 enum TPROffset_t {
00115 PanOffset=0,
00116 TiltOffset,
00117 NodOffset = TiltOffset,
00118 RollOffset
00119 };
00120
00121
00122
00123 enum LegOffset_t {
00124 RFrLegOffset = LegOffset+RFrLegOrder*JointsPerLeg,
00125 LFrLegOffset = LegOffset+LFrLegOrder*JointsPerLeg,
00126 RMdLegOffset = LegOffset+RMdLegOrder*JointsPerLeg,
00127 LMdLegOffset = LegOffset+LMdLegOrder*JointsPerLeg,
00128 RBkLegOffset = LegOffset+RBkLegOrder*JointsPerLeg,
00129 LBkLegOffset = LegOffset+LBkLegOrder*JointsPerLeg,
00130 };
00131
00132
00133 enum ArmOffset_t {
00134 ArmShoulderOffset=ArmOffset,
00135 ArmElbowOffset,
00136 WristOffset,
00137 WristYawOffset = WristOffset,
00138 WristPitchOffset,
00139 GripperLeftOffset,
00140 GripperRightOffset,
00141 GripperOffset
00142 };
00143
00144
00145 enum HeadOffset_t {
00146 HeadPanOffset = HeadOffset,
00147 HeadTiltOffset,
00148 };
00149
00150
00151
00152 enum LEDOffset_t {
00153 RFrKneeLEDOffset = LEDOffset,
00154 LFrKneeLEDOffset,
00155 RMdKneeLEDOffset,
00156 LMdKneeLEDOffset,
00157 RBkKneeLEDOffset,
00158 LBkKneeLEDOffset,
00159 HeadLEDOffset
00160 };
00161
00162 typedef unsigned int LEDBitMask_t;
00163
00164 const LEDBitMask_t RFrKneeLEDMask = 1<<(RFrKneeLEDOffset-LEDOffset);
00165 const LEDBitMask_t LFrKneeLEDMask = 1<<(LFrKneeLEDOffset-LEDOffset);
00166 const LEDBitMask_t RMdKneeLEDMask = 1<<(RMdKneeLEDOffset-LEDOffset);
00167 const LEDBitMask_t LMdKneeLEDMask = 1<<(LMdKneeLEDOffset-LEDOffset);
00168 const LEDBitMask_t RBkKneeLEDMask = 1<<(RBkKneeLEDOffset-LEDOffset);
00169 const LEDBitMask_t LBkKneeLEDMask = 1<<(LBkKneeLEDOffset-LEDOffset);
00170 const LEDBitMask_t HeadLEDMask = 1<<(HeadLEDOffset-LEDOffset);
00171
00172
00173 const LEDBitMask_t FaceLEDMask = HeadLEDMask;
00174
00175 const LEDBitMask_t AllLEDMask = (LEDBitMask_t)~0;
00176
00177
00178
00179
00180
00181
00182
00183
00184
00185
00186
00187
00188
00189
00190
00191
00192
00193
00194
00195
00196
00197 enum ButtonOffset_t { GreenButOffset, RedButOffset, YellowButOffset };
00198
00199
00200 const char* const buttonNames[NumButtons+1] = { "GreenBut", "RedBut", "YellowBut", NULL };
00201
00202
00203
00204 enum SensorOffset_t {
00205 LeftIRDistOffset,
00206 CenterIRDistOffset,
00207 IRDistOffset = CenterIRDistOffset,
00208 RightIRDistOffset,
00209 LeftLuminosityOffset,
00210 CenterLuminosityOffset,
00211 RightLuminosityOffset,
00212 MicVolumeOffset,
00213 MicSpikeCountOffset,
00214 PowerRemainOffset,
00215 PowerThermoOffset,
00216 PowerVoltageOffset,
00217 };
00218
00219
00220 const char* const sensorNames[NumSensors+1] = {
00221 "LeftIRDist", "CenterIRDist", "RightIRDist",
00222 "LeftLuminosity", "CenterLuminosity", "RightLuminosity",
00223 "MicVolume", "MicSpikeCount",
00224 "PowerRemain", "PowerThermo", "PowerVoltage", NULL
00225 };
00226
00227
00228
00229
00230
00231 const char* const outputNames[NumReferenceFrames+1] = {
00232
00233 "RFr:rotor",
00234 "RFr:sweep","RFr:elvtr","RFr:knee",
00235 "LFr:sweep","LFr:elvtr","LFr:knee",
00236 "RMd:sweep","RMd:elvtr","RMd:knee",
00237 "LMd:sweep","LMd:elvtr","LMd:knee",
00238 "RBk:sweep","RBk:elvtr","RBk:knee",
00239 "LBk:sweep","LBk:elvtr","LBk:knee",
00240 "ARM:shldr","ARM:elbow","ARM:wristYaw","ARM:wristPitch","ARM:gripperLeft","ARM:gripperRight",
00241 "NECK:pan","NECK:tilt",
00242
00243
00244
00245 "LED:RFr:knee","LED:LFr:knee",
00246 "LED:RMd:knee","LED:LMd:knee",
00247 "LED:RBk:knee","LED:LBk:knee",
00248 "LED:NECK:pan",
00249
00250
00251 "BaseFrame",
00252 "RFrFootFrame",
00253 "LFrFootFrame",
00254 "RMdFootFrame",
00255 "LMdFootFrame",
00256 "RBkFootFrame",
00257 "LBkFootFrame",
00258 "GripperFrame",
00259 "CameraFrame",
00260 "LeftIRFrame",
00261 "CenterIRFrame",
00262 "RightIRFrame",
00263 NULL
00264 };
00265
00266
00267 class ChiaraCapabilities : public Capabilities {
00268 public:
00269
00270 ChiaraCapabilities()
00271 : Capabilities(TargetName,NumReferenceFrames,outputNames,NumButtons,buttonNames,NumSensors,sensorNames,PIDJointOffset,NumPIDJoints,LEDOffset,NumLEDs,NumOutputs)
00272 {
00273 frameToIndex["ARM:Wrist"] = WristYawOffset;
00274 frameToIndex["NECK:nod"] = HeadTiltOffset;
00275 frameToIndex["IRFrame"] = IRFrameOffset;
00276 sensorToIndex["IRDist"]=IRDistOffset;
00277 }
00278 };
00279
00280 extern const ChiaraCapabilities capabilities;
00281
00282
00283
00284
00285 const float DefaultPIDs[NumPIDJoints][3] = {
00286 {32,32,0},
00287 {32,32,0}, {32,32,0}, {32,32,0},
00288 {32,32,0}, {32,32,0}, {32,32,0},
00289 {32,32,0}, {32,32,0}, {32,32,0},
00290 {32,32,0}, {32,32,0}, {32,32,0},
00291 {32,32,0}, {32,32,0}, {32,32,0},
00292 {32,32,0}, {32,32,0}, {32,32,0},
00293 {32,32,0}, {32,32,0}, {32,32,0}, {32,32,0}, {32,32,0}, {32,32,0},
00294 {32,32,0}, {32,32,0},
00295 };
00296
00297
00298
00299
00300
00301
00302
00303
00304 const float MaxOutputSpeed[NumOutputs] = {
00305
00306 0.4f,
00307 0.4f,0.4f,0.4f,
00308 0.4f,0.4f,0.4f,
00309 0.4f,0.4f,0.4f,
00310 0.4f,0.4f,0.4f,
00311 0.4f,0.4f,0.4f,
00312 0.4f,0.4f,0.4f,
00313 0.4f,0.4f,0.4f,0.4f,0.4f,0.4f,
00314 0.4f,0.4f,
00315
00316 0,0,
00317 0,0,
00318 0,0,
00319 0,
00320 };
00321
00322 #ifndef RAD
00323
00324 #define RAD(deg) (((deg) * (float)M_PI ) / 180.0f)
00325
00326 #define __RI_RAD_FLAG
00327 #endif
00328
00329
00330 const float outputRanges[NumOutputs][2] = {
00331
00332 {RAD(-17),RAD(90)},
00333 {RAD(-102),RAD(67)}, {RAD(-100),RAD(100)}, {RAD(-55),RAD(160)},
00334 {RAD(-53),RAD(58)}, {RAD(-100),RAD(100)}, {RAD(-55),RAD(160)},
00335 {RAD(-53),RAD(53)}, {RAD(-100),RAD(100)}, {RAD(-55),RAD(160)},
00336 {RAD(-53),RAD(53)}, {RAD(-100),RAD(100)}, {RAD(-55),RAD(160)},
00337 {RAD(-53),RAD(53)}, {RAD(-100),RAD(100)}, {RAD(-55),RAD(160)},
00338 {RAD(-73),RAD(53)}, {RAD(-100),RAD(100)}, {RAD(-55),RAD(160)},
00339 {RAD(-95),RAD(95)}, {RAD(-95),RAD(95)}, {RAD(-100),RAD(100)}, {RAD(-90),RAD(90)}, {RAD(-90),RAD(90)}, {RAD(-90),RAD(90)},
00340 {RAD(-150),RAD(150)}, {RAD(-92),RAD(75)},
00341
00342
00343 {0,1}, {0,1},
00344 {0,1}, {0,1},
00345 {0,1}, {0,1},
00346 {0,1},
00347 };
00348
00349
00350
00351 const float mechanicalLimits[NumOutputs][2] = {
00352
00353 {RAD(-17),RAD(90)},
00354 {RAD(-102),RAD(67)}, {RAD(-100),RAD(100)}, {RAD(-55),RAD(160)},
00355 {RAD(-53),RAD(58)}, {RAD(-100),RAD(100)}, {RAD(-55),RAD(160)},
00356 {RAD(-53),RAD(53)}, {RAD(-100),RAD(100)}, {RAD(-55),RAD(160)},
00357 {RAD(-53),RAD(53)}, {RAD(-100),RAD(100)}, {RAD(-55),RAD(160)},
00358 {RAD(-53),RAD(53)}, {RAD(-100),RAD(100)}, {RAD(-55),RAD(160)},
00359 {RAD(-73),RAD(53)}, {RAD(-100),RAD(100)}, {RAD(-55),RAD(160)},
00360 {RAD(-95),RAD(95)}, {RAD(-95),RAD(95)}, {RAD(-100),RAD(100)}, {RAD(-90),RAD(90)}, {RAD(-90),RAD(90)}, {RAD(-90),RAD(90)},
00361 {RAD(-150),RAD(150)}, {RAD(-92),RAD(75)},
00362
00363
00364 {0,1}, {0,1},
00365 {0,1}, {0,1},
00366 {0,1}, {0,1},
00367 {0,1},
00368 };
00369
00370 #ifdef __RI_RAD_FLAG
00371 #undef RAD
00372 #undef __RI_RAD_FLAG
00373 #endif
00374 }
00375
00376
00377
00378
00379
00380
00381 #endif