26 lines
828 B
C
26 lines
828 B
C
#include "pid.h"
|
|
|
|
void pid_init(pid_t *pid, float kp, float ki, float kd, float max_out, float max_i)
|
|
{
|
|
pid->kp = kp;
|
|
pid->ki = ki;
|
|
pid->kd = kd;
|
|
pid->integral = 0.0f;
|
|
pid->last_error = 0.0f;
|
|
pid->max_out = max_out;
|
|
pid->max_integral = max_i;
|
|
}
|
|
|
|
float pid_calc(pid_t *pid, float target, float actual)
|
|
{
|
|
float error = target - actual;
|
|
pid->integral += error;
|
|
if (pid->integral > pid->max_integral) pid->integral = pid->max_integral;
|
|
if (pid->integral < -pid->max_integral) pid->integral = -pid->max_integral;
|
|
float output = pid->kp * error + pid->ki * pid->integral
|
|
+ pid->kd * (error - pid->last_error);
|
|
pid->last_error = error;
|
|
if (output > pid->max_out) output = pid->max_out;
|
|
if (output < -pid->max_out) output = -pid->max_out;
|
|
return output;
|
|
} |