Radar Robot con Arduino

Como estan? En esta ocación voy a mostrarles como construir un radar con un sensor ultrasónico de distancia us-020, Arduino Duemilanove (Atmega328) y un servo Tower Pro SG90. Despues vamos a hacer unos calculos para tener mejor presicion en la deteccion de los objetos y por ultimo lo veremos montado sobre un robot.


Para empezar se conecto todo de la siguientemanera:

Servo rojo -> +5V
Servo marron -> GND
Servo naranja -> Pin 8 Arduino
Sensor Trigger ->Pin 12 Arduino
Sensor echo -> Pin 11 Arduino
Sensor VCC -> +5V
Sensor GND -> GND

Para manejar el servo se usa la libreria Servo.h que viene con la IDE y la libreria NewPing.h. Cuando se monta el sensor ultrasonico sobre un servo hay tomar la precaucion de atachar y desatachar el servo en cada movimiento porque genera interferencias en la señal del sensor.

Este es el codigo:

#include <Servo.h>
#include <NewPing.h>

#define TRIGGER_PIN  12
#define ECHO_PIN     11
#define MAX_DISTANCE 400
#define SERVO_PIN 8

Servo myservo;
int i;
NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);

void setup()
{
        myservo.attach(SERVO_PIN);
        myservo.write(0);
        delay(1000);
        myservo.detach();
        Serial.begin(115200);
}



void loop()
{
        for(i=0;i<181;i+=2)
        {
                myservo.attach(SERVO_PIN);
                myservo.write(i);
                delay(50);
                myservo.detach();
                unsigned int uS = sonar.ping();
                int cm = uS / US_ROUNDTRIP_CM;
                Serial.print("Ping: ");
                Serial.print(cm);
                Serial.println("cm");
        }
        myservo.attach(SERVO_PIN);
        myservo.write(0);
        delay(1000);
        myservo.detach();
}

Explicacion:

En la funcion setup se posiciona el servo en 0 y se abre el puerto serie para la transmicion de los valores. En la la funcion loop se mueve el servo de a dos grados, se mide la distancia, se convierte a centimetros, se envia el valor por serie y se vuelve a repetir el ciclo. Despues se vuelve a posicionar el servo en 0 grados y se repite todo el ciclo indefinidamente

El resultado es este:



Con esto logramos medir distancias en un rango de 180 grados (200 contando el angulo de error del sensor), pero no detecta objetos, ahora vamos hacer los calculos para que detecte objetos y el angulo en el que se encuentran.

Empecemos por el estudio del sensor ultrasonico, el datasheet del sensor dice que el engulo de deteccion es de menos de 15 grados, en la practica a mi me dio un angulo mayor, de 20 rgados (en realidad depende del sensor, de la forma del objeto y su textura principalmente), o sea podriamos hablar de un error de medicion de +/- 10 grados.

Esto quiere decir que cuando hay un objeto frente al barrido de nuestro radar el sensor lo detecta 10 grados antes de donde esta y termina de verlo 10 grados despues. Entonces llamemos A1 y A2 a los angulos reales donde apunta el sensor cuando aparece un objeto y E al engulo de error, los angulos reales donde empieza y termina el objeto son A1+E/2 y A2-E/2. Para que se entienda mas aca va lo mismo en un grafico:

De aca podemos deducir la siguiente formula para obtener el centro del objeto con respecto a A1+E/2:

Aplicando este razonamiento perdemos 20 grados en el rango de deteccion de nuestro radar pero ganamos en presicion:

Con todo esto se puede escribir el siguiente codigo que detecta un objeto cuando se produce una diferencia entre dos mediciones (seteada en UMBRAL_DISTANCIA) y nos devuelve el angulo central del objeto

#include <Servo.h>
#include <NewPing.h>

#define TRIGGER_PIN  12
#define ECHO_PIN     11
#define MAX_DISTANCE 400
#define SERVO_PIN 8
#define UMBRAL_DISTANCIA

Servo myservo;
int i, angulo, angulo1, angulo2, hay_obstaculo = false;
unsigned int distancia_cm, distancia_ant_cm = 1;
NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);

void setup()
{
        myservo.attach(SERVO_PIN);
        myservo.write(0);
        delay(1000);
        myservo.detach();
        Serial.begin(115200);
}



void loop()
{
        for(i=0;i<181;i+=2)
        {
                myservo.attach(SERVO_PIN);
                myservo.write(i);
                delay(50);
                myservo.detach();
                unsigned int uS = sonar.ping();
                distancia_cm = uS / US_ROUNDTRIP_CM;

                if ( (distancia_cm * 100 / distancia_ant_cm) < UMBRAL_DISTANCIA && hay_obstaculo == false)
                {
                        hay_obstaculo = true;
                        angulo1 = i;
                }
                if ( (distancia_ant_cm * 100 / distancia_cm) < UMBRAL_DISTANCIA && hay_obstaculo == true)
                {
                        angulo2 = i;
                        angulo = (angulo2 - angulo1)/2 + angulo1 - 10;
                        Serial.print("******************Angulo*****************: ");
                        Serial.println(angulo);
                        hay_obstaculo = false;
                }


                Serial.print("Ping: ");
                Serial.print(distancia_cm);
                Serial.println("cm");
                distancia_ant_cm = distancia_cm;

        }
        hay_obstaculo = false;
        myservo.attach(SERVO_PIN);
        myservo.write(0);
        delay(1000);
        myservo.detach();
}

En este video se puede ver este ejemplo aplicado en un robot que se dirige hacia calcuier objeto que haya:

Comentarios