#include "PID.h"

PID::PID ()
{
	lastCalculationTime = 0;
	lastValue = 0;
	integratedI = 0.;	
}


uint8_t PID::calculatePID(int32_t value){
	uint32_t time = millis();

	int32_t error = Target - value;
	int16_t errorConstrained = constrain(error,-2550,2550);

	integratedI = constrain(integratedI + ((float)errorConstrained)*I,-255.,255.);

	float speed = (lastValue - value) / (((float)(time-lastCalculationTime))/1000.);
	lastCalculationTime = time;
	lastValue = value;

	float calculation = ((float)error)*P + integratedI + speed*D;
	
	return (uint8_t)(constrain(calculation,0.,255.));	
}

