00001 /*************************************************************************** 00002 * Copyright (C) 2005-2011 LpzRobots development team * 00003 * Georg Martius <georg dot martius at web dot de> * 00004 * Frank Guettler <guettler at informatik dot uni-leipzig dot de * 00005 * Frank Hesse <frank at nld dot ds dot mpg dot de> * 00006 * Ralf Der <ralfder at mis dot mpg dot de> * 00007 * * 00008 * This program is free software; you can redistribute it and/or modify * 00009 * it under the terms of the GNU General Public License as published by * 00010 * the Free Software Foundation; either version 2 of the License, or * 00011 * (at your option) any later version. * 00012 * * 00013 * This program is distributed in the hope that it will be useful, * 00014 * but WITHOUT ANY WARRANTY; without even the implied warranty of * 00015 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * 00016 * GNU General Public License for more details. * 00017 * * 00018 * You should have received a copy of the GNU General Public License * 00019 * along with this program; if not, write to the * 00020 * Free Software Foundation, Inc., * 00021 * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * 00022 * * 00023 ***************************************************************************/ 00024 #ifndef __PID_H 00025 #define __PID_H 00026 00027 namespace lpzrobots { 00028 00029 class PID 00030 { 00031 //*********************attributes*************** 00032 //private: 00033 public: 00034 00035 double position; 00036 double lastposition; 00037 double last2position; 00038 00039 double error; 00040 double lasterror; 00041 double derivative; 00042 double targetposition; 00043 00044 double KP; 00045 double KI; 00046 double KD; 00047 double tau; 00048 00049 double P; 00050 double D; 00051 double I; 00052 00053 double force; 00054 double lasttime; // last update time (to calc stepsize) 00055 00056 //*********************methods****************** 00057 public : 00058 /// KP is used as a general koefficient. KI and KD can be tuned without dependence of KP 00059 PID ( double KP = 100 , double KI = 2.0 , double KD = 0.3 ); 00060 00061 void setKP(double KP); 00062 00063 void setTargetPosition ( double newpos ); 00064 00065 double getTargetPosition (); 00066 00067 /// perform one step of the PID controller with cutoff for large forces 00068 double step ( double newsensorval, double time); 00069 /// perform one step of the PID controller without cutoffs used for Center-Servos 00070 double stepNoCutoff ( double newsensorval, double time); 00071 /** perform one step of the PID controller for velocity control. 00072 Meaning the misfit is in position space but the output is 00073 the nominal velocity. The velocity is also limited. such that 00074 the maximal velocity cannot be so that the error is overcompenstated 00075 in one timestep. 00076 */ 00077 double stepVelocity ( double newsensorval, double time); 00078 00079 }; 00080 00081 } 00082 00083 #endif
1.6.3