Частный блог

В этой статье хотелось бы поделиться описанием того, как построить Lego-вездеход с управлением по bluetooth и вещанием видео. Описана будет электроника, программная часть и некоторые конструктивные моменты. Сам же вездеход придётся собрать самостоятельно.

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

• Web-камера Logitech C270
• Xiaomi Mi Power Bank 20000mAh
• Raspberry Pi Model B
• L293D motor shield
• 8 аккумуляторных батареек и корпус для них
• Два L-мотора Lego
• Много деталей от 9398, 42038, 42009, 42029 и других наборов
• Bluetooth adapter

Предполагается, что на Raspberry уже установлена Jetty вместе с Java от Oracle. Так же предполагается, что вы знакомы с программированием на Java. Как поставить нужную версию JDK:

sudo apt-get install oracle-java7-jdk

1. Моторы

Начать стоит с подключение к моторам Lego, которые представляют из себя обычный коллекторный двигатель на 9V. Схема типичного подключения представлена ниже. На ней видно, какие провода используются при работе с моторами (обозначены сплошной линией).

И расположение контактов:

Нам нужны контакты C1 и C2. Способ подключения их к Motor shield каждый выбирает сам. Можно разрезать удлинитель Lego 8886. А можно распечатать на 3D принтере основу и сделать самостоятельно. У меня получились такие переходники на Lego-моторы.

На схеме ниже показано, как подключать моторы и питание (9V) к Motor Shield. Контролировать их будет Raspberry Pi, которая обозначена как MCU I/O.

2. Питание для моторов

Lego моторы питаются от блока с батарейками напряжением 9V. Для того, чтоб моторы хорошо работали от аккумуляторов (1.2V), нужно использовать 7 и более элементов. В моём случае используется блок на 8 штук.

3. Управление моторами

После подготовки моторов их нужно подключить к Raspberry Pi и попробовать ими управлять. Для этой цели на плате есть набор контактов GIPO, с помощью  библиотеки PI4J можно подавать на их высокий или низкий уровень напряжения.

В проекте используются контакты GIPO 0-3.

Ниже представлен класс для работы с двигателями.

package info.privateblog.gpio;

import com.pi4j.io.gpio.GpioController;
import com.pi4j.io.gpio.GpioFactory;
import com.pi4j.io.gpio.GpioPinDigitalOutput;
import com.pi4j.io.gpio.RaspiPin;

public class GPIOUtil {
    private static final GpioController gpio = GpioFactory.getInstance();

    private static final GpioPinDigitalOutput rightBackward = gpio.provisionDigitalOutputPin(RaspiPin.GPIO_00);
    private static final GpioPinDigitalOutput rightForward = gpio.provisionDigitalOutputPin(RaspiPin.GPIO_01);
    private static final GpioPinDigitalOutput leftBackward = gpio.provisionDigitalOutputPin(RaspiPin.GPIO_02);
    private static final GpioPinDigitalOutput leftForward = gpio.provisionDigitalOutputPin(RaspiPin.GPIO_03);

    public static void forward(int timeout) {
        rightForward.high();
        leftForward.high();
        sleep(timeout);
        rightForward.low();
        leftForward.low();
    }
    
    public static void backward(int timeout) {
    	rightBackward.high();
    	leftBackward.high();
    	sleep(timeout);
        rightBackward.low();
        leftBackward.low();
    }
    
    public static void left(int timeout) {
        rightForward.high();
        leftBackward.high();
        sleep(timeout);
        rightForward.low();
        leftBackward.low();
    }

    public static void right(int timeout) {
        rightBackward.high();
        leftForward.high();
        sleep(timeout);
        rightBackward.low();
        leftForward.low();
    }
    
    public static void sleep(int timeout) {
        try {
		Thread.currentThread().sleep(timeout);
	} catch (InterruptedException e) {}    	
    }
}

4. Камера

В простом варианте можно было ограничиться только управлением по Bluetooth и сделать всё на Arduino Nano, но это слишком просто. Хочется какой-то изюминки и это будет вещание картинки. Для получения изображения я буду использовать камеру Logitech C270.

Ранее я уже описывал свою версию библиотеки для работы с камерой на Raspberry. Но у неё выяснился серьёзный недостаток — утечка памяти, которая обездвиживает вездеход уже через несколько минут. Поэтому своей версии пришлось пока отказаться и вернуться к OpenCV:

sudo apt-get install libopencv-dev

Сборка моего проекта для Raspberry осуществляется на Windows, поэтому я скопировал c Raspberry файлы opencv-249.jar и libopencv_java249.so в проект, как показано на картинке. При экспорте проекта в виде запускаемого jar все библиотеки оказываются в одном файле и это избавляет от хлопот с копированием проекта обратно на Raspberry.
Для работы с камерой нужно загрузить библиотеку OpenCV и подготовить объект класса org.opencv.highgui.VideoCapture. Bluetooth не достаточно быстрый интерфейс, поэтому картинка ограничена размером 320×240.

static {
  try {
        System.loadLibrary("opencv_java249");
  } catch (UnsatisfiedLinkError e) {
    try {
        NativeUtils.loadLibraryFromJar("/libopencv_java249.so");
    } catch (IOException e1) {
        throw new RuntimeException(e1);
    }
  }
}

public static VideoCapture camera = new VideoCapture(0);      

static {
    camera.set(Highgui.CV_CAP_PROP_FRAME_WIDTH, WIDTH);      
    camera.set(Highgui.CV_CAP_PROP_FRAME_HEIGHT, HEIGHT); 
}

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

String time = sdf.format(new Date());
if (LegoRobot.camera.read(frame)) {
	BufferedImage out = VideoUtil.convertMatToBufferedImage(frame);
	Graphics graphics = out.getGraphics();
	graphics.drawString(time, 5, 13);
		        	
	outStream.reset();
		        	
	ImageIO.write(out, "jpg", outStream);
	byte[]result = outStream.toByteArray();
...

5. Bluetooth

Настройку Bluetooth на Raspberry я уже описывал. Этого достаточно, чтоб потом подключиться с Android, т.к. там есть метод для работы через протокол RFCOMM, но недостаточно, чтоб управлять с ноутбука. Windows отказывается видеть bluetooth без сервиса SerialPort. Поэтому его придётся добавить:

sudo nano /etc/systemd/system/dbus-org.bluez.service

и убедиться, что в файле прописаны следующие строки

ExecStart=/usr/lib/bluetooth/bluetoothd -C
ExecStartPost=/usr/bin/sdptool add SP

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

sdptool records local

На консоли должна появиться запись типа такой:

Service Name: Serial Port
Service Description: COM Port
Service Provider: BlueZ
Service RecHandle: 0x10006
Service Class ID List:
«Serial Port» (0x1101)
Protocol Descriptor List:
«L2CAP» (0x0100)
«RFCOMM» (0x0003)
Channel: 1
Language Base Attr List:
code_ISO639: 0x656e
encoding: 0x6a
base_offset: 0x100
Profile Descriptor List:
«Serial Port» (0x1101)
Version: 0x0100

6. Программа для Raspberry PI.

Многие моменты уже были описаны выше (используется библиотека Bluecove). Весь код будет выложен на github, здесь будет приведён только кусок для создания Bluetooth сервера на Raspberry

try {
    local = LocalDevice.getLocalDevice();
    local.setDiscoverable(DiscoveryAgent.GIAC);
            
    UUID uuid = new UUID(80087355);
    String url = "btspp://localhost:" + uuid.toString() + ";name=RemoteBluetooth;authenticate=false";
    notifier = (StreamConnectionNotifier)Connector.open(url);
} catch (Exception e) {
    e.printStackTrace();
    return;
}

while(true) {
    try {
       System.out.println("waiting for connection...");
       connection = notifier.acceptAndOpen();

       Thread processThread = new Thread(new ProcessConnectionThread(connection));
       processThread.start();
    } catch (Exception e) {
       e.printStackTrace();
       return;
    }
}

7. Bluetooth клиент

У меня не получилось настроить клиентскую программу на BlueCove, поэтому пришлось прибегнуть к хитрости и настроить соединение как Serial порт. Это упрощает работу и позволяет использовать ту же программу для работы с Arduino.

 

Пример создания подключения показан ниже.

logger.log(Level.INFO, "Toggle clicked");

serialPort = (SerialPort) comPort.open("SimpleReadApp", 1000);
inputStream = serialPort.getInputStream();
outputStream = serialPort.getOutputStream();
	
serialPort.setSerialPortParams(921600,
        SerialPort.DATABITS_8,
        SerialPort.STOPBITS_1,
        SerialPort.PARITY_NONE);
	
thread = new UARTThread();
thread.setInputStream(inputStream);
thread.addImageLoadedEvents(cameraView);
thread.start();

8. Питание Raspberry Pi

Питание вездехода осуществляется от Power Bank в 20000mA, который позволяет системе работать более 6 часов. Плюсом оказалось то, что он прекрасно вписался в корпус из Lego.

9. Запуск

Теперь можно скомпилировать серверную часть, экспортировать в виде запускаемого jar:

Запускаем на Raspberry Pi:

sudo java -jar ./legorobot.jar &

После этого можно запустить клиента и управлять через com порт. В этого получится вот это:

Код доступен тут:

https://github.com/Kanaris/LegoVehicle