Введение
Лирика
Добрый день. Мотивированный многочисленными постами на Хабре о самодельных роботах решил сделать и что-нибудь свое более менее стоящее и интересное.
Вообще роботами я увлекаюсь давно, но до нормального проекта руки не доходили, в основном только игрался. Немного подумав, придумал свой проект, поискал детали, нарисовал наброски, пофантазировал на тему будущих возможностей робота. Детали заказал не небезызвестном сайте, и пока детали преодолевают путь из поднебесной решил реализовать один из модулей будущего робота из того что есть под рукой. Вернее даже не реализовать сам модуль, а собрать прототип и написать софт, чтобы потом не отвлекаться на написание программы, да и тем более пока идут все детали есть море свободного времени, а желание что-либо сделать, не дает покоя.
Под рукой у меня оказалась платка Arduino Diecimila, несколько сервоприводов, веб-камера, джойстик и ультразвуковой дальномер. Соответственно сразу возникло желание сделать «компьютерное зрение» на основе веб-камеры, с возможностью как автономной работы, так и ручного управления (джойстиком).
Что меня сподвигло написать эту статью?
Порывшись в интернете, я в основном находил всякий мусор, невнятные вопросы на форумах, отрывки из статей, немного отдаленных от потребностей. В общем и целом я не нашел хорошей, полноценной статьи, которая бы от начала и до конца описывала создание двигающейся веб-камеры, с примерами кода, а уж тем более совмещенные с дальномером и джойстиком.
Тогда решено было ничего больше не искать, так как времени на обработку статей и собирание во едино всей информации уходить стало больше, чем если делать все с нуля самому, тем более, что большинство статей уже давно устарело.
Задача ведь тривиальная, посылать информацию с джойстика на Arduino, которая на определенный угол будет поворачивать 2 сервопривода с прикрепленной веб-камерой, и по необходимости считывать информацию с дальномера, отсылая ее в SerialPort.
Обдумав все еще раз, решил приступить к созданию данного прототипа самостоятельно. Поехали!
Основная часть
Сборка прототипа
Прототип был создан в течение 5 минут. Внешний вид прототипа не интересует вообще, основная его цель — отработка программной части до приезда деталей для робота.
А сделал я его из первой попавшейся баночки из под каких-то витаминов, двух сервоприводов, веб-камеры, скрепки, изоленты и клеевого пистолета. Получилось следующее:
Сборка завершена, сервоприводы и ультразвуковой дальномер подключены к 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 был тут же переписан и принял следующий вид:
#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();
}
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
