Simple PID controller simulator

gistfile1.c

// PV :: process variable. Current measurement
// SP :: Setpoint. Desired measurement.

#include <math.h>
#include <stdio.h>

typedef struct {
	double previous;
} derivative_context;

typedef struct {
	double sum;
} integral_context;

typedef struct {
	const double setpoint;
	const double proportional_gain; //Kp
	const double integral_gain; //Ki
	const double derivative_gain; // Kd
	integral_context error_integral;
	derivative_context error_derivative;
} pid_context;

double integrate(integral_context* context, double error){
	context->sum += error;
	return context->sum;
}

double differentiate(derivative_context* context, double error){
	double result = error - context->previous;
	context->previous = error;
	return result;
}

double u(pid_context* context, double sample){
	const double error = context->setpoint - sample;
	const double proportional = context->proportional_gain * error;
	const double integral     = context->integral_gain     * integrate(&context->error_integral, error);
	const double derivative   = context->derivative_gain   * differentiate(&context->error_derivative, error);
	//printf("pro: %f  int: %f  der: %f\n", proportional, integral, derivative);
	return proportional + integral + derivative;
}

typedef struct {
	double x_pos;
	double velocity;
} object;

typedef struct {
	double gravity;
	object ball;
} physical_system;

void step(physical_system* system, double angle, double dt){
	double acceleration = system->gravity * sin(angle);
	double dv = acceleration * dt;
	double dx = (system->ball.velocity + (dv / 2)) * dt;
	system->ball.velocity += dv;
	system->ball.x_pos += dx;
}

double bound(double a, double b, double x){
	if(x < a) return a;
	if(x > b) return b;
	return x;
}

int main(int argc, char** argv){
	const double delta_t = .001; // Time between samples
	const double gain = .0001;
	pid_context context = {
		.setpoint = 85,
		.proportional_gain = gain,
		.integral_gain = 0, //gain*(.1)*delta_t,
		.derivative_gain = gain*(50)/delta_t,
		.error_integral = {},
		.error_derivative = {}
	};

	physical_system environment = {
		.gravity = 9.8,
		.ball = {
			.x_pos = 11.0,
			.velocity = 10,
		}
	};

	for(long i = 0; i<1000000; i++){
		double angle = bound(-1, 1, u(&context, environment.ball.x_pos));
		step(&environment, angle, delta_t);
		printf("0");
		int location = (int) environment.ball.x_pos;
		int px = 0;
		for(;px<location;px++){
			printf("_");
		}
		printf("O");
		px++;
		for(;px<200;px++){
			printf("_");
		}
		printf("200\n");
		//printf("Ball pos: %f  Ball velocity: %f\n  Platform angle: %f\n", environment.ball.x_pos, environment.ball.velocity, angle);
	}


	return 0;
}