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(){
[Link](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);
}
//[Link](j);
// [Link](media);
//[Link](" 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();
// [Link](t-tiempo, DEC); [Link](" ");
[Link](salida, DEC); [Link](" ");
[Link](nivel, DEC); [Link](" ");
// [Link](referencia); [Link](" ");
// [Link](error, DEC); [Link](" ");
[Link]("");
}
////////////////////// 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;
}
}