#include<Servo.

h>
#include<PID_v1.h>
#include<NewPing.h>
#define TRIGGER_PIN 5
Pin for UltraSonic Sensor
#define ECHO_PIN 6
for UltraSonic Sensor
#define MAX_DISTANCE 30
ance to be sensed (cm)
const int servoPin = 9;
n
int potPin = A0;
otentiometer Pin

//Trigger

float Kp = 0.8;
Proportional Gain
float Ki = 0.07;
Integral Gain
float Kd = 0.12;
Derivative Gain
double Setpoint, Input, Output;

//Initial

NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);

e NewPing object, which is in the class

//Echo Pin
//Max Dist
//Servo Pi
//Analog P

//Initial
//Intitial

//Initializ
// NewPing

that calcualtes and digitally filters

// Ultraso
nic Sensor data
PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT);
e PID object, which is in the class PID.

//Initializ
// This cl

ass 'filters' the error signal based on

// where t
he Setpoint and Input is. The Output is
// sent to
the Servo in terms of degrees.
Servo myServo;
ervo.

//Initial S

void setup() {
Serial.begin(9600);
ial (to show readings)
myServo.attach(servoPin);
rvo

//Begin Ser

Input = readPosition();
ction readPosition() and sets the balls

//Calls fun

//Attach Se

// positio
n as the input to the PID algorithm
Setpoint = map( analogRead(potPin), 0, 255, 5, 20);
ocation of ball in terms of potentiometer redading
myPID.SetMode(AUTOMATIC);
bject myPID to AUTOMATIC (see playground for more details)

//Desired l
//Set PID o

myPID.SetOutputLimits(80, 100);
t limits to 80 and 100 degrees. This keeps my servo from

//Set Outpu
// going t

oo far each way and messing up the plant.

}
void loop()
{
Setpoint = map( analogRead(potPin), 0, 255, 5, 20);
//Setpoint
same as above in function setup() (see above for more details)
Input = readPosition();
//Input sam
e as above in function setup() (see above for more details)
myPID.Compute();
//The 'magi
c' happens and algorithm computes Output in range of 80 to 100 degrees
myServo.write(Output);
//Writes va
lue of Output to servo
printData(Setpoint, Input, Output);
ction printData(). Prints Setpoint, Input, and Output to Serial
newGains();
ction newGains(). Changes Kp, Ki, and Kd and prints to Serial
}

//Calls fun
//Calls fun

//******************************************************************************
****************************************************************************
//PrintData() prints Setpoint, Input, and Output to Serial each time PID loop ru
ns. This may cause PID loop to run a tad slower than wanted
//
so it may be helpful to use this only when testing
//******************************************************************************
****************************************************************************
void printData(float set, float in, float out) {
Serial.print("Setpoint = ");
Serial.print(set);
Serial.print(" Input = ");
Serial.print(in);
Serial.print(" Output = ");
Serial.print(out);
Serial.print("\n");
}
//******************************************************************************
****************************************************************************
//newGains() watis to see if anything is typed in serial port. If there is, the
values are put into Kp, Ki, and Kd. Then the values are printed to Serial.
//******************************************************************************
****************************************************************************
void newGains() {
if (Serial.available() > 0) {
//If ther
e is any data in serial input, then it starts reading.
delay(100);
//Pause f
or a tenth of a second to readjust plant or anything else you need to do.
// Can b
e as long as needed.
for (int i = 0; i < 4; i = i + 1) {
switch (i) {
case 0:
//Reads 1
st value in
Kp = Serial.parseFloat();
break;

case 1:
st value in
Ki = Serial.parseFloat();
break;
case 2:
st value in
Kd = Serial.parseFloat();
break;
case 3:
any remaining parts.
for (int j = Serial.available(); j == 0; j = j - 1) {
Serial.read();
}
break;
}
}
Serial.print(" Kp, Ki, Kd = ");
s new gain values to Serial
Serial.print(Kp);
Serial.print(", ");
Serial.print(Ki);
Serial.print(", ");
Serial.println(Kd);
myPID.SetTunings(Kp, Ki, Kd);
}

//Reads 2

//Reads 3

//Clears

//Print

}
//******************************************************************************
****************************************************************************
//readPosition() reads the position of the ball and returns balls position in cm
.
//******************************************************************************
****************************************************************************
float readPosition() {
delay(36);
//Don't
set too low or echos will run into eachother.
unsigned int uS = sonar.ping_median(4);
//ping_m
edian() function is in class NewPing. It is a digital filter that takes the
// the
average of 4 (in this case) pings and returns that distance value. This helps
// to f
ilter out the pesy noise in those $5.00 UltraSonic Sensors. You know which
// ones
I am talking about....
return uS / US_ROUNDTRIP_CM;
//Return
s distance value.
}