PID_Controller.cpp
Go to the documentation of this file.
1 /*+-------------------------------------------------------------------------+
2  | MultiVehicle simulator (libmvsim) |
3  | |
4  | Copyright (C) 2014 Jose Luis Blanco Claraco (University of Almeria) |
5  | Copyright (C) 2017 Borys Tymchenko (Odessa Polytechnic University) |
6  | Distributed under GNU General Public License version 3 |
7  | See <http://www.gnu.org/licenses/> |
8  +-------------------------------------------------------------------------+ */
9 
10 #include <mvsim/PID_Controller.h>
11 
12 using namespace mvsim;
13 
15  : KP(1.0),
16  KI(.0),
17  KD(.0),
18  max_out(0),
19  lastOutput(0),
20  e_n(0),
21  e_n_1(0),
22  e_n_2(0)
23 {
24 }
25 
27 double PID_Controller::compute(double err, double dt)
28 {
29  e_n_2 = e_n_1;
30  e_n_1 = e_n;
31  e_n = err;
32 
33  double output = lastOutput + KP * (e_n - e_n_1) + KI * e_n * dt +
34  KD * (e_n - 2 * e_n_1 + e_n_2) / dt;
35 
36  // prevent integral windup
37  if (max_out != 0.0 && (output < -max_out || output > max_out))
38  {
39  output -= KI * e_n * dt;
40  }
41 
42  lastOutput = output;
43 
44  if (max_out != 0.0)
45  {
46  if (output < -max_out) output = -max_out;
47  if (output > max_out) output = max_out;
48  }
49 
50  return output;
51 }
double max_out
For clamping (0=no clamp)
double compute(double err, double dt)


mvsim
Author(s):
autogenerated on Thu Jun 6 2019 19:36:40