00001
00002 #ifndef INCLUDED_MantisLegInfo_h
00003 #define INCLUDED_MantisLegInfo_h
00004
00005 #include <cmath>
00006 #include <stdlib.h>
00007 #include "CommonInfo.h"
00008 using namespace RobotInfo;
00009
00010
00011 #if defined(TGT_MANTISLEG)
00012 # define TGT_IS_MANTISLEG
00013 # define TGT_HAS_LEGS 1
00014 # define TGT_HAS_LEDS 4
00015 # define TGT_HAS_POWER_STATUS
00016 #endif
00017
00018
00019 namespace MantisLegInfo {
00020
00021
00022
00023
00024
00025 extern const char* const TargetName;
00026
00027 const unsigned int FrameTime = 32;
00028 const unsigned int NumFrames = 1;
00029 const unsigned int SoundBufferTime = 32;
00030
00031
00032 const unsigned NumWheels = 0;
00033
00034 const unsigned JointsPerArm = 0;
00035 const unsigned NumArms = 0;
00036 const unsigned NumArmJoints = JointsPerArm*NumArms;
00037
00038 const unsigned JointsPerLeg = 4;
00039 const unsigned NumLegs = 1;
00040 const unsigned NumLegJoints = JointsPerLeg*NumLegs;
00041
00042 const unsigned NumHeadJoints = 0;
00043 const unsigned NumTailJoints = 0;
00044 const unsigned NumMouthJoints = 0;
00045 const unsigned NumEarJoints = 0;
00046 const unsigned NumButtons = 3;
00047 const unsigned NumSensors = 11;
00048 const unsigned NumFacePanelLEDs = 0;
00049
00050 const unsigned NumPIDJoints = NumArmJoints + NumLegJoints + NumHeadJoints + NumTailJoints + NumMouthJoints;
00051 const unsigned NumLEDs = NumPIDJoints;
00052
00053 const unsigned NumOutputs = NumWheels + NumPIDJoints + NumLEDs;
00054 const unsigned NumReferenceFrames = NumOutputs + NumLegs + NumArms + 1;
00055
00056 using namespace Camera75DOF;
00057
00058 const float BallOfFootRadius = 0;
00059
00060
00061
00062
00063
00064
00065
00066
00067
00068 const unsigned PIDJointOffset = 0;
00069 const unsigned LegOffset = PIDJointOffset;
00070 const unsigned LEDOffset = PIDJointOffset + NumPIDJoints;
00071 const unsigned BaseFrameOffset = NumOutputs;
00072 const unsigned FootFrameOffset = BaseFrameOffset+1;
00073 const unsigned GripperFrameOffset = FootFrameOffset+NumLegs;
00074 const unsigned CameraFrameOffset = GripperFrameOffset+NumArms;
00075
00076 const unsigned LeftIRFrameOffset = CameraFrameOffset+1;
00077 const unsigned CenterIRFrameOffset = LeftIRFrameOffset+1;
00078 const unsigned IRFrameOffset = CenterIRFrameOffset;
00079 const unsigned RightIRFrameOffset = CenterIRFrameOffset+1;
00080
00081
00082
00083 enum LegOrder_t {
00084 MdLegOrder = 0,
00085 };
00086
00087
00088 enum SEKOffset_t {
00089 SweepOffset = 0,
00090 ElevatorOffset,
00091 RotorOffset,
00092 KneeOffset
00093 };
00094
00095
00096 enum TPROffset_t {
00097 PanOffset = 0,
00098 TiltOffset,
00099 NodOffset = TiltOffset,
00100 RollOffset
00101 };
00102
00103
00104
00105 enum LegOffset_t {
00106 MdLegOffset = LegOffset+MdLegOrder*JointsPerLeg,
00107 };
00108
00109
00110
00111 enum LEDOffset_t {
00112 PowerRedLEDOffset=LEDOffset,
00113 PowerGreenLEDOffset,
00114 PlayLEDOffset,
00115 AdvanceLEDOffset
00116 };
00117
00118
00119 const LEDOffset_t RedLEDOffset = PowerRedLEDOffset;
00120 const LEDOffset_t BlueLEDOffset = AdvanceLEDOffset;
00121 const LEDOffset_t GreenLEDOffset = PlayLEDOffset;
00122 const LEDOffset_t YellowLEDOffset = AdvanceLEDOffset;
00123
00124 typedef unsigned int LEDBitMask_t;
00125
00126 const LEDBitMask_t BlueLEDMask = (1<<(AdvanceLEDOffset-LEDOffset)) | (1<<(PowerRedLEDOffset-LEDOffset));
00127 const LEDBitMask_t GreenLEDMask = 1<<(GreenLEDOffset-LEDOffset);
00128 const LEDBitMask_t YellowLEDMask = 1<<(YellowLEDOffset-LEDOffset);
00129 const LEDBitMask_t RedLEDMask = 1<<(RedLEDOffset-LEDOffset);
00130
00131 const LEDBitMask_t PowerRedLEDMask = 1<<(PowerRedLEDOffset-LEDOffset);
00132 const LEDBitMask_t PowerGreenLEDMask = 1<<(PowerGreenLEDOffset-LEDOffset);
00133 const LEDBitMask_t PlayLEDMask = 1<<(PlayLEDOffset-LEDOffset);
00134 const LEDBitMask_t AdvanceLEDMask = 1<<(AdvanceLEDOffset-LEDOffset);
00135
00136
00137 const LEDBitMask_t FaceLEDMask = 0;
00138
00139 const LEDBitMask_t AllLEDMask = (LEDBitMask_t)~0;
00140
00141
00142
00143
00144 const fmat::Column<3> AgentBoundingBoxBaseFrameOffset = fmat::pack(400,70,350);
00145
00146
00147 const fmat::Column<3> AgentBoundingBoxHalfDims = fmat::pack(100,100,100);
00148
00149
00150
00151
00152
00153
00154
00155
00156
00157
00158
00159
00160
00161
00162
00163
00164
00165
00166
00167
00168
00169 enum ButtonOffset_t { GreenButOffset, RedButOffset, YellowButOffset };
00170
00171
00172 const char* const buttonNames[NumButtons+1] = { "GreenBut", "RedBut", "YellowBut", NULL };
00173
00174
00175
00176 enum SensorOffset_t {
00177 LeftIRDistOffset,
00178 CenterIRDistOffset,
00179 IRDistOffset = CenterIRDistOffset,
00180 RightIRDistOffset,
00181 LeftLuminosityOffset,
00182 CenterLuminosityOffset,
00183 RightLuminosityOffset,
00184 MicVolumeOffset,
00185 MicSpikeCountOffset,
00186 PowerRemainOffset,
00187 PowerThermoOffset,
00188 PowerVoltageOffset,
00189 };
00190
00191
00192 const char* const sensorNames[NumSensors+1] = {
00193 "LeftIRDist", "CenterIRDist", "RightIRDist",
00194 "LeftLuminosity", "CenterLuminosity", "RightLuminosity",
00195 "MicVolume", "MicSpikeCount",
00196 "PowerRemain", "PowerThermo", "PowerVoltage", NULL
00197 };
00198
00199
00200
00201
00202
00203 const char* const outputNames[NumReferenceFrames+1] = {
00204
00205 "sweep","elevator","rotor","knee",
00206
00207
00208 "LED:sweep","LED:elevator",
00209 "LED:rotor","LED:knee",
00210
00211
00212 "BaseFrame", "FootFrame",
00213
00214 NULL
00215 };
00216
00217
00218 class MantisLegCapabilities : public Capabilities {
00219 public:
00220
00221 MantisLegCapabilities()
00222 : Capabilities(TargetName,NumReferenceFrames,outputNames,NumButtons,buttonNames,NumSensors,sensorNames,PIDJointOffset,NumPIDJoints,LEDOffset,NumLEDs,NumOutputs)
00223 {
00224 frameToIndex["IRFrame"] = IRFrameOffset;
00225 sensorToIndex["IRDist"] = IRDistOffset;
00226 }
00227 };
00228
00229
00230 extern const MantisLegCapabilities capabilities;
00231
00232
00233
00234 enum ServoParam_t {
00235 DYNAMIXEL_SLOPE = 0,
00236 DYNAMIXEL_PUNCH,
00237 DYNAMIXEL_MARGIN
00238 };
00239
00240
00241
00242
00243 const float DefaultPIDs[NumPIDJoints][3] = {
00244 {32,32,0},{32,32,0},
00245 {32,32,0},{32,32,0}
00246 };
00247
00248
00249
00250
00251
00252
00253
00254
00255 const float MaxOutputSpeed[NumOutputs] = {
00256
00257 0.4f, 0.4f, 0.4f, 0.4f,
00258
00259 0,0,0,0
00260 };
00261
00262 #ifndef RAD
00263
00264 #define RAD(deg) (((deg) * (float)M_PI ) / 180.0f)
00265
00266 #define __RI_RAD_FLAG
00267 #endif
00268
00269
00270 const float outputRanges[NumOutputs][2] = {
00271
00272 {RAD(-100),RAD(100)},
00273 {RAD(-100),RAD(100)},
00274 {RAD(-100),RAD(100)},
00275 {RAD(-100),RAD(100)},
00276
00277
00278 {0,1},{0,1},
00279 {0,1},{0,1},
00280 };
00281
00282
00283
00284 const float mechanicalLimits[NumOutputs][2] = {
00285
00286 {RAD(-100),RAD(100)},
00287 {RAD(-100),RAD(100)},
00288 {RAD(-100),RAD(100)},
00289 {RAD(-100),RAD(100)},
00290
00291 {0,1},{0,1},
00292 {0,1},{0,1},
00293 };
00294
00295 #ifdef __RI_RAD_FLAG
00296 #undef RAD
00297 #undef __RI_RAD_FLAG
00298 #endif
00299 }
00300
00301
00302
00303
00304
00305
00306 #endif