pidcontrol.cpp

00001 #include "pidcontrol.h"
00002 
00003 //-----------------------------------------------------------------------------
00004 
00005 PIDControl::PIDControl()  :
00006     m_dState( 0.0 ),
00007     m_iState( 0.0 ),
00008     m_iMax( 1.0 ),
00009     m_iMin( 0.0 ),
00010     m_iGain( 0.1 ),
00011     m_pGain( 0.075 ),
00012     m_dGain( 0.9 )
00013 {
00014 }
00015 
00016 //-----------------------------------------------------------------------------
00017 
00018 PIDControl::~PIDControl()
00019 {
00020 }
00021 
00022 //-----------------------------------------------------------------------------
00023 
00024 PIDControl & PIDControl::setPIDGains( double pGain, double iGain, double dGain )
00025 {
00026     m_pGain = pGain;
00027     m_iGain = iGain;
00028     m_dGain = dGain;
00029     return *this;
00030 }
00031 
00032 //-----------------------------------------------------------------------------
00033 
00034 PIDControl & PIDControl::setIntegratorLimits( double iMin, double iMax )
00035 {
00036     m_iMin = iMin;
00037     m_iMax = iMax;
00038     return *this;
00039 }
00040 
00041 //-----------------------------------------------------------------------------
00042 
00043 double PIDControl::update( double error, double position )
00044 {
00045     // calculate proportional term
00046     double pTerm = m_pGain * error;
00047 
00048     // calculate integral state with appropriate limiting
00049     m_iState += error;
00050     if ( m_iState > m_iMax )
00051         m_iState = m_iMax;
00052     else if ( m_iState < m_iMin )
00053         m_iState = m_iMin;
00054 
00055     // calculate integral term
00056     double iTerm = m_iGain * m_iState;
00057 
00058     // calculate derivative term
00059     double dTerm = m_dGain * (m_dState - position);
00060     m_dState = position;
00061 
00062     return pTerm + dTerm + iTerm;
00063 }