iCub-main
pidfilter.h
Go to the documentation of this file.
1 // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
2 
3 /*
4 * Copyright (C) 2010 RobotCub Consortium, European Commission FP6 Project IST-004370
5 * Author: Lorenzo Natale
6 * email: lorenzo.natale@iit.it
7 * website: www.robotcub.org
8 * Permission is granted to copy, distribute, and/or modify this program
9 * under the terms of the GNU General Public License, version 2 or any
10 * later version published by the Free Software Foundation.
11 *
12 * A copy of the license can be found at
13 * http://www.robotcub.org/icub/license/gpl.txt
14 *
15 * This program is distributed in the hope that it will be useful, but
16 * WITHOUT ANY WARRANTY; without even the implied warranty of
17 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
18 * Public License for more details
19 */
28 #ifndef __PidFilterh__
29 #define __PidFilterh__
30 
31 #include <math.h>
32 
33 class PidFilter
34 {
35 private:
36  double error_old; //error at previous step
37  double Kp,Kd,Ki; //proportional, derivative and integral gains
38 
39  // integrative stuff
40  double Umax; //maximum value of the control
41  double Sn; //integal value
42 
43  //computes the pd portion of the control
44  inline double pd(double error)
45  {
46  double ret=Kp*error+Kd*(error-error_old);
47  return ret;
48  }
49 
50 public:
51  PidFilter(void);
52  PidFilter(double kp, double kd=0, double ki = 0, double u = 0.0);
53  PidFilter(const PidFilter& f);
54  void operator=(const PidFilter& f);
55  virtual ~PidFilter(void);
56 
57  inline void setKs(double kp, double kd=0.0, double ki=0.0, double u_max = 0.0)
58  {
59  Kp = kp;
60  Kd = kd;
61  Ki = ki;
62 
63  Sn = 0;
64  Umax = u_max;
65  }
66 
67  inline void reset(double error = 0.0)
68  {
69  Sn = 0.0;
70  error_old = error;
71  }
72 
73  // computes the PID control with anti reset wind up scheme
74  inline double pid (double error)
75  {
76  double u_tmp;
77  double Sn_tmp;
78  double u_pd;
79  double u;
80 
81  //compute the pd part
82  u_pd = pd(error);
83 
84  //compute the temporary integral part
85  Sn_tmp = Sn + Ki * error;
86 
87  //compute the temporary control
88  u_tmp = u_pd + Sn_tmp;
89 
90  //if no saturation occur, then temporary works fine
91  Sn = Sn_tmp;
92 
93  //if saturation occur, redifine integral part
94  if (u_tmp > Umax)
95  Sn = Umax - u_pd;
96  if (u_tmp < -Umax)
97  Sn = -Umax - u_pd;
98 
99  //redifine error_old
100  error_old = error;
101 
102  //compute the control
103  u = Sn + u_pd;
104 
105  return u;
106  }
107 
108  inline double getProportional(void) const { return Kp; }
109  inline double getDerivative(void) const { return Kd; }
110  inline double getIntegrative(void) const { return Ki; }
111 };
112 
118 {
119 protected:
120  double fc; // cut frequency
121  double Ts; // sample time
122  double y; // filter current output
123  double yold; // old output
124  double uold; // old input
125  double a1, a2;
126  double b1, b2;
127 
129  {
130  double tau = 1.0/(2.0*3.1415926535897932384626433832795029*fc);
131  b1 = b2 = Ts;
132  a1 = 2.0*tau+Ts;
133  a2 = Ts-2.0*tau;
134  }
135 
136 public:
143  FirstOrderLowPassFilter(const double cutFrequency, const double sampleTime, const double y0){
144  fc = cutFrequency;
145  Ts = sampleTime;
146  computeCoeff();
147  init(y0);
148  }
149 
154  void init(const double y0)
155  {
156  y = y0;
157  yold = y0;
158  uold = (a1+a2)/(b1+b2)*y0;
159  }
160 
165  double getCutFrequency() { return fc; }
166 
171  double getSampleTime() { return Ts; }
172 
178  double filt(double u)
179  {
180  y = (b1*u + b2*uold - a2*yold) / a1;
181  uold = u;
182  yold = y;
183  return y;
184  }
185 
190  double output() { return y; }
191 };
192 
193 #endif
double pid(double error)
Definition: pidfilter.h:74
double getCutFrequency()
Retrieve the cut frequency of the filter.
Definition: pidfilter.h:165
double getDerivative(void) const
Definition: pidfilter.h:109
PidFilter(void)
Definition: pidfilter.cpp:33
void init(const double y0)
Internal state reset.
Definition: pidfilter.h:154
void operator=(const PidFilter &f)
Definition: pidfilter.cpp:70
void reset(double error=0.0)
Definition: pidfilter.h:67
double getSampleTime()
Retrieve the sample time of the filter.
Definition: pidfilter.h:171
double getProportional(void) const
Definition: pidfilter.h:108
double filt(double u)
Performs filtering on the actual input.
Definition: pidfilter.h:178
virtual ~PidFilter(void)
Definition: pidfilter.cpp:55
FirstOrderLowPassFilter(const double cutFrequency, const double sampleTime, const double y0)
Creates a filter with specified parameters.
Definition: pidfilter.h:143
void setKs(double kp, double kd=0.0, double ki=0.0, double u_max=0.0)
Definition: pidfilter.h:57
First order low pass filter implementing the transfer function H(s) = {1}{1+ s}.
Definition: pidfilter.h:117
double output()
Return current filter output.
Definition: pidfilter.h:190
double getIntegrative(void) const
Definition: pidfilter.h:110