PID controller. Uses the bilinear transformation.

PID controller.cpp

#include <stdio.h>
#include <iostream>
#include <fstream>
#include "daq.h"
using namespace std;

// Easily toggle the P, I and D parts of the controller
#define CONTROLLER_P true
#define CONTROLLER_I true
#define CONTROLLER_D true

// DAQ params
int timeoutS = 1;
float64 samplingRateHZ = 100, vMin = -10, vMax = 10;

// Reference voltage
float64 refVoltage = 1;

// PID controller parameters (used for the Ziegler–Nichols tuning method)
float64 Kc = 5; // Gain at which stability limit is reached, determined in (3)
float64 K = 0.6 * Kc;
float64 Tc = 0.14993; // Period of oscillation, determined in (3)
float64 Ti = Tc / 2;
float64 Td = Tc / 8;
float64 alpha = 400 * 3.14159265; // Used for the filter (derivative component of the PID)

// Final PID parameters
float64 Kp = K;
float64 Ki = K / Ti;
float64 Kd = K * Td;

// T used in the bilinear transformation, etc.
float64 T = 1 / samplingRateHZ;

// Signals whether the controller has become saturated
bool CONTROLLER_IS_SATURATED = false;


float64 bilinearTransformationPID(float64 e){	
	// Return u(k) given e(k)
	static float64 u = 0, u_p = 0, u_i = 0, u_d = 0;
	// Notation: e_1 = e(k-1)
	static float64 e_1 = 0, e_2 = 0, u_1 = 0, u_2 = 0;

	// Proportional gain
	if(CONTROLLER_P) u_p = Kp * e;

	// Integral gain
	if(CONTROLLER_I) u_i = Ki * T * e_1 + u_1;

	// Derivative gain
	if(CONTROLLER_D) u_d = (1 / (T + alpha)) * ((Kd * alpha * T + Kd * alpha * alpha) * e + (alpha * T * Kd + Kd * alpha * T + Kd * Kd) * e_1 + (Kd * alpha * T + Kd * alpha * alpha) * e_2 - (2 * (T + alpha)) * u_1 - (T + alpha) * u_2);

	if(CONTROLLER_IS_SATURATED){
		u = u_p + u_d;
	    	u_1 = 0;
		e_1 = 0;
	}else{
		u = u_p + u_i + u_d;
		e_1 = e;
		u_1 = u;
	}

	return u;
}

int main(){
	AI_Configuration(samplingRateHZ, timeoutS, vMin, vMax);
	AO_Configuration(vMin, vMax);

	cout << "Enter a reference voltage between -5V to +5V: ";
	cin >> refVoltage;
	
	// Save output data
	FILE *dataFile = fopen("feedback_lab3.txt", "w");

	float64 y, e = 0;
	float64 outputVoltage;
	while(true){
		y = Read_Voltage();
		e = refVoltage - y;

		outputVoltage = bilinearTransformationPID(e);

		// Clip value into the motor and check if the controller has become saturated
		if(outputVoltage > 6){
			outputVoltage = 6;
			CONTROLLER_IS_SATURATED = true;
		
		}else if(outputVoltage < -6){
			outputVoltage = -6;
			CONTROLLER_IS_SATURATED = true;
		}

		Write_Voltage(outputVoltage);

		cout << outputVoltage << endl;
		fprintf(dataFile, "%f\n", outputVoltage);
	}
}