Из таких же комплектующих многие собирают свои автономные роботы-платформы, но мое решение немного отличается от того, что можно найти в интернете. В частности упор был сделан на автономность и перехват управления по Bluetooth.
Для проекта нам понадобится плата Arduino Mega 2560. Она достаточно мощная, что бы справиться с нашими задачами. На нее отлично встает Motor Shield L293D.
Корпус автомобиля мы взяли из набора Car Chassis Kit for Arduino. В него входят рама, колеса, двигатели.
Основной датчик для ориентирования в пространстве - Ультразвуковой дальномер HC-SR04. Он закреплен на сервоприводе MG995.
Для перехвата управления я использовал Bluetooth модуль. В качестве пульта использовал телефон на Android с установленной программой Bluetooth Arduino Bluetooth RC Car (скачать можно с апстора).
| Моторы | Motor Shield L293D | Фары | Arduino Mega 2560 |
| П.Л. | М3 | П.Л. | «-» на GND, «+» на Pin 22 |
| П.П. | М4 | П.П. | «-» на GND, «+» на Pin 24 |
| З.Л. | М1 | З.Л. | «-» на GND, «+» на Pin 34 |
| З.П. | М2 | З.П. | «-» на GND, «+» на Pin 35 |
| Дальномер HC-SR04 | Arduino Mega 2560 | Bluetooth HC-05 | Arduino Mega 2560 | Buzzer | Arduino Mega 2560 |
| Vcc | 5V | VCC | 5V | SIG | Pin A9 |
| Trig | Pin 31 | GND | GND | VCC | Pin A8 |
| Echo | Pin 30 | TXD | Pin 50 | GND | GND |
| Gnd | GND | RXD | Pin 51 |
| Сервопривод MG995 | Motor Shield L293D |
| тройная клема | SERVO_2 (центральный провод «+» откусываем и запитываем напрямую от «+» Motor Shield; при управлении по Bluetooth — клему снимайте, чтобы серва не дёргалась) |
Если следовать указаниям по подключению проводов к пинам, то напутать что-то сложно. На плате всегда есть подписи.
Особое внимание стоило бы обратить на подключение проводов к двигателям что бы не напутать полярность - направления вращения.
При подключении телефона к Bluetooth нашего робота вылезет запрос пин-кода. Обычно он "1234". В программе Bluetooth Arduino Bluetooth RC Car доступны два способа управления: клавишами влево-вправо и с помощью гироскопа наклоном телефона.
Механизм работы автопилота таков, что машина едет вперед до тех пор пока до препятствия спереди не будет 30 см, то запускаем алгоритм сканирования препятствия. Двигатели останавливаются, серво-привод начинает вращать ультразвуковой дальномер на 180 градусов с шагом 15 градусов. Если в массиве измерений находятся точки расстояние до которых больше 30 см, то мы определяем с какой стороны эти точки и разворачиваем машину в этом направлении, запускаем двигатели.
Для зашивки программы вам необходимо будет скачать библиотеку AFMotor.
Управление по Bluetuth:
#include <AFMotor.h>
#include <SoftwareSerial.h>
int ledPin1 = 22; // передние фары
int ledPin2 = 24; // передние фары
int ledPin3 = 34; // задние фары
int ledPin4 = 35; // задние фары
int ledPin5 = A8; // клаксон
int ledPin6 = A9; // питание для клаксона
//Создаем объекты для двигателей
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 — передний правый
//Время мигания Аварийки
int blinkTime = 15; //частота мигания
int leftTime = 0;
boolean signalOn = LOW;
boolean avaOn = LOW;
SoftwareSerial BTSerial(50, 51); // RX, TX
// Создаем переменную для команд Bluetooth
char vcmd;
// Создаем переменные для запоминания скорости двигателей
int vspdL, vspdR;
/* Создаем переменную, на значение которой будет уменьшаться скорость при плавных поворотах.
Текущая скорость должна быть больше этого значения. В противном случае двигатели со стороны направления поворота просто не будут вращаться */
int vspd = 200;
void setup() {
pinMode(ledPin1, OUTPUT);
pinMode(ledPin2, OUTPUT);
pinMode(ledPin3, OUTPUT);
pinMode(ledPin4, OUTPUT);
pinMode(ledPin5, OUTPUT);
pinMode(ledPin6, OUTPUT);
digitalWrite(ledPin6, HIGH);
// Устанавливаем скорость передачи данных по Bluetooth
BTSerial.begin(9600);
// Устанавливаем скорость передачи данных по кабелю
Serial.begin(9600);
// Устанавливаем максимальную скорость вращения двигателей
vspeed(255,255);
}
void loop() {
// Если есть данные
if (BTSerial.available())
{
// Читаем команды и заносим их в переменную. char преобразует код символа команды в символ
vcmd = (char)BTSerial.read();
// Отправляем команду в порт, чтобы можно было их проверить в "Мониторе порта"
//Serial.println(vcmd);
// Передние фары ВКЛ
if (vcmd == 'W') {
digitalWrite(ledPin1, HIGH);
digitalWrite(ledPin2, HIGH);
}
// Передние фары ВЫКЛ
if (vcmd == 'w') {
digitalWrite(ledPin1, LOW);
digitalWrite(ledPin2, LOW);
}
// Задние фары ВКЛ
if (vcmd == 'U') {
digitalWrite(ledPin3, HIGH);
digitalWrite(ledPin4, HIGH);
}
// Задние фары ВЫКЛ
if (vcmd == 'u') {
digitalWrite(ledPin3, LOW);
digitalWrite(ledPin4, LOW);
}
// Аварийка ВКЛ
if (vcmd == 'X') {
avaOn = HIGH;
}
// Аварийка ВЫКЛ
if (vcmd == 'x') {
avaOn = LOW;
digitalWrite(ledPin1, LOW);
digitalWrite(ledPin2, LOW);
digitalWrite(ledPin3, LOW);
digitalWrite(ledPin4, LOW);
}
if (vcmd != 'S') {
Serial.println(vcmd);
}
if (avaOn) {
if (leftTime == 0) {
leftTime = blinkTime;
signalOn = !signalOn;
}
Serial.print("leftTime = ");
Serial.println(leftTime);
if (signalOn) {
digitalWrite(ledPin1, HIGH);
digitalWrite(ledPin2, HIGH);
digitalWrite(ledPin3, HIGH);
digitalWrite(ledPin4, HIGH);
delay(1);
leftTime = leftTime - 1;
}
else
{
digitalWrite(ledPin1, LOW);
digitalWrite(ledPin2, LOW);
digitalWrite(ledPin3, LOW);
digitalWrite(ledPin4, LOW);
delay(1);
leftTime = leftTime - 1;
}
} //закончилась Аварийка
// Клаксон ВКЛ
if (vcmd == 'V') {
analogWrite(ledPin5, 200);
}
// Клаксон ВЫКЛ
if (vcmd == 'v') {
analogWrite(ledPin5, LOW);
}
// Вперед
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);
}
}
}
// Вперед
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);
}
Пример программы для автоматической навигации робота-автомобиля:
#include <AFMotor.h>
#include <Servo.h>
#include <SoftwareSerial.h>
int ledPin1 = 22;
int ledPin2 = 24;
int ledPin3 = 34;
int ledPin4 = 35;
//Создаем объекты для двигателей
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;
// Создаем переменные для запоминания скорости левых и правых двигателей
int vspdL, vspdR;
/* Создаем переменную, на значение которой будет уменьшаться скорость при плавных поворотах.
Текущая скорость должна быть больше этого значения. В противном случае двигатели со стороны направления поворота просто не будут вращаться */
const int vspd = 200;
// Массив для хранения углов поворота сервопривода (шаг 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() {
pinMode(ledPin1, OUTPUT);
pinMode(ledPin2, OUTPUT);
pinMode(ledPin3, OUTPUT);
pinMode(ledPin4, OUTPUT);
// Устанавливаем скорость передачи данных по кабелю
Serial.begin(9600);
// Выбираем пин к которому подключен сервопривод
vservo.attach(9); // или 10, если воткнули в крайний разъём
// Поворачиваем сервопривод в положение 90 градусов при каждом включении
vservo.write(90);
// Устанавливаем максимальную скорость вращения двигателей
vspeed(255,255);
// Устанавливаем значение для пинов, к которым подключен ультразвуковой дальномер
pinMode(vTrig, OUTPUT);
pinMode(vEcho, INPUT);
}
void loop() {
digitalWrite(ledPin1, HIGH);
digitalWrite(ledPin2, HIGH);
digitalWrite(ledPin3, HIGH);
digitalWrite(ledPin4, HIGH);
vultrasoundmode();
}
/* Режим работы с использованием ультразвукового дальномера */
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();
}
}
/* Функция определения расстояния с дальномера */
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);
}