Robotsystem from Robot Group Leipzig: pid.cpp Source File

- Main Page
- Modules
- Namespaces
- Classes
- Files
- Directories
- Examples

- File List
- File Members

# pid.cpp

Go to the documentation of this file.

    00001 /***************************************************************************
    00002  *   Copyright (C) 2005 by Robot Group Leipzig                             *
    00003  *    martius@informatik.uni-leipzig.de                                    *
    00004  *    fhesse@informatik.uni-leipzig.de                                     *
    00005  *    der@informatik.uni-leipzig.de                                        *
    00006  *                                                                         *
    00007  *   This program is free software; you can redistribute it and/or modify  *
    00008  *   it under the terms of the GNU General Public License as published by  *
    00009  *   the Free Software Foundation; either version 2 of the License, or     *
    00010  *   (at your option) any later version.                                   *
    00011  *                                                                         *
    00012  *   This program is distributed in the hope that it will be useful,       *
    00013  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
    00014  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
    00015  *   GNU General Public License for more details.                          *
    00016  *                                                                         *
    00017  *   You should have received a copy of the GNU General Public License     *
    00018  *   along with this program; if not, write to the                         *
    00019  *   Free Software Foundation, Inc.,                                       *
    00020  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
    00021  *                                                                         *
    00022  *   $Log: pid.cpp,v $
    00023  *   Revision 1.8.4.3  2006/02/07 15:51:56  martius
    00024  *   axis, setpower
    00025  *
    00026  *   Revision 1.8.4.2  2006/01/10 14:48:28  martius
    00027  *   indentation
    00028  *
    00029  *   Revision 1.8.4.1  2005/12/20 17:53:42  martius
    00030  *   changed to Joints from joint.h
    00031  *   new servos for universal and hinge2
    00032  *
    00033  *   Revision 1.8  2005/11/09 14:08:48  martius
    00034  *   *** empty log message ***
    00035  *
    00036  *   Revision 1.7  2005/11/09 13:28:24  fhesse
    00037  *   GPL added
    00038  *                                                                *
    00039  ***************************************************************************/
    00040 #include <ode/ode.h>
    00041
    00042 #include "pid.h"
    00043
    00044 namespace lpzrobots {
    00045
    00046   PID::PID ( double KP , double KI , double KD)
    00047   {
    00048     this->KP = KP;
    00049     this->KI = KI;
    00050     this->KD = KD;
    00051
    00052     P=D=I=0;
    00053
    00054     targetposition = 0;
    00055
    00056     position = 0;
    00057     lastposition = 0;
    00058     error = 0;
    00059     alpha = 0.95;
    00060   }
    00061
    00062   void PID::setKP(double KP){
    00063     this->KP = KP;
    00064   }
    00065
    00066   void PID::setTargetPosition ( double newpos )
    00067   {
    00068     targetposition = newpos;
    00069   }
    00070
    00071   double PID::getTargetPosition ( )
    00072   {
    00073     return targetposition;
    00074   }
    00075
    00076   double PID::step ( double newsensorval )
    00077   {
    00078     last2position = lastposition;
    00079     lastposition = position;
    00080     position = newsensorval;
    00081
    00082     return stepWithD(newsensorval, lastposition - position);
    00083   }
    00084
    00085   double PID::stepWithD ( double newsensorval, double derivative ){
    00086     position = newsensorval;
    00087
    00088     lasterror = error;
    00089     error = targetposition - position;
    00090
    00091     P = error;
    00092     I += (1-alpha) * (error * KI - I);
    00093     D = -derivative * KD;
    00094     // D = -( 3*position - 4 * lastposition + last2position ) * KD;
    00095     //double maxforce = KP/10;
    00096     //P = P > maxforce ? maxforce : (force < -maxforce ? -maxforce : P);
    00097
    00098     force = KP*(P + I + D);
    00099     return force;
    00100
    00101   }
    00102
    00103 }