PID_Controller.cpp
Go to the documentation of this file.
00001 /*+-------------------------------------------------------------------------+
00002   |                       MultiVehicle simulator (libmvsim)                 |
00003   |                                                                         |
00004   | Copyright (C) 2014  Jose Luis Blanco Claraco (University of Almeria)    |
00005   | Copyright (C) 2017  Borys Tymchenko (Odessa Polytechnic University)     |
00006   | Distributed under GNU General Public License version 3                  |
00007   |   See <http://www.gnu.org/licenses/>                                    |
00008   +-------------------------------------------------------------------------+ */
00009 
00010 #include <mvsim/PID_Controller.h>
00011 
00012 using namespace mvsim;
00013 
00014 PID_Controller::PID_Controller()
00015         : KP(1.0),
00016           KI(.0),
00017           KD(.0),
00018           max_out(0),
00019           lastOutput(0),
00020           e_n(0),
00021           e_n_1(0),
00022           e_n_2(0)
00023 {
00024 }
00025 
00027 double PID_Controller::compute(double err, double dt)
00028 {
00029         e_n_2 = e_n_1;
00030         e_n_1 = e_n;
00031         e_n = err;
00032 
00033         double output = lastOutput + KP * (e_n - e_n_1) + KI * e_n * dt +
00034                                         KD * (e_n - 2 * e_n_1 + e_n_2) / dt;
00035 
00036         // prevent integral windup
00037         if (max_out != 0.0 && (output < -max_out || output > max_out))
00038         {
00039                 output -= KI * e_n * dt;
00040         }
00041 
00042         lastOutput = output;
00043 
00044         if (max_out != 0.0)
00045         {
00046                 if (output < -max_out) output = -max_out;
00047                 if (output > max_out) output = max_out;
00048         }
00049 
00050         return output;
00051 }


mvsim
Author(s):
autogenerated on Thu Jun 6 2019 22:08:35