/*
 * pid.c
 *
 *  Created on: Jan 18, 2022
 *      Author: mcfly
 */

#include "pid.h"

void PID_Init(PIDController *pid) {
	//Clear controller values
	pid->integrator = 0.0f;
	pid->prevError  = 0.0f;

	pid->differentiator  = 0.0f;
	pid->prevMeasurement = 0.0f;

	pid->out = 0.0f;
}


float PID_Update(PIDController *pid, float setpoint, float measurement) {
	// PID = Kp + Ki * 1/s + Kd * s/(s*tau+1)

	// e = error tothe setpoint
	// p[n] = Kp * e[n]
	// i[n] = Ki*T/2 * (e[n]+e[n-1]) + i[n-1]
	// d[n] = 2*Kd/(2*tau+T) * (e[n]-e[n-1]) + (2*tau-T)/(2*tau+T) * d[n-1]
	// PID = p[n]+i[n]+d[n]


	//Error signal
	float error = setpoint - measurement;

	//Proportional
	float proportional = pid->Kp * error;

	//Integral
	pid->integrator += 0.5f * pid->Ki * pid->T * (error + pid->prevError);

	//Anti-wind-up via dynamic integrator clamping
	float limMinInt, limMaxInt;

	//Compute integrator limits
	if (pid->limMax > proportional) {
		limMaxInt = pid->limMax - proportional;
	} else {
		limMaxInt = 0.0f;
	}
	if (pid->limMin < proportional) {
		limMinInt = pid->limMin - proportional;
	} else {
		limMinInt = 0.0f;
	}

	//Clamp integrator
	if (pid->integrator > limMaxInt) {
		pid->integrator = limMaxInt;
	} else if (pid->integrator < limMinInt) {
		pid->integrator = limMinInt;
	}

	//Derivative (band-limited differentiator)
	pid->differentiator = (2.0f * pid->Kd * (measurement - pid->prevMeasurement)		//Note: derivative on measurement
						+ (2.0f * pid->tau - pid->T) * pid->differentiator)
						/ (2.0f * pid->tau + pid->T);

	//Compute output and apply limits
	pid->out = proportional + pid->integrator + pid->differentiator;
	if (pid->out > pid->limMax) {
		pid->out = pid->limMax;
	} else if (pid->out < pid->limMin) {
		pid->out = pid->limMin;
	}

	//Store error and measurement for later use
	pid->prevError		 = error;
	pid->prevMeasurement = measurement;

	//Return controller output
	return pid->out;

}