mirror of
https://github.com/WallabyLester/RBF-aPID-Controller.git
synced 2026-02-08 14:07:28 +08:00
21 lines
606 B
C++
21 lines
606 B
C++
#include "apid_controller.h"
|
|
|
|
/**
|
|
* @brief Constructor to initialize PID gains and time step.
|
|
*/
|
|
aPIDController::aPIDController(double kp, double ki, double kd, double dt)
|
|
: Kp(kp), Ki(ki), Kd(kd), dt(dt), integral(0.0), prev_err(0.0) {}
|
|
|
|
/**
|
|
* @brief Update the PID output based on the target and measured value.
|
|
*/
|
|
double aPIDController::update(double target, double measured_value) {
|
|
double error = target - measured_value;
|
|
|
|
integral += error * dt;
|
|
double derivative = (error - prev_err) / dt;
|
|
prev_err = error;
|
|
|
|
return (Kp * error) + (Ki * integral) + (Kd * derivative);
|
|
}
|