#include <Arduino.h>
#define trig_pin 5
#define echo_pin 18
#define red_area_pin 15
#define yellow_area_pin 2
#define green_area_pin 4
#define SOUND_SPEED 0.034
#define CM_TO_INCH 0.393701
#define Alpha_EMA .2
#define kalman 23
#define ema 22
long duration; //duracion del sonido en regresar
float distanceCm; //variable para almacenar la distancia en cm
float distanceInch; //variable para almacenar la distancia en in
float signal_filter_ema;
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;
void setup() {
Serial.begin(1152000);
pinMode(trig_pin, OUTPUT);
pinMode(echo_pin, OUTPUT);
pinMode(red_area_pin, OUTPUT);
pinMode(yellow_area_pin, OUTPUT);
pinMode(green_area_pin, OUTPUT);
pinMode(kalman, INPUT);
pinMode(ema, INPUT);
Serial.println("Ultrasonic Sensor with filters"); //filtro EMA, filtro kalman
}
void loop() {
digitalWrite(trig_pin, LOW); //asegurar el apagado del trig
delayMicroseconds(2); //espera en microsegundos
digitalWrite(trig_pin, HIGH); //encender el trig
delayMicroseconds(10); //esperar 10 microsegundos para la señal de regreso
digitalWrite(trig_pin, LOW);
duration = pulseIn(echo_pin, HIGH);
distanceCm = duration * SOUND_SPEED/2; //ecuacion del proveedor
distanceInch = distanceCm * CM_TO_INCH; //conversion en pulgadas
signal_filter_ema = Alpha_EMA*distanceCm + (1-Alpha_EMA)*(signal_filter_ema);
K = P*H/(H*P*H+R);
U_hat = U_hat + K*(distanceCm-H*U_hat);
P = (1-K*H)*P+Q;
if (digitalRead(ema) == 1 && digitalRead(kalman) == 0)
{
if (signal_filter_ema > 30 && signal_filter_ema < 120){
digitalWrite(red_area_pin, HIGH);
Serial.println("we are at red zone");
}
else{
digitalWrite(red_area_pin, LOW);
Serial.println("we area at no zone");
}
if (signal_filter_ema > 120 && signal_filter_ema < 210){
digitalWrite(yellow_area_pin, HIGH);
Serial.println("we are at yellow zone");
}
else{
digitalWrite(yellow_area_pin, LOW);
Serial.println("we area at no zone");
}
if (signal_filter_ema > 210 && signal_filter_ema < 400){
digitalWrite(green_area_pin, HIGH);
Serial.println("we are at green zone");
}
else{
digitalWrite(green_area_pin, LOW);
Serial.println("we area at no zone");
}
Serial.print("Distance (cm): ");
Serial.println(distanceCm);
Serial.print("Distance (filter): ");
Serial.println(signal_filter_ema);
delay(100);
}
if (digitalRead(ema) == 0 && digitalRead(kalman) == 1)
{
if (U_hat > 30 && U_hat < 120){
digitalWrite(red_area_pin, HIGH);
Serial.println("we are at red zone");
}
else{
digitalWrite(red_area_pin, LOW);
Serial.println("we area at no zone");
}
if (U_hat > 120 && U_hat < 210){
digitalWrite(yellow_area_pin, HIGH);
Serial.println("we are at yellow zone");
}
else{
digitalWrite(yellow_area_pin, LOW);
Serial.println("we area at no zone");
}
if (U_hat > 210 && U_hat < 400){
digitalWrite(green_area_pin, HIGH);
Serial.println("we are at green zone");
}
else{
digitalWrite(green_area_pin, LOW);
Serial.println("we area at no zone");
}
Serial.print("Distance (cm): ");
Serial.println(distanceCm);
Serial.print("Distance (filter): ");
Serial.println(U_hat);
delay(100);
}
}