Простой робот с ультразвуковым дальномером

Данная тема посвящается созданию простенького робота на основе Arduino nano. Предполагается, что читатель является новичком и обладаешь лишь начальными знаниями данного вопроса. Я постарался изложить все как-можно более подробно и понятно.

Введение в задачу

Начнем с концепции: мы хотим робота, который может самостоятельно передвигаться по комнате, при этом объезжать все препятствия, встречаемые на своем пути.
Задачу поставили. Теперь бегом по магазинам! 1) Платформа. Есть такие варианты: сделать самому всё, купить детальки (например Tamiya ) и собрать из них, либо же купить готовое. Я остановился на последнем варианте. Вид танка, ну или трактора мне почему-то пришелся более по душе, и в итоге я остановился на таком варианте (платформа от DF robot):

В комплекте — платформа (по одному мотору на каждой гусенице) и отсек для батареек.
Ну, тут ничего сложного, поехали дальше.

Дальномер

Сонар (он же дальномер, он же Ultrasonic module) В качестве дальномера изначально выбор был между ультразвуковым и инфракрасным. Поскольку характеристики ультразвукового существенно лучше (максимальная дальность около 4-5 метров, против 30-60 см), а цена примерно одинаковая, то выбор пал на Ultrasonic. Наиболее распространена модель HC-SR04.

Что бы понять, как устроен этот фрукт — есть даташит + достаточно информации в интернете.
Расскажу основное. На фотографии видны 2 цилиндра. Один из них приемник, другой передатчик. Приемник генерирует ультразвуковые волны, передатчик принимает отраженную волну от объекта, и сообщаем нам об этом. На его плате 4 контакта ( 5V, GND, Trig, Echo).
Алгоритм работы таков:

Подаем на ножку Trig сигнал, длительностью 10мкс, что запускает генератор, создающий пачку коротких импульсов на передатчике ( 8 шт ). Далее, приемник получает отраженный сигнал и на ножке Echo генерируется прямоугольный сигнал, длина которого пропорциональна времени между излучением импульсов и детектированием их приемником.

Реальное время, за которое звук дойдет до приемника, конечно же, составит копейки. Что бы по нему определить расстояние, можно воспользоваться нехитрой формулой:

s=vt/2, s — расстояние, v — скорость звука, t — время получения сигнала на приемнике.

Ну почему пополам делим, думаю всем понятно. Только в данном случае эта формула не нужна. Привожу ее здесь исключительно для понимания физики процесса.
С выхода Echo идет уже сформированный сигнал, с достаточно большой длительностью. Заглянув в даташит, мы увидим формулу пересчета: s = t/58, s — расстояние, t — длительность импульса Echo, s — расстояние в сантиметрах.

Ок, вроде все основы разобрали. Перейдем к коду под Arduino:

const int Trig = 3; // обозначим к какой ножке и что подключаем const int Echo = 2;
void setup()
{
pinMode(Trig, OUTPUT);
pinMode(Echo, INPUT);
Serial.begin(9600); // Инициализируем сериал порт, дабы вывести результат на монитор
}

void loop()
{
digitalWrite(Trig, HIGH); // Подаем сигнал на выход микроконтроллера
delayMicroseconds(10); // Удерживаем 10 микросекунд
digitalWrite(Trig, LOW); // Затем убираем time_us=pulseIn(Echo, HIGH); // Замеряем длину импульса
distance_sm=time_us/58; // Пересчитываем в сантиметры
Serial.print(distance_sm); // Выводим на порт
Serial.print(" ");
delay(500);
}

Драйвер

Ну что же, с сонаром вроде разобрались. Продолжим.
Платформа содержит 2 мотора. Ими надо как-то управлять. Казалось бы — подключил их напрямую, подавай то HIGH то LOW и радуйся. Тут одно существенное «НО» — с атмеги не получишь ток выше ~40мА, а мотору надо где-то на порядок больше.

Как быть? Первое что приходит в голову это — поставить на выход микроконтроллера транзистор и с него уже питать моторы. Это конечно хорошо, но не прокатит, если мы захотим мотор в другую сторону пустить… Зато с этой задачей хорошо справится H — мост, который представляем собой немного более сложную схему, чем пара транзисторов. Но в данном случае их полно в виде готовых интегральных схем, так что думаю велосипед изобретать незачем — купим готовый. К тому же цена располагает — 2-3 доллара…

Немного почитать об этих приборах можно, например, здесь.

Двинемся дальше. Для этих целей я себе купил микросхему L293D, собственно о которой речь дальше и пойдет. Она проста в использовании, повсеместно доступна и имеет удобный корпус Dip16.
Её максимальный ток сравнительно небольшой ( 600 мА ), что для конкретной задачи более чем достаточно. Если нужно больше, то есть, например, L293B (1А) и т.д…
Чуть не забыл, сей мост позволяет подключить к нему 2 мотора, по одному с каждой стороны.
Что бы понять, как взаимодействовать с ним, я нашел хорошую статью, ею и воспользуемся:

Все просто и наглядно. Внимательно изучив первую часть статьи, остановим взор на рисунке:

— схема включения данной микросхемы, собственно, взятая из даташита.

Кратко пробежимся по её ножкам:

1) Инициализация мотора1. Пока вы не подадите на эту ножку HIGH, что бы вы не делали с остальными, моторчик не заработает. Хоть и написано 1,2E — мотор там один. Не путайте. Дело в том, что для управления одним мотором вам понадобится 2 ножки микроконтроллера, а соответственно и H — моста. Подадим на одну ножку HIGH, другую LOW — мотор закрутился в одну сторону. Подадим на первую LOW, вторую HIGH — закрутится в противоположную. подадим на обе LOW — остановится.
2) 1A. На эту ножку вы будите посылать сигнал с микроконтроллера( слаботочный ) для управления 1 входом мотора.
3) 1Y. А это уже сигнал( большой ток ), который идет непосредственно на мотор. По своему виду он полностью повторяет сигнал, подаваемый на вход 1A.
4) — 5) Земля
6) 2A Сигнал с микроконтроллера для управления втором входом мотора.
7) 2Y Сюда подключаем вторую ножку мотора.
8) Сюда мы подаем напряжение, которым будут питаться моторы. По-сути, что подадим на этот вход, то и будет отпираться на ножках 1Y, 2Y.
9) — 16) Полная аналогия с первыми восемью, но для второго мотора.

Далее, схема включения:

Дабы убрать скачки напряжения при включении мотора, используем конденсатор, как показано ниже:

Ну и напоследок, приводится исходный код, с моей небольшой редакцией, который резюмирует все вышесказанное:

const int motor1Pin = 3; // H-bridge leg 1 (pin 2, 1A)
const int motor2Pin = 4; // H-bridge leg 2 (pin 7, 2A)
const int enablePin = 9; // H-bridge enable pin
void setup()
{ // set all the other pins you're using as outputs:
pinMode(motor1Pin, OUTPUT);
pinMode(motor2Pin, OUTPUT);
pinMode(enablePin, OUTPUT); // set enablePin high so that motor can turn on:
digitalWrite(enablePin, HIGH);
}

Сервомашинка

Итак, с работой дальномера мы разобрались. Двинемся дальше. Дальномер у нас один, смотреть надо как вперед, так и по сторонам, что бы знать куда поворачивать в случае чего. Для этих целей воспользуемся серво (сервомашинка, сервопривод, servo).

Эти игрушки используются в основном в авиамоделизме, но для роботов тоже очень даже ничего.
Данное устройство может поворачиваться на углы от 0 до 180 градусов. От корпуса идет трехжильный кабель:

Черный — GND
Красный — 5V
Белый — Сигнал

Мотор управляется контроллером (не пугайтесь — ничего покупать не надо, он уже есть внутри серво), который, получая внешний сигнал — контролирует, что бы мотор повернулся на заданный угол. Для этих целей с мотора заведена обратная связь на контроллер, которая представляет собой переменный резистор, меняющий своё сопротивление в зависимости от угла поворота. Сам контроллер управляется длиной входного импульса. Как правило: 380 — 400 мкс — 0 градусов, 2200мкс — 180 градусов. Приведем простой алгоритм управления серво для Arduino:

#define ServoPin 2 // На эту ножку мы подключим наше серво (его белый провод)
void setup()
{
pinMode(2,OUTPUT);
}

Но в дальнейшем, мы будем использовать специальную библиотеку для управления серво, вот её описание:

www.arduino.cc/en/Reference/Servo

arduino.cc/en/Tutorial/Sweep

Данный пример (2 ссылка) проделывает ровным счётом тоже самое что и программа, описанная выше. Там приведено красочное описание кода с рисунками, картинками, комментариями, так что думаю — особых затруднений не возникнет. Ограничусь лишь небольшими комментариями — при проверки данного кода не забудьте переставить серво на на цифровой порт 9, либо поправить в том коде вот эту строчку:

myservo.attach(9); // attaches the servo on pin 9 to the servo object


А то ничего не заработает. И последнее что хотелось бы добавить — данный пример доступен как по вышеуказанной ссылке, так и в среде разработки Arduino во вкладке «Examples».

Сборка

Перейдем к сборке нашего творения. Поскольку плату я не делал, то и принципиальной схемы, у меня нету, к сожалению. Но я думаю, это не сильно нам помешает — схема простая, все понятно. Фотографий и небольших комментариев вполне хватит. На данном этапе возникает Arduino nano, как вы уже могли догадаться, поскольку весь предыдущий код был сделан с расчетом на него. Описывать сей прибор занятие довольно трудоемкое и утомительное, поэтому для тех, кто не знает — ссылки:

arduino.cc/en/Guide/HomePage
freeduino.ru/arduino/index.html
arduino.ru/

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

4 ножки — входы H моста, по 2 на каждый мотор:
1A — 11
2A — 6
3A — 10
4A — 5

enablePin — 12

1 Ножка под 1,2EN и 3,4EN — посадил их вместе, так как оба мотора все равно по отдельности нам не нужны. В принципе вообще, можно эти 2 ножки моста к Arduino не подключать, а просто подать на них 5V.

2 ножки для сонара:
Trig — 3
Echo — 2

Ножка для подключения серво:
Servo — 8

На этом вроде бы и всё. Далее, в процессе сборки робота, я столкнулся с одной проблемой — периодически робот останавливался, Arduino перезагружалось. Немного подумав, я понял что Arduino nano неспособен питать всю эту систему ( H-мост, серво, сонар) от своего штатного стабилизатора. Потому на помощь мне пришел стабилизатор напряжения 7805 (L7805, LM7805). Прибор прост в применении, имеет 3 ножки: вход( 6 — 35 В ), земля, выход( ~5В). Даташит к нему можно повсеместно найти в интернете. Объединив его землю, с землёй Arduino и, соответственно с минусом аккумулятора тоже. Я сделал так — от Arduino я питаю только H — мост, а всё остальное ( серво, сонар ) от стабилизатора. После этого робот стал отлично работать без сбоев. Да, не забывайте важное правило — земля в любой схеме должна быть общей для всех элементов! Ну, по поводу самих моторов, я думаю понятно — подаем напряжение с аккумулятора на вход моста — Vcc2. Ну вроде с подключением разобрались, проиллюстрирую вышесказанное фотографиями:

Вся схема:

Стабилизатор напряжения (конденсаторы можно не ставить):

Шлейф от сонара:

H – мост:

Немного о самой конструкции: обошлось без излишеств). Вырезал из пластика крышку на платформу, в ней было проделано отверстие для крепления серво. Из того же пластика выгнута ( предварительно нагрев промышленным феном) Г — образная скобка. К ней приклеен четырехжильный шлейф (под PLS вилку, с шагом 2.54мм), в который уже и вставляется сам сонар.

Программирование

Итак, робот собран. Переходим к заключительному этапу — прошивка. Здесь я опишу мой вариант реализации данного алгоритма. Заранее отмечу, что все можно было существенно упростить, например, вращать сонар не постоянно, а остановиться, когда на пути встречается преграда, «осмотреться» и повернуть в наилучшее направление. Либо вообще не вращать головой.

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

Объявление переменных:

Переменная, для реализации алгоритма работы сонара — unsigned int time_us=0;
Расстояние, определяемое сонаром — unsigned int distance_sm=0;
Данная переменная используется в цикле loop для того, что бы при включении робот «осмотрелся» на месте, а потом уже поехал —
unsigned int circle=0;
Расстояние до ближайшего объекта спереди — unsigned int dist_f=0;
Расстояние до ближайшего объекта слева — unsigned int dist_l=0;
Расстояние до ближайшего объекта справа — unsigned int dist_r=0;
Расстояние до ближайшего объекта под углом 45 градусов — unsigned int dist_45=0;
Расстояние до ближайшего объекта под углом 135 градусов — unsigned int dist_135=0;

Константа времени(мс), определяющая минимальный шаг движения робота. Подобрана экспериментально. В зависимости от скорости движения и скорости вращения серво вашего робота, возможно придется её изменить. Позже станет более понятно для чего она нужна —
unsigned int t=15;

Функции:

sonar() — реализует алгоритм работы сонара, возвращает расстояние [см].
forward (), back (), right (), left () — наши базовые функции движения.
Основная функция, реализующая движение —

void motion (char dimention, int prev_angle, int next_angle, int time)
{
/*Данная функция одновременно управляет как и вращением моторов, так и серво.
char dimention - направление движения
int prev_angle - предыдущее положение серво
int next_angle - положение, на которое хотим установить серво
int time - временной шаг одного движения робота*/

}
void motion_back( int time )
{
/* Движение робота назад в течении времени 2*time, с поворотом серво от угла 180 градусов, на угол 180 градусов*/
motion('b',180,90,2*time);
}
void loop()
{
// Наша главная функция, реализующая итоговый алгоритм работы
if (circle==0)
{
//Если робота только что включили, установим серво в начальное положение.
myservo.write(0); //И "осмотримся" по сторонам
dist_r=sonar();
motion('w',0,45,t);
dist_45=sonar();
motion('w',45,90,t);
dist_f=sonar();
motion('w',90,135,t);
dist_135=sonar();
motion('w',135,180,t);
dist_l=sonar(); } // Больше мы данное действие производить не будем
circle++; i
f(dist_f>=25)
{ // Если до ближайшего объекта спереди более 25 сантиметров
a: //Двигаемся вперед, при этом осуществляем поворот серво от 180 до 135 градусов
motion('f',180,135,t); //Сделаем замер расстояния до объектов под углом 135 градусов
dist_135=sonar(); //Если необходимо, сделаем доворот
front_motion(t); //Далее аналогично, но с другими значениями
motion('f',135,90,t);
dist_f=sonar();
front_motion(t);
motion('f',90,45,t);
dist_45=sonar();
front_motion(t);
motion('f',45,0,t);
dist_r=sonar();
front_motion(t);
motion('f',0,45,t);
dist_45=sonar();
front_motion(t);
motion('f',45,90,t);
dist_f=sonar();
front_motion(t);
motion('f',90,135,t);
dist_135=sonar();
front_motion(t);
motion('f',135,180,t);
dist_l=sonar(); front_motion(t); // Если расстояние спереди все еще больше 25 сантиметров, то вернемся в точку 'a'
if (dist_f>=25)
goto a;
}
else
{ //Если нет
if(dist_f<5)
{ // Если робот уже слишком близко к ближайшему объекту, то делаем движение назад
motion_back(t);
// Производим новый замер расстояния
dist_f=sonar();
} //При этом поворачиваем в ту сторону, где больше свободного места
if(dist_l>=dist_r || dist_135>dist_r)
{
motion('l',180,90,t);
dist_f=sonar();
}
if(dist_l<dist_r)
{
motion('r',180,90,t);
dist_f=sonar();
}
} // Далее новый круг
}


Полную версию данной программы можно скачать вот тут:
maxim.wf/arduino_code/Robot_compilation.pde

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

Спасибо за внимание!
Буду рад любым вопросам, предложениям и комментариям.

Автор: MaxFilippov