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

Tekkotsu v2.4.1
Generated Tue Aug 16 16:32:48 2005 by Doxygen 1.4.4