0,0
рейтинг
16 апреля 2012 в 12:16

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

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

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

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

image

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

Дальномер

Сонар (он же дальномер, он же 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); // Инициализируем сериал порт, дабы вывести результат на монитор
}

unsigned int time_us=0; // Переменная для хранения временного интервала
unsigned int distance_sm=0; // Переменная для хранения расстояния в сантиметрах

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 мотора, по одному с каждой стороны.
Что бы понять, как взаимодействовать с ним, я нашел хорошую статью, ею и воспользуемся:

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

image

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

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

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

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

image

image

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

image

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

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);
}

void loop()
{ // Вращаем мотор в одну сторону
digitalWrite(motor1Pin, LOW); // set leg 1 of the H-bridge low
digitalWrite(motor2Pin, HIGH); // set leg 2 of the H-bridge high delay(1000); // А через секунду в другую digitalWrite(motor1Pin, HIGH); // set leg 1 of the H-bridge high
digitalWrite(motor2Pin, LOW); // set leg 2 of the H-bridge low delay(1000);
// А теперь всё сначала
}

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

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

image

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

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


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

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

void Servo_motion(int angle) // функция управления серво
{
int time=390+10*angle; // Пересчитываем заданный угол поворота в длину импульса, который подадим на //серво
digitalWrite(ServoPin, HIGH); // Сигнал пошел
delayMicroseconds(time); // Удерживаем его заданное время
digitalWrite(ServoPin, LOW); // Выключаем его
delayMicroseconds(20000-time); // Даем серво время, что бы повернуться (20000 мкс — 50 гц)
}

void loop()
{

for(int i=0;i<=180;i++)
{
Servo_motion(i); // Прокрутим серво в одну сторону
delay(10); // C задержкой 10 миллисекунд на каждом градусе
}

for(int i=180; i>=0; i--)
{
Servo_motion(i); // Затем в другую сторону
delay(10);
}

}

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

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. Ну вроде с подключением разобрались, проиллюстрирую вышесказанное фотографиями:

Вся схема:
image

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

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

H – мост:
image

Немного о самой конструкции: обошлось без излишеств). Вырезал из пластика крышку на платформу, в ней было проделано отверстие для крепления серво. Из того же пластика выгнута ( предварительно нагрев промышленным феном) Г — образная скобка. К ней приклеен четырехжильный шлейф (под 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 — временной шаг одного движения робота*/

// Величина, на которую изменяется угол в процессе движения —
int a;
if(next_angle>=prev_angle)
a=15;
else
a=-15;
if (dimention=='f')
{
// Если сказано двигаться вперед, то
int i=prev_angle;
while( i!=next_angle)
{
/*Пока не достигли заданного значения угла, будем в цикле постепенно изменять текущее положение серво на величину a*/
i+=a; myservo.write(i); // И передавать это значение на серво
forward(); // После чего делаем движение вперед
delay(time); // В течении временного интервала time
}
}

/* Аналогичный алгоритм для движения влево, вправо, назад и стоянии на месте*/



}

void front_motion( int time )
{
/* Функция, которая осуществляет небольшой «доворот» робота в одну из сторон, если объект расположен под углами 45 и 135 градусов*/
if(dist_45<=9)
{ // Если расстояние до объекта под углом 45 градусов меньше 9см, поворачиваем налево
left();
delay(3*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
карма
6,0
рейтинг 0,0
Реклама помогает поддерживать и развивать наши сервисы

Подробнее
Реклама

Самое читаемое

Комментарии (23)

  • 0
    Спасибо, отличная статья. Было бы полезно еще отписать по ценам на детали и сколько вышло всего.
    • 0
      Спасибо. Платформа — 40 баксов, дальномер — 10, ардуино 15. Примерно такие цены. Остальное — мелочь.
  • +1
    Ох, уж этот радикал
    • 0
      Вечером постараюсь перезалить
    • +1
      — скажите, а вы бы смогли полюбить сервис Радикала?
      — ради чегоооо?
  • 0
    Напомнило похожую игрушку из Lego Mindstorms

    www.youtube.com/watch?v=NZu6-AnXRFw
  • 0
    6) 2A Сигнал с микроконтроллера для управления втором входом мотора.
    7) 2Y Сюда подключаем вторую ножку мотора.

    вы перепутали порядок ножек 2Y — 6 нога, а 2А — 7 нога
    • 0
      да, действительно. спасибо за найденный баг
  • +2
    Режут глаз однотипные строки вида

    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);
    


    Вы бы хоть массив сделали, что ли… Типа

    unsigned int distance[5];
    enum DIRECTION {DIR_FORWARD, DIR_LEFT, DIR_RIGHT, DIR_45DEG, DIR_135DEG};
    
    //...
    
    angle=0;
    for(int I=0;i<5;i++)
    {
        angle+=45;
        look(angle);
        WaitWhileTurningServo();
        distance[i]=sonar;
    }
    
    • 0
      Согласен. некрасиво… Делал, как было удобнее)
  • 0
    Музыка из видеоролика вставляет :)
  • +1
    Все же движки на 400мА для такой штуки — это ппц круто. А L298x микросхемы очень многими ругаемы, ибо очень много энергии преобразуют в тепло из-за конструктива — биполяры используются.
    • 0
      тут L293D. В дип корпусах 298х, насколько знаю, не бывает. Ну, это был по-сути мой первый проект. Так что брал самое распространенное и проверенное.
  • НЛО прилетело и опубликовало эту надпись здесь
    • 0
      что б по кругу вращать — надо вместо серво что-то другое поставить)
      Не, не фича. Видюшка сама первая и старая. Там глюк был в проге. На двух других он в разные стороны объезжает.
  • 0
    Пару лет назад делал подобное из Lego Mindstorms и выяснилось, что у ультразвукового дальномера есть недостаток — он не «видит» кота. Кот мягкий и хорошо поглощает звук. После нескольких наездов робота бедный Юникс зашипел и ушёл в другую комнату. Такой вот неожиданный «стелс» в домашних условиях.
    • 0
      Ну, любые аморфные тела поглощают колебания. Энергия тратится на трение.
  • 0
    Дорогой Макс, позволь маленькую ремарку. Ты пишешь, что

    Далее, приемник получает отраженный сигнал и на ножке Echo генерируется прямоугольный сигнал, длина которого пропорциональна времени между излучением импульсов и детектированием их приемником.

    Вот длина этого сигнала не пропорциональна времени, а в точности равна ему. И поэтому и возникает эта формула
    s = t/58. Потому что скорость звука и есть 2*(1/58)=0,0344 сантиметров в миллисекунду
    • 0
      Ok.У меня недостаточно кармы лайкнуть твой пост

Только зарегистрированные пользователи могут оставлять комментарии. Войдите, пожалуйста.