Homepage Demos Overview Downloads Tutorials 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 
00053   PIDMC(unsigned int low, unsigned int high, float powerlevel, float w=1) : MotionCommand(), dirty(false) {
00054     setRangePowerLevel(PIDJointOffset,low,1.f,0.f);
00055     setRangePowerLevel(low,high,powerlevel,w);
00056     setRangePowerLevel(high,PIDJointOffset+NumPIDJoints,1.f,0.f);
00057   }
00058   //!Destructor
00059   virtual ~PIDMC() {}
00060 
00061   //!Inherited
00062   //@{
00063   virtual int updateOutputs() {
00064     for(unsigned int i=PIDJointOffset; i<PIDJointOffset+NumPIDJoints; i++)
00065       motman->setOutput(this,i+PIDJointOffset,PIDs[i]);
00066     int wasDirty=dirty; dirty=false; return wasDirty;
00067   }
00068   virtual int isDirty() { return dirty; }
00069   virtual int isAlive() { return dirty; }
00070   //@}
00071 
00072   //!Sets the PIDs to the defaults specified in RobotInfo
00073   void setDefaults(float weight=1) {
00074     setAllPowerLevel(1.f,weight);
00075   }
00076 
00077   //!Sets the PIDs to a percentage of default for a given joint, and sets weight
00078   void setJointPowerLevel(unsigned int i, float p, float w=1) {
00079     i-=PIDJointOffset;
00080     for(unsigned int j=0;j<3;j++)
00081       PIDs[i].pid[j]=DefaultPIDs[i][j]*p;
00082     PIDs[i].weight=w;
00083     dirty=true;
00084   }
00085 
00086   //!Sets the PIDs to a percentage of default for all joints
00087   void setAllPowerLevel(float p, float w=1) {
00088     for(unsigned int i=0; i<NumPIDJoints; i++) {
00089       for(unsigned int j=0;j<3;j++)
00090         PIDs[i].pid[j]=DefaultPIDs[i][j]*p;
00091       PIDs[i].weight=w;
00092     }
00093     dirty=true;
00094   }
00095 
00096   //!Sets a range of joints' PIDs to a given power level and weight
00097   void setRangePowerLevel(unsigned int low, unsigned int high, float p, float w=1) {
00098     low-=PIDJointOffset;
00099     high-=PIDJointOffset;
00100     for(unsigned int i=low; i<high; i++) {
00101       for(unsigned int j=0;j<3;j++)
00102         PIDs[i].pid[j]=DefaultPIDs[i][j]*p;
00103       PIDs[i].weight=w;
00104     }
00105     dirty=true;
00106   }
00107 
00108 
00109   //!Use this to set the PID value and weight
00110   void setPID(unsigned int i, const OutputPID& pid) {
00111     i-=PIDJointOffset;
00112     PIDs[i]=pid;
00113     dirty=true;
00114   }
00115 
00116   //!Use this if you want to double check the PID you set
00117   OutputPID& getPID(unsigned int i) {
00118     return PIDs[i-PIDJointOffset];
00119   }
00120 
00121   //!Use this if you want to double check the PID you set
00122   const OutputPID& getPID(unsigned int i) const {
00123     return PIDs[i-PIDJointOffset];
00124   }
00125 
00126 protected:
00127   //! returns true if the output i is a PID joint
00128   static inline bool isPID(unsigned int i) {
00129     return ((int)i>=(int)PIDJointOffset && i<PIDJointOffset+NumPIDJoints); //casting to int just to get rid of compiler warning.. sigh
00130   }
00131 
00132   bool dirty; //!< true if there are changes that have not been picked up by Motion
00133   OutputPID PIDs[NumPIDJoints]; //!< the PIDs being requested
00134 };
00135 
00136 /*! @file
00137  * @brief Defines PIDMC, a nice little MotionCommand for manually manipulating the PID values
00138  * @author ejt (Creator)
00139  *
00140  * $Author: ejt $
00141  * $Name: tekkotsu-2_2 $
00142  * $Revision: 1.11 $
00143  * $State: Exp $
00144  * $Date: 2004/01/14 20:45:01 $
00145  */
00146 
00147 #endif

Tekkotsu v2.2
Generated Tue Oct 19 14:19:15 2004 by Doxygen 1.3.9.1