Перед вами обновлённая версия машинки!
Основные нововведения: FPV-система и датчик чёрной линии. Справа на фотографии - пульт для управления роботом в 2-х режимах: оператора (навигация за счёт двух джойстиков от приставки) и в режиме автономного движения по линии (навигация за счёт датчика линии). На заднем плане находятся картонные очки виртуальной реальности, обеспечивающие глубокое погружение в процесс управления и наблюдения. Сервопривод в верхней крышке поворачивает камеру.
Под капотом:
Компоненты машинки:
Визуальная система:
Для более сосредоточенного погружения в процесс наблюдения при автономном движении по маршруту или в процесс управления роботом в режиме оператора вне маршрута были созданы очки из картона и двух луп малого диаметра. Изображение поступает по радио-каналу на смартфон с воткнутым UVC OTG приёмником и раздваивается на оба глаза. На телефоне установлено ПО.
Т.к. корпус машинки сделан из светорассеивающего оргстекла, подсветка робота обеспечиваться обычным светодиодом. Из-за малых размеров (10x15 см) на неё легко наступить, особенно в темноте.
Основные нововведения: FPV-система и датчик чёрной линии. Справа на фотографии - пульт для управления роботом в 2-х режимах: оператора (навигация за счёт двух джойстиков от приставки) и в режиме автономного движения по линии (навигация за счёт датчика линии). На заднем плане находятся картонные очки виртуальной реальности, обеспечивающие глубокое погружение в процесс управления и наблюдения. Сервопривод в верхней крышке поворачивает камеру.
Под капотом:
Компоненты машинки:
- Arduino Nano
- NRF24L01 с антенной и адаптером
- Аккумулятор 610мАч 7.4В (две банки)
- Драйвер моторов двухканальный Pololu на TB6612FNG v2 <1А
- Мотор-редуктор Gekko MR12-050 turbo
- Колесо Pololu 32x7
- QTR-8RC Reflectance Sensor Array
- FPV Camera System Eachine TX02
- Micro Servo SG90
Контакты драйвера двигателя:
- AIN1 – Значение №1 (цифровой сигнал)
- AIN2 – Значение №2 (цифровой сигнал)
- STBY – Аварийное торможение (цифровой сигнал)
- PWMA – Скорость вращения (ШИМ сигнал, 0-255)
Зачем нужен драйвер двигателей?
По силе тока присоединенной нагрузки вся плата Arduino имеет ограничения в 800mA, для каждого отдельного вывода в 40mA. Любой из двигателей в момент запуска или остановки создаёт пиковые броски тока, превышающие эти пределы, и поэтому мы не можем подключить двигатель напрямую. Один из вариантов решения – использовать плату драйвера двигателей.
Что такое NRF24L01?
NRF24L01 – радиомодуль приёма-передачи, работающий на частоте 2,4 ГГц. На пульте он без антенны, на роботе с антенной, разница заключается в дальности связи. Дальность связи на открытой местности составляет около 1 км. Радиомодуль на пульте отсылает значения, полученные с джойстиков, на машинку. В свою очередь, модуль на машинке принимает их и посылает на моторы.
Внутренности пульта управления:Линейка сенсорных пар, датчик линии.
На модуле датчика линии расположено 8 сенсорных пар. Одна такая пара включает в себя источник инфракрасного сигнала и его приемник. В зависимости сколько света отразилось и пришло в приёмник датчик определяет цвет поверхности, над которой он находиться. От белой поверхности отражается больше света чем от чёрной.
Схема пульта управления:
Компоненты пульта управления:
- Arduino Nano
- NRF24L01 с антенной и адаптером
- Аккумулятор 610мАч 7.4В (две банки)
- Джойстик от геймпада
Джойстик...
Существует большое разнообразие способов передачи информации микроконтроллеру. Один из них - это использование джойстика. Аналоговый джойстик представляет собой ручку, закреплённую на шаровом шарнире с двумя взаимно перпендикулярными осями. При наклоне ручки ось вращает подвижный контакт потенциометра, благодаря чему изменяется напряжение на его выходе. Также аналоговый джойстик имеет тактовую кнопку, которая срабатывает при вертикальном надавливании на ручку. Кнопки на джойстиках я использую для переключения между режимами работы робота и для включения-выключения подсветки. Ось Y на первом джойстике используется для снятия скорости вперёд-назад, а ось X для поворота камеры влево-вправо. На втором джойстике ось X используется для снятия коэффициента поворота, а ось Y не используется.
Алгоритм режима оператора
В этом режиме мы можем довольно маневренно управлять машинкой с помощью двух осей джойстиков. Т.е. есть два аналоговых значения от 0 до 1023, которые считываются с джойстиков в реальном времени. Также есть два мотора, которым можно задавать скорость от 0 до 255 и направление вращения. Алгоритм взаимосвязывает полученные значения с джойстиков и режим вращения колеса так, чтобы получилась удобная и эффективная система управления машинкой. В обоих режимах мы можем поворачивать камеру на 180o, значения для поворота считываются с оси Х перового джойстика.
Алгоритм режима автономного движения
Данный режим обеспечивает движение по маршруту задаваемого чёрной линией. К примеру, можно нарисовать чёрной изолентой траекторию на нескольких ватманах и в режиме оператора наехать на сделанный трек так, чтобы датчик линии оказался над чёрной полосой, затем переключиться нажатием на джойстик в режим автономного движения. Для следования по линии используется 1, 3, 4, 5, 6 и 8 сенсорная пара, которые задействованы в ПД (пропорционально-дифференциальном регуляторе). В будущем можно реализовать и полный, ПИД (пропорционально-интегрально-дифференциальный регулятор) Зачем они нужны? Представьте, что машинка это маятник, который пропорционально колеблется около чёрной линии, съезжая то в лево, то вправо, пытаясь следовать ей. Чтобы устаканить его, мы используем дифференциальную составляющую - ошибку: то на сколько сильно мы отклонились от линии(если чёрная линия под 4 сенсорной парой, то не сильно и поворачивать нужно меньше, а если под 8, то гораздо сильней) и интегральную составляющую: позволяет сгладить малые колебания нашего маятника, мы буквально смотрим в прошлое, сравнивая с предыдущим отклонением нынешнее.
RX | Код приёмника (машинки):
#include "nRF24L01.h"
#include "RF24.h"
#include <Servo.h>
//Пины подключения драйвера двигателя//
#define PWMA_PIN 6 //левый мотор
#define AIN2_PIN 8 //левый мотор
#define AIN1_PIN 7 //левый мотор
#define STBY_PIN 4
#define BIN1_PIN A0 //правый мотор
#define BIN2_PIN A1 //правый мотор
#define PWMB_PIN 5 //правый мотор
//Пины подключения драйвера двигателя//
RF24 radio(9,10);//Пины подключения nrf24l01
#define LED_PIN 3 //Пин подключения подсветки
#define SERVO_PIN 2 //Пин подключения сервопривода
//Пины подключения датчика линии (8шт. сенсорных пар, но используем только 6шт.)//
#define SEN1_PIN A2
#define SEN3_PIN A3
#define SEN4_PIN A4
#define SEN5_PIN A5
#define SEN6_PIN A6
#define SEN8_PIN A7
//Пины подключения датчика линии//
Servo myservo;
boolean isFolow_line=false;
boolean isLed=false;
const int d36min=50;
const int d36max=100;
const int d18min=50;
const int d18max=255;
byte recieved_data[4];
void setup(){
Serial.begin(9600);
myservo.attach(SERVO_PIN);
pinMode(PWMA_PIN, OUTPUT);
pinMode(AIN2_PIN, OUTPUT);
pinMode(AIN1_PIN, OUTPUT);
pinMode(STBY_PIN, OUTPUT);
pinMode(BIN1_PIN, OUTPUT);
pinMode(BIN2_PIN, OUTPUT);
pinMode(PWMB_PIN, OUTPUT);
pinMode(LED_PIN, OUTPUT);
digitalWrite(STBY_PIN, HIGH);
radio.begin(); //активировать модуль
radio.setChannel(0x60);
radio.enableAckPayload();
radio.setDataRate(RF24_2MBPS);
radio.setPALevel(RF24_PA_MAX);
radio.openReadingPipe(0, 0x0123456789LL);
radio.startListening();
}
void loop(){
while(radio.available()){ // слушаем эфир со всех труб
radio.read(&recieved_data, sizeof(recieved_data)); //читаем присланный массив
Serial.print(recieved_data[0]);Serial.print(" | ");Serial.print(recieved_data[1]);Serial.print(" | ");Serial.print(recieved_data[2]);Serial.print(" | ");Serial.print(recieved_data[3]);Serial.println();
if(recieved_data[3]==1||recieved_data[3]==3) isFolow_line=!isFolow_line;
if(recieved_data[3]==2||recieved_data[3]==3){isLed=!isLed; digitalWrite(LED_PIN, isLed);}
myservo.write(constrain(map(recieved_data[0], 0, 255, 180, 0), 0, 180));
if(isFolow_line){
int d1 = analogRead(SEN1_PIN);
int d3 = analogRead(SEN3_PIN);
int d4 = analogRead(SEN4_PIN);
int d5 = analogRead(SEN5_PIN);
int d6 = analogRead(SEN6_PIN);
int d8 = analogRead(SEN8_PIN);
if(d3>100){
go(d36max, d36min);
}else if(d6>100){
go(d36min, d36max);
}else if(d1>100){
go(d18max, -d18min);
}else if(d8>100){
go(-d18min, d18max);
}else{
go(d36max,d36max);
}
}else{
int y1f = constrain(map(recieved_data[1], 128, 255, 0, 255), 0, 255); //обрезаем и переворачиваем значения
int y1b = constrain(map(recieved_data[1], 128, 0, 0, 255), 0, 255);
int x2r = constrain(map(recieved_data[2], 122, 255, 100, 1), 0, 100);
int x2l = constrain(map(recieved_data[2], 122, 0, 100, 1), 0, 100);
int stl = constrain(map(recieved_data[2], 123, 255, 0, 255), 0, 255);
int str = constrain(map(recieved_data[2], 122, 0, 0, 255), 0, 255);
int spr = y1f * x2r / 100; //скорость правого колеса
int spl = y1f * x2l / 100; //скорость левого колеса
int bspr = -y1b * x2r / 100; //скорость правого колеса назад
int bspl = -y1b * x2l / 100; //скорость левого колеса назад
if (recieved_data[1]>138){
go(spl,spr);
}else if (recieved_data[1]<118){
go(bspl,bspr);
}else if (118<recieved_data[1]<138){
go(stl,str);
}
}
}Serial.println("none");
}
void go(int speedLeft, int speedRight){
digitalWrite(AIN1_PIN, speedLeft<0);
digitalWrite(AIN2_PIN, speedLeft>0);
digitalWrite(BIN2_PIN, speedRight<0);
digitalWrite(BIN1_PIN, speedRight>0);
analogWrite(PWMA_PIN, abs(speedLeft));
analogWrite(PWMB_PIN, abs(speedRight));
}
TX | Код передатчика (пульта):
#include "nRF24L01.h"
#include "RF24.h"
#include "GyverButton.h"
#define X1_PIN A0
#define Y1_PIN A1
GButton butt1(7);
#define X2_PIN A2
#define Y2_PIN A3
GButton butt2(8);
RF24 radio(9, 10);
byte transmit_data[4];
void setup(){
Serial.begin(9600);
radio.begin();
radio.setChannel(0x60);
radio.enableAckPayload();
radio.setDataRate(RF24_2MBPS);
radio.setPALevel(RF24_PA_MAX);
radio.openWritingPipe(0x0123456789LL);
}
void loop(){
butt1.tick();
butt2.tick();
transmit_data[0] = map(analogRead(X1_PIN), 0, 1023, 0, 255); //считываем и обрезаем показания с джостика 1 оси X
transmit_data[1] = map(analogRead(Y1_PIN), 0, 1023, 0, 255); //считываем и обрезаем показания с джостика 1 оси Y
transmit_data[2] = map(analogRead(X2_PIN), 0, 1023, 0, 255); //считываем и обрезаем показания с джостика 2 оси X
transmit_data[3] = butt1.isClick()+2*butt2.isClick(); //0=нет; 1=butt1; 2=butt2; 3=butt1&&butt2;
radio.write(&transmit_data, sizeof(transmit_data)); //отправляем получившийся массив
//Serial.print(transmit_data[0]); Serial.print(" | "); Serial.print(transmit_data[1]); Serial.print(" | "); Serial.print(transmit_data[2]); Serial.print(" | "); Serial.print(transmit_data[3]); Serial.println();
}
Для более сосредоточенного погружения в процесс наблюдения при автономном движении по маршруту или в процесс управления роботом в режиме оператора вне маршрута были созданы очки из картона и двух луп малого диаметра. Изображение поступает по радио-каналу на смартфон с воткнутым UVC OTG приёмником и раздваивается на оба глаза. На телефоне установлено ПО.
Т.к. корпус машинки сделан из светорассеивающего оргстекла, подсветка робота обеспечиваться обычным светодиодом. Из-за малых размеров (10x15 см) на неё легко наступить, особенно в темноте.
Автономное движение. Справа внизу вид из VR-очков
Изменено: