#include <PID_v1.

h>

#include <Servo.h>

#define PIN_ECHO 8

#define PIN_MOT 9

#define PIN_TRIG 7

#define Nivel_servo 90

Servo myservo;

const float VelSon = 34000.0;

long distancia;

//Define Variables we'll be connecting to

double posicion, Setpoint,Output, error,ServoOutput;

//Specify the links and initial tuning parameters

double Kp=1, Ki=0, Kd=1;

PID myPID(&posicion, &Output, &Setpoint, Kp, Ki, Kd, DIRECT);

void setup(){
 Setpoint = 1; //posicion referencia Voltaje

myservo.attach(PIN_MOT);

pinMode(PIN_TRIG, OUTPUT);

pinMode(PIN_ECHO, INPUT);

posicion = ping(PIN_TRIG, PIN_ECHO);

Serial.begin(9600); //Iniciamos la comunicación con el puerto monitor serial

myservo.write(Nivel_servo);

myPID.SetMode(AUTOMATIC);

myPID.SetOutputLimits(-90,90);

myPID.SetSampleTime(50);

void loop(){

posicion = ping(PIN_TRIG, PIN_ECHO);

if (posicion < 0 && posicion > 1)

Ki=0.1;

else

Ki=0;

if(posicion > 1 || posicion < 1)

myPID.Compute();

error = Setpoint - posicion;

ServoOutput=Nivel_servo-Output;

myservo.write(ServoOutput);

else

myservo.write(Nivel_servo);

//delay(1000);
Serial.println(posicion);

////// Serial.print("cm");

////// Serial.println();

// Serial.println(error);

//

//Serial.println(ServoOutput);

////

int ping(int TriggerPin, int EchoPin) {

long duration;

long distanceCm;

delay(100);

digitalWrite(TriggerPin, LOW); //para generar un pulso limpio ponemos a LOW 4us

delayMicroseconds(10);

digitalWrite(TriggerPin, HIGH); //generamos Trigger (disparo) de 10us

delayMicroseconds(10);

digitalWrite(TriggerPin, LOW);

duration = pulseIn(EchoPin, HIGH); //medimos el tiempo entre pulsos, en

microsegundos

distanceCm = duration * 1000/ 2000/ 1000; //convertimos a distancia, en cm

return distanceCm;