1
0
Fork 0
mirror of https://git.rwth-aachen.de/acs/public/villas/node/ synced 2025-03-09 00:00:00 +01:00
VILLASnode/common/lib/dsp/pid.cpp
Steffen Vogel 7d10544d79 Apply clang-format changes
Signed-off-by: Steffen Vogel <steffen.vogel@opal-rt.com>
2024-02-29 18:58:07 +01:00

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;
}