Робот на smart car, объезжающий препятствия

В этой статье я покажу, как собрать простого робота объезжающего препятствия, используя Arduino.

Для этого нам понадобится следующие компоненты:

• Плата Arduino UNO
• Motor Drive Shield dual L239D
• Сервопривод SG90
• Ультразвуковой дальномер Ultrasonic Module HC-SR04
• Платформа smart car на 4 колесах + 4 указателя скорости
• Держатель для 6 батареек AA

Все компоненты можно приобрести на этом сайте 🙂

Как вы уже догадались, в качестве «глаз» мы будем использовать ультразвуковой дальномер. Он имеет угол обзора примерно 15 градусов. К сожалению, этого не достаточно для обеспечения наших задач. Сервопривод позволит нам обойти это ограничение и выступит в роли «шеи». Все двигатели, сервопривод и дальномер подключать будем к шилду (Motor Drive Shield dual L239D). Возможности шилда позволяют нам подключить 2 сервопривода + на выбор:

• 4 мотора постоянного тока с потреблением не больше 600mA, 4.5 — 36В
• 2 мотора постоянного тока и один шаговый двигатель
• 2 шаговых двигателя

На борту данного шилда имеется две микросхемы L293D (1). L-ка позволяет управлять слаботочными двигателями с током потребления до 600 мА на канал. Моторы подключаются к разъемам M1, M2, M3, M4 на шилде (2). Центральные выводы на пятипиновых клеммниках соединены с землей и служат для удобства при подключении пятипроводных шаговых двигателей. Для управления на прямую выводами L-ки (IN1, IN2, IN3, IN4), отвечающимими за выбор направления вращения, необходимо 4 вывода, а для двух микросхем целых 8. Для уменьшения количества управляющих выводов в игру вступает сдвиговый регистр 74НС595 (3). Благодаря регистру, управление сводится с 8-ми пинов к 4-ем.

Также, на плату выведены 2 разъема для подключения сервоприводов (4). Управление сервоприводами стандартное с помощью библиотеки Servo.h и никак не связано с библиотекой которую мы будем рассматривать далее.

Питание силовой части производится либо от внешнего клеммника (5) либо замыканием джампера (6).

К явным минусам данного шилда можно отнести то, что он задействует практически все цифровые пины:

Выводы отвечающие за скорость вращения двигателей:

• Цифровой вывод 11 — DC Мотор №1 / Шаговый №1
• Цифровой вывод 3 — DC Мотор №2 / Шаговый №1
• Цифровой вывод 5 — DC Мотор №3 / Шаговый №2
• Цифровой вывод 6 — DC Мотор №4 / Шаговый №2

Выводы отвечающие за выбор направления вращения двигателей (подключены к микросхеме 74HC595): Цифровые выводы 4, 7, 8 и 12.

Выводы для управления сервоприводами (выведены на штырьки на краю платы):

• Цифровой вывод 9 — Сервопривод №1
• Цифровой вывод 10 — Сервопривод №2

В итоге незадействованными цифровыми выводами остаются только пины 2 и 13. Однако есть выход из данной ситуации. У нас остались незадействованные аналоговые входы A0-A5, их можно использовать как цифровые. К ним мы и будем подключить наш ультразвуковой дальномер. В коде они будут записываться как цифровые с 14 по 19.

Следующая проблема, которую необходимо решить — определиться с источником питания для робота. Варианты такие:

• Питаем Arduino и моторы от одного источника постоянного тока: подключаем этот источник к DC-входу Arduino или EXT_PWR на шилде, замыкаем джампер JP1. Вообще, питать моторы и Arduino от одного источника питания — плохая идея, Arduino может постоянно сбрасываться и вообще нестабильно работать.
• Питаем Arduino от USB, а моторы — от отдельного источника постоянного тока. Удаляем джампер JP1 на шилде, подключаем USB к Arduino, а питание моторов — на шилд в EXT_PWR.
• Питаем Arduino и моторы от двух независимых источников постоянного тока. Удаляем джампер JP1 на шилде, подключаем первый источник через DC-вход к Arduino, а второй — в EXT_PWR на шилде.

Внимание: соблюдайте полярность на EXT_PWR: оно подается после высоковольтного защитного диода D1 на Arduino. Если вы все правильно подключили — светодиод должен загореться!

Я решил остановиться на последнем варианте и использовать крону для питания платы Arduino и 6 батареек типа AA для питания моторов.

Настало время собрать все воедино:

• втыкаем шилд в плату Arduino
• подключаем моторы к шилду (разъемы M1, M2, M3, M4)
• подключаем сервопривод к шилду (смотрим на маркировку на плате, чтобы не ошибиться пинами)
• подключаем дальномер к шилду (Vcc — 5В, Gnd — земля, Trig — к пину A0, Echo — к пину A1)
• подключаем питание

И последний штрих — запускаем тестовую программу, которая будет управлять моторами: (для запуска потребуется библиотека AFMotor).

#include <AFMotor.h>  // Подключаем библиотеку для работы с шилдом 

// Подключаем моторы к клеммникам M1, M2, M3, M4
AF_DCMotor motor1(1);
AF_DCMotor motor2(2);
AF_DCMotor motor3(3);
AF_DCMotor motor4(4);

void setup(){
	// Задаем максимальную скорость вращения моторов (аналог работы PWM) 
	motor1.setSpeed(255);
	motor1.run(RELEASE);
	motor2.setSpeed(255);
	motor2.run(RELEASE);
	motor3.setSpeed(255);
	motor3.run(RELEASE);
	motor4.setSpeed(255);
	motor4.run(RELEASE);
}

void loop(){
	// Двигаемся условно вперед одну секунду 
	motor1.run(FORWARD); // Задаем движение вперед
	motor2.run(FORWARD);
	motor3.run(FORWARD);
	motor4.run(FORWARD);
	motor1.setSpeed(255); // Задаем скорость движения
	motor2.setSpeed(255); 
	motor3.setSpeed(255); 
	motor4.setSpeed(255); 
	delay(1000);

	// Останавливаем двигатели
	/* Очень не рекомендуем резко переключать направление вращения двигателей.
	Лучше дать небольшой промежуток времени.*/

	motor1.run(RELEASE); 
	motor2.run(RELEASE);
	motor3.run(RELEASE);
	motor4.run(RELEASE);
	delay(500);

	// Двигаемся в обратном направлении
	motor1.run(BACKWARD);  // Задаем движение назад
	motor2.run(BACKWARD);
	motor3.run(BACKWARD);
	motor4.run(BACKWARD);
	motor1.setSpeed(255);  // Задаем скорость движения 
	motor2.setSpeed(255); 
	motor3.setSpeed(255); 
	motor4.setSpeed(255); 
	delay(1000);

	// Останавливаем двигатели  
	motor1.run(RELEASE);
	motor2.run(RELEASE);
	motor3.run(RELEASE);
	motor4.run(RELEASE);
	delay(500);
}
			

Надеюсь у вас все получилось и ваш робот сделал первые «шаги» 🙂

Мы имеем следующие способности и компоненты робота:

• 4 моторчика, каждым мотором мы можем управлять независимо от других
• сервопривод с рабочим ходом +\- 85-90 градусов и большой скоростью поворота
• ультразвуковой дальномер с углом обзора примерно 15 градусов

Зная характеристики и возможности железа, мы можем накидать алгоритм для программы:

• робот должен ехать прямо, пока не увидит препятствие
• увидев препятствие, он должен оглядеться и определить оптимальный маршрут (поехать в ту сторону, где помеха находятся дальше или отсутствует)
• если препятствие оказалось слишком близко, робот по каким либо причинам его не заметил и уткнулся в него (слабая отражающая способность, крохотная преграда, или робот подъезжал к нему под таким углом, что звук просто не мог отразиться обратно) — необходимо отъехать назад и дальше следовать второму пункту данного алгоритма
• после завершения маневра ехать по прямой линии до следующей преграды

Итак, задача ясна. Ниже представлена готовая и откомментированная программа. По-моему мнению, это наилучший способ разъяснить, как она работает 🙂

// Подключаем нужные библиотеки
#include <Servo.h> // для работы с сервоприводом
#include <AFMotor.h> // для работы с шилдом

// задаем пины, к которым мы подключили ультразвуковой дальномер
// на шилде Motor Drive Shield dual L239D оставались пустыми только аналоговые пины
// их можно свободно использовать, как цифровые
// пин A0 соответсвует 14-му цифровому пину, A1 - 15-му
#define trigPin 14 // через этот пин будем отправлять сигнал
#define echoPin 15 // здесь будем считывать ответ

// даем Arduino знать, что подключено 4 моторчика

// это задние моторы
AF_DCMotor motor_b1(1); // мотор подключен к порту M1 на шилде
AF_DCMotor motor_b2(2); // порт M2

// это передние моторы
AF_DCMotor motor_f1(3); // порт M3
AF_DCMotor motor_f2(4); // порт M4

// объект для управления сервоприводом
Servo neck;

// вспомогательные переменные, за что они отвечают написано ниже...
int d1, d2;

// для упрощения программы, устранения повторения кода и просто для удобства, будем использовать функции...
// ниже описаны функции, которые нужны для реализации того алгоритма, который мы написали в начале статьи
// с помощью данной функции мы сможем задать скорость, с которой будет двигаться наш робот
// у нее всего один параметр - speed - он должен быть в диапазоне 0-255
// соотвественно 0 - робот никуда не едет, 255 - едет на максимальной скорости (максимально сжирая батарейки :))
// для прямолинейной езды скорость всем моторам нужно установить одинаковую
void set_speed(int speed){
	motor_b1.setSpeed(speed);
	motor_b2.setSpeed(speed);
	motor_f1.setSpeed(speed);
	motor_f2.setSpeed(speed);
}

// библиотека AFMotor позволяет установить моторы в 3 режима:
// 1. FORWARD - моторы будут крутиться "вперед", если можно так выразиться :)
// 2. BACKWARD - моторы будут крутиться "назад"
// 3. RELEASE - остановка, моторы крутиться не будут
// и после переключения режима, желательно дать время на переход из одного состояния в другое

// Ниже реализованы 3 функции, которые позволят переключаться между этими режимами.

// едем вперед
void robot_forward(){
	motor_b1.run(FORWARD);
	motor_b2.run(FORWARD);
	motor_f1.run(FORWARD);
	motor_f2.run(FORWARD);
	delay(1000);
}

// едем назад
void robot_backward(){
	motor_b1.run(BACKWARD);
	motor_b2.run(BACKWARD);
	motor_f1.run(BACKWARD);
	motor_f2.run(BACKWARD);
	delay(1000);
}

// останавливаемся
void robot_release(){
	motor_b1.run(RELEASE);
	motor_b2.run(RELEASE);
	motor_f1.run(RELEASE);
	motor_f2.run(RELEASE);
	delay(1000);
}

// конструкция платформы не позволяет поворачивать передние колеса для изменения направления движения
// это легко обойти, если вспомнить как поворачивает танк :)
// колеса на одной стороне мы вращаем в одну сторону, на другой стороне - в противоположную
// следующие функции это делают...

// поворот направо
void turning_to_the_right(){
	motor_b1.run(FORWARD);
	motor_f2.run(FORWARD);
	motor_b2.run(BACKWARD);
	motor_f1.run(BACKWARD);
	delay(1000);
}

// поворот налево
void turning_to_the_left(){
	motor_b2.run(FORWARD);
	motor_f1.run(FORWARD);
	motor_b1.run(BACKWARD);
	motor_f2.run(BACKWARD);
	delay(1000);
}

// такой алгоритм разворота разумеется не идеальный - этот метод я использовал для простоты
// для плавного поворота на ходу, нужно просто задавать моторам разную скорость

// и последняя функция
// она нужна для определения расстояния до препятствия
// для обеспечения большей точности, показания считываются дважды
int get_distance(){
	// вспомогательные переменные
	long duration;
	int d_1, d_2, average = -1;
	
	// это стандартный способ определения расстояния для дальномера
	// описывать его не буду, в интернете есть куча информации по этому поводу - нам важен результат
	// результат будет записан в переменную d_1, а именно будет записано количество сантиметров до препятствия
	// -------------------
	digitalWrite(trigPin, LOW);
	delayMicroseconds(2);
	digitalWrite(trigPin, HIGH);
	delayMicroseconds(10);
	digitalWrite(trigPin, LOW);
	duration = pulseIn(echoPin, HIGH);
	d_1 = (int) duration / 58.2;
	// -------------------
	
	
	// как я уже писал, повторяем процедуру для более точных показаний
	// и записываем значение в переменную d_2
	// -------------------
	digitalWrite(trigPin, LOW);
	delayMicroseconds(2);
	digitalWrite(trigPin, HIGH);
	delayMicroseconds(10);
	digitalWrite(trigPin, LOW);
	duration = pulseIn(echoPin, HIGH);
	d_2 = (int) duration / 58.2;
	// -------------------
	
	// подсчитываем среднее арифметическое для d_1 и d_2
	// это и будет считаться окончательным ответом
	average = (int) (d_1 + d_2) / 2;
	Serial.print("distance: ");
	Serial.println(average);
	return average;
}


void setup(){
	Serial.begin(19200);
	// устанавливаем пин A0 на вывод, A1 - на ввод
	pinMode(trigPin, OUTPUT);
	pinMode(echoPin, INPUT);
	// сервопривод подключен к 10-му пину, даем знать Arduino, что он там есть
	neck.attach(10);
	// и поворачиваем на нужный угол (угол подбирайте сами, нужно чтобы дальномер смотрел вперед)
	neck.write(120);
	// устанавливаем скорость двигателей. я выбрал значение 200, при 150 он еле тащился :)
	set_speed(200);
	// режим FORWARD, робот будет ехать вперед
	robot_forward();
}

void loop(){
	// вот мы и дошли до реализации самого алгоритма
	// в функции setup() мы уже установили необходимые значения для движения вперед по прямой
	
	// следующее условие будет проверять не наткнулся ли робот на препятствие
	// если наткнулся - совершить маневр, и после его завершения опять ехать вперед
	
	if (get_distance() < 20){
		// сюда попадем, если мы оказались впритык к препятствию (до него меньше 20 сантиметров)
		// нам не хватает места для разворота
		Serial.println("backward");
		// сдаем немного назад
		robot_backward();
		delay(1000);
		Serial.println("forward");
		// потом мы опять едем вперед с надеждой, что сдалека мы обнаружим помеху :)
		robot_forward();
		// если ее обнаружим - то выполнится нижний участок кода
		// если не обнаружим - либо опять сдадим назад, либо врежимся :)
	} else if (get_distance() < 50){
		// если до препятствия остается 50 сантиметров, совершаем поворот
		// останавливаем робота
		robot_release();
		// поворачиваем "голову" направо на 50 градусов
		neck.write(70);
		delay(500);
		// считываем расстояние до препятствия справа от робота
		d1 = get_distance();
		// поворачиваем "голову" налево
		neck.write(170);
		delay(500);
		// считываем расстояние до препятствия слева от робота
		d2 = get_distance();
		// выбираем оптимальный маршрут
		if (d1 > d2){
			// сюда попадем, если справа от робота больше свободного места для езды
			Serial.println("turning to the right");
			// начинаем поворот направо
			turning_to_the_right();
		} else {
			// сюда попадем, если слева от робота больше свободного места для езды
			Serial.println("turning to the left");
			// начинаем поворот налево
			turning_to_the_left();
		}
		// вернем "голову" в исходную позицию
		neck.write(120);
		delay(500);
		// поворачиваем до тех пор, пока препятствие не выйдет из поля зрения
		while (get_distance() < 50){
			delay(500);
		}
		// поедем вперед, когда поворот завершится
		robot_forward();
	}
	// небольшая задержка, чтобы дать время роботу поменять свое положение
	delay(500);
}
			

Помните, что робот имеет только один сервопривод — следовательно осматриваться он может только в одной плоскости и помехи, которые будут находится ниже и выше угла обзора дальномера, будут просто игнорироваться и робот в них врежется. Частично это можно обойти, играясь с этим условием (get_distance() < 50).

На этом все, спасибо за внимание 🙂

Видео с очень похожим роботом:

Инфомрация взята с:

• http://zelectro.com.ua/Adafruit_motor_shield
• http://mk90.blogspot.ru/2009/03/motorshield-adafruit.html
• http://learn.adafruit.com/adafruit-motor-shield/overview

Материал прислал: Александр Укштейн

Возможно, вам потребуются файлы:

• библиотека AFMotor