Coche Robot Seguidor de Rostros Autónomo con Arduino Nano y OpenCV

Componentes y suministros

Aplicaciones y servicios online

• Arduino IDE
• OpenCV (visión por computadora)

Sobre este proyecto

Un coche que sigue tu rostro: si te alejas, él te sigue.

Requisitos en la placa Ci20 (Linux)

• OpenCV instalado
• Python
• Permisos puerto serie

Distro usada: Debian 8. Componentes: cámara portátil vieja (reciclada), HC-SR04, 2× baterías 3.7V 4000mAh (recicladas), 2 servos, conversor DC-DC, Arduino Nano, base robot GoPiGo, 2× L298.

Instalación en Linux (terminal)

# Actualizar sistema
sudo apt-get update
sudo apt-get upgrade
sudo apt-get dist-upgrade

# OpenCV 2.4.9.1 (no la última, pero funciona)
sudo apt-get install libopencv-dev python-opencv

# PySerial
sudo apt-get install python python-serial

# Permisos puerto serie (USB Arduino o nativo Ci20)
sudo chown ci20:ci20 /dev/ttyUSB0
sudo chown ci20:ci20 /dev/ttyS1

Ejecución

sudo python facetrackingcar.py 0

El argumento 0 selecciona la fuente de vídeo (cámbialo si tienes varias). Para ver la imagen capturada, descomenta en el script:

#cv.ShowImage("result", img)
#cv.NamedWindow("result", 1)
#cv.DestroyWindow("result")

Necesitas el archivo haarcascade_frontalface_alt.xml en el mismo directorio que el script.

Código

faceser.py (Python – OpenCV + envío serie a Arduino)

#!/usr/bin/python
"""
Demostración de detección de rostros u objetos con características tipo Haar.
Encuentra rostros en imagen de cámara/video, dibuja caja roja y centra la
cámara vía dos servos para que el rostro quede en el centro.
Basado en facedetect.py de ejemplos OpenCV.
"""
import sys
from optparse import OptionParser
import serial
import cv2.cv as cv
import time

# Parámetros detección Haar
min_size = (20, 20)
image_scale = 2
haar_scale = 1.2
min_neighbors = 2
haar_flags = cv.CV_HAAR_DO_CANNY_PRUNING

max_pwm = 180
min_pwm = 1
midScreenWindow = 20   # margen aceptable centro pantalla
panStepSize = 1        # grados por actualización pan
tiltStepSize = 1       # grados por actualización tilt
servoPanPosition = 90  # posición inicial pan (x)
servoTiltPosition = 45 # posición inicial tilt (y)
panGpioPin = 2         # ID servo pan en Arduino (NO pin físico)
tiltGpioPin = 1        # ID servo tilt en Arduino
var = 0

def detect_and_draw(img, cascade):
    gray = cv.CreateImage((img.width, img.height), 8, 1)
    small_img = cv.CreateImage((cv.Round(img.width / image_scale),
                                cv.Round(img.height / image_scale)), 8, 1)
    cv.CvtColor(img, gray, cv.CV_BGR2GRAY)
    cv.Resize(gray, small_img, cv.CV_INTER_LINEAR)
    cv.EqualizeHist(small_img, small_img)
    midFace = None
    if cascade:
        t = cv.GetTickCount()
        faces = cv.HaarDetectObjects(small_img, cascade, cv.CreateMemStorage(0),
                                     haar_scale, min_neighbors, haar_flags, min_size)
        t = cv.GetTickCount() - t
        if faces:
            for ((x, y, w, h), n) in faces:
                pt1 = (int(x * image_scale), int(y * image_scale))
                pt2 = (int((x + w) * image_scale), int((y + h) * image_scale))
                cv.Rectangle(img, pt1, pt2, cv.RGB(255, 0, 0), 3, 8, 0)
                x1, x2 = pt1[0], pt2[0]
                y1, y2 = pt1[1], pt2[1]
                midFaceX = x1 + ((x2 - x1) / 2)
                midFaceY = y1 + ((y2 - y1) / 2)
                midFace = (midFaceX, midFaceY)
    return midFace

def move(servo, angle):
    """Mueve servo especificado al ángulo (0-180)."""
    if min_pwm <= angle <= max_pwm:
        ser.write(chr(255))
        ser.write(chr(servo))
        ser.write(chr(angle))
    else:
        print "Ángulo servo debe ser entero entre 0 y 180."

if __name__ == '__main__':
    ser = serial.Serial(port='/dev/ttyUSB0', baudrate=115200, timeout=1)
    # ser = serial.Serial(port='/dev/ttyS0', baudrate=115200, timeout=1) # nativo
    # ser = serial.Serial(port='COM14', baudrate=115200, timeout=1)      # Windows

    parser = OptionParser(usage="usage: %prog [options] [camera_index]")
    parser.add_option("-c", "--cascade", action="store", dest="cascade",
                      type="str", help="Archivo cascade Haar, default %default",
                      default="./haarcascade_frontalface_alt.xml")
    (options, args) = parser.parse_args()
    cascade = cv.Load(options.cascade)
    if len(args) != 1:
        parser.print_help()
        sys.exit(1)
    input_name = args[0]
    if input_name.isdigit():
        capture = cv.CreateCameraCapture(int(input_name))
        cv.SetCaptureProperty(capture, cv.CV_CAP_PROP_FRAME_WIDTH, 320)
        cv.SetCaptureProperty(capture, cv.CV_CAP_PROP_FRAME_HEIGHT, 240)
    else:
        print "¡Se necesita entrada de cámara! Especifica índice ej. 0"
        sys.exit(0)

    if capture:
        frame_copy = None
        move(panGpioPin, servoPanPosition)
        move(tiltGpioPin, servoTiltPosition)
        while True:
            start = time.time()
            frame = cv.QueryFrame(capture)
            if not frame:
                cv.WaitKey(0)
                break
            if not frame_copy:
                frame_copy = cv.CreateImage((frame.width, frame.height),
                                            cv.IPL_DEPTH_8U, frame.nChannels)
            if frame.origin == cv.IPL_ORIGIN_TL:
                cv.Copy(frame, frame_copy)
            else:
                cv.Flip(frame, frame_copy, 0)
            midScreenX = frame.width / 2
            midScreenY = frame.height / 2
            midFace = detect_and_draw(frame_copy, cascade)
            if midFace is not None:
                midFaceX, midFaceY = midFace
                # PAN (eje X)
                if midFaceX < (midScreenX - midScreenWindow):
                    servoPanPosition += panStepSize
                    print str(midFaceX) + " > " + str(midScreenX) + " : Pan Right : " + str(servoPanPosition)
                elif midFaceX > (midScreenX + midScreenWindow):
                    servoPanPosition -= panStepSize
                    print str(midFaceX) + " < " + str(midScreenX) + " : Pan Left : " + str(servoPanPosition)
                else:
                    print str(midFaceX) + " ~ " + str(midScreenX) + " : " + str(servoPanPosition)
                servoPanPosition = min(max(servoPanPosition, min_pwm), max_pwm)
                move(panGpioPin, servoPanPosition)
                # TILT (eje Y) — nota: eje Y invertido en imagen
                if midFaceY < (midScreenY - midScreenWindow):
                    if servoTiltPosition <= 90:
                        servoTiltPosition -= tiltStepSize
                        print str(midFaceY) + " > " + str(midScreenY) + " : Tilt Down : " + str(servoTiltPosition)
                elif midFaceY > (midScreenY + midScreenWindow):
                    if servoTiltPosition >= 1:
                        servoTiltPosition += tiltStepSize
                        print str(midFaceY) + " < " + str(midScreenY) + " : Tilt Up : " + str(servoTiltPosition)
                else:
                    print str(midFaceY) + " ~ " + str(midScreenY) + " : " + str(servoTiltPosition)
                servoTiltPosition = min(max(servoTiltPosition, min_pwm), max_pwm)
                move(tiltGpioPin, servoTiltPosition)
            else:
                end = time.time() + 0.1
                var += 0.1
                time_elapsed = int(end - start + var)
                print 'time elapsed:\t{}'.format(time_elapsed)
                print 'var:\t{}'.format(var)
                if time_elapsed == 20:
                    move(3, servoTiltPosition)  # servo 3? (ver código Arduino)
                    servoPanPosition = 90
                    servoTiltPosition = 45
                    var = 0
            if cv.WaitKey(1) >= 0:
                break

Car.ino (Arduino – control motores y servos)

#include 
#define MA_1 2
#define MA_2 3
#define MB_1 4
#define MB_2 5
#define MC_1 6
#define MC_2 7
#define MD_1 8
#define MD_2 9
#define SERVOX_PIN 11
#define SERVOY_PIN 10
#define trigPin 13
#define echoPin 12

Servo servoy;
Servo servox;
int x = 90, prevX;
int y = 45, prevY;
int userInput[3];
int startbyte;
int servo;
int pos;
int i;
int State = LOW;
unsigned long previousMillis = 0, val = 100;
const long interval = 100;
bool scana = false;
unsigned long currentMillis;

void setup() {
  inicializate();
  currentMillis = millis();
}

void loop() {
  if (Serial.available() > 2) {
    startbyte = Serial.read();
    if (startbyte == 255) {
      for (i = 0; i < 2; i++) userInput[i] = Serial.read();
      servo = userInput[0];
      pos = userInput[1];
      if (pos == 255) servo = 255; // recuperación error
      switch (servo) {
        case 1: servoy.write(pos); break;           // tilt
        case 2: servox.write(pos); range(pos); break; // pan + movimiento
        case 3: scan(45); scan(65); break;          // barrido
        default: break;
      }
    }
  }
}

void scan(int val) {
  while (Serial.available() == 0) {
    servoy.write(val);
    currentMillis = millis();
    if (currentMillis - previousMillis >= interval) {
      previousMillis = currentMillis;
      servox.write(pos);
      if (State == LOW) {
        pos += 1;
        if (pos == 130) State = HIGH;
      } else {
        pos -= 1;
        if (pos == 40) { State = LOW; break; }
      }
    }
  }
}

long distancia() {
  long duration, distance;
  digitalWrite(trigPin, LOW); delayMicroseconds(2);
  digitalWrite(trigPin, HIGH); delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  duration = pulseIn(echoPin, HIGH);
  distance = (duration / 2) / 29.1;
  return distance;
}

void range(int pos) {
  if (pos >= 80 && pos <= 100) moves();
  else if (pos >= 100 && pos <= 180) { left(); delay(10); Stop(); }
  else if (pos >= 1 && pos <= 80) { right(); delay(10); Stop(); }
}

void moves() {
  long distance, previusdistance;
  distance = distancia();
  if (val <= 80) { reverse(); delay(50); Stop(); }
  else if (val >= 140) { forward(); delay(50); Stop(); }
  if (distance >= previusdistance) val = distance - previusdistance;
  else val = previusdistance - distance;
  previusdistance = distance;
}

void inicializate() {
  Serial.begin(115200);
  pinMode(MA_1, OUTPUT); pinMode(MA_2, OUTPUT);
  pinMode(MB_1, OUTPUT); pinMode(MB_2, OUTPUT);
  pinMode(MC_1, OUTPUT); pinMode(MC_2, OUTPUT);
  pinMode(MD_1, OUTPUT); pinMode(MD_2, OUTPUT);
  pinMode(trigPin, OUTPUT); pinMode(echoPin, INPUT);
  servox.attach(SERVOX_PIN); servoy.attach(SERVOY_PIN);
}

void forward()  { digitalWrite(MA_1,HIGH); digitalWrite(MA_2,LOW);  digitalWrite(MB_1,HIGH); digitalWrite(MB_2,LOW);  digitalWrite(MC_1,HIGH); digitalWrite(MC_2,LOW);  digitalWrite(MD_1,HIGH); digitalWrite(MD_2,LOW); }
void reverse()  { digitalWrite(MA_1,LOW);  digitalWrite(MA_2,HIGH); digitalWrite(MB_1,LOW);  digitalWrite(MB_2,HIGH); digitalWrite(MC_1,LOW);  digitalWrite(MC_2,HIGH); digitalWrite(MD_1,LOW);  digitalWrite(MD_2,HIGH); }
void right()    { digitalWrite(MA_1,LOW);  digitalWrite(MA_2,HIGH); digitalWrite(MB_1,LOW);  digitalWrite(MB_2,HIGH); digitalWrite(MC_1,HIGH); digitalWrite(MC_2,LOW);  digitalWrite(MD_1,HIGH); digitalWrite(MD_2,LOW); }
void left()     { digitalWrite(MA_1,HIGH); digitalWrite(MA_2,LOW);  digitalWrite(MB_1,HIGH); digitalWrite(MB_2,LOW);  digitalWrite(MC_1,LOW);  digitalWrite(MC_2,HIGH); digitalWrite(MD_1,LOW);  digitalWrite(MD_2,HIGH); }
void Stop()     { digitalWrite(MA_1,LOW); digitalWrite(MA_2,LOW); digitalWrite(MB_1,LOW); digitalWrite(MB_2,LOW); digitalWrite(MC_1,LOW); digitalWrite(MC_2,LOW); digitalWrite(MD_1,LOW); digitalWrite(MD_2,LOW); }
void StopH()    { digitalWrite(MA_1,HIGH); digitalWrite(MA_2,HIGH); digitalWrite(MB_1,HIGH); digitalWrite(MB_2,HIGH); digitalWrite(MC_1,HIGH); digitalWrite(MC_2,HIGH); digitalWrite(MD_1,HIGH); digitalWrite(MD_2,HIGH); }

scanlinux.py (Python – escáner puertos serie Linux)

#! /usr/bin/env python
"""
Escanea puertos serie en Linux. Incluye adaptadores USB/Serie.
Parte de pySerial (https://pyserial.sf.net)
"""
import serial
import glob

def scan():
    return (glob.glob('/dev/ttyS*') + glob.glob('/dev/ttyUSB*') +
            glob.glob('/dev/ttyACM*') + glob.glob('/dev/serial/by-id/*'))

if __name__ == '__main__':
    print "Found ports:"
    for name in scan():
        print name

Partes personalizadas y cajas

Script Python OpenCV + código Arduino empaquetados en FaceTrakingCar.rar.

Esquemático

Control de motores y servos con Arduino; pinout puede variar. Ver imagen del esquemático original.