From e521c124f4adca8e960b6eb06dd48735db6607e8 Mon Sep 17 00:00:00 2001
From: FluffyCube9343 <learningninja007@gmail.com>
Date: Sat, 13 Aug 2022 14:23:20 -0400
Subject: [PATCH] Added PID code for Ultrasonic demo (may not work)

---
 FakePID/PID_Practice/PID_Practice.ino | 44 +++++++++++++++++++++++++
 FakePID/Pid2/Pid2.ino                 | 47 +++++++++++++++++++++++++++
 2 files changed, 91 insertions(+)
 create mode 100644 FakePID/PID_Practice/PID_Practice.ino
 create mode 100644 FakePID/Pid2/Pid2.ino

diff --git a/FakePID/PID_Practice/PID_Practice.ino b/FakePID/PID_Practice/PID_Practice.ino
new file mode 100644
index 0000000..4e668ba
--- /dev/null
+++ b/FakePID/PID_Practice/PID_Practice.ino
@@ -0,0 +1,44 @@
+#include <SR04.h>
+//PID constants
+double kp = 2;
+double ki = 5;
+double kd = 1;
+
+unsigned long currentTime, previousTime;
+double elapsedTime;
+double error;
+double lastError;
+double input, output, setPoint;
+double cumError, rateError;
+double Setpoint = 100;
+long dist;
+SR04 sr04 = SR04(2,3);
+
+void setup(){
+        setPoint = 0;
+        Serial.begin(115200);//set point at zero degrees
+}    
+
+void loop(){
+        dist = sr04.Distance();                //read from rotary encoder connected to A0
+        output = computePID(input);
+        delay(100);
+        Serial.println("Read: "+String(dist)+", Move: "+String(output));
+
+}
+
+double computePID(double inp){     
+        currentTime = millis();                //get current time
+        elapsedTime = (double)(currentTime - previousTime);        //compute time elapsed from previous computation
+        
+        error = Setpoint - inp;                                // determine error
+        cumError += error * elapsedTime;                // compute integral
+        rateError = (error - lastError)/elapsedTime;   // compute derivative
+
+        double out = kp*error + ki*cumError + kd*rateError;                //PID output               
+
+        lastError = error;                                //remember current error
+        previousTime = currentTime;                        //remember current time
+
+        return out;                                        //have function return the PID output
+}
diff --git a/FakePID/Pid2/Pid2.ino b/FakePID/Pid2/Pid2.ino
new file mode 100644
index 0000000..cfc3bdf
--- /dev/null
+++ b/FakePID/Pid2/Pid2.ino
@@ -0,0 +1,47 @@
+/*
+sketch belongs to this video: https://youtu.be/crw0Hcc67RY
+write by Moz for YouTube changel logMaker360
+4-12-2017
+*/
+
+#include <PID_v1.h>
+#include <SR04.h>
+double Setpoint ; // will be the desired value
+double Input; // photo sensor
+double Output ; //LED
+//PID parameters
+double Kp=0, Ki=10, Kd=0; 
+long dist;
+SR04 sr04 = SR04(2,3);
+ 
+//create PID instance 
+PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT);
+ 
+void setup()
+{
+  
+  Serial.begin(9600);   
+  //Hardcode the brigdness value
+  Setpoint = 100;
+  //Turn the PID on
+  myPID.SetMode(AUTOMATIC);
+  //Adjust PID values
+  myPID.SetTunings(Kp, Ki, Kd);
+}
+ 
+void loop()
+{
+  //Read the value from the light sensor. Analog input : 0 to 1024. We map is to a value from 0 to 255 as it's used for our PWM function.
+  Input = sr04.Distance();  // photo senor is set on analog pin 5
+  //PID calculation
+  myPID.Compute();
+  //Write the output as calculated by the PID function
+  analogWrite(3,Output); //LED is set to digital 3 this is a pwm pin. 
+  //Send data by serial for plotting 
+  //Serial.print(Input);
+  //Serial.print(" ");
+  Serial.println(Output);
+  //Serial.print(" ");  
+  //Serial.println(Setpoint);
+//  delay(100); 
+}