TECNOLOGICO NACIONAL DE MEXICO

INSTITUTO TECNOLOGICO DE CIUDAD JUAREZ

INSTRUMENTACIÓN
Sensores con Filtro

EQUIPO 3:
- GONZALEZ MONTOYA JESUS ALFONSO
– LOPEZ MARCELINO BRANDON DAVID
- ORIGINALES MONTOYA DIEGO
- PEREZ ALVARADO NAYELI

31/03/2023
 1. Introduccion
En esta práctica utilizamos un sensor ultrasónico HC-SR04 el cual detecta la distancia
de algún objeto que este en frente de los transductores. Utilizamos dos filtros: el filtro
EMA y el filtro de Kalman.

2. Desarrollo
2.1 Código

#include <Arduino.h>

#define trig_pin 23
#define echo_pin 22
#define red_area_pin 2
#define yellow_area_pin 4
#define green_area_pin 5
#define start 19
#define stop 21

#define SOUND_SPEED 0.034

//Duracion del sonido en regresar

float distanceCm; // Variable para almacenar la distancia en cm
float distanceInch; // Variable para almacenar la distancia en in

double distance, duration;

double kaldist;

double kalman(double U){

static const double R = 40;
static const double H = 1.00;
static double Q = 10;
static double P = 0;
static double U_hat = 0;
static double K = 0;
K = P*H/(H*P*H+R);
U_hat += + K*(U-H*U_hat);
P = (1-K*H)*P+Q;
return U_hat;
}

void setup() {
// put your setup code here, to run once:
Serial.begin(115200);
 pinMode(trig_pin, OUTPUT);
pinMode(echo_pin, INPUT);
pinMode(red_area_pin, OUTPUT);
pinMode(yellow_area_pin, OUTPUT);
pinMode(green_area_pin, OUTPUT);
pinMode(start, INPUT);
pinMode(stop, INPUT);

Serial.println("Ultrasonic Sensor with Filters"); //Filtro EMA, Filtro Kalman

void loop() {
if (digitalRead(start) == HIGH){
// put your main code here, to run repeatedly:
digitalWrite(trig_pin, LOW); //Asegurar el apagado del trig
delayMicroseconds(2);
digitalWrite(trig_pin, HIGH); //Asegurar el encendido del trig
delayMicroseconds(10);

//duration = pulseIn(echo_pin, HIGH); //Cuanto dura ECHO en HIGH, leer el

tiempo de regreso del sonido

//distanceInch = distanceCm * CM_TO_INCH;

//FILTRO kalman

duration = pulseIn(echo_pin, HIGH);

distance = duration * SOUND_SPEED/2;
kaldist = kalman(distance);

Serial.print("Distance (in cm): ");

Serial.println(distance);
Serial.print("Corrected distance (in cm): %f");
Serial.println(kaldist);

if (kaldist >260 && kaldist <400){

digitalWrite(red_area_pin, HIGH);
 digitalWrite(yellow_area_pin, LOW);
digitalWrite(green_area_pin, LOW);
Serial.println("Red Zone");
}

if (kaldist >130 && kaldist <260){

digitalWrite(yellow_area_pin, HIGH);
digitalWrite(green_area_pin, LOW);
digitalWrite(red_area_pin, LOW);
Serial.println("Yellow Zone");
}

if (kaldist >= 0 && kaldist <=130){

digitalWrite(green_area_pin, HIGH);
digitalWrite(yellow_area_pin, LOW);
digitalWrite(red_area_pin, LOW);
Serial.println("Green Zone");
}

Serial.print("Distance in cm: ");

Serial.println(distance);
Serial.print("Distance FILTER: ");
Serial.println(kaldist);

delay(100);
}
if (digitalRead(start) == LOW && digitalRead(stop) == HIGH){
digitalWrite(yellow_area_pin, LOW);
digitalWrite(red_area_pin, LOW);
digitalWrite(green_area_pin, LOW);
delay(110);
}
}
}
2.2 Simulación
2.2 Circuito en fisico