Ведроид-мобиль — робот на Arduino — Часть 4. Подключаем ультразвуковой дальномер

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

А прошлой статье я описывал процесс реализации управления роботом по Bluetooth через смартфон на Android. Это был первый режим работы робота. После этой статьи у нас появится второй режим. В будущем я планирую добавить еще несколько.

Создание шилда управления роботом

Для переключения между различными режимами работы я решил сделать отдельный шилд. В прошлый раз я уже использовал Proto Shield. Сегодня я его немного модернизирую под свои потребности.

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

• Светодиоды - 7 штук
• Кнопки - 3 штуки
• Резисторы на 220 Ом - 6 штук
• Резисторы на 10 кОм - 2 штуки
• Перемычки - 5 штук
• Разъём PBS (мама) на 16 контактов - 1 штука
• Миниатюрная макетная плата - 1 штука

Все компоненты запаиваем на Proto Shield по указанной схеме (вид со стороны деталей):

Размещение элементов на плате:

В левом нижнем углу я вывел кнопку RESET. Она замыкает выводы RESET и GND. В левом верхнем - разъем подключения сервоприводов. Вверху светодиод с 13 дискретного вывода. Справа 2 кнопки переключения режимов (след / пред).

Каждая кнопка подтянута к земле резистором на 10 кОм.

Далее размещены 6 светодиодов, которые подключены к панельке контактов через резисторы на 220 Ом.

Светодиоды будут служить индикаторами режимов работы робота.

Для разводки дополнительных модулей, я еще решил добавить в шилд управления миниатюрную макетную плату.

Подключение шилда управления роботом

Подключим провода к разъему на 16 контактов. Описывать буду назначение контактов сверху вниз. LED 1..6 - выводы светодиодов, BTN 1..2 - кнопки.

• 1 - LED 1 - к 22 дискретному выводу Arduino
• 2
• 3
• 4 - LED 2 - к 23 дискретному выводу Arduino Mega 2560
• 5
• 6 - BTN 1 - к 28 дискретному выводу Arduino Mega 2560
• 7 - LED 3 - к 24 дискретному выводу Arduino Mega 2560
• 8 - GND - к GND Arduino Mega 2560
• 9 - +5 V - к +5 V Arduino Mega 2560
• 10 - LED 4 - к 25 дискретному выводу Arduino Mega 2560
• 11 - BTN 2 - к 29 дискретному выводу Arduino Mega 2560
• 12
• 13 - LED 5 - к 26 дискретному выводу Arduino Mega 2560
• 14
• 15
• 16 - LED 6 - к 27 дискретному выводу Arduino Mega 2560

Сразу восстановим подключение Bluetooth модуля JY-MCU к Arduino Mega 2560

• VCC на JY-MCU подключаем к +5В Arduino
• GND на JY-MCU подключаем к GND Arduino
• TXT на JY-MCU подключаем к дискретному PIN 50 на Arduino
• RXD на JY-MCU подключаем к дискретному PIN 51 на Arduino

Подключение ультразвукового дальномера HC-SR04

Процесс подключения ультразвукового дальномера HC-SR04 к Arduino  и работу с ним я описывал ранее. В тонкости в этот раз вдаваться не буду, а лучше подробно распишу как я его закрепил на сервоприводе.

Для создания крепления под ультразвуковой дальномер HC-SR04 я использовал кусок платы от Proto Shield и панельку контактов.

Откусил от панельки контактов две части по 4 контакта и запаял их на плату параллельно.

Перед подключением дальномера я сперва “раскорячил” его контакты, чтобы они плотно держались в разъеме.

Проковырял в плате дырку под болт.

Закрепил модуль на сервоприводе.

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

Осталось только подключить провода к контактной панельке на плате дальномера и завести их на контакты Arduino Mega 2560. Этим и займемся.

• VCC HC-SR04 подключим к +5V на Arduino Mega 2560
• Trig HC-SR04 к цифровому пину 31 на Arduino Mega 2560
• Echo HC-SR04 к цифровому пину 30 на Arduino Mega 2560
• GND HC-SR04 к GND на Arduino Mega 2560

Алгоритм объезда препятствий

С алгоритмом я особо не заморачивался. Все достаточно просто и интуитивно понятно.

Из исходного положения проверяем расстояние впереди. Если оно больше 30 сантиметров, то продолжаем двигаться вперед, иначе:

• останавливаем двигатели
• поворачиваем сервопривод на углы от 0 до 180 градусов с шагом в 15 градусов и измеряем расстояния на этих углах
• заносим полученные значения в массив
• поворачиваем сервопривод на угол 90 градусов (прямо)
• ищем в массиве позицию с максимальным значением данных
• если это значение меньше 30 сантиметров, то едем назад
• если это значение больше 30 сантиметров, то проверяем какому углу поворота сервопривода оно соответствует и в зависимости от этого поворачиваем влево или вправо

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

Скетч реализации алгоритма объезда препятствий

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

//Создаем объекты для двигателей
AF_DCMotor motor1(1); //канал М1 на Motor Shield — задний левый
AF_DCMotor motor2(2); //канал М2 на Motor Shield — задний правый
AF_DCMotor motor3(3); //канал М3 на Motor Shield — передний левый
AF_DCMotor motor4(4); //канал М4 на Motor Shield — передний правый
// Создаем объект для сервопривода
Servo vservo;
// Прописываем пины используемые модулем Bluetooth
SoftwareSerial BTSerial(50, 51); // RX, TX
// Создаем переменную для команд Bluetooth
char vcmd;
// Создаем переменные для запоминания скорости левых и правых двигателей
int vspdL, vspdR;
/* Создаем переменную, на значение которой будет уменьшаться скорость при плавных поворотах.
Текущая скорость должна быть больше этого значения.  В противном случае двигатели со стороны направления поворота просто не будут вращаться */
const int vspd = 200;
// Заносим в массив пины, к которым подключены светодиоды
const int vledpins[6]={
22,23,24,25,26,27};
// Создаем переменную для сохранения режима работы
int vmode;
// Создаем переменную для сохранения предыдущего режима работы
int vmodeprev = -1;
// Заносим в массив пины, к которым подключены кнопки
const int vbtn[2]={
28,29};
// Массив для хранения углов поворота сервопривода (шаг 15 градусов)
const int vservo_array[13]={
0,15,30,45,60,75,90,105,120,135,150,165,180};
// Массив для хранения данных о расстоянии под различными углами поворота сервопривода
int vHC_SR04_array[13];
// Пины, используемые ультразвуковым дальномером
const int vTrig = 31;
const int vEcho = 30;
// Переменные, для хранения данных с дальномера
unsigned int vtime_us=0;
unsigned int vdistance_sm=0;
// Минимальное расстояние в сантиметрах, при котором нужно искать новый маршрут движения
const int vmindistance = 30;
// Переменная для циклов перебора значения массивов vservo_array и vHC_SR04_array
int vservo_int;
// Переменные для цикла поиска максимального значения в массивах
int vmaxarrayindex_int;
int vmaxarrayvalue_int;

void setup() {
// Устанавливаем скорость передачи данных по Bluetooth
BTSerial.begin(9600);
// Устанавливаем скорость передачи данных по кабелю
Serial.begin(9600);
// Выбираем пин к которому подключен сервопривод
vservo.attach(9); // или 10, если воткнули в крайний разъём
// Поворачиваем сервопривод в положение 90 градусов при каждом включении
vservo.write(90);
// Устанавливаем максимальную скорость вращения двигателей
vspeed(255,255);
/* Устанавливаем все выводы, к которым подключены светодиоды,
в OUTPUT. Зажигаем и гасим светодиоды с интервалом в 0.5 сек
для проверки */
for (vmode = 0; vmode < 6; vmode = vmode + 1) {
pinMode(vledpins[vmode], OUTPUT);
digitalWrite(vledpins[vmode], HIGH);
delay (500);
digitalWrite(vledpins[vmode], LOW);
}
/* Устанавливаем выводы, к которым подключены кнопки, в INPUT. */
pinMode(vbtn[0], INPUT);
pinMode(vbtn[1], INPUT);
// Устанавливаем значение первого режима работы робота
vmode = 0;
// Устанавливаем значение для пинов, к которым подключен ультразвуковой дальномер
pinMode(vTrig, OUTPUT);
pinMode(vEcho, INPUT);
}

void loop() {
/* Переключение режимов работы робота */
// Кнопка переключения на следующий режим - BTN1
if (digitalRead(vbtn[0]) == HIGH) {
vmode = vmode + 1;
vmodeprev = vmode - 1;
if (vmode > 5) {
vmode = 0;
vmodeprev = 5;
}
vrelease();
delay (500);
}
// Кнопка переключения на предыдущий режим - BTN2
if (digitalRead(vbtn[1]) == HIGH) {
vmode = vmode - 1;
vmodeprev = vmode + 1;
if (vmode < 0) {
vmode = 5;
vmodeprev = 0;
}
vrelease();
delay (500);
}
// Засвечиваем светодиод текущего режима работы
digitalWrite(vledpins[vmode], HIGH);
// Гасим светодиод предыдущего режима работы
if (vmodeprev > -1) {
digitalWrite(vledpins[vmodeprev], LOW);
}
/* Выбор режима работы */
switch (vmode) {
case 0:
// Режим ожидания
break;
case 1:
// Режим работы с использованием ультразвукового дальномера
vultrasoundmode();
break;
case 2:
// Режим
break;
case 3:
// Режим управления через Bluetooth
vbluetoothmode();
break;
case 4:
// Режим
break;
case 5:
// Режим
break;
}
}
/* Режим работы с использованием ультразвукового дальномера */
void vultrasoundmode(){
vservo.write(90);
delay(200);
Serial.print("Now ");
Serial.println(vHC_SR04());
// Если расстояние меньше наименьшего, то
if (vHC_SR04() < vmindistance) {
// Останавливаем двигатели
vrelease();
// Крутим серву измеряя расстояния и занося данные в массив
for (vservo_int = 0; vservo_int < 13; vservo_int = vservo_int + 1) {
vservo.write(vservo_array[vservo_int]);
delay(200);
vHC_SR04_array[vservo_int] = vHC_SR04();
// Выводим данные для отладки
Serial.print(vservo_int);
Serial.print(" ");
Serial.println(vHC_SR04_array[vservo_int]);
}
vservo.write(90);
delay(500);
// Поиск в массиве позиции с максимальным значением
vmaxarrayindex_int = 0;
vmaxarrayvalue_int = 0;
for (vservo_int = 0; vservo_int < 13; vservo_int = vservo_int + 1) {
if (vHC_SR04_array[vservo_int] > vmaxarrayvalue_int) {
vmaxarrayindex_int = vservo_int;
vmaxarrayvalue_int = vHC_SR04_array[vservo_int];
}
}
Serial.print("Max index ");
Serial.println(vmaxarrayindex_int);
// Проверка - если максимальное значение массива меньше минимального расстояния, то едем назад
if (vHC_SR04_array[vmaxarrayindex_int] < vmindistance) {
vbackward();
delay(500);
}
/* Проверка - если индекс максимального значения массива меньше 6 то поворачиваем вправо,
иначе влево */
if (vmaxarrayindex_int < 6) {
vright();
delay(500);
}
else
{
vleft();
delay(500);
}
}
else
{
// Едем прямо
vforward();
}
}

/* Режим управления через Bluetooth */
void vbluetoothmode() {
// Если есть данные с Bluetooth
if (BTSerial.available())
{
/* Читаем команды и заносим их в переменную.
(char) преобразует код символа команды в символ */
vcmd = (char)BTSerial.read();
// Отправляем команду в порт, чтобы можно было их проверить в "Мониторе порта"
Serial.println(vcmd);

// Вперед
if (vcmd == 'F') {
vforward();
}
// Назад
if (vcmd == 'B')
{
vbackward();
}
// Влево
if (vcmd == 'L')
{
vleft();
}
// Вправо
if (vcmd == 'R')
{
vright();
}
// Прямо и влево
if (vcmd == 'G')
{
vforwardleft();
}
// Прямо и вправо
if (vcmd == 'I')
{
vforwardright();
}
// Назад и влево
if (vcmd == 'H')
{
vbackwardleft();
}
// Назад и вправо
if (vcmd == 'J')
{
vbackwardright();
}
// Стоп
if (vcmd == 'S')
{
vrelease();
}
// Скорость 0%
if (vcmd == '0')
{
vspeed(0,0);
}
// Скорость 10%
if (vcmd == '1')
{
vspeed(25,25);
}
// Скорость 20%
if (vcmd == '2')
{
vspeed(50,50);
}
// Скорость 30%
if (vcmd == '3')
{
vspeed(75,75);
}
// Скорость 40%
if (vcmd == '4')
{
vspeed(100,100);
}
// Скорость 50%
if (vcmd == '5')
{
vspeed(125,125);
}
// Скорость 60%
if (vcmd == '6')
{
vspeed(150,150);
}
// Скорость 70%
if (vcmd == '7')
{
vspeed(175,175);
}
// Скорость 80%
if (vcmd == '8')
{
vspeed(200,200);
}
// Скорость 90%
if (vcmd == '9')
{
vspeed(225,225);
}
// Скорость 100%
if (vcmd == 'q')
{
vspeed(255,255);
}
}
}

/* Функция определения расстояния с дальномера */
int vHC_SR04() {
digitalWrite(vTrig, HIGH); // Подаем сигнал на выход микроконтроллера
delayMicroseconds(10); // Удерживаем 10 микросекунд
digitalWrite(vTrig, LOW); // Затем убираем
vtime_us=pulseIn(vEcho, HIGH); // Замеряем длину импульса
vdistance_sm=vtime_us/58; // Пересчитываем в сантиметры
return vdistance_sm; // Возвращаем значение
}

/* Функции управления двигателями */

// Вперед
void vforward() {
vspeed(vspdL,vspdR);
vforwardRL();
}

// Вперед для RL
void vforwardRL() {
motor1.run(FORWARD);
motor2.run(FORWARD);
motor3.run(FORWARD);
motor4.run(FORWARD);
}

// Назад
void vbackward() {
vspeed(vspdL,vspdR);
vbackwardRL();
}

// Назад для RL
void vbackwardRL() {
motor1.run(BACKWARD);
motor2.run(BACKWARD);
motor3.run(BACKWARD);
motor4.run(BACKWARD);
}

// Влево
void vleft() {
vspeed(vspdL,vspdR);
motor1.run(BACKWARD);
motor2.run(FORWARD);
motor3.run(BACKWARD);
motor4.run(FORWARD);
}

// Вправо
void vright() {
vspeed(vspdL,vspdR);
motor1.run(FORWARD);
motor2.run(BACKWARD);
motor3.run(FORWARD);
motor4.run(BACKWARD);
}

// Вперед и влево
void vforwardleft() {
if (vspdL > vspd) {
vspeed(vspdL-vspd,vspdR);
}
else
{
vspeed(0,vspdR);
}
vforwardRL();
}

// Вперед и вправо
void vforwardright() {
if (vspdR > vspd) {
vspeed(vspdL,vspdR-vspd);
}
else
{
vspeed(vspdL,0);
}
vforwardRL();
}

// Назад и влево
void vbackwardleft() {
if (vspdL > vspd) {
vspeed(vspdL-vspd,vspdR);
}
else
{
vspeed(0,vspdR);
}
vbackwardRL();
}

// Назад и вправо
void vbackwardright() {
if (vspdR > vspd) {
vspeed(vspdL,vspdR-vspd);
}
else
{
vspeed(vspdL,0);
}
vbackwardRL();
}

// Стоп
void vrelease(){
motor1.run(RELEASE);
motor2.run(RELEASE);
motor3.run(RELEASE);
motor4.run(RELEASE);
}

// Изменение скорости
void vspeed(int spdL,int spdR){
if (spdL == spdR) {
vspdL=spdL;
vspdR=spdR;
}
motor1.setSpeed(spdL);
motor2.setSpeed(spdR);
motor3.setSpeed(spdL);
motor4.setSpeed(spdR);
}

В строках, на которые будет ругаться компилятор, поменяйте тире на минусы.

Демонстрация робота на Arduino

P.S.

В следующий раз я попробую реализовать алгоритм прохождения лабиринта по правилу правой руки или движение по белой линии.