Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

CalliopeComponents.h

Go to the documentation of this file.
00001 //-*-c++-*-
00002 #ifndef INCLUDED_CalliopeComponents_h
00003 #define INCLUDED_CalliopeComponents_h
00004 
00005 //**** This file defines Calliope components (arms, cameras, AX-S1
00006 //**** sensor module) that are used by CalliopeInfo.h to define
00007 //**** specific robot models such as Calliope2SP.
00008 
00009 #include <cmath>
00010 #include <stdlib.h>
00011 #include "CommonInfo.h"
00012 using namespace RobotInfo;
00013 
00014 // see http://tekkotsu.org/porting.html#configuration for more information on TGT_HAS_* flags
00015 
00016 #if defined(TGT_CALLIOPE)
00017 #  define TGT_IS_CALLIOPE
00018 #endif
00019 
00020 #if defined(TGT_CALLIOPESP) || defined(TGT_CALLIOPELP) || defined(TGT_CALLIOPEKP)
00021 #  define TGT_IS_CALLIOPE
00022 #  define TGT_IS_CALLIOPE0
00023 #endif
00024 
00025 #if defined(TGT_CALLIOPE2SP) || defined(TGT_CALLIOPE2LP) || defined(TGT_CALLIOPE2KP)
00026 #  define TGT_IS_CALLIOPE
00027 #  define TGT_IS_CALLIOPE2
00028 #endif
00029 
00030 #if defined(TGT_CALLIOPE5SP) || defined(TGT_CALLIOPE5LP) || defined(TGT_CALLIOPE5KP)
00031 #  define TGT_IS_CALLIOPE
00032 #  define TGT_IS_CALLIOPE5
00033 #endif
00034 
00035 #if defined(TGT_CALLIOPEKP) || defined(TGT_CALLIOPE2KP) || defined(TGT_CALLIOPE5KP)
00036 #  define TGT_HAS_KINECT
00037 #endif
00038 
00039 // Common to all Calliope models
00040 #if defined(TGT_IS_CALLIOPE)
00041 #  define TGT_IS_CREATE
00042 #  define TGT_HAS_WHEELS 2
00043 #  define TGT_HAS_BUTTONS 15
00044 #  define TGT_HAS_LEDS 4
00045 #  define TGT_HAS_CAMERA 1
00046 #endif
00047 
00048 // Everything but the base model (old TGT_CREATE) has a pan-tilt
00049 #if !defined(TGT_CALLIOPE)
00050 #  define TGT_HAS_HEAD 1
00051 #endif
00052 
00053 // 2-DOF arm with open-close gripper
00054 #if defined(TGT_IS_CALLIOPE2)
00055 #  define TGT_HAS_ARMS 1
00056 #  define TGT_HAS_GRIPPER 1
00057 #endif
00058 
00059 // 5-DOF arm with independent fingers
00060 #if defined(TGT_IS_CALLIOPE5)
00061 #  define TGT_HAS_ARMS 1
00062 #  define TGT_HAS_GRIPPER 1
00063 #  define TGT_HAS_FINGERS 2
00064 #endif
00065 
00066 // Models with a head but not a Kinect have an AX-S1 with 3 IR rangefinders
00067 #if defined(TGT_HAS_HEAD) && !defined(TGT_HAS_KINECT)
00068 #  define TGT_HAS_IR_DISTANCE 3
00069 #endif
00070 
00071 #ifndef RAD
00072 //!Just a little macro for converting degrees to radians
00073 #define RAD(deg) (((deg) * (float)M_PI ) / 180.0f)
00074 //!a flag so we undef these after we're done - do you have a cleaner solution?
00075 #define __RI_RAD_FLAG
00076 #endif
00077 
00078 
00079 //! Contains information about a Calliope robot, such as number of joints, LEDs, etc.
00080 namespace CalliopeComponents {
00081   
00082   // *******************************
00083   //       ROBOT CONFIGURATION
00084   // *******************************
00085 
00086   const unsigned int FrameTime=30;        //!< time between frames in the motion system (milliseconds)
00087   const unsigned int NumFrames=1;        //!< the number of frames per buffer (don't forget also double buffered)
00088   const unsigned int SoundBufferTime=32; //!< the number of milliseconds per sound buffer... I'm not sure if this can be changed
00089   
00090   //!@name Output Types Information
00091   const unsigned NumWheels      =  2;
00092   
00093   const unsigned JointsPerLeg   =  0; //!< The number of joints per leg
00094   const unsigned NumLegs        =  0; //!< The number of legs
00095   const unsigned NumLegJoints   =  JointsPerLeg*NumLegs; //!< the TOTAL number of joints on ALL legs
00096   const unsigned NumTailJoints  =  0; //!< The number of joints assigned to the tail
00097   const unsigned NumMouthJoints =  0; //!< the number of joints that control the mouth
00098   const unsigned NumEarJoints   =  0; //!< The number of joints which control the ears (NOT per ear, is total)
00099   const unsigned NumButtons     =  15; //!< the number of buttons that are available
00100   const unsigned NumLEDs        =  4; //!< The number of LEDs which can be controlled
00101   const unsigned NumFacePanelLEDs = 0; //!< The number of face panel LEDs
00102   //@}
00103 
00104 
00105   // *******************************
00106   //         OUTPUT OFFSETS
00107   // *******************************
00108 
00109   //!Corresponds to entries in outputNames, defined at the end of this file
00110   //!@name Output Offsets
00111 
00112   
00113   const unsigned PIDJointOffset = 0; //!< The beginning of the PID Joints
00114   const unsigned WheelOffset = PIDJointOffset;
00115 
00116   enum WheelOffset_t {
00117     LWheelOffset=WheelOffset,
00118     RWheelOffset
00119   };
00120   
00121   //! Offset needed so that the centroid of the robot is correct related to the bounding box
00122   const fmat::Column<3> AgentBoundingBoxBaseFrameOffset = fmat::pack(838.4/2-304.8/2,0,0);
00123 
00124   //! Half of the length, width, and height of the robot.
00125   const fmat::Column<3> AgentBoundingBoxHalfDims = fmat::pack(838.4/2, 304.8/2, 0);
00126 
00127   // *******************************
00128   //          INPUT OFFSETS
00129   // *******************************
00130 
00131 
00132   //! The order in which inputs should be stored
00133   //!@name Input Offsets
00134 
00135   //! holds offsets to different buttons in WorldState::buttons[]
00136   /*! Should be a straight mapping to the ButtonSourceIDs
00137    *
00138    *  @see WorldState::buttons
00139    * @hideinitializer */
00140   enum ButtonOffset_t {
00141     PlayButOffset, //!< 1 if play button is down
00142     AdvanceButOffset, //!< 1 if advance button is down
00143     WallButOffset, //!< 1 if wall is detected (note correspondence to WALL_SIGNAL_OFFSET's value, avoiding problems if the two are swapped)
00144     DropCasterButOffset, //!< 1 if caster detects dropoff
00145     DropLeftWheelButOffset, //!< 1 if left wheel detects dropoff
00146     DropRightWheelButOffset, //!< 1 if right wheel detects dropoff
00147     BumpLeftButOffset, //!< 1 if left bumper is pressed
00148     BumpRightButOffset, //!< 1 if right bumper is pressed
00149     OvercurrentLeftWheelOffset, //!< 1 if the left wheel is drawing more than 1 amp
00150     OvercurrentRightWheelOffset, //!< 1 if the right wheel is drawing more than 1 amp
00151     LowSideDriver0ButOffset, //!< 1 if low side driver 0 is pulling more than 0.5 amps
00152     LowSideDriver1ButOffset, //!< 1 if low side driver 1 is pulling more than 0.5 amps
00153     LowSideDriver2ButOffset, //!< 1 if low side driver 2 is pulling more than 1.6 amps
00154     BaseChargerButOffset, //!< 1 if the home base charger is available
00155     InternalChargerButOffset //!< 1 if the internal charger is available
00156   };
00157 
00158   const ButtonOffset_t GreenButOffset = PlayButOffset;
00159   const ButtonOffset_t YellowButOffset = AdvanceButOffset;
00160   const ButtonOffset_t RedButOffset = PlayButOffset;
00161 
00162   //! Provides a string name for each button
00163   const char* const buttonNames[NumButtons+1] = { 
00164     "Play", "Advance",
00165     "Wall",
00166     "CasterDrop", "LWheelDrop", "RWheelDrop",
00167     "LBump", "RBump",
00168     "LOverCurrent", "ROverCurrent",
00169     "DriverLow0", "DriverLow1", "DriverLow2",
00170     "BaseCharger", "InternalCharger",
00171     NULL
00172   };
00173 
00174   //! holds offset to different sensor values in WorldState::sensors[]
00175   /*! @see WorldState::sensors[] */
00176   enum SensorOffset_t {
00177     DigitalInput0Offset, //!< the digital input pins in bits 0 through 4
00178     DigitalInput1Offset, //!< the digital input pins in bits 0 through 4
00179     DigitalInput2Offset, //!< the digital input pins in bits 0 through 4
00180     DigitalInput3Offset, //!< the digital input pins in bits 0 through 4
00181     AnalogSignalOffset, //!< voltage on cargo bay pin 4
00182     WallSignalOffset, //!< strength of the wall sensor's signal (note correspondence to WALL_OFFSET's value, avoid problems if the two are swapped)
00183     IRCommOffset, //!< value received by the infrared communication receiver, see IRComm_t for values sent by standard hardware
00184     CliffLeftSignalOffset, //!< strength of the left cliff sensor
00185     CliffFrontLeftSignalOffset, //!< strength of the front left cliff sensor
00186     CliffFrontRightSignalOffset, //!< strength of the front right cliff sensor
00187     CliffRightSignalOffset, //!< strength of the right cliff sensor
00188     EncoderDistanceOffset, //!< average distance (mm) traveled by the wheels since last update
00189     EncoderAngleOffset, //!< average angle (radians) rotated since the last update
00190     VoltageOffset, //!< mV measured at battery
00191     CurrentOffset, //!< mA flowing into battery (negative when discharging)
00192     BatteryChargeOffset, //!< mAh remaining in battery (may not be accurate with alkaline battery pack)
00193     BatteryTempOffset, //!< degrees celsius
00194     ChargingStateOffset, //!< one of #ChargingState_t
00195     ModeStateOffset, //!< one of #ModeState_t
00196     GPSXOffset, //!< x-coordinate of robot from GPS or Mirage
00197     GPSYOffset, //!< y-coordinate of robot from GPS or Mirage
00198     GPSHeadingOffset, //!< heading of robot from GPS or Mirage
00199   };
00200   
00201   enum IRComm_t {
00202     IR_REMOTE_LEFT=129,
00203     IR_REMOTE_FORWARD,
00204     IR_REMOTE_RIGHT,
00205     IR_REMOTE_SPOT,
00206     IR_REMOTE_MAX,
00207     IR_REMOTE_SMALL,
00208     IR_REMOTE_MEDIUM,
00209     IR_REMOTE_LARGE,
00210     IR_REMOTE_PAUSE,
00211     IR_REMOTE_POWER,
00212     IR_REMOTE_ARC_LEFT, 
00213     IR_REMOTE_ARC_RIGHT,
00214     IR_REMOTE_STOP,
00215     IR_REMOTE_SEND,
00216     IR_REMOTE_DOCK,
00217     IR_BASE_RED=248,
00218     IR_BASE_GREEN=244,
00219     IR_BASE_FORCE=242,
00220     IR_BASE_RED_GREEN=252,
00221     IR_BASE_RED_FORCE=250,
00222     IR_BASE_GREEN_FORCE=246,
00223     IR_BASE_RED_GREEN_FORCE=254
00224   };
00225   /*const unsigned IR_BASE_MASK=240;
00226   const unsigned IR_BASE_RED_MASK=8;
00227   const unsigned IR_BASE_GREEN_MASK=4;
00228   const unsigned IR_BASE_FORCE_MASK=2;*/
00229   
00230   enum ChargingState_t {
00231     CHARGING_OFF,
00232     CHARGING_RECONDITIONING,
00233     CHARGING_FULL,
00234     CHARGING_TRICKLE,
00235     CHARGING_WAITING,
00236     CHARGING_FAULT
00237   };
00238 
00239   enum ModeState_t {
00240     MODE_OFF,
00241     MODE_PASSIVE,
00242     MODE_SAFE,
00243     MODE_FULL
00244   };
00245     
00246   //@}
00247 
00248 
00249   namespace WithoutAXS1Sensors {
00250     const unsigned NumSensors     =  22;  //!< the number of sensors available: 19 Create
00251     
00252     //! Provides a string name for each sensor
00253     const char* const sensorNames[NumSensors+1] = { 
00254       "DigitalIn0",
00255       "DigitalIn1",
00256       "DigitalIn2",
00257       "DigitalIn3",
00258       "AnalogIn",
00259       "WallSignal",
00260       "IR",
00261       "CliffLeftSignal",
00262       "CliffFrontLeftSignal",
00263       "CliffFrontRightSignal",
00264       "CliffRight",
00265       "Distance",
00266       "Angle",
00267       "BatteryVoltage",
00268       "BatteryCurrent",
00269       "BatteryCharge",
00270       "BatteryTemp",
00271       "ChargingState",
00272       "ModeState",
00273       "GPSX",
00274       "GPSY",
00275       "GPSHeading",
00276       NULL
00277     };
00278     
00279   }
00280   
00281   namespace WithAXS1Sensors {
00282     const unsigned NumSensors     =  WithoutAXS1Sensors::NumSensors+3+3+2;  //!< the number of sensors available: 19 Create plus 3 IR, 3 luminosity, 2 mic from AX-S1
00283     
00284     enum AXS1SensorOffset_t {
00285       LeftIRDistOffset = GPSHeadingOffset+1,
00286       CenterIRDistOffset,
00287       IRDistOffset = CenterIRDistOffset,
00288       RightIRDistOffset,
00289       LeftLuminosityOffset,
00290       CenterLuminosityOffset,
00291       RightLuminosityOffset,
00292       MicVolumeOffset,
00293       MicSpikeCountOffset
00294     };
00295     
00296     //! Provides a string name for each sensor
00297     const char* const sensorNames[NumSensors+1] = { 
00298       "DigitalIn0",
00299       "DigitalIn1",
00300       "DigitalIn2",
00301       "DigitalIn3",
00302       "AnalogIn",
00303       "WallSignal",
00304       "IR",
00305       "CliffLeftSignal",
00306       "CliffFrontLeftSignal",
00307       "CliffFrontRightSignal",
00308       "CliffRight",
00309       "Distance",
00310       "Angle",
00311       "BatteryVoltage",
00312       "BatteryCurrent",
00313       "BatteryCharge",
00314       "BatteryTemp",
00315       "ChargingState",
00316       "ModeState",
00317       "GPSX",
00318       "GPSY",
00319       "GPSHeading",
00320       "LeftIRDist", "CenterIRDist", "RightIRDist",
00321       "LeftLuminosity", "CenterLuminosity", "RightLuminosity",
00322       "MicVolume", "MicSpikeCount",
00323       NULL
00324     };
00325   }
00326   
00327   namespace WithHead {
00328     const unsigned NumHeadJoints = 2; //!< The number of joints in the pan/tilt
00329     const unsigned HeadOffset  = WheelOffset+NumWheels;   //!< the offset of the beginning of the head joints, add TPROffset_t to get specific joint
00330     
00331     //! The offsets of appendages with pan (heading), tilt (elevation), note that this should be added to HeadOffset, otherwise use HeadOffset_t (#HeadPanOffset and #HeadTiltOffset)
00332     enum TPROffset_t {
00333       PanOffset = 0,      //!< pan/heading (horizontal)
00334       TiltOffset, //!< tilt/elevation (vertical)
00335       NodOffset = TiltOffset //!< replicated tilt (could be left undefined instead...)
00336     };
00337     
00338     //! These are 'absolute' offsets for the neck joints, don't need to add to HeadOffset like TPROffset_t values do
00339     enum HeadOffset_t {
00340       HeadPanOffset = HeadOffset,      //!< pan/heading (horizontal)
00341       HeadTiltOffset, //!< tilt/elevation (vertical)
00342     };
00343   }
00344   
00345   // No arm, no head
00346   namespace CalliopeInfo {
00347     const unsigned FingerJointsPerArm   =  0;
00348     const unsigned JointsPerArm   =  0;
00349     const unsigned NumArms        =  0;
00350     const unsigned NumArmJoints   =  JointsPerArm*NumArms;
00351     const unsigned NumHeadJoints = 0;
00352     const unsigned NumPIDJoints   = NumWheels + NumHeadJoints + NumArmJoints; //!< number of motors and servos
00353     const unsigned NumOutputs     = NumPIDJoints + NumLEDs + 1 /*Create mode*/; //!< the total number of outputs
00354 
00355     const unsigned LEDOffset   = WheelOffset+NumWheels;
00356     const unsigned ModeOffset = LEDOffset + NumLEDs;
00357     
00358     const unsigned BaseFrameOffset   = NumOutputs; //!< Use with kinematics to refer to base reference frame
00359     const unsigned CameraFrameOffset = BaseFrameOffset+1; //!< Use with kinematics to refer to camera reference frame
00360 
00361     //! The offsets of the individual LEDs
00362     /*! @hideinitializer */
00363     enum LEDOffset_t {
00364       PowerRedLEDOffset=LEDOffset,
00365       PowerGreenLEDOffset,
00366       PlayLEDOffset,
00367       AdvanceLEDOffset
00368     };
00369     
00370     const LEDOffset_t RedLEDOffset = PowerRedLEDOffset;
00371     const LEDOffset_t BlueLEDOffset = AdvanceLEDOffset; //!< Create has no blue LED: use Advance LED here, and Advance+PowerRED in BlueLEDMask
00372     const LEDOffset_t GreenLEDOffset = PlayLEDOffset;
00373     const LEDOffset_t YellowLEDOffset = AdvanceLEDOffset;
00374     
00375     typedef unsigned int LEDBitMask_t; //!< So you can be clear when you're refering to a LED bitmask
00376     
00377     const LEDBitMask_t BlueLEDMask = (1<<(AdvanceLEDOffset-LEDOffset)) |
00378     (1<<(PowerRedLEDOffset-LEDOffset)); //!< Create has no blue LED, so use Advance (green) + Power (red)
00379     const LEDBitMask_t GreenLEDMask = 1<<(GreenLEDOffset-LEDOffset); //!< mask corresponding to GreenLEDOffset
00380     const LEDBitMask_t YellowLEDMask = 1<<(YellowLEDOffset-LEDOffset); //!< mask corresponding to YellowLEDOffset
00381     const LEDBitMask_t RedLEDMask = 1<<(RedLEDOffset-LEDOffset); //!< mask corresponding to RedLEDOffset
00382     
00383     const LEDBitMask_t PowerRedLEDMask = 1<<(PowerRedLEDOffset-LEDOffset); //!< mask corresponding to BlueLEDOffset
00384     const LEDBitMask_t PowerGreenLEDMask = 1<<(PowerGreenLEDOffset-LEDOffset); //!< mask corresponding to GreenLEDOffset
00385     const LEDBitMask_t PlayLEDMask = 1<<(PlayLEDOffset-LEDOffset); //!< mask corresponding to YellowLEDOffset
00386     const LEDBitMask_t AdvanceLEDMask = 1<<(AdvanceLEDOffset-LEDOffset); //!< mask corresponding to RedLEDOffset
00387     
00388     //! LEDs for the face panel (all FaceLEDPanelMask<<(0:NumFacePanelLEDs-1) entries)
00389     const LEDBitMask_t FaceLEDMask = 0;
00390     //! selects all of the leds
00391     const LEDBitMask_t AllLEDMask  = (LEDBitMask_t)~0;
00392     //@}
00393 
00394     //! This table holds the default PID values for each joint.  see PIDMC
00395     const float DefaultPIDs[NumPIDJoints][3] = {
00396       {1,0,0},
00397       {1,0,0},
00398     };
00399     
00400     //!These values are our recommended maximum joint velocities, in rad/ms
00401     const float MaxOutputSpeed[NumOutputs] = {
00402       0, 0,   // wheels
00403       0,      // leds
00404       0,
00405       0,
00406       0
00407     };
00408     
00409     //! This table holds the software limits of each of the outputs, first index is the output offset, second index is MinMaxRange_t (i.e. MinRange or MaxRange)
00410     const float outputRanges[NumOutputs][2] =
00411     {
00412       { -500 , 500 }, // left wheel
00413       { -500 , 500 }, // right wheel
00414       {  0 , 1 },
00415       {  0 , 1 },
00416       {  0 , 1 },
00417       {  0 , 1 },
00418       { MODE_PASSIVE, MODE_SAFE }
00419     };
00420     
00421   }
00422   
00423   // no arm, with pan/tilt head and AX-S1
00424   namespace Calliope0 {
00425     using namespace CalliopeComponents::WithHead;
00426     
00427     const unsigned FingerJointsPerArm = 0;
00428     const unsigned JointsPerArm   =  0;
00429     const unsigned NumArms        =  0;
00430     const unsigned NumArmJoints   =  JointsPerArm*NumArms;
00431     const unsigned NumPIDJoints   = NumWheels + NumHeadJoints + NumArmJoints; //!< number of motors and servos
00432     const unsigned NumOutputs     = NumPIDJoints + NumLEDs + 1 /*Create mode*/; //!< the total number of outputs
00433 
00434     const unsigned LEDOffset   = WheelOffset+NumWheels;
00435     const unsigned ModeOffset = LEDOffset + NumLEDs;
00436     
00437     const unsigned BaseFrameOffset   = NumOutputs; //!< Use with kinematics to refer to base reference frame
00438     const unsigned CameraFrameOffset = BaseFrameOffset+1; //!< Use with kinematics to refer to camera reference frame
00439     const unsigned LeftIRFrameOffset = CameraFrameOffset+1; //!< Use with kinematics to refer to left IR distance rangefinder reference frame
00440     const unsigned CenterIRFrameOffset = LeftIRFrameOffset+1; //!< Use with kinematics to refer to center IR distance rangefinder reference frame
00441     const unsigned IRFrameOffset = CenterIRFrameOffset; //!< alias for CenterIRFrameOffset
00442     const unsigned RightIRFrameOffset = CenterIRFrameOffset+1; //!< Use with kinematics to refer to right IR distance rangefinder reference frame
00443     
00444     //! The offsets of the individual LEDs
00445     /*! @hideinitializer */
00446     enum LEDOffset_t {
00447       PowerRedLEDOffset=LEDOffset,
00448       PowerGreenLEDOffset,
00449       PlayLEDOffset,
00450       AdvanceLEDOffset
00451     };
00452     
00453     const LEDOffset_t RedLEDOffset = PowerRedLEDOffset;
00454     const LEDOffset_t BlueLEDOffset = AdvanceLEDOffset; //!< Create has no blue LED: use Advance LED here, and Advance+PowerRED in BlueLEDMask
00455     const LEDOffset_t GreenLEDOffset = PlayLEDOffset;
00456     const LEDOffset_t YellowLEDOffset = AdvanceLEDOffset;
00457     
00458     typedef unsigned int LEDBitMask_t; //!< So you can be clear when you're refering to a LED bitmask
00459     
00460     const LEDBitMask_t BlueLEDMask = (1<<(AdvanceLEDOffset-LEDOffset)) |
00461     (1<<(PowerRedLEDOffset-LEDOffset)); //!< Create has no blue LED, so use Advance (green) + Power (red)
00462     const LEDBitMask_t GreenLEDMask = 1<<(GreenLEDOffset-LEDOffset); //!< mask corresponding to GreenLEDOffset
00463     const LEDBitMask_t YellowLEDMask = 1<<(YellowLEDOffset-LEDOffset); //!< mask corresponding to YellowLEDOffset
00464     const LEDBitMask_t RedLEDMask = 1<<(RedLEDOffset-LEDOffset); //!< mask corresponding to RedLEDOffset
00465     
00466     const LEDBitMask_t PowerRedLEDMask = 1<<(PowerRedLEDOffset-LEDOffset); //!< mask corresponding to BlueLEDOffset
00467     const LEDBitMask_t PowerGreenLEDMask = 1<<(PowerGreenLEDOffset-LEDOffset); //!< mask corresponding to GreenLEDOffset
00468     const LEDBitMask_t PlayLEDMask = 1<<(PlayLEDOffset-LEDOffset); //!< mask corresponding to YellowLEDOffset
00469     const LEDBitMask_t AdvanceLEDMask = 1<<(AdvanceLEDOffset-LEDOffset); //!< mask corresponding to RedLEDOffset
00470     
00471     //! LEDs for the face panel (all FaceLEDPanelMask<<(0:NumFacePanelLEDs-1) entries)
00472     const LEDBitMask_t FaceLEDMask = 0;
00473     //! selects all of the leds
00474     const LEDBitMask_t AllLEDMask  = (LEDBitMask_t)~0;
00475     //@}
00476     //! This table holds the default PID values for each joint.  see PIDMC
00477     const float DefaultPIDs[NumPIDJoints][3] = {
00478       {1,0,0},
00479       {1,0,0},
00480       {32,32,1},
00481       {32,32,1}
00482     };
00483     
00484     //!These values are our recommended maximum joint velocities, in rad/ms
00485     const float MaxOutputSpeed[NumOutputs] = {
00486       0, 0,       // wheels
00487       1.f, 1.f,   // head
00488       0,          // leds
00489       0,
00490       0,
00491       0
00492     };
00493     
00494     //! This table holds the software limits of each of the outputs, first index is the output offset, second index is MinMaxRange_t (i.e. MinRange or MaxRange)
00495     const float outputRanges[NumOutputs][2] =
00496     {
00497       { -500 , 500 }, // left wheel
00498       { -500 , 500 }, // right wheel
00499       {RAD(-150),RAD(150)}, // neck pan
00500       {RAD(-92),RAD(75)}, // neck tilt
00501       {  0 , 1 },
00502       {  0 , 1 },
00503       {  0 , 1 },
00504       {  0 , 1 },
00505       { MODE_PASSIVE, MODE_SAFE }
00506     };
00507     
00508   }
00509   
00510   // 2-dof arm, with pan/tilt head and AX-S1
00511   namespace Calliope2 {
00512     using namespace CalliopeComponents::WithHead;
00513     
00514     const unsigned FingerJointsPerArm = 1;
00515     const unsigned JointsPerArm   =  3; //!< 2 arm joints plus gripper
00516     const unsigned NumArms        =  1;
00517     const unsigned NumArmJoints   =  JointsPerArm*NumArms;
00518     const unsigned NumPIDJoints   = NumWheels + NumHeadJoints + NumArmJoints; //!< number of motors and servos
00519     const unsigned NumOutputs     = NumPIDJoints + NumLEDs + 1 /*Create mode*/; //!< the total number of outputs
00520 
00521     const unsigned ArmOffset = HeadOffset+NumHeadJoints;
00522     const unsigned LEDOffset   = ArmOffset+NumArmJoints;
00523     const unsigned ModeOffset = LEDOffset + NumLEDs;
00524     
00525     const unsigned BaseFrameOffset   = NumOutputs; //!< Use with kinematics to refer to base reference frame
00526     const unsigned GripperFrameOffset = BaseFrameOffset + 1; //!< Use with kinematics to refer to gripper reference frame
00527     const unsigned CameraFrameOffset = GripperFrameOffset+NumArms; //!< Use with kinematics to refer to camera reference frame
00528     const unsigned LeftIRFrameOffset = CameraFrameOffset+1; //!< Use with kinematics to refer to left IR distance rangefinder reference frame
00529     const unsigned CenterIRFrameOffset = LeftIRFrameOffset+1; //!< Use with kinematics to refer to center IR distance rangefinder reference frame
00530     const unsigned IRFrameOffset = CenterIRFrameOffset; //!< alias for CenterIRFrameOffset
00531     const unsigned RightIRFrameOffset = CenterIRFrameOffset+1; //!< Use with kinematics to refer to right IR distance rangefinder reference frame
00532 
00533     //! These are 'absolute' offsets for the arm joints, don't need to add to ArmOffset like TPROffset_t values do
00534     enum ArmOffset_t {
00535       ArmBaseOffset=ArmOffset,
00536       ArmShoulderOffset,
00537       GripperOffset
00538     };
00539     
00540     //! The offsets of the individual LEDs
00541     /*! @hideinitializer */
00542     enum LEDOffset_t {
00543       PowerRedLEDOffset=LEDOffset,
00544       PowerGreenLEDOffset,
00545       PlayLEDOffset,
00546       AdvanceLEDOffset
00547     };
00548     
00549     const LEDOffset_t RedLEDOffset = PowerRedLEDOffset;
00550     const LEDOffset_t BlueLEDOffset = AdvanceLEDOffset; //!< Create has no blue LED: use Advance LED here, and Advance+PowerRED in BlueLEDMask
00551     const LEDOffset_t GreenLEDOffset = PlayLEDOffset;
00552     const LEDOffset_t YellowLEDOffset = AdvanceLEDOffset;
00553     
00554     typedef unsigned int LEDBitMask_t; //!< So you can be clear when you're refering to a LED bitmask
00555     
00556     const LEDBitMask_t BlueLEDMask = (1<<(AdvanceLEDOffset-LEDOffset)) |
00557     (1<<(PowerRedLEDOffset-LEDOffset)); //!< Create has no blue LED, so use Advance (green) + Power (red)
00558     const LEDBitMask_t GreenLEDMask = 1<<(GreenLEDOffset-LEDOffset); //!< mask corresponding to GreenLEDOffset
00559     const LEDBitMask_t YellowLEDMask = 1<<(YellowLEDOffset-LEDOffset); //!< mask corresponding to YellowLEDOffset
00560     const LEDBitMask_t RedLEDMask = 1<<(RedLEDOffset-LEDOffset); //!< mask corresponding to RedLEDOffset
00561     
00562     const LEDBitMask_t PowerRedLEDMask = 1<<(PowerRedLEDOffset-LEDOffset); //!< mask corresponding to BlueLEDOffset
00563     const LEDBitMask_t PowerGreenLEDMask = 1<<(PowerGreenLEDOffset-LEDOffset); //!< mask corresponding to GreenLEDOffset
00564     const LEDBitMask_t PlayLEDMask = 1<<(PlayLEDOffset-LEDOffset); //!< mask corresponding to YellowLEDOffset
00565     const LEDBitMask_t AdvanceLEDMask = 1<<(AdvanceLEDOffset-LEDOffset); //!< mask corresponding to RedLEDOffset
00566     
00567     //! LEDs for the face panel (all FaceLEDPanelMask<<(0:NumFacePanelLEDs-1) entries)
00568     const LEDBitMask_t FaceLEDMask = 0;
00569     //! selects all of the leds
00570     const LEDBitMask_t AllLEDMask  = (LEDBitMask_t)~0;
00571     //@}
00572     //! This table holds the default PID values for each joint.  see PIDMC
00573     const float DefaultPIDs[NumPIDJoints][3] = {
00574       {1,0,0},
00575       {1,0,0},
00576       {32,32,1},
00577       {32,32,1},
00578       {32,32,1},
00579       {32,32,1},
00580       {32,32,1}
00581     };
00582     
00583     //!These values are our recommended maximum joint velocities, in rad/ms
00584     const float MaxOutputSpeed[NumOutputs] = {
00585       0, 0,   // wheels
00586       1.f, 1.f,   // head
00587       1.f, 1.f, 1.f, // arm
00588       0,
00589       0,
00590       0,
00591       0,
00592       0
00593     };
00594     
00595     //! This table holds the software limits of each of the outputs, first index is the output offset, second index is MinMaxRange_t (i.e. MinRange or MaxRange)
00596     const float outputRanges[NumOutputs][2] =
00597     {
00598       { -500 , 500 }, // left wheel
00599       { -500 , 500 }, // right wheel
00600       {RAD(-150),RAD(150)}, // neck pan
00601       {RAD(-92),RAD(75)}, // neck tilt
00602       {RAD(-150),RAD(75)}, // arm base
00603       {RAD(-60),RAD(49)}, // arm shoulder
00604       {RAD(0),RAD(130)}, // gripper
00605       {  0 , 1 },
00606       {  0 , 1 },
00607       {  0 , 1 },
00608       {  0 , 1 },
00609       { MODE_PASSIVE, MODE_SAFE }
00610     };
00611     
00612   }
00613   
00614   // 5-dof arm, with pan/tilt head (but see sub-spaces re: AX-S1...)
00615   namespace Calliope5 {
00616     using namespace CalliopeComponents::WithHead;
00617     
00618     const unsigned FingerJointsPerArm = 2;
00619     const unsigned JointsPerArm   =  7; //!< 5 arms joints plus two fingers
00620     const unsigned NumArms        =  1;
00621     const unsigned NumArmJoints   =  JointsPerArm*NumArms;
00622     const unsigned NumPIDJoints   = NumWheels + NumHeadJoints + NumArmJoints; //!< number of motors and servos
00623     const unsigned NumOutputs     = NumPIDJoints + NumLEDs + 1 /*Create mode*/; //!< the total number of outputs
00624 
00625     const unsigned ArmOffset = HeadOffset+NumHeadJoints;
00626     const unsigned LEDOffset   = ArmOffset+NumArmJoints;
00627     const unsigned ModeOffset = LEDOffset + NumLEDs;
00628     
00629     const unsigned BaseFrameOffset   = NumOutputs; //!< Use with kinematics to refer to base reference frame
00630     const unsigned GripperFrameOffset = BaseFrameOffset + 1; //!< Use with kinematics to refer to gripper reference frame
00631     const unsigned LeftFingerFrameOffset = GripperFrameOffset+1; //!< Use with kinematics to refer to the left finger reference frame
00632     const unsigned RightFingerFrameOffset = LeftFingerFrameOffset+1; //!< Use with kinematics to refer to the right finger reference frame
00633     const unsigned CameraFrameOffset = RightFingerFrameOffset+NumArms; //!< Use with kinematics to refer to camera reference frame
00634 
00635     //! These are 'absolute' offsets for the arm joints, don't need to add to ArmOffset like TPROffset_t values do
00636     enum ArmOffset_t {
00637       ArmBaseOffset=ArmOffset,
00638       ArmShoulderOffset,
00639       ArmElbowOffset,
00640       ArmWristOffset,
00641       WristRotateOffset,
00642       LeftFingerOffset,
00643       RightFingerOffset
00644     };
00645     
00646     namespace WithoutAXS1Sensors {
00647       using namespace Calliope5;
00648       using namespace CalliopeComponents::WithoutAXS1Sensors;
00649     }
00650     namespace WithAXS1Sensors {
00651       using namespace Calliope5;
00652       using namespace CalliopeComponents::WithAXS1Sensors;
00653       const unsigned LeftIRFrameOffset = CameraFrameOffset+1; //!< Use with kinematics to refer to left IR distance rangefinder reference frame
00654       const unsigned CenterIRFrameOffset = LeftIRFrameOffset+1; //!< Use with kinematics to refer to center IR distance rangefinder reference frame
00655       const unsigned IRFrameOffset = CenterIRFrameOffset; //!< alias for CenterIRFrameOffset
00656       const unsigned RightIRFrameOffset = CenterIRFrameOffset+1; //!< Use with kinematics to refer to right IR distance rangefinder reference frame
00657     }
00658 
00659     //! The offsets of the individual LEDs
00660     /*! @hideinitializer */
00661     enum LEDOffset_t {
00662       PowerRedLEDOffset=LEDOffset,
00663       PowerGreenLEDOffset,
00664       PlayLEDOffset,
00665       AdvanceLEDOffset
00666     };
00667     
00668     const LEDOffset_t RedLEDOffset = PowerRedLEDOffset;
00669     const LEDOffset_t BlueLEDOffset = AdvanceLEDOffset; //!< Create has no blue LED: use Advance LED here, and Advance+PowerRED in BlueLEDMask
00670     const LEDOffset_t GreenLEDOffset = PlayLEDOffset;
00671     const LEDOffset_t YellowLEDOffset = AdvanceLEDOffset;
00672     
00673     typedef unsigned int LEDBitMask_t; //!< So you can be clear when you're refering to a LED bitmask
00674     
00675     const LEDBitMask_t BlueLEDMask = (1<<(AdvanceLEDOffset-LEDOffset)) |
00676     (1<<(PowerRedLEDOffset-LEDOffset)); //!< Create has no blue LED, so use Advance (green) + Power (red)
00677     const LEDBitMask_t GreenLEDMask = 1<<(GreenLEDOffset-LEDOffset); //!< mask corresponding to GreenLEDOffset
00678     const LEDBitMask_t YellowLEDMask = 1<<(YellowLEDOffset-LEDOffset); //!< mask corresponding to YellowLEDOffset
00679     const LEDBitMask_t RedLEDMask = 1<<(RedLEDOffset-LEDOffset); //!< mask corresponding to RedLEDOffset
00680     
00681     const LEDBitMask_t PowerRedLEDMask = 1<<(PowerRedLEDOffset-LEDOffset); //!< mask corresponding to BlueLEDOffset
00682     const LEDBitMask_t PowerGreenLEDMask = 1<<(PowerGreenLEDOffset-LEDOffset); //!< mask corresponding to GreenLEDOffset
00683     const LEDBitMask_t PlayLEDMask = 1<<(PlayLEDOffset-LEDOffset); //!< mask corresponding to YellowLEDOffset
00684     const LEDBitMask_t AdvanceLEDMask = 1<<(AdvanceLEDOffset-LEDOffset); //!< mask corresponding to RedLEDOffset
00685     
00686     //! LEDs for the face panel (all FaceLEDPanelMask<<(0:NumFacePanelLEDs-1) entries)
00687     const LEDBitMask_t FaceLEDMask = 0;
00688     //! selects all of the leds
00689     const LEDBitMask_t AllLEDMask  = (LEDBitMask_t)~0;
00690     //@}
00691     //! This table holds the default PID values for each joint.  see PIDMC
00692     const float DefaultPIDs[NumPIDJoints][3] = {
00693       {1,0,0},
00694       {1,0,0},
00695       {32,32,1},
00696       {32,32,1},
00697       {32,32,1},
00698       {32,32,1},
00699       {32,32,1},
00700       {32,32,1},
00701       {32,32,1},
00702       {32,32,1},
00703       {32,32,1}
00704     };
00705     
00706     //!These values are our recommended maximum joint velocities, in rad/s
00707     const float MaxOutputSpeed[NumOutputs] = {
00708       0, 0,       // wheels
00709       1.0, 1.0,   // head pan and tilt
00710       1.f,        // arm base
00711       1.f,        // shoulder
00712       1.f,        // elbow
00713       1.f,        // wrist
00714       1.f,        // wrist rotate
00715       1.f, 1.f,   // left and right finger
00716       0,0,0,0     // Create LEDs
00717     };
00718     
00719     //! This table holds the software limits of each of the outputs, first index is the output offset, second index is MinMaxRange_t (i.e. MinRange or MaxRange)
00720     const float outputRanges[NumOutputs][2] =
00721     {
00722       {    -500 ,    500 }, // left wheel
00723       {    -500 ,    500 }, // right wheel
00724       {RAD(-150),RAD(150)}, // neck pan
00725       {RAD(-99) ,RAD(107)}, // neck tilt
00726       {RAD( -95),RAD( 84)}, // arm base
00727       {RAD( -55),RAD(149)}, // arm shoulder
00728       {RAD(-149),RAD(149)}, // arm elbow
00729       {RAD(-132),RAD(130)}, // arm wrist
00730       {RAD(-149),RAD(149)}, // arm wristrot
00731       {RAD( -70),RAD( 25)}, // arm left finger
00732       {RAD( -25),RAD( 70)}, // arm right finger
00733       {       0 ,      1 }, // LED power red
00734       {       0 ,      1 }, // LED power green
00735       {       0 ,      1 }, // LED play
00736       {       0 ,      1 }, // LED advance
00737       { MODE_PASSIVE, MODE_SAFE }
00738     };
00739     
00740   }
00741   
00742 }
00743 
00744 #endif

Tekkotsu v5.1CVS
Generated Mon May 9 04:58:36 2016 by Doxygen 1.6.3