PID.c

/*
 * PID.c
 *
 *  Created on: Nov 10, 2014
 *      Author: ucart
 */

#include "PID.h"
#include <math.h>
#include <float.h>

// The generic PID diagram. This function takes in pid parameters (PID_t * pid) and calculates the output "pid_correction"
// part based on those parameters.
//
//              +  ---    error    ------------------		P		+  ---			  ----------------------------
// setpoint ---> / sum \ --------->| Kp * error	    |--------------->/ sum \ -------->| output: "pid_correction" |
//				 \     /	|	   ------------------				 \	   /		  ----------------------------
//				   ---		|										   ---					||
//                -	^       |	   									+ ^  ^ +				||
//					|		|	   -------------------------------	  |	 |			------- \/------------
//					|		|----->| Ki * accumulated error * dt |----+	 |			|					 |
//					|		|	   -------------------------------	I	 |			|		SYSTEM		 |
//					|		|											 |			|					 |
//					|		|											 |			--------||------------
//					|		|											 |					||
//					|		|      ----------------------------------	 |					||
//					|		|----->| Kd * (error - last error) / dt |----+					||
//				    |			   ----------------------------------  D					||
//					|																		||
//					|															 -----------\/-----------
//					|____________________________________________________________| Sensor measurements: |
//																				 |	 "current point"	|
//																				 ------------------------
//
PID_values pid_computation(PID_t *pid) {

	float P = 0.0, I = 0.0, D = 0.0;

	// calculate the current error
	float error = pid->setpoint - pid->current_point;

	// Accumulate the error (if Ki is less than epsilon, rougly 0,
	// then reset the accumulated error for safety)
	if (fabs(pid->Ki) <= FLT_EPSILON) {
		pid->acc_error = 0;
	} else {
		pid->acc_error += error;
	}

	float change_in_error = error - pid->prev_error;

	// Compute each term's contribution
	P = pid->Kp * error;
	I = pid->Ki * pid->acc_error * pid->dt;
	D = pid->Kd * (change_in_error / pid->dt);

	PID_values ret = {P, I, D, error, change_in_error, P + I + D};

	pid->prev_error = error; // Store the current error into the PID_t

	pid->pid_correction = P + I + D; // Store the computed correction
	return ret;
}