Homepage Demos Overview Downloads Tutorials Reference
Credits
Main Page | Namespace List | Class Hierarchy | Alphabetical List | Compound List | File List | Namespace Members | Compound Members | File Members | Related Pages | Search

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 can be a one shot deal if you set the autoprune flag as you're adding it (which is the default) */
00011 class PIDMC : public MotionCommand {
00012 public:
00013   //!Constructor, uses default PIDs and 0 weight for all
00014   PIDMC() : MotionCommand(), dirty(false) {
00015     setDefaults(0);
00016     dirty=false;
00017   }
00018   //!Constructor, sets general power level of all 
00019   PIDMC(float powerlevel, float w=1) : MotionCommand(), dirty(false) {
00020     setAllPowerLevel(powerlevel,w);
00021   }
00022   //!Constructor, sets general power level of a range of joints, uses default and 0 weight for others 
00023   PIDMC(unsigned int low, unsigned int high, float powerlevel, float w=1) : MotionCommand(), dirty(false) {
00024     setRangePowerLevel(PIDJointOffset,low,1.f,0.f);
00025     setRangePowerLevel(low,high,powerlevel,w);
00026     setRangePowerLevel(high,PIDJointOffset+NumPIDJoints,1.f,0.f);
00027   }
00028   //!Destructor
00029   virtual ~PIDMC() {}
00030 
00031   //!Inherited
00032   //@{
00033   virtual int updateOutputs() {
00034     for(unsigned int i=PIDJointOffset; i<PIDJointOffset+NumPIDJoints; i++)
00035       motman->setOutput(this,i+PIDJointOffset,PIDs[i]);
00036     int wasDirty=dirty; dirty=false; return wasDirty;
00037   }
00038   virtual int isDirty() { return dirty; }
00039   virtual int isAlive() { return dirty; }
00040   //@}
00041 
00042   //!Sets the PIDs to the defaults specified in RobotInfo
00043   void setDefaults(float weight=1) {
00044     setAllPowerLevel(1.f,weight);
00045   }
00046 
00047   //!Sets the PIDs to a percentage of default for a given joint, and sets weight
00048   void setJointPowerLevel(unsigned int i, float p, float w=1) {
00049     i-=PIDJointOffset;
00050     for(unsigned int j=0;j<3;j++)
00051       PIDs[i].pid[j]=DefaultPIDs[i][j]*p;
00052     PIDs[i].weight=w;
00053     dirty=true;
00054   }
00055 
00056   //!Sets the PIDs to a percentage of default for all joints
00057   void setAllPowerLevel(float p, float w=1) {
00058     for(unsigned int i=0; i<NumPIDJoints; i++) {
00059       for(unsigned int j=0;j<3;j++)
00060         PIDs[i].pid[j]=DefaultPIDs[i][j]*p;
00061       PIDs[i].weight=w;
00062     }
00063     dirty=true;
00064   }
00065 
00066   //!Sets a range of joints' PIDs to a given power level and weight
00067   void setRangePowerLevel(unsigned int low, unsigned int high, float p, float w=1) {
00068     low-=PIDJointOffset;
00069     high-=PIDJointOffset;
00070     for(unsigned int i=low; i<high; i++) {
00071       for(unsigned int j=0;j<3;j++)
00072         PIDs[i].pid[j]=DefaultPIDs[i][j]*p;
00073       PIDs[i].weight=w;
00074     }
00075     dirty=true;
00076   }
00077 
00078 
00079   //!Use this to set the PID value and weight
00080   void setPID(unsigned int i, const OutputPID& pid) {
00081     i-=PIDJointOffset;
00082     PIDs[i]=pid;
00083     dirty=true;
00084   }
00085 
00086   //!Use this if you want to double check the PID you set
00087   OutputPID& getPID(unsigned int i) {
00088     return PIDs[i-PIDJointOffset];
00089   }
00090 
00091   //!Use this if you want to double check the PID you set
00092   const OutputPID& getPID(unsigned int i) const {
00093     return PIDs[i-PIDJointOffset];
00094   }
00095 
00096 protected:
00097   //! returns true if the output i is a PID joint
00098   static inline bool isPID(unsigned int i) {
00099     return ((int)i>=(int)PIDJointOffset && i<PIDJointOffset+NumPIDJoints); //casting to int just to get rid of compiler warning.. sigh
00100   }
00101 
00102   bool dirty; //!< true if there are changes that have not been picked up by Motion
00103   OutputPID PIDs[NumPIDJoints]; //!< the PIDs being requested
00104 };
00105 
00106 /*! @file
00107  * @brief Defines PIDMC, a nice little MotionCommand for manually manipulating the PID values
00108  * @author ejt (Creator)
00109  *
00110  * $Author: ejt $
00111  * $Name: tekkotsu-1_4_1 $
00112  * $Revision: 1.9 $
00113  * $State: Exp $
00114  * $Date: 2003/06/12 23:41:40 $
00115  */
00116 
00117 #endif

Tekkotsu v1.4
Generated Sat Jul 19 00:06:31 2003 by Doxygen 1.3.2