mirror of
https://git.rwth-aachen.de/acs/public/villas/node/
synced 2025-03-09 00:00:00 +01:00
48 lines
1.1 KiB
C++
48 lines
1.1 KiB
C++
/* A PID controller.
|
|
*
|
|
* Author: Steffen Vogel <post@steffenvogel.de>
|
|
* SPDX-FileCopyrightText: 2014-2023 Institute for Automation of Complex Power Systems, RWTH Aachen University
|
|
* SPDX-License-Identifier: Apache-2.0
|
|
*/
|
|
|
|
#include <cmath>
|
|
#include <iostream>
|
|
#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), pre_error(0),
|
|
integral(0) {}
|
|
|
|
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;
|
|
}
|