File content as of revision 2:fd1023193cc4:

#include "PID.h"

pid* pid_create(pid* pid, float* input, float* output, float* setpoint, float kp, float ki, float kd)
{
	pid->input = input;
	pid->output = output;
	pid->setpoint = setpoint;
	pid->sumError = 0;
	pid->lastInput = 0;
	pid->lastError = 0;
	pid_limits(pid,-254, 254);

	pid_tune(pid, kp, ki, kd);

	return pid;
}

void pid_compute(pid* pid)
{

	
	float in = *(pid->input);
	// Compute error
	float error =(*(pid->setpoint)) - in;
	// Compute integral
	pid->sumError += (pid->Ki * error);
	/*if ((error>0 && pid->lastError<0) || (error<0 && pid->lastError>0) ){
		pid->sumError = 0;
	}*/
	if (pid->sumError > 254)
		pid->sumError = 255;
	else if (pid->sumError < -254)
		pid->sumError = -255;
	// Compute differential on input
	float dinput = in - pid->lastInput;
	// Compute PID output
	float out = pid->Kp * error + pid->sumError - pid->Kd * dinput;
	// Apply limit to output value
	int absOut=abs((int)out);
	if (out > pid->maxOutput){
		out = 255;
		//pid->sumError=0;
	}
	else if (out < pid->minOutput){
		out = -255;
		//pid->sumError=0;
	}
	// Output to pointed variable
	(*pid->output) = out;
	// Keep track of some variables for next execution
	pid->lastInput = in;
	pid->lastError = error;
}

void pid_tune(pid* pid, float kp, float ki, float kd)
{
	// Check for validity
	pid->Kp = kp;
	pid->Ki = ki ;
	pid->Kd = kd ;

}


void pid_limits(pid* pid, float min, float max)
{
	if (min >= max) return;
	pid->minOutput = min;
	pid->maxOutput = max;
	if (pid->sumError > pid->maxOutput)
			pid->sumError = pid->maxOutput;
	else if (pid->sumError < pid->minOutput)
			pid->sumError = pid->minOutput;
}