PID Position Control of a Levitating Balsa Disc Assembly Inside a Graduated Cylinder

1#include <Wire.h>
2#include <LiquidCrystal_I2C.h>
3LiquidCrystal_I2C lcd(0x27, 20, 4); // I2C address 0x27, 20 , 4
4
5String command;
6int n;
7int trigPin = 9;    // TRIG pin
8int echoPin = 8;    // ECHO pin
9int fan = 11;// Main Graduated Cylinder Fan
10int disturb = 10; // Disturbance Fan
11float disturbanceFan;
12
13float propBand=64;
14float integralTime=2;
15float derivativeTime=0.3;
16float sampleTime=10;
17
18bool enableDisturbance=false;
19
20float Setpoint_Potentiometer();
21float Ultrasonic_filterFunction(float timeConstant, float processGain,float blockIn, float intervalTime);
22float normalizesDistance();
23float PID_output(float process, float setpoint, float Prop, float Integ, float deriv, int Interval);
24float DerivativefilterFunction(float timeConstant, float processGain,float blockIn, float intervalTime); 
25
26void Disturbance_Potentiometer();
27void printText();// Print static text to display
28
29void setup() 
30{
31  lcd.init(); //initialize the lcd
32  lcd.backlight(); //open the backlight
33  // begin serial port
34  Serial.begin (19200);
35
36  // configure the trigger pin to output mode
37  pinMode(trigPin, OUTPUT);
38  
39  // configure the echo pin to input mode
40  pinMode(echoPin, INPUT);
41  // power to fans
42  pinMode(fan, OUTPUT);
43  pinMode(disturb, OUTPUT);
44  printText();
45}
46//****************Start of Looping***********************************
47void loop() 
48{
49  float controlledVariable;
50  float contOutNorm;
51  float setPoint;
52  if (Serial.available()) 
53    {
54      command = Serial.readStringUntil('\n');
55      command.trim();
56      
57      if (command.equals("PB+")) 
58      {
59        propBand=propBand*2;
60      }
61  
62      else if (command.equals("PB-")) 
63      {
64        propBand=propBand/2;
65        if(propBand<=10)
66          propBand=10;
67      }
68
69      else if (command.equals("Ti+")) 
70      {
71        integralTime=integralTime*2;
72      }
73      else if (command.equals("Ti-")) 
74      {
75        integralTime=integralTime/2;
76        if (integralTime<=1)
77          integralTime=1;
78      }
79      else if (command.equals("Td+")) 
80      {
81        derivativeTime=derivativeTime+0.1;
82      }
83      else if (command.equals("Td-")) 
84      {
85        derivativeTime=derivativeTime-0.1;
86        if(derivativeTime<=0)
87          derivativeTime=0;
88      }
89      else if (command.equals("enDist")) 
90      {
91        enableDisturbance=true;
92      }
93      else if (command.equals("disDist")) 
94      {
95        enableDisturbance=false;
96      }
97      else 
98      {
99      Serial.println("bad command");
100      }
101    Serial.print("Command: ");
102    Serial.println(command);
103   }
104  
105  
106  if(enableDisturbance==false)//enable/disble disturbace potentiometer
107  {
108  Disturbance_Potentiometer();
109 
110  } 
111  else if(enableDisturbance==true)//square wave disturbance
112   {
113    n++;
114    
115    if(n<=500)
116      {
117       analogWrite(disturb,255);
118       disturbanceFan=10.0;
119      }
120    if(n>500)
121    {
122       analogWrite(disturb,0);
123       disturbanceFan=0.0;
124    }
125    if(n==1000)
126      n=0;
127  }
128  
129  
130  controlledVariable=normalizesDistance();
131  setPoint=-Setpoint_Potentiometer() + 1.0;// reveresing pot
132  contOutNorm=PID_output(controlledVariable, setPoint,propBand, integralTime, derivativeTime, sampleTime); 
133  analogWrite( fan,((float)contOutNorm)*255);
134  delay(sampleTime);
135}
136//************End of Looping***********************************************
137
138float normalizesDistance()
139{
140float duration_us;
141float distance_cm;
142float filtered_distance;
143// generate 10-microsecond pulse to TRIG pin
144  digitalWrite(trigPin, HIGH);
145  delayMicroseconds(10);
146  digitalWrite(trigPin, LOW);
147
148  // measure duration of pulse from ECHO pin
149  duration_us = pulseIn(echoPin, HIGH);
150
151  // calculate the distance
152  distance_cm = 0.017 * duration_us;
153  filtered_distance = Ultrasonic_filterFunction(0.5, 1.0,distance_cm, sampleTime);
154  if (filtered_distance>31) 
155    filtered_distance=0;
156  return filtered_distance/31;  // range 0 to 31 cm
157  //return distance_cm/31;
158}
159
160float Ultrasonic_filterFunction(float timeConstant, float processGain,float blockIn, float intervalTime)
161{
162float static blockOut;
163blockOut=blockOut+(intervalTime/1000/(timeConstant+intervalTime/1000))*(processGain*blockIn-blockOut);
164return blockOut;  
165}
166
167
168float Setpoint_Potentiometer()
169{
170int potA3;  
171potA3=analogRead(A3);
172return (float)potA3/1023;
173}
174
175void Disturbance_Potentiometer()
176{
177int potA1;  
178potA1=analogRead(A1);
179analogWrite(disturb,(float)potA1/1023*255);// temporary for test
180disturbanceFan=(float)potA1/1023*10;
181}
182
183float PID_output(float process, float setpoint, float Prop, float Integ, float deriv, int Interval)
184{
185float Er;
186static float Olderror, Cont;
187static int Limiter_Switch;
188static float Integral=0.82;
189float derivative;
190float proportional;
191float deltaT;
192float filteredDerivative;
193deltaT=float(Interval)/1000;
194Limiter_Switch = 1;
195//delay(Interval);  // Interval in msec is delta t in the integral and derivative  calculations
196 Er=(process-setpoint); //forward acting
197//Limiter switch turns integration OFF if controller is already at 100% output or 0% output
198//Prevents integral windup, where controller keeps integrating when controller output can no longer
199//affect the process.
200
201if ((Cont >= 1 && Er > 0) || (Cont <= 0 && Er < 0) || (Integ >= 3600)) 
202        Limiter_Switch = 0;
203else
204        Limiter_Switch = 1;     
205  
206Integral = Integral + 100 / Prop / Integ * Er *deltaT * Limiter_Switch;// Integral calculator
207derivative = 100 / Prop * deriv * (Er - Olderror) / deltaT;// Derivative calculator
208//filteredDerivative=DerivativefilterFunction(0.1, 1.0,derivative, sampleTime);
209proportional = 100 / Prop * Er;// Proportional calculator
210        
211Cont = proportional + Integral + derivative;
212Olderror = Er;// remember previous error for deriative calculator
213
214if (Cont > 1) // limit controller output between 0.0 and 1.0 a normalized value
215    Cont = 1;
216
217if (Cont < 0) 
218    Cont = 0;
219 // Serial Plotter Variables 
220  Serial.print(-31*process+31);
221  Serial.print(" = Process");
222  Serial.print(",");
223  Serial.print(-31*setpoint+31);
224  Serial.print(" = Set Point");
225  Serial.print(",");
226  Serial.print(disturbanceFan);
227  Serial.println(" = disturbance Fan"); 
228  Serial.print(",");
229 // LCD Display Variables
230lcd.setCursor(9 , 1); 
231lcd.print("    ");    
232lcd.setCursor(9 , 1); 
233lcd.print((int)(Cont*100.0));
234
235lcd.setCursor(15 , 0); 
236lcd.print("     ");    
237lcd.setCursor(15 , 0);
238lcd.print((int)(proportional*100.0));
239
240lcd.setCursor(15 , 1); 
241lcd.print("    ");    
242lcd.setCursor(15 , 1); 
243lcd.print((int)(Integral*100.0));
244
245lcd.setCursor(15 , 2); 
246lcd.print("     ");    
247lcd.setCursor(15 , 2); 
248lcd.print((int)(derivative*100.0));
249
250lcd.setCursor(9 , 2); 
251lcd.print("   ");
252lcd.setCursor(9 , 2); 
253lcd.print((int)(-31*setpoint+31)); 
254
255lcd.setCursor(9, 0); 
256lcd.print("    ");
257lcd.setCursor(9, 0); 
258lcd.print((int)(-31*process+31)); 
259
260//PID Parameters
261lcd.setCursor(3 , 3); 
262lcd.print("    ");    
263lcd.setCursor(3 , 3); 
264lcd.print((int)Prop);
265lcd.setCursor(11 , 3); 
266lcd.print("   ");    
267lcd.setCursor(11 , 3); 
268lcd.print((int)Integ);
269lcd.setCursor(17 , 3); 
270lcd.print("   ");    
271lcd.setCursor(17 , 3); 
272lcd.print(deriv);
273lcd.setCursor(0, 0);
274lcd.print("P");
275
276return  Cont;
277}
278
279void printText()// Print Static Text
280  {
281 
282  lcd.setCursor(0, 0);
283  lcd.print("Posit cm ");
284  lcd.setCursor(0, 1);
285  lcd.print("C Out  % ");
286  lcd.setCursor(0, 2);
287  lcd.print("SetPt cm  ");
288
289  lcd.setCursor(13, 0);
290  lcd.print("P "); 
291  lcd.setCursor(13, 1);
292  lcd.print("I ");
293  lcd.setCursor(13, 2);
294  lcd.print("D ");
295
296  lcd.setCursor(0, 3);
297  lcd.print("PB=  ");
298  lcd.setCursor(8, 3);
299  lcd.print("Ti=  ");
300  lcd.setCursor(14, 3);
301  lcd.print("Td=  ");
302  
303  }