You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
67 lines
1.1 KiB
67 lines
1.1 KiB
/*
|
|
PID regulator.
|
|
*/
|
|
|
|
#include <stdio.h>
|
|
#include <stdint.h>
|
|
#include <sys/time.h>
|
|
#include "pid.h"
|
|
|
|
PID::PID(double min, double max, double kp, double kd, double ki) {
|
|
Min = min;
|
|
Max = max;
|
|
Kp = kp;
|
|
Kd = kd;
|
|
Ki = ki;
|
|
Integral = 0;
|
|
PreviousError = 0;
|
|
PreviousTime = getTime();
|
|
}
|
|
|
|
PID::~PID() {
|
|
}
|
|
|
|
int64_t PID::getTime(void) {
|
|
|
|
struct timeval current_time;
|
|
int64_t t;
|
|
|
|
gettimeofday (¤t_time, NULL);
|
|
t = current_time.tv_sec * 1000000L + current_time.tv_usec;
|
|
return t;
|
|
}
|
|
|
|
double PID::Update(double target, double value, int64_t time) {
|
|
double error, out, dt;
|
|
double P, I, D;
|
|
double Derivative;
|
|
|
|
// error
|
|
error = target - value;
|
|
|
|
// proportional part
|
|
P = Kp * error;
|
|
|
|
// integral part
|
|
dt = time - PreviousTime;
|
|
Integral += error * dt;
|
|
I = Ki * Integral;
|
|
|
|
// derivative part
|
|
Derivative = (error - PreviousError) / dt;
|
|
D = Kd * Derivative;
|
|
|
|
// calculate output of regulator
|
|
out = P + I + D;
|
|
|
|
// Min/Max
|
|
if (out > Max) out = Max;
|
|
if (out < Min) out = Min;
|
|
|
|
// save previous error value and time
|
|
PreviousError = error;
|
|
PreviousTime = time;
|
|
|
|
return out;
|
|
}
|