#include <Wire.

h>
#include <LiquidCrystal_I2C.h>
LiquidCrystal_I2C lcd(0x27, 20, 4); // I2C address 0x27, 20 , 4

String command;
int n;
int trigPin = 9; // TRIG pin
int echoPin = 8; // ECHO pin
int fan = 11;// Main Graduated Cylinder Fan
int disturb = 10; // Disturbance Fan
float disturbanceFan;

float propBand=64;
float integralTime=2;
float derivativeTime=0.3;
float sampleTime=10;

bool enableDisturbance=false;

float Setpoint_Potentiometer();
float Ultrasonic_filterFunction(float timeConstant, float processGain,float
blockIn, float intervalTime);
float normalizesDistance();
float PID_output(float process, float setpoint, float Prop, float Integ, float
deriv, int Interval);
float DerivativefilterFunction(float timeConstant, float processGain,float blockIn,
float intervalTime);

void Disturbance_Potentiometer();
void printText();// Print static text to display

void setup()
{
lcd.init(); //initialize the lcd
lcd.backlight(); //open the backlight
// begin serial port
Serial.begin (19200);

// configure the trigger pin to output mode

pinMode(trigPin, OUTPUT);

// configure the echo pin to input mode

pinMode(echoPin, INPUT);
// power to fans
pinMode(fan, OUTPUT);
pinMode(disturb, OUTPUT);
printText();
}
//****************Start of Looping***********************************
void loop()
{
float controlledVariable;
float contOutNorm;
float setPoint;
if (Serial.available())
{
command = Serial.readStringUntil('\n');
command.trim();
 if (command.equals("PB+"))
{
propBand=propBand*2;
}

else if (command.equals("PB-"))
{
propBand=propBand/2;
if(propBand<=10)
propBand=10;
}

else if (command.equals("Ti+"))
{
integralTime=integralTime*2;
}
else if (command.equals("Ti-"))
{
integralTime=integralTime/2;
if (integralTime<=1)
integralTime=1;
}
else if (command.equals("Td+"))
{
derivativeTime=derivativeTime+0.1;
}
else if (command.equals("Td-"))
{
derivativeTime=derivativeTime-0.1;
if(derivativeTime<=0)
derivativeTime=0;
}
else if (command.equals("enDist"))
{
enableDisturbance=true;
}
else if (command.equals("disDist"))
{
enableDisturbance=false;
}
else
{
Serial.println("bad command");
}
Serial.print("Command: ");
Serial.println(command);
}

if(enableDisturbance==false)//enable/disble disturbace potentiometer

{
Disturbance_Potentiometer();

}
else if(enableDisturbance==true)//square wave disturbance
{
n++;

if(n<=500)
 {
analogWrite(disturb,255);
disturbanceFan=10.0;
}
if(n>500)
{
analogWrite(disturb,0);
disturbanceFan=0.0;
}
if(n==1000)
n=0;
}

controlledVariable=normalizesDistance();
setPoint=-Setpoint_Potentiometer() + 1.0;// reveresing pot
contOutNorm=PID_output(controlledVariable, setPoint,propBand, integralTime,
derivativeTime, sampleTime);
analogWrite( fan,((float)contOutNorm)*255);
delay(sampleTime);
}
//************End of Looping***********************************************

float normalizesDistance()
{
float duration_us;
float distance_cm;
float filtered_distance;
// generate 10-microsecond pulse to TRIG pin
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);

// measure duration of pulse from ECHO pin

duration_us = pulseIn(echoPin, HIGH);

// calculate the distance

distance_cm = 0.017 * duration_us;
filtered_distance = Ultrasonic_filterFunction(0.5, 1.0,distance_cm, sampleTime);
if (filtered_distance>31)
filtered_distance=0;
return filtered_distance/31; // range 0 to 31 cm
//return distance_cm/31;
}

float Ultrasonic_filterFunction(float timeConstant, float processGain,float

blockIn, float intervalTime)
{
float static blockOut;
blockOut=blockOut+(intervalTime/1000/(timeConstant+intervalTime/
1000))*(processGain*blockIn-blockOut);
return blockOut;
}

float Setpoint_Potentiometer()
{
int potA3;
potA3=analogRead(A3);
return (float)potA3/1023;
}

void Disturbance_Potentiometer()
{
int potA1;
potA1=analogRead(A1);
analogWrite(disturb,(float)potA1/1023*255);// temporary for test
disturbanceFan=(float)potA1/1023*10;
}

float PID_output(float process, float setpoint, float Prop, float Integ, float
deriv, int Interval)
{
float Er;
static float Olderror, Cont;
static int Limiter_Switch;
static float Integral=0.82;
float derivative;
float proportional;
float deltaT;
float filteredDerivative;
deltaT=float(Interval)/1000;
Limiter_Switch = 1;
//delay(Interval); // Interval in msec is delta t in the integral and derivative
calculations
Er=(process-setpoint); //forward acting
//Limiter switch turns integration OFF if controller is already at 100% output or
0% output
//Prevents integral windup, where controller keeps integrating when controller
output can no longer
//affect the process.

if ((Cont >= 1 && Er > 0) || (Cont <= 0 && Er < 0) || (Integ >= 3600))
Limiter_Switch = 0;
else
Limiter_Switch = 1;

Integral = Integral + 100 / Prop / Integ * Er *deltaT * Limiter_Switch;// Integral

calculator
derivative = 100 / Prop * deriv * (Er - Olderror) / deltaT;// Derivative calculator
//filteredDerivative=DerivativefilterFunction(0.1, 1.0,derivative, sampleTime);
proportional = 100 / Prop * Er;// Proportional calculator

Cont = proportional + Integral + derivative;

Olderror = Er;// remember previous error for deriative calculator

if (Cont > 1) // limit controller output between 0.0 and 1.0 a normalized value
Cont = 1;

if (Cont < 0)
Cont = 0;
// Serial Plotter Variables
Serial.print(-31*process+31);
Serial.print(" = Process");
Serial.print(",");
Serial.print(-31*setpoint+31);
Serial.print(" = Set Point");
Serial.print(",");
 Serial.print(disturbanceFan);
Serial.println(" = disturbance Fan");
Serial.print(",");
// LCD Display Variables
lcd.setCursor(9 , 1);
lcd.print(" ");
lcd.setCursor(9 , 1);
lcd.print((int)(Cont*100.0));

lcd.setCursor(15 , 0);
lcd.print(" ");
lcd.setCursor(15 , 0);
lcd.print((int)(proportional*100.0));

lcd.setCursor(15 , 1);
lcd.print(" ");
lcd.setCursor(15 , 1);
lcd.print((int)(Integral*100.0));

lcd.setCursor(15 , 2);
lcd.print(" ");
lcd.setCursor(15 , 2);
lcd.print((int)(derivative*100.0));

lcd.setCursor(9 , 2);
lcd.print(" ");
lcd.setCursor(9 , 2);
lcd.print((int)(-31*setpoint+31));

lcd.setCursor(9, 0);
lcd.print(" ");
lcd.setCursor(9, 0);
lcd.print((int)(-31*process+31));

//PID Parameters
lcd.setCursor(3 , 3);
lcd.print(" ");
lcd.setCursor(3 , 3);
lcd.print((int)Prop);
lcd.setCursor(11 , 3);
lcd.print(" ");
lcd.setCursor(11 , 3);
lcd.print((int)Integ);
lcd.setCursor(17 , 3);
lcd.print(" ");
lcd.setCursor(17 , 3);
lcd.print(deriv);
lcd.setCursor(0, 0);
lcd.print("P");

return Cont;
}

void printText()// Print Static Text

{

lcd.setCursor(0, 0);
lcd.print("Posit cm ");
lcd.setCursor(0, 1);
lcd.print("C Out % ");
lcd.setCursor(0, 2);
lcd.print("SetPt cm ");

lcd.setCursor(13, 0);
lcd.print("P ");
lcd.setCursor(13, 1);
lcd.print("I ");
lcd.setCursor(13, 2);
lcd.print("D ");

lcd.setCursor(0, 3);
lcd.print("PB= ");
lcd.setCursor(8, 3);
lcd.print("Ti= ");
lcd.setCursor(14, 3);
lcd.print("Td= ");