mirror of
https://git.rwth-aachen.de/acs/public/villas/node/
synced 2025-03-09 00:00:00 +01:00
dsp: add PID controller
This commit is contained in:
parent
994d9ed6a6
commit
2346846541
3 changed files with 126 additions and 0 deletions
57
common/include/villas/dsp/pid.hpp
Normal file
57
common/include/villas/dsp/pid.hpp
Normal file
|
@ -0,0 +1,57 @@
|
|||
/** A PID controller.
|
||||
*
|
||||
* @file
|
||||
* @author Steffen Vogel <stvogel@eonerc.rwth-aachen.de>
|
||||
* @copyright 2014-2019, Institute for Automation of Complex Power Systems, EONERC
|
||||
* @license GNU General Public License (version 3)
|
||||
*
|
||||
* VILLAScommon
|
||||
*
|
||||
* This program is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*********************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
namespace villas {
|
||||
namespace dsp {
|
||||
|
||||
class PID {
|
||||
|
||||
protected:
|
||||
double dt;
|
||||
double max;
|
||||
double min;
|
||||
double Kp;
|
||||
double Kd;
|
||||
double Ki;
|
||||
double pre_error;
|
||||
double integral;
|
||||
|
||||
public:
|
||||
/**
|
||||
* Kp - proportional gain
|
||||
* Ki - Integral gain
|
||||
* Kd - derivative gain
|
||||
* dt - loop interval time
|
||||
* max - maximum value of manipulated variable
|
||||
* min - minimum value of manipulated variable
|
||||
*/
|
||||
PID(double _dt, double _max, double _min, double _Kp, double _Kd, double _Ki);
|
||||
|
||||
/** Returns the manipulated variable given a setpoint and current process value */
|
||||
double calculate(double setpoint, double pv);
|
||||
};
|
||||
|
||||
} // namespace dsp
|
||||
} // namespace villas
|
|
@ -27,6 +27,7 @@ add_library(villas-common SHARED
|
|||
compat.cpp
|
||||
crypt.cpp
|
||||
hist.cpp
|
||||
dsp/pid.cpp
|
||||
kernel/kernel.cpp
|
||||
kernel/kernel.cpp
|
||||
kernel/rt.cpp
|
||||
|
|
68
common/lib/dsp/pid.cpp
Normal file
68
common/lib/dsp/pid.cpp
Normal file
|
@ -0,0 +1,68 @@
|
|||
/** A PID controller.
|
||||
*
|
||||
* @author Steffen Vogel <stvogel@eonerc.rwth-aachen.de>
|
||||
* @copyright 2014-2019, Institute for Automation of Complex Power Systems, EONERC
|
||||
* @license GNU General Public License (version 3)
|
||||
*
|
||||
* VILLAScommon
|
||||
*
|
||||
* This program is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*********************************************************************************/
|
||||
|
||||
#include <iostream>
|
||||
#include <cmath>
|
||||
#include <villas/dsp/pid.hpp>
|
||||
|
||||
using namespace std;
|
||||
using namespace villas::dsp;
|
||||
|
||||
PID::PID(double _dt, double _max, double _min, double _Kp, double _Kd, double _Ki) :
|
||||
dt(_dt),
|
||||
max(_max),
|
||||
min(_min),
|
||||
Kp(_Kp),
|
||||
Kd(_Kd),
|
||||
Ki(_Ki)
|
||||
{ }
|
||||
|
||||
double PID::calculate(double setpoint, double pv)
|
||||
{
|
||||
/* Calculate error */
|
||||
double error = setpoint - pv;
|
||||
|
||||
/* Proportional term */
|
||||
double Pout = Kp * error;
|
||||
|
||||
/* Integral term */
|
||||
integral += error * dt;
|
||||
double Iout = Ki * integral;
|
||||
|
||||
/* Derivative term */
|
||||
double derivative = (error - pre_error) / dt;
|
||||
double Dout = Kd * derivative;
|
||||
|
||||
/* Calculate total output */
|
||||
double output = Pout + Iout + Dout;
|
||||
|
||||
/* Restrict to max/min */
|
||||
if (output > max)
|
||||
output = max;
|
||||
else if (output < min)
|
||||
output = min;
|
||||
|
||||
/* Save error to previous error */
|
||||
pre_error = error;
|
||||
|
||||
return output;
|
||||
}
|
Loading…
Add table
Reference in a new issue