#include "pid.h"

float pid_update(pid_t* pid, float error, float gyro_rate, float dt)
{
	float p = pid->kp * error;
	
	pid->integral += error * dt;
	if (pid->integral > 100) pid->integral = 100;
	if (pid->integral < -100) pid->integral = -100;
	
	float i = pid->ki * pid->integral;
	
	float d = - pid->kd * gyro_rate;
	
	pid->prev_error = error;
	
	float spid = p + i + d;
	
	if (spid > 300) spid = 300;
	if (spid < -300) spid = -300;
	
	return spid;
}