You cannot select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
35 lines
949 B
C
35 lines
949 B
C
#include <util/atomic.h>
|
|
#include "pid.h"
|
|
|
|
float pid_step(volatile struct pid* controller, float dt, float error) {
|
|
//TODO convert to integer; floats cause trouble ...
|
|
|
|
// Calculate p term
|
|
float p = error * controller->kP;
|
|
|
|
// Calculate i term
|
|
ATOMIC_BLOCK(ATOMIC_FORCEON){
|
|
controller->integral += error * dt * controller->kI;
|
|
if(controller->integral > 80)
|
|
controller->integral = 80;
|
|
if(controller->integral < -80)
|
|
controller->integral = -80;
|
|
}
|
|
|
|
// Calculate d term, taking care to not divide by zero
|
|
float d = dt == 0 ? 0 : ((error - controller->lastError) / dt) * controller->kD;
|
|
controller->lastError = error;
|
|
|
|
return p + controller->integral + d;
|
|
}
|
|
|
|
void init_pid(volatile struct pid* controller, float p, float i, float d){
|
|
*controller = (struct pid){
|
|
.kP = p,
|
|
.kI = i,
|
|
.kD = d,
|
|
.lastError = 0,
|
|
.integral = 0
|
|
};
|
|
}
|