double Kp=4;//THe proportional gain
double Ki=0;//The integral gain
double Kd=2;//The derivative gain
volatile double set_point=-80;//The equilibrium point
double integration_limit=.09;//This prevents against integral wind-up, not an issue when Ki=0
double kp_scale=0;
volatile double output;//The output signal


void PID(double theta_actual){
	
	
	ADMUX=2;//Switch the ADC channel
	ADCSRA |= (1<<ADSC);//start the conversion process
	while(ADCSRA&_BV(ADSC));//wait for the conversion process
	Kp=ADC*2.75 /1024.0;//Scale Kp from the potentiometer knob
	ADMUX=3;//Switch the ADC channel
	ADCSRA |= (1<<ADSC);//start the conversion process 
	while(ADCSRA&_BV(ADSC));//wait for the conversion process
	set_point=ADC*500.0/1024.0-250;//Scale the set point from the pot knob
	ADMUX=4;//Switch the ADC channel
	ADCSRA |= (1<<ADSC);//start the conversion process
	while(ADCSRA&_BV(ADSC));//wait for the conversion process
	Kd=ADC*150.0/1024.0;//Scale the derivative gain from the pot knob

	
static double error_sum;//static variable to keep track of the integral
static double error_not;//Static variable to remember the last error

double error=angle_set-theta_actual;//Error is calculated
double d_error=error_not-error;//Differntial error is calculated

if(fabs(error_sum)<integration_limit){ //Cuts off the integral term if there is windup
	output= Kp*error+Ki*error_sum+Kd*(d_error);
	}
else{
	output= Kp*error+Ki*integration_limit+Kd*(d_error);
	}


if(output>=255){//Cap the output so that it does not overflow when casted to 8-bit
	output=255;
}
if(output<=-255){
	output=-255;
}

if(output<0){PORTC|=(1<<DIR1)|(1<<DIR2);}//Set the direction pin
if(output>0){PORTC=0b00000000;}

OCR1A = (int)(255-fabs(output));// the 255- is for inverting (optoisolator is inverting)
OCR1B = (int)(255-fabs(output));// make a copy on another timer for back up

error_not=error;//Store the last error

}