pid_controller.cpp
Go to the documentation of this file.
00001 /*
00002  *    This file is part of ACADO Toolkit.
00003  *
00004  *    ACADO Toolkit -- A Toolkit for Automatic Control and Dynamic Optimization.
00005  *    Copyright (C) 2008-2014 by Boris Houska, Hans Joachim Ferreau,
00006  *    Milan Vukov, Rien Quirynen, KU Leuven.
00007  *    Developed within the Optimization in Engineering Center (OPTEC)
00008  *    under supervision of Moritz Diehl. All rights reserved.
00009  *
00010  *    ACADO Toolkit is free software; you can redistribute it and/or
00011  *    modify it under the terms of the GNU Lesser General Public
00012  *    License as published by the Free Software Foundation; either
00013  *    version 3 of the License, or (at your option) any later version.
00014  *
00015  *    ACADO Toolkit is distributed in the hope that it will be useful,
00016  *    but WITHOUT ANY WARRANTY; without even the implied warranty of
00017  *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00018  *    Lesser General Public License for more details.
00019  *
00020  *    You should have received a copy of the GNU Lesser General Public
00021  *    License along with ACADO Toolkit; if not, write to the Free Software
00022  *    Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
00023  *
00024  */
00025 
00026 
00034 #include <acado/control_law/pid_controller.hpp>
00035 
00036 
00037 
00038 BEGIN_NAMESPACE_ACADO
00039 
00040 
00041 
00042 //
00043 // PUBLIC MEMBER FUNCTIONS:
00044 //
00045 
00046 PIDcontroller::PIDcontroller( ) : ControlLaw( ), ClippingFunctionality( )
00047 {
00048         nInputs  = 0;
00049         nOutputs = 0;
00050 
00051         setStatus( BS_NOT_INITIALIZED );
00052 }
00053 
00054 
00055 PIDcontroller::PIDcontroller(   uint _nInputs,
00056                                                                 uint _nOutputs,
00057                                                                 double _samplingTime
00058                                                                 ) : ControlLaw( _samplingTime ), ClippingFunctionality( _nOutputs )
00059 {
00060         if ( ( _nOutputs != _nInputs ) && ( _nOutputs != 1 ) )
00061         {
00062                 _nOutputs = 1;
00063                 ACADOERROR( RET_INVALID_PID_OUTPUT_DIMENSION );
00064         }
00065 
00066         nInputs  = _nInputs;
00067         nOutputs = _nOutputs;
00068 
00069         pWeights.init( nInputs );
00070         pWeights.setZero( );
00071 
00072         iWeights.init( nInputs );
00073         iWeights.setZero( );
00074 
00075         dWeights.init( nInputs );
00076         dWeights.setZero( );
00077 
00078         iValue.init( nInputs );
00079         iValue.setZero( );
00080 
00081         lastError.init( nInputs );
00082         lastError.setZero( );
00083 
00084         setStatus( BS_READY );
00085 }
00086 
00087 
00088 PIDcontroller::PIDcontroller( const PIDcontroller& rhs ) : ControlLaw( rhs ), ClippingFunctionality( rhs )
00089 {
00090         nInputs  = rhs.nInputs;
00091         nOutputs = rhs.nOutputs;
00092 
00093         pWeights = rhs.pWeights;
00094         iWeights = rhs.iWeights;
00095         dWeights = rhs.dWeights;
00096 
00097         iValue    = rhs.iValue;
00098         lastError = rhs.lastError;
00099 }
00100 
00101 
00102 PIDcontroller::~PIDcontroller( )
00103 {
00104 }
00105 
00106 
00107 PIDcontroller& PIDcontroller::operator=( const PIDcontroller& rhs )
00108 {
00109         if ( this != &rhs )
00110         {
00111                 ControlLaw::operator=( rhs );
00112                 ClippingFunctionality::operator=( rhs );
00113 
00114                 nInputs  = rhs.nInputs;
00115                 nOutputs = rhs.nOutputs;
00116                 
00117                 pWeights = rhs.pWeights;
00118                 iWeights = rhs.iWeights;
00119                 dWeights = rhs.dWeights;
00120 
00121                 iValue    = rhs.iValue;
00122                 lastError = rhs.lastError;
00123         }
00124 
00125     return *this;
00126 }
00127 
00128 
00129 ControlLaw* PIDcontroller::clone( ) const
00130 {
00131         return new PIDcontroller( *this );
00132 }
00133 
00134 
00135 
00136 returnValue PIDcontroller::setProportionalWeights(      const DVector& _pWeights
00137                                                                                                         )
00138 {
00139         if ( _pWeights.getDim() != getNumInputs( ) )
00140                 return ACADOERROR( RET_VECTOR_DIMENSION_MISMATCH );
00141 
00142         pWeights = _pWeights;
00143         
00144         return SUCCESSFUL_RETURN;
00145 }
00146 
00147 
00148 returnValue PIDcontroller::setIntegralWeights(          const DVector& _iWeights
00149                                                                                                         )
00150 {
00151         if ( _iWeights.getDim() != getNumInputs( ) )
00152                 return ACADOERROR( RET_VECTOR_DIMENSION_MISMATCH );
00153 
00154         iWeights = _iWeights;
00155         
00156         return SUCCESSFUL_RETURN;
00157 }
00158 
00159 
00160 returnValue PIDcontroller::setDerivativeWeights(        const DVector& _dWeights
00161                                                                                                         )
00162 
00163 {
00164         if ( _dWeights.getDim() != getNumInputs( ) )
00165                 return ACADOERROR( RET_VECTOR_DIMENSION_MISMATCH );
00166 
00167         dWeights = _dWeights;
00168         
00169         return SUCCESSFUL_RETURN;
00170 }
00171 
00172 
00173 returnValue PIDcontroller::init(        double startTime,
00174                                                                         const DVector &x0_,
00175                                                                         const DVector &p_,
00176                                                                         const VariablesGrid& _yRef
00177                                                                         )
00178 {
00179         if ( x0_.getDim( ) != getNumInputs( ) )
00180                 return ACADOERROR( RET_VECTOR_DIMENSION_MISMATCH );
00181 
00182         // Use reference trajectory if it is defined, 
00183         // otherwise set default reference to zero
00184         DVector xRef( x0_.getDim() );
00185 
00186         if ( _yRef.getNumPoints( ) > 0 )
00187         {
00188                 if ( _yRef.getNumValues( ) != getNumInputs( ) )
00189                         return ACADOERROR( RET_VECTOR_DIMENSION_MISMATCH );
00190 
00191                 xRef = _yRef.getVector( 0 );
00192         }
00193         else
00194         {
00195                 xRef.setZero( );
00196         }
00197 
00198 
00199         // initialize control and parameter signals
00200         u.init( getNumOutputs() );
00201         u.setZero( );
00202         
00203         p = p_;
00204 
00205 
00206         lastError = xRef - x0_;
00207 
00208         setStatus( BS_READY );
00209 
00210         return SUCCESSFUL_RETURN;
00211 }
00212 
00213 
00214 returnValue PIDcontroller::step(        double currentTime,
00215                                                                         const DVector& _x,
00216                                                                         const DVector& _p,
00217                                                                         const VariablesGrid& _yRef
00218                                                                         )
00219 {
00220         if ( getStatus( ) != BS_READY )
00221                 return ACADOERROR( RET_BLOCK_NOT_READY );
00222 
00223         if ( _x.getDim( ) != getNumInputs( ) )
00224                 return ACADOERROR( RET_VECTOR_DIMENSION_MISMATCH );
00225 
00226 
00227         /* 1) Use reference trajectory if it is defined */
00228         // set default reference to zero
00229         DVector xRef( _x.getDim() );
00230 
00231         if ( _yRef.getNumPoints( ) > 0 )
00232         {
00233                 if ( _yRef.getNumValues( ) != getNumInputs( ) )
00234                         return ACADOERROR( RET_VECTOR_DIMENSION_MISMATCH );
00235 
00236                 xRef = _yRef.getVector( 0 );
00237         }
00238         else
00239         {
00240                 xRef.setZero( );
00241         }
00242 
00243 
00244         /* 2) Determine PID control action. */
00245         if ( getNumOutputs( ) > 0 )
00246         {
00247                 if ( determineControlAction( xRef-_x,u ) != SUCCESSFUL_RETURN )
00248                         return ACADOERROR( RET_CONTROLLAW_STEP_FAILED );
00249         }
00250         else
00251                 u.init();
00252 
00253         p = _p;
00254 
00255 
00256         /* 3) Call output transformator. */
00257         if ( clipSignals( u,p ) != SUCCESSFUL_RETURN )
00258                 return ACADOERROR( RET_OUTPUTTRANSFORMATOR_STEP_FAILED );
00259 
00260         return SUCCESSFUL_RETURN;
00261 }
00262 
00263 
00264 
00265 uint PIDcontroller::getNX( ) const
00266 {
00267         return getNumInputs( );
00268 }
00269 
00270 
00271 uint PIDcontroller::getNXA( ) const
00272 {
00273         return 0;
00274 }
00275 
00276 
00277 uint PIDcontroller::getNU( ) const
00278 {
00279         return getNumOutputs( );
00280 }
00281 
00282 
00283 uint PIDcontroller::getNP( ) const
00284 {
00285         return 0;
00286 }
00287 
00288 
00289 uint PIDcontroller::getNW( ) const
00290 {
00291         return 0;
00292 }
00293 
00294 
00295 uint PIDcontroller::getNY( ) const
00296 {
00297         return getNX( );
00298 }
00299 
00300 
00301 BooleanType PIDcontroller::isDynamic( ) const
00302 {
00303         return BT_FALSE;
00304 }
00305 
00306 
00307 BooleanType PIDcontroller::isStatic( ) const
00308 {
00309         if ( isDynamic() == BT_TRUE )
00310                 return BT_FALSE;
00311         else
00312                 return BT_TRUE;
00313 }
00314 
00315 
00316 
00317 //
00318 // PROTECTED MEMBER FUNCTIONS:
00319 //
00320 
00321 returnValue PIDcontroller::determineControlAction(      const DVector& error,
00322                                                                                                         DVector& output
00323                                                                                                         )
00324 {
00325         uint i;
00326         double tmp;
00327         
00328         output.init( getNumOutputs() );
00329         output.setZero( );
00330 
00331         // update integral value
00332         for( i=0; i<getNumInputs(); ++i )
00333                 iValue(i) += error(i) * getSamplingTime( );
00334 
00335         // determine outputs
00336         for( i=0; i<getNumInputs(); ++i )
00337         {
00338                 tmp  = pWeights(i) * error(i);
00339                 tmp += iWeights(i) * iValue(i);
00340                 tmp += dWeights(i) * (error(i) - lastError(i)) / getSamplingTime( );
00341 
00342                 if ( getNumOutputs( ) > 1 )
00343                         output(i) = tmp;
00344                 else
00345                         output(0) += tmp;
00346         }
00347 
00348         // update last error
00349         lastError = error;
00350 
00351         return SUCCESSFUL_RETURN;
00352 }
00353 
00354 
00355 
00356 CLOSE_NAMESPACE_ACADO
00357 
00358 // end of file.


acado
Author(s): Milan Vukov, Rien Quirynen
autogenerated on Sat Jun 8 2019 19:38:41