72 lines
1.6 KiB
C++
72 lines
1.6 KiB
C++
#include "PID.h"
|
|
#include <Arduino.h>
|
|
|
|
PID::PID(float kp, float ki, float kd) {
|
|
this->kp = kp;
|
|
this->ki = ki;
|
|
this->kd = kd;
|
|
this->minOutput = 0;
|
|
this->maxOutput = 255;
|
|
this->lastOutput = 0;
|
|
this->sampleTime = 100; // Default sample time in milliseconds
|
|
this->lastTime = millis();
|
|
this->integral = 0;
|
|
this->lastInput = 0;
|
|
}
|
|
|
|
void PID::setTunings(float kp, float ki, float kd) {
|
|
this->kp = kp;
|
|
this->ki = ki;
|
|
this->kd = kd;
|
|
}
|
|
|
|
void PID::setOutputLimits(float min, float max) {
|
|
this->minOutput = min;
|
|
this->maxOutput = max;
|
|
}
|
|
|
|
void PID::setSampleTime(int sampleTime) {
|
|
this->sampleTime = sampleTime;
|
|
}
|
|
|
|
void PID::reset() {
|
|
this->integral = 0;
|
|
this->lastInput = 0;
|
|
this->lastTime = millis();
|
|
}
|
|
|
|
float PID::compute(float setpoint, float input) {
|
|
unsigned long now = millis();
|
|
unsigned long timeChange = now - lastTime;
|
|
|
|
if (timeChange >= sampleTime) {
|
|
// Calculate error
|
|
float error = setpoint - input;
|
|
|
|
// Proportional term
|
|
float Pout = kp * error;
|
|
|
|
// Integral term
|
|
integral += (ki * error * timeChange / 1000.0);
|
|
integral = constrain(integral, minOutput, maxOutput);
|
|
|
|
// Derivative term
|
|
float derivative = (input - lastInput) / (timeChange / 1000.0);
|
|
float Dout = kd * derivative;
|
|
|
|
// Compute output
|
|
float output = Pout + integral - Dout;
|
|
output = constrain(output, minOutput, maxOutput);
|
|
|
|
// Remember some variables for next time
|
|
lastInput = input;
|
|
lastTime = now;
|
|
|
|
lastOutput = output
|
|
|
|
return output;
|
|
}
|
|
|
|
return lastOutput;
|
|
}
|