/* Demo program of a PID control loop

From http://en.wikipedia.org/wiki/PID_controller 

Pseudocode:

	previous_error = 0
	integral = 0 
	start:
	  error = setpoint - actual_position
	  integral = integral + (error * dt)
	  derivative = (error - previous_error) / dt
	  output = (Kp * error) + (Ki * integral) + (Kd * derivative)
	  previous_error = error
	  wait(dt)
	  goto start

Where dt is delta time, Kp is the Proportional constant,
	Ki the Integral contant, and Kd the Derivative constant

*/

#include<stdio.h>

int main()
{
	int i;
	float setpoint, actual_position, error, integral;
	float derivative, previous_error, Kp, Ki, Kd, dt;
	float output;

	/* set initial setting */
	previous_error = 0.0;
	integral = 0.0;
	setpoint = 100.0;
	actual_position = 0.0;
	Kp = 0.2;
	Ki = 0.1;
	Kd = 0.02;
	dt = 0.25;

	/* print settings */
	printf("Setpoint: %f\n", setpoint);
	printf("Kp: %f\n", Kp);
	printf("Ki: %f\n", Ki);
	printf("Kd: %f\n\n\n", Kd);

	/* print header */
	printf("Actual_pos, output,    error,     P,        I,        D\n");

	/* main loop */
	for (i=0; i<100; i++)
	{
		/* guts of the control loop */
		error = setpoint - actual_position;
		integral = integral + (error * dt);
		derivative = (error - previous_error) / dt;
		output = (Kp * error) + (Ki * integral) + (Kd * derivative);
		previous_error = error;

		/* we would wait here for the system to
		   respond if this were an actual control
		   program and not a sim */

		printf("%f  %f  %f  %f  %f  %f\n", actual_position, output, error, (Kp * error), integral, derivative);

		/* since this is a sim, we'll adjust the position
		   ourselves based on the output signal */
		actual_position = actual_position + output;
	}


}