46 lines
810 B
C
46 lines
810 B
C
/*
|
|
* Copyright 2022 Oleg Borodin <borodin@unix7.org>
|
|
*
|
|
*/
|
|
|
|
|
|
#include <stdint.h>
|
|
#include <pidcont.h>
|
|
|
|
void pidcont_init(pidcont_t* p) {
|
|
p->perror = 0.0;
|
|
p->integ = 0.0;
|
|
|
|
p->kp = 0.0;
|
|
p->ki = 0.0;
|
|
p->kd = 0.0;
|
|
}
|
|
|
|
|
|
void pidcont_setup(pidcont_t* p, double kp, double ki, double kd) {
|
|
p->kp = kp;
|
|
p->ki = ki;
|
|
p->kd = kd;
|
|
}
|
|
|
|
|
|
double pidcont_next(pidcont_t* p, double target, double actual, double dt) {
|
|
|
|
double error = 0.0;
|
|
double deriv = 0.0;
|
|
double integ = 0.0;
|
|
double output = 0.0;
|
|
|
|
const double kk = 100;
|
|
|
|
error = (target - actual);
|
|
integ += (error * dt);
|
|
deriv = (error - p->perror) * dt;
|
|
|
|
output = (p->kp * error / kk) + (p->ki * integ / kk) + (p->kd * deriv / kk);
|
|
|
|
p->perror = error;
|
|
p->integ = integ;
|
|
return output;
|
|
}
|