#include <Servo.

h
>
const int trigPin = 9;
const int echoPin = 10;
const int servoPin = 6;
long duration; //Tiempo que toma el eco para la vuelta en el ultrasonico.
int distance; //Ubicacion de la bola en el sensor.
int pos = 0; //Posicion del servomotor.
double StartAngle = 112; //Angulo del servomotor cuando la bola esta al paralelo
de la superficie.
double pError,iError,dError; //Valores de error asociado con el PID.
double Output; //Suma de valores de error multiplicados con su
correspondiente ganancia.
double prevError; //Error de previa prevencion de error.
unsigned long prevTime,now; //Tiempo previo y tiempo corriendo.
double dt; //Cambio del tiempo.
int Setpoint = 0; //Punto deseado de la pelota.
float Kp = 3.400; //Constante proporcional.
float Ki = 0.159; //Constante de integracion.
float Kd = 17.836; //Constante derivativa.

Servo servo; //Crea la variable servo para controlar el servomotor.

void setup() {
pinMode(trigPin, OUTPUT); //Configura el trigPin como salida.
pinMode(echoPin, INPUT); //Configura el echoPin como una entrada

servo.attach(servoPin); //Configura el pin 6 como salida del servomotor.

servo.write(StartAngle); //Iniciar el servomotor en el angulo horizontal.
Serial.begin(9600); //Inicia la comunicacion serial.
}
void loop() {

digitalWrite(trigPin, LOW);
delayMicroseconds(2);
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
duration = pulseIn(echoPin, HIGH);
distance= duration*0.034/2 - 15;
//Imprime la distancia en el monitor serial.
if (distance > 40 or distance < -40) {
distance = -12; //pone el valor de distancia como 12 para evitar que
genere valores aleatorios.
}
if (distance == 0 or distance == 0) {
distance = 0; //pone el valor de distancia como 0 para evitar que
genere valores aleatorios.
}

//Algoritmo PID
now = millis();
dt = (now - prevTime);
pError = Setpoint - distance;
dError = (pError - prevError) / dt;
iError = iError + (pError * dt);

if (iError > 10) {

 iError = 10; //Si el error es mas grande que 10 que no haga nada, porque
es ruido.
}

Output = Kp * pError + Ki * iError + Kd * dError;

//Aqui se evita que se generen valores aleatorios.

if (Output >40) {
Output = 40;
}
if (Output <-40) {
Output = -40;
}

servo.write(StartAngle + Output);

//Salida serial
Serial.print("PID: ");
Serial.println(Output);

//Configura el error previo y el tiempo.

prevError = pError;
prevTime = now;