Управляем веб-камерой с помощью джойстика

в 10:23, , рубрики: arduino, веб-камера, джойстик, робототехника, метки: , , , ,

Введение

Лирика

Добрый день. Мотивированный многочисленными постами на Хабре о самодельных роботах решил сделать и что-нибудь свое более менее стоящее и интересное.

Вообще роботами я увлекаюсь давно, но до нормального проекта руки не доходили, в основном только игрался. Немного подумав, придумал свой проект, поискал детали, нарисовал наброски, пофантазировал на тему будущих возможностей робота. Детали заказал не небезызвестном сайте, и пока детали преодолевают путь из поднебесной решил реализовать один из модулей будущего робота из того что есть под рукой. Вернее даже не реализовать сам модуль, а собрать прототип и написать софт, чтобы потом не отвлекаться на написание программы, да и тем более пока идут все детали есть море свободного времени, а желание что-либо сделать, не дает покоя.

Под рукой у меня оказалась платка Arduino Diecimila, несколько сервоприводов, веб-камера, джойстик и ультразвуковой дальномер. Соответственно сразу возникло желание сделать «компьютерное зрение» на основе веб-камеры, с возможностью как автономной работы, так и ручного управления (джойстиком).

Что меня сподвигло написать эту статью?

Порывшись в интернете, я в основном находил всякий мусор, невнятные вопросы на форумах, отрывки из статей, немного отдаленных от потребностей. В общем и целом я не нашел хорошей, полноценной статьи, которая бы от начала и до конца описывала создание двигающейся веб-камеры, с примерами кода, а уж тем более совмещенные с дальномером и джойстиком.
Тогда решено было ничего больше не искать, так как времени на обработку статей и собирание во едино всей информации уходить стало больше, чем если делать все с нуля самому, тем более, что большинство статей уже давно устарело.

Задача ведь тривиальная, посылать информацию с джойстика на Arduino, которая на определенный угол будет поворачивать 2 сервопривода с прикрепленной веб-камерой, и по необходимости считывать информацию с дальномера, отсылая ее в SerialPort.
Обдумав все еще раз, решил приступить к созданию данного прототипа самостоятельно. Поехали!

Основная часть

Сборка прототипа

Прототип был создан в течение 5 минут. Внешний вид прототипа не интересует вообще, основная его цель — отработка программной части до приезда деталей для робота.
А сделал я его из первой попавшейся баночки из под каких-то витаминов, двух сервоприводов, веб-камеры, скрепки, изоленты и клеевого пистолета. Получилось следующее:

Фото

image

Сборка завершена, сервоприводы и ультразвуковой дальномер подключены к Arduino, Arduino к ПК, приступаем к программированию Arduino.

Программируем Arduino

Тут все казалось очень просто, так как джойстик подключается к ПК, основная обработка видео тоже будет на ПК, то Arduino займется лишь приемом и обработкой информации с ПК и управлением сервоприводами. Поэтому нам надо лишь читать Serial Port, обрабатывать каким-то образом поступающую информацию и как-то на нее реагировать.

Забегая немного вперед сразу скажу, тут и произошла ошибка, к которой мне пришлось вернуться уже после написания программы на C#. Ошибка была вот в чем — я, наивный и полный энтузиазма, написал программку которая разбирает поступающую в Serial Port строку примерно следующего вида «90:90» на две части, соответственно первая часть это градусы по координате X, вторая часть Y. При помощи монитора порта все было оттестировано и работало прекрасно, но когда была написана программа для управления с джойстика, при усиленной атаке порта строками с изменяющимися значениями, Arduino просто не успевала считывать все последовательно, поэтому зачастую строки превращались в «0:909», ":9090" и тому подобное.
Соответственно сервоприводы сходили с ума и принимали все положения, кроме тех, что нужны нам.

Поэтому, не долго думая, я пришел к выводу что нам нужен символ начала строки и символ конца строки. Опять же, не долго думая, символом начала строки был выбран первый символ латинского алфавита — «a», концом строки последний — «z», а символы начала значений осей «x» и «y» соответственно. Итого входная строка принимала следующий вид: «ax90y90z».

Все бы хорошо, если бы не дальномер. Дальномер ультразвуковой, расстояние он определяет на ура, но есть несколько нюансов. Во-первых, если угол между дальномером и стеной острее 45 градусов (плюс-минус), то звук отражается от стены по касательной, и значение, не соответствует действительности. Во-вторых довольно большой угол испускания сигнала, около 30 градусов(по мануалу), а замеряется расстояние до ближайшего объекта, благо что сигнал от объектов к которым датчик находится под углом, отражается в другую сторону, и мы получаем более менее реальное расстояние по прямой, но помехи все же бывают, и довольно часто. Поэтому я дописал еще одну функцию, которая берет n замеров расстояния, складывает их и делит на кол-во, выставил n=10, так помехи стали более сглажены и менее заметны.

Код на Arduino был тут же переписан и принял следующий вид:

Код Arduino

#include <Servo.h>
#include <String.h>
/*
Тут реализован алгоритм приема строки
строка должна быть вида ax180y180z
Где a - символ начала строки
x - символ начала координат x
y - символ начала координат y
z - символ конца строки
*/
String str_X="";
String str_Y="";

int XY_Flag=0; // 1 = X, 2 = Y

Servo X_Servo;
Servo Y_Servo;

const int distancePin = 12;
const int distancePin2 = 11;

void setup()
{
  Serial.begin(115200);
  X_Servo.attach(7);
  Y_Servo.attach(8);
}

void loop()
{
  delay(50);
  if(Serial.available()>0) //считываем значения из порта
  {
    int inChar=Serial.read(); //считываем байт
    if(inChar == 97) { // Если это начало строки

      while(Serial.available()>0)
      {
        inChar=Serial.read(); //считываем байт
        if(inChar==120){ // x
          XY_Flag=1; 
          continue;
        }
        if(inChar==121){ // y
          XY_Flag=2;
          continue;
        }
        if(inChar==122){ // z (конец строки)
          XY_Flag=0;
        }

        if(XY_Flag==0)
          break; // Если конец строки, то досрочный выход из цикла
        if(XY_Flag==1)
          str_X +=(char)inChar; //если X, то пишем в X
        if(XY_Flag==2)
          str_Y +=(char)inChar; //Если Y, то пишем в Y
      }
      if(XY_Flag==0) // Если был конец строки, то выполняем...
      {
        servo(str_X.toInt(), str_Y.toInt());     
        
        str_X=""; 
        str_Y=""; //очищаем переменные
        
       Serial.println("d" + String(trueDistance()) + "z");
      }
    }
  }
}

void servo(int x, int y){ //говорим сервоприводам сколько градусов им нужно взять :)
  X_Servo.write(x);
  Y_Servo.write(y);
}

long trueDistance() //считываем датчик n раз и возвращаем среднее значение
{ 
  int n=10;
  long _value=0;
  
  for(int i =0; i<n; i++)
    _value += distance();
 
  return _value/n;
}

long distance() //считываем показания ультразвукового дальномера
{
  long duration, cm;
  
  pinMode(distancePin, OUTPUT);
  digitalWrite(distancePin, LOW);
  delayMicroseconds(2);
  digitalWrite(distancePin, HIGH);
  delayMicroseconds(10);
  digitalWrite(distancePin, LOW);

  pinMode(distancePin, INPUT);
  duration = pulseIn(distancePin, HIGH);

  cm = microsecondsToCentimeters(duration);
 return cm;
}

long microsecondsToCentimeters(long microseconds) //переводим микросекунды в сантиметры
{
  return microseconds / 29 / 2;
}

Проблема с неправильным разбором координат исчезла на совсем, 100 из 100 испытаний пройдены успешно.

Основная управляющая программа (C#)

По началу хотел писать все на C++ под Qt, но в последствии все же пришлось писать на C#, ну да ладно.

Что хотелось получить:
1. Распознавание лиц людей.
2. Слежение за лицом человека.
3. Ручное управление с помощью джойстика.
4. Определение расстояния до объекта.

Для распознавания лиц и вывода изображения с веб-камеры, без всяких вопросов, была выбрана библиотека OpenCV, а вернее ее оболочка для C# — Emgu CV.

Для считывания положения джойстика по началу использовалась библиотека Microsoft.DirectX.DirectInput, которая мне жутко не понравилась, и я применил библиотеку SharpDX, притом довольно успешно.

Что требовалось от программы:
1. Захватывать изображение с веб-камеры и выводить его на экран.
2. Распознавать лица на изображении, обводить их и получать координаты лица на изображении.
3. Формировать строку вида «ax90y90z» и отправлять ее в Serial Port для управления сервоприводами.
4. Считывать значения положения джойстика.
5. Считывать показания с дальномера.

Сформулировав задачи, приступаем к программированию.

Библиотечка SharpDX позволяет нам находить подключенный джойстик и получать с него значения осей (от 0 до 65535), нажатие и отпускание клавиш джойстика. Сервоприводы могут поворачиваться от 0 до 180 градусов, соответственно нужно преобразовывать значения осей джойстика от 0 до 180. Я просто поделил возвращаемое значение на 363, и получил на выходе значения от 0 до 180. Далее написал функцию которая формирует строку положения сервоприводов и отправляет ее в порт.

Вывод изображения и распознавание лиц написаны с использованием OpenCV и ничего сложного не представляют (для нас).

Дальше поинтереснее, имея под рукой дальномер, конечно же захотелось сделать радар, и построить хоть какую-то приблизительную картину местности.

Повторив тригонометрию и вектора, написал процедуру, которая вычисляет координаты точки относительно нашего дальномера с камерой по углу поворота сервопривода и расстоянию до объекта, и рисует полученные результаты в PictureBox, по кнопке запускаю процедуру в потоке, все работает, но все же из за рельефа комнаты получаются довольно большие помехи, но примерное очертание совпадает с действительностью. Пытался сглаживать данные с датчика, выбирая лишь пиковые значения и рисуя между ними отрезки, в принципе получилось не плохо, но решил отказаться от этого, так как часто пиковыми значениями становятся именно помехи.

Код (на всякий случай с подробными комментариями, по возможности):

Класс формы

Capture myCapture;
        private bool captureInProgress = false;

        string _distance = "0";

        string coords;
        int X_joy = 90;
        int Y_joy = 90;
        SerialPort _serialPort = new SerialPort();

        Image<Bgr, Byte> image;

        DirectInput directInput;
        Guid joystickGuid;
        Joystick joystick;

        Thread th;

        private int GRAD_TURN_X = 2;
        private int GRAD_TURN_Y = 2;

        private void GetVideo(object sender, EventArgs e)
        {
            myCapture.FlipHorizontal = true;
            image = myCapture.QueryFrame();

            try
            {
                //   Image<Gray, Byte> gray = image.Convert<Gray, Byte>().Canny(100, 60);
                // CamImageBoxGray.Image = gray;
            }
            catch { }


            /*детектор лиц */
            if (FaceCheck.Checked)
            {
                List<System.Drawing.Rectangle> faces = new List<System.Drawing.Rectangle>();

                DetectFace.Detect(image, "haarcascade_frontalface_default.xml", "haarcascade_eye.xml", faces);

                foreach (System.Drawing.Rectangle face in faces)
                {
                    image.Draw(face, new Bgr(System.Drawing.Color.Red), 2);

                    int faceX = face.X + face.Width / 2;
                    int faceY = face.Y + face.Height / 2;

                    if ((faceX - 320 > 120) || (faceX - 320 < -120)) //Чем дальше от центра изображения лицо, тем быстрее двигаем камеру
                        GRAD_TURN_X = 4;
                    else if ((faceX - 320 > 80) || (faceX - 320 < -80))
                        GRAD_TURN_X = 3;
                    else
                        GRAD_TURN_X = 2;

                    if ((faceY - 240 > 120) || (faceY - 240 < -120))
                        GRAD_TURN_Y = 4;
                    else if ((faceY - 240 > 80) || (faceY - 240 < -80))
                        GRAD_TURN_Y = 3;
                    else
                        GRAD_TURN_Y = 2;

                    label7.Text = faceX.ToString();
                    label8.Text = faceY.ToString();

                    if (!JoyCheck.Checked)
                    {
                        if (faceX > 370)
                            X_joy += GRAD_TURN_X;
                        else if (faceX < 290)
                            X_joy -= GRAD_TURN_X;

                        if (faceY > 270)
                            Y_joy -= GRAD_TURN_Y;
                        else if (faceY < 210)
                            Y_joy += GRAD_TURN_Y;
                        serialPortWrite(X_joy, Y_joy);
                    }

                }
            }
            /*=============*/



            System.Drawing.Rectangle rect1 = new System.Drawing.Rectangle(305, 240, 30, 1);
            System.Drawing.Rectangle rect2 = new System.Drawing.Rectangle(320, 225, 1, 30);
            System.Drawing.Rectangle rect3 = new System.Drawing.Rectangle(0, 0, 640, 22);
            image.Draw(rect1, new Bgr(System.Drawing.Color.Yellow), 1);
            image.Draw(rect2, new Bgr(System.Drawing.Color.Yellow), 1);
            image.Draw(rect3, new Bgr(System.Drawing.Color.Black), 22);

            MCvFont f = new MCvFont(FONT.CV_FONT_HERSHEY_TRIPLEX, 0.9, 0.9);
            image.Draw("Distance: " + _distance + " cm", ref f, new System.Drawing.Point(0, 30), new Bgr(0, 255, 255));


            CamImageBox.Image = image;


            if (JoyCheck.Checked)
            {
                th = new Thread(joy); // ручное управление, запускаем в потоке
                th.Start();
            }


            label1.Text = X_joy.ToString();
            label2.Text = Y_joy.ToString();
            label3.Text = coords;
        }

        private void ReleaseData()
        {
            if (myCapture != null)
                myCapture.Dispose();
        }

        public Form1()
        {
            InitializeComponent();
        }

        private void serialPortWrite(int X, int Y) //отсылаем ардуине координаты и читаем из порта дистанцию
        {
            try
            {
                coords = "ax" + X + "y" + Y + "z";
                _serialPort.Write(coords);

                _distance = _serialPort.ReadLine();
                if (_distance[0] == 'd')
                    if (_distance[_distance.Length - 2] == 'z')
                    {
                        _distance = _distance.Remove(_distance.LastIndexOf('z')).Replace('d', ' ');
                    }
                    else _distance = "0";
                else _distance = "0";
            }
            catch { }
        }

        private void joy() //ручное управление джойстиком
        {
            joystick.Poll();
            var datas = joystick.GetBufferedData();
            foreach (var state in datas)
            {
                if (state.Offset.ToString() == "X")
                    X_joy = 180 - (state.Value / 363);
                else if (state.Offset.ToString() == "Y")
                    Y_joy = state.Value / 363;
            }
            serialPortWrite(X_joy, Y_joy);

        }

        private void Form1_Load(object sender, EventArgs e)
        {


            if (myCapture == null)
            {
                try
                {
                    myCapture = new Capture();
                }
                catch (NullReferenceException excpt)
                {
                    MessageBox.Show(excpt.Message);
                }
            }

            if (myCapture != null)
            {
                if (captureInProgress)
                {
                    Application.Idle -= GetVideo;

                }
                else
                {
                    Application.Idle += GetVideo;

                }
                captureInProgress = !captureInProgress;
            }

            _serialPort.PortName = "COM3";
            _serialPort.BaudRate = 115200;
            if (_serialPort.IsOpen)
                _serialPort.Close();
            if (!_serialPort.IsOpen)
                _serialPort.Open();

            directInput = new DirectInput();

            joystickGuid = Guid.Empty;

            foreach (var deviceInstance in directInput.GetDevices(DeviceType.Gamepad, DeviceEnumerationFlags.AllDevices))
                joystickGuid = deviceInstance.InstanceGuid;

            if (joystickGuid == Guid.Empty)
                foreach (var deviceInstance in directInput.GetDevices(DeviceType.Joystick, DeviceEnumerationFlags.AllDevices))
                    joystickGuid = deviceInstance.InstanceGuid;

            joystick = new Joystick(directInput, joystickGuid);

            joystick.Properties.BufferSize = 128;

            joystick.Acquire();
        }

        private void JoyCheck_CheckedChanged(object sender, EventArgs e)
        {
            if (FaceCheck.Checked)
                FaceCheck.Checked = !JoyCheck.Checked;

        }

        private void FaceCheck_CheckedChanged(object sender, EventArgs e)
        {
            if (JoyCheck.Checked)
                JoyCheck.Checked = !FaceCheck.Checked;
        }

        private void RadarPaint()
        {
            Bitmap map = new Bitmap(pictureBox1.Size.Width, pictureBox1.Size.Height);

            Graphics g = Graphics.FromImage(map);

            var p = new Pen(System.Drawing.Color.Black, 2);

            System.Drawing.Point p1 = new System.Drawing.Point();
            System.Drawing.Point p2 = new System.Drawing.Point();

            System.Drawing.Point p3 = new System.Drawing.Point();
            System.Drawing.Point p4 = new System.Drawing.Point();

            p1.X = pictureBox1.Size.Width/2 ; //начало координат переводим в удобное нам место
            p1.Y = pictureBox1.Size.Height; //посередине pictureBox'a внизу

            for (int i = 0; i < 181; i++)
            {        
                serialPortWrite(i, 90);

                p2.X = Convert.ToInt32(Math.Ceiling(320 + int.Parse(_distance) * Math.Cos(i * Math.PI / 180))); //считаем координаты точки
                p2.Y = Convert.ToInt32(Math.Ceiling(480 - int.Parse(_distance) * Math.Sin(i * Math.PI / 180)));

                if (i > 0)
                    g.DrawLine(p, p2, p3);

                if (i % 18 == 0)
                {
                    p4 = p2;
                    p4.Y -= 50;
                    g.DrawString(_distance, new Font("Arial", 18), new SolidBrush(System.Drawing.Color.Red), p4);
                }

                p3.X = p2.X;
                p3.Y = p2.Y;
                 g.DrawLine(p, p1, p2);
                try
                {
                    pictureBox1.Image = map;
                }
                catch (Exception e)
                {
                    MessageBox.Show(e.Message);
                }
            }
        }


        private void button1_Click(object sender, EventArgs e)
        {
            if (FaceCheck.Checked || JoyCheck.Checked)
            {
                FaceCheck.Checked = false; JoyCheck.Checked = false;
            }
            Thread t = new Thread(RadarPaint);
            t.Start();
        }

Класс DetectFace

  class DetectFace
    {
        public static void Detect(Image<Bgr, Byte> image, String faceFileName, String eyeFileName, List<Rectangle> faces)
        {
            CascadeClassifier face = new CascadeClassifier(faceFileName);
           // CascadeClassifier eye = new CascadeClassifier(eyeFileName);

            Image<Gray, Byte> gray = image.Convert<Gray, Byte>();

            gray._EqualizeHist();

            Rectangle[] facesDetected = face.DetectMultiScale(
               gray,
               1.1,
               5,
               new Size(70, 70),
               Size.Empty);
            faces.AddRange(facesDetected);
        }
    }

В итоге получаем все, что хотели. Компьютер распознает лица и автоматически следит за ними. ручное управление джойстиком работает на ура. Радар, хоть и не совсем точно, но работает. Основные функции модуля зрения робота отработаны и остается лишь дорабатывать и усовершенствовать их.

Видео

Вот, что получилось по завершении.



Заключение

Итоги

Оказалось все довольно просто. Цель достигнута, прототип готов. Есть над чем работать и заняться в свободное время, ожидая посылку с компонентами для робота.

Планы на будущее

Следующим шагом будет построение колесной платформы для робота, настройка удаленного управления (WiFi, 3G)., навешивание датчиков (температура, давление и прочее), синтез речи. В хотелках так же имеются планы по поводу механической руки.

Думаю, если будет интерес к данной статье и ее продолжению, то оно обязательно последует! Исправления и критика приветствуются!

Спасибо за внимание!

Автор: Slicker

Источник

Поделиться

* - обязательные к заполнению поля