float distancia;

float tiempo;
float media;
int j=1;
int var=1;
int g;
int i;
const int PWMPin = 11;

void setPwmFrequency(int pin, int divisor);

void setup(){
Serial.begin(9600);

pinMode(6, OUTPUT); /*activacin del pin 6 como salida: para el pulso

ultrasnico*/
pinMode(7, INPUT); /*activacin del pin 7 como entrada: tiempo del rebote del
ultrasonido*/
pinMode(11, OUTPUT); //motor
pinMode(8, OUTPUT); // motor
pinMode(13, OUTPUT);// motor

}
void loop(){

setPwmFrequency(11,1024);

for (i=0;i<20;i++)
{

digitalWrite(6,LOW); /* Por cuestin de estabilizacin del sensor*/

delayMicroseconds(5);
digitalWrite(6, HIGH); /* envo del pulso ultrasnico*/
delayMicroseconds(10);
tiempo=pulseIn(7, HIGH); /* Funcin para medir la longitud del pulso entrante.
Mide el tiempo que transcurrido entre el envo
del pulso ultrasnico y cuando el sensor recibe el rebote, es decir: desde que
el pin 12 empieza a recibir el rebote, HIGH, hasta que
deja de hacerlo, LOW, la longitud del pulso entrante*/
distancia= float(0.0171*tiempo); /*frmula para calcular la distancia
obteniendo un valor entero*/
/*Monitorizacin en centmetros por el monitor serial*/
delayMicroseconds(10);
if (i==0){
media=distancia;
j++;
}

if ((distancia <= media*1.01 && distancia >= media*0.99) && i>1)

{
media=(media+distancia)/j;
j++;

}
// delay(1);
 }
//Serial.println(j);
// Serial.println(media);
//Serial.println(" cm");

j=1;

/////////////////////////// PID ///////////////////////////////////////

///////////////////////////
float referencia=13;// CENTIMETROS
///////////////////////////
int Kp=180;
float Ti=20; //1500
//int Td=0.000001;
float t;
float error;
float T=5;
float nivel;
float nivelA;
float salida;
float salidaA;
float n;
float nAnterior;
float error0;

/* Cada T milisegundos se ejecuta T */

delay(T);

/* Mido el tiempo cada vez que hago un calculo */

tiempo = micros();

nivel = media;

/* Calculamos el error de referencia */

error = referencia - nivel;

/*calculamos la salida del controlador, es decir, nuestra entrada al sistema

entrada al sistema*/
n = nAnterior + error;
salida = Kp * (error + (T * i) / Ti + (Td / T) * (error - error0)); //salida =215
-(Kp * error + ((T*n) / Ti ) );

salidaA = salida;
nivelA = nivel;

/* Limitamos el rango entre el minimo y el mximo */

if (salida > 255){ salida=255;}
if (salida < 0) {salida=0;}

/*Aplicamos la salida*/

digitalWrite(13, HIGH);
digitalWrite(8, LOW);
analogWrite(11,salida);
 /* Actualizamos valores */
nAnterior = n;
error0 = error;

/*Si var = 1 imprimimos por serial los valores de tiempo, entrada

referencia y error del sistema*/
if (var == 1){
t=micros();

// Serial.print(t-tiempo, DEC); Serial.print(" ");

Serial.print(salida, DEC); Serial.print(" ");
Serial.print(nivel, DEC); Serial.print(" ");
// Serial.print(referencia); Serial.print(" ");
// Serial.print(error, DEC); Serial.print(" ");
Serial.println("");
}

////////////////////// FUNCION PARA CAMBIAR LA FRECUENCIA SI LA BOMBA NO FUNCIONA

CORRECTAMENTE////////////////////////////

void setPwmFrequency(int pin, int divisor) {

byte mode;
if(pin == 5 || pin == 6 || pin == 9 || pin == 10) {
switch(divisor) {
case 1: mode = 0x01; break;
case 8: mode = 0x02; break;
case 64: mode = 0x03; break;
case 256: mode = 0x04; break;
case 1024: mode = 0x05; break;
default: return;
}
if(pin == 5 || pin == 6) {
TCCR0B = TCCR0B & 0b11111000 | mode;
} else {
TCCR1B = TCCR1B & 0b11111000 | mode;
}
} else if(pin == 3 || pin == 11) {
switch(divisor) {
case 1: mode = 0x01; break;
case 8: mode = 0x02; break;
case 32: mode = 0x03; break;
case 64: mode = 0x04; break;
case 128: mode = 0x05; break;
case 256: mode = 0x06; break;
case 1024: mode = 0x7; break;
default: return;
}
TCCR2B = TCCR2B & 0b11111000 | mode;
}
}