Tekkotsu Homepage | Demos | Overview | Downloads | Dev. Resources | Reference | Credits |
PIDMC.hGo 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 |