Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

PIDMC.h

Go to the documentation of this file.
00001 //-*-c++-*-
00002 #ifndef INCLUDED_PIDMC_h_
00003 #define INCLUDED_PIDMC_h_
00004 
00005 #include "MotionCommand.h"
00006 #include "MotionManager.h"
00007 #include "OutputPID.h"
00008 
00009 //! A nice little MotionCommand for manually manipulating the PID values
00010 /*! This will, by default, autoprune after its PID values have been set.
00011  *  
00012  *  Although this doesn't handle directly sending PID commands to the
00013  *  system (the MotionManager::updatePIDs() does that) a little
00014  *  documentation on how Tekkotsu ad OPEN-R handle the PIDs might be
00015  *  nice.
00016  *
00017  *  In Tekkotsu, each of P,I, and D are a single floating point
00018  *  number.  However, OPEN-R has, in essence, a limited precision
00019  *  floating point number.  Each value is broken into a gain
00020  *  (mantissa) and shift (exponent).  The shifts appear to be 4 bit
00021  *  values, and are inverted.  In other words, x = g/(1<<(0x10-s)), or
00022  *  @f$ x = \frac{g}{2^{16-s}} @f$ The gain is probably 6-8 bits of
00023  *  resolution.
00024  *
00025  *  On the ERS-2xx series, each joint is completely independent.  One
00026  *  caveat is that the shift value 0x0A gives a warning
00027  *  (AGRMSDriver::SetGain() : 0x0A IS USED FOR GAIN SHIFT VALUE.) for
00028  *  some unknown reason.
00029  *
00030  *  On the ERS-7, all the joints share the last set shift values, so
00031  *  a global set of shifts must be enforced.  This of course, pretty
00032  *  much kills the whole point of having the shifts.
00033  *
00034  *  To understand the conversion from Tekkotsu format to the
00035  *  OPEN-R format, see MotionManager::updatePIDs().
00036  *  
00037  *  A final note: the OPENR::SetJointGain function seems to be
00038  *  a rather costly function call.  You should probably try to avoid
00039  *  setting PIDs at too high a frequency.
00040  */
00041 class PIDMC : public MotionCommand {
00042 public:
00043   //!Constructor, uses default PIDs and 0 weight for all
00044   PIDMC() : MotionCommand(), dirty(false) {
00045     setDefaults(0);
00046     dirty=false;
00047   }
00048   //!Constructor, sets general power level of all 
00049   PIDMC(float powerlevel, float w=1) : MotionCommand(), dirty(false) {
00050     setAllPowerLevel(powerlevel,w);
00051   }
00052   //!Constructor, sets general power level of a range of joints, uses default and 0 weight for others, see setRangePowerLevel()
00053   /*! @param low the first joint to apply @a powerlevel to
00054    *  @param high one-past last joint to apply @a powerlevel to (i.e. exclusive upper limit)
00055    *  @param powerlevel scaling factor for all of the default PID parameters
00056    *  @param w MotionManager weight for averaging conflicting commands
00057    *
00058    *  Note that if you want to set a single joint @e i, then @a low = @e i, and @a high = @e i + 1 because high is an exclusive upper limit*/
00059   PIDMC(unsigned int low, unsigned int high, float powerlevel, float w=1) : MotionCommand(), dirty(false) {
00060     setRangePowerLevel(PIDJointOffset,low,1.f,0.f);
00061     setRangePowerLevel(low,high,powerlevel,w);
00062     setRangePowerLevel(high,PIDJointOffset+NumPIDJoints,1.f,0.f);
00063   }
00064   //!Destructor
00065   virtual ~PIDMC() {}
00066 
00067   //!Inherited
00068   //@{
00069   virtual int updateOutputs() {
00070     for(unsigned int i=PIDJointOffset; i<PIDJointOffset+NumPIDJoints; i++)
00071       motman->setOutput(this,i+PIDJointOffset,PIDs[i]);
00072     int wasDirty=dirty; dirty=false; return wasDirty;
00073   }
00074   virtual int isDirty() { return dirty; }
00075   virtual int isAlive() { return dirty; }
00076   virtual void DoStart() { MotionCommand::DoStart(); dirty=true; } //!< marks this as dirty each time it is added
00077   //@}
00078 
00079   //!Sets the PIDs to the defaults specified in RobotInfo
00080   void setDefaults(float weight=1) {
00081     setAllPowerLevel(1.f,weight);
00082   }
00083 
00084   //!Sets the PIDs to a percentage of default for a given joint, and sets weight
00085   void setJointPowerLevel(unsigned int i, float p, float w=1) {
00086     i-=PIDJointOffset;
00087     for(unsigned int j=0;j<3;j++)
00088       PIDs[i].pid[j]=DefaultPIDs[i][j]*p;
00089     PIDs[i].weight=w;
00090     dirty=true;
00091   }
00092 
00093   //!Sets the PIDs to a percentage of default for all joints
00094   void setAllPowerLevel(float p, float w=1) {
00095     for(unsigned int i=0; i<NumPIDJoints; i++) {
00096       for(unsigned int j=0;j<3;j++)
00097         PIDs[i].pid[j]=DefaultPIDs[i][j]*p;
00098       PIDs[i].weight=w;
00099     }
00100     dirty=true;
00101   }
00102 
00103   //!Sets a range of joints' PIDs to a given power level and weight
00104   /*! @param low the first joint to apply power level @a p to
00105    *  @param high one-past last joint to apply power level @a p to (i.e. exclusive upper limit)
00106    *  @param p scaling factor for all of the default PID parameters
00107    *  @param w MotionManager weight for averaging conflicting commands
00108    *
00109    *  Note that if you want to set a single joint @e i with this function (as opposed to setJointPowerLevel() which is designed for that...)
00110    *  then you would need to pass @a low = @e i, and @a high = @e i + 1 because high is an exclusive upper limit*/
00111   void setRangePowerLevel(unsigned int low, unsigned int high, float p, float w=1) {
00112     low-=PIDJointOffset;
00113     high-=PIDJointOffset;
00114     for(unsigned int i=low; i<high; i++) {
00115       for(unsigned int j=0;j<3;j++)
00116         PIDs[i].pid[j]=DefaultPIDs[i][j]*p;
00117       PIDs[i].weight=w;
00118     }
00119     dirty=true;
00120   }
00121 
00122 
00123   //!Use this to set the PID value and weight
00124   void setPID(unsigned int i, const OutputPID& pid) {
00125     i-=PIDJointOffset;
00126     PIDs[i]=pid;
00127     dirty=true;
00128   }
00129 
00130   //!Use this if you want to double check the PID you set
00131   OutputPID& getPID(unsigned int i) {
00132     return PIDs[i-PIDJointOffset];
00133   }
00134 
00135   //!Use this if you want to double check the PID you set
00136   const OutputPID& getPID(unsigned int i) const {
00137     return PIDs[i-PIDJointOffset];
00138   }
00139 
00140 protected:
00141   //! returns true if the output i is a PID joint
00142   static inline bool isPID(unsigned int i) {
00143     return ((int)i>=(int)PIDJointOffset && i<PIDJointOffset+NumPIDJoints); //casting to int just to get rid of compiler warning.. sigh
00144   }
00145 
00146   bool dirty; //!< true if there are changes that have not been picked up by Motion
00147   OutputPID PIDs[NumPIDJoints]; //!< the PIDs being requested
00148 };
00149 
00150 /*! @file
00151  * @brief Defines PIDMC, a nice little MotionCommand for manually manipulating the PID values
00152  * @author ejt (Creator)
00153  *
00154  * $Author: ejt $
00155  * $Name: tekkotsu-4_0 $
00156  * $Revision: 1.13 $
00157  * $State: Exp $
00158  * $Date: 2006/10/30 20:21:50 $
00159  */
00160 
00161 #endif

Tekkotsu v4.0
Generated Thu Nov 22 00:54:54 2007 by Doxygen 1.5.4