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 }