Автоматизация системы управления квадрокоптера

convDef.add(new MatOfInt4());

// функция ищет дефекты

Imgproc.convexityDefects(finalContours.get(0), convexHull.get(0),

convDef.get(0));

// оставляем только первый

cdList = convDef.get(0).toList();

Point data[] = finalContours.get(0).toArray();

// разбираем дефекты на состанвые точки

Point defect[] = new Point[20];

Point start[] = new Point[20];

Point end [] = new Point [20];

int defectCounter = 0;

int defectFilteredCounter = 0;

// переносим в массив точек

for (int j = 0; j < cdList.size(); j = j + 4)

{

defect[defectCounter] = data[cdList.get(j + 2)];

end [defectCounter] = data[cdList.get(j + 1)];

start [defectCounter] = data[cdList.get(j)];

defectCounter++;

}

// массивы для отфильтрованных точек

Point defectFiltered[] = new Point[15];

Point startFiltered[] = new Point[15];

// переменная для угла и ее счетчик

double angle;

double angleCount = 0;

// фильтрация

for (int i = 0; i < defectCounter; i++)

{

// координаты, по которым считается угол

double dx1 = Math.abs (start[i].x - defect[i].x);

double dy1 = Math.abs (start[i].y - defect[i].y);

double dx2 = Math.abs (defect[i].x - end[i].x);

double dy2 = Math.abs (defect[i].y - end[i].y);

// вычисление угла

angle = (dx1*dx2 + dy1*dy2)/Math.sqrt((dx1*dx1 + dy1*dy1)*(dx2*dx2 + dy2*dy2));

// вычисление расстояния между соответствующими точками end и start

double lenght = Math.sqrt (Math.pow((start[i].x - end[i].x), 2) + Math.pow((start[i].y - end[i].y), 2);

// условие с подобранными значениями

if ((start[i].y < 450 || start[i].y > 50) && angle > 4.6 && lenght < 5.1)

{

// записываем точки в массив

defectFiltered [defectFilteredCounter] = defect [i];

startFiltered [defectFilteredCounter] = start [i];

// служит количеством отфильтрованных контуров

defectFilteredCounter++;

angleCount = angleCount + angle;

}

}

//выводим количество стартов

String numberOfFingers = Integer.toString(defectFilteredCounter);

Point textPoint = new Point (20, 80);

// среднее арифметическое углов

double angleThresh = angleCount/defectFilteredCounter;

//переводим в цвет

Imgproc.cvtColor(frame, frame, Imgproc.COLOR_GRAY2BGR);

// если среднее арифметическое угла больше определенного значения, цвет меняется

if (angleThresh > 0.75 || defectFilteredCounter <= 5)

{

Imgproc.putText(frame, numberOfFingers, textPoint, 1, 6, new Scalar(0, 255, 0), 4);

Imgproc.drawContours(frame, finalContours, -1, new Scalar(255, 0, 0), 5);

}

else

{

Imgproc.drawContours(frame, finalContours, -1, new Scalar(0, 0, 255), 5);

}

// обводящий контур

Imgproc.drawContours(frame, hullmop, -1, new Scalar(0, 0, 255), 2);

// точки старт и дефект обозначаются кругами разных цветов

for (int i = 0; i < defectFilteredCounter; i++)

{

Imgproc.circle(frame, defectFiltered [i], 5, new Scalar(0, 255, 0), 5);

}

for (int i = 0; i < defectFilteredCounter; i++)

{

Imgproc.circle(frame, startFiltered [i], 5, new Scalar(0, 255, 255), 5);

}

//делаем байт

if (angleThresh > 0.75 || defectFilteredCounter <= 5)

{

sendByte = (byte)127;

}

if (angleThresh > 0.75)

{

sendByte = (byte)defectFilteredCounter;

}

}

}

serial.sendSingleByte(sendByte);

// возвращаем обработанный кадр

return frame;

}

// остановить захват и отпустить все ресурсы

private void stopAcquisition()

{

if (this.timer!=null && !this.timer.isShutdown())

{

try

{

// остановить таймер

this.timer.shutdown();

this.timer.awaitTermination(33, TimeUnit.MILLISECONDS);

}

catch (InterruptedException e)

{

System.err.println("Exception in stopping the frame capture, trying to release the camera now... " + e);

}

}

if (this.capture.isOpened())

{

// отпустить камеру

this.capture.release();

}

}

// обновляем {@link ImageView} в основной тред JavaFX

private void updateImageView(ImageView view, Image image)

{

Utils.onFXThread(view.imageProperty(), image);

}

// При закрытии приложения останавливаем захват:

protected void setClosed()

{

this.stopAcquisition();

serial.close();

}

}

Serial.java

package sample;

// импортирование средств ввода-вывода

import gnu.io.CommPortIdentifier;

import gnu.io.SerialPort;

import gnu.io.SerialPortEvent;

import gnu.io.SerialPortEventListener;

import java.io.InputStream;

import java.io.OutputStream;

import java.util.Enumeration;

public class Serial implements SerialPortEventListener

{

// создание объектов и констант

SerialPort serialPort;

private InputStream input;

private OutputStream output;

private static final int TIME_OUT = 2000;

private static final int DATA_RATE = 115200;

private static final String PORT_NAMES[] = {

"/dev/tty.usbserial-A9007UX1", // Mac OS X

"/dev/ttyUSB0", // Linux

"COM4", // Windows

};

// инициализация

public void initialize() {

CommPortIdentifier portId = null;

Enumeration portEnum = CommPortIdentifier.getPortIdentifiers();

while (portEnum.hasMoreElements()) {

CommPortIdentifier currPortId = (CommPortIdentifier) portEnum.nextElement();

for (String portName : PORT_NAMES) {

if (currPortId.getName().equals(portName)) {

portId = currPortId;

break;

}

}

}

try {

// открываем серийный порт

serialPort = (SerialPort) portId.open(this.getClass().getName(),TIME_OUT);

// устанавливаем параметры порта

serialPort.setSerialPortParams(DATA_RATE,

SerialPort.DATABITS_8,

SerialPort.STOPBITS_1,

SerialPort.PARITY_NONE);

// открываем входные и выходные потоки

input = serialPort.getInputStream();

output = serialPort.getOutputStream();

// добавляем слушаетелей событий

serialPort.addEventListener(this);

serialPort.notifyOnDataAvailable(true);

} catch (Exception e) {

System.err.println(e.toString());

}

}

// метод для закрытия порта

public synchronized void close() {

if (serialPort != null) {

serialPort.removeEventListener();

serialPort.close();

}

}

// метод для отправки одного байта

public void sendSingleByte(byte myByte){

try {

output.write(myByte);

output.flush();

} catch (Exception e) {

System.err.println(e.toString());

}

}

// метод для приема данных (не используется)

public synchronized void serialEvent (SerialPortEvent oEvent) {

if (oEvent.getEventType() == SerialPortEvent.DATA_AVAILABLE) {

try {

int myByte=input.read();

// перевод из типа byte в int

int value = myByte & 0xff;

if(value>=0 && value<256){

System.out.println(value);

}

} catch (Exception e) {

System.err.println(e.toString());

}

}

}

}



Приложение 2

Arduino-передатчик

#include <NewPing.h>

// объявление пинов для дальномера

#define TRIGGER_PIN 7

#define ECHO_PIN 8

// максимальная дистанция в сантиметрах, на которую реагирует дальномер

#define MAX_DISTANCE 200

// создаем объект дальномера

NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);

// библиотеки для радиосвязи

#include <SPI.h> // библиотека для работы с шиной SPI

#include "nRF24L01.h" // библиотека радиомодуля

#include "RF24.h" // ещё библиотека радиомодуля

// создать объект на пинах 9 и 10

RF24 radio(9, 10);

//возможные номера труб

byte address[][6] = {"1Node", "2Node", "3Node", "4Node", "5Node", "6Node"};

// Инициализация функции фильтрации

float kalman (float val);

// функции обработки кнопок

void buttons();

// Объявление переменных для фильтра Калмана

float varVolt = 21.47; // среднее отклонение (ищем в excel)

float varProcess = 0.02; // скорость реакции на изменение (подбирается вручную)

float Pc = 0.0;

float G = 0.0;

float P = 1.0;

float Xp = 0.0;

float Zp = 0.0;

float Xe = 0.0;

// Конец объявления переменных для фильтра Калмана

// Переменные для кнопки

int buttonPin = 6;

boolean button1S; // храним состояния кнопок (S - State)

boolean button1F; // флажки кнопок (F - Flag)

boolean button1R; // флажки кнопок на отпускание (R - Release)

boolean button1P; // флажки кнопок на нажатие (P - Press)

boolean button1H; // флажки кнопок на удержание (H - Hold)

boolean button1D; // флажки кнопок на двойное нажатие (D - Double)

boolean button1DP; // флажки кнопок на двойное нажатие и отпускание (D - Double Pressed)

#define double_timer 100 // время (мс), отведённое на второе нажатие

#define hold 500 // время (мс), после которого кнопка считается зажатой

#define debounce 80 // (мс), антидребезг

unsigned long button1_timer; // таймер последнего нажатия кнопки

unsigned long button1_double; // таймер двойного нажатия кнопки

// библиотека для работы IІC

#include <Wire.h>

// библиотека для работы с модулями IMU

#include <TroykaIMU.h>

// множитель фильтра Магвика

#define BETA 0.22

// создаём объект для работы с акселерометром

Accelerometer accel;

// создаём объект для работы с гироскопом

Gyroscope gyro;

// создаём объект для работы с компасом

Compass compass;

// создаём объект для фильтра Магвика

Madgwick filter;

// переменные для данных с гироскопа, акселерометра и компаса

float gx, gy, gz, ax, ay, az, mx, my, mz;

// получаемые углы ориентации (Эйлера)

float yaw, pitch, roll, throttle, yawCenter = 0;

// обработанные значения каналов

int sendYaw, sendPitch, sendThrottle, sendRoll;

// переменная для хранения частоты выборок фильтра

float fps = 100;

// перменные для приема из серийного порта

byte getByte;

int mode1, mode2;

//флаг готовности к передаче

bool check = false;

// калибровочные значения компаса

// полученные в калибровочной матрице из примера «compassCalibrateMatrixx»

const double compassCalibrationBias[3] = {

524.21,

3352.214,

-1402.236

};

const double compassCalibrationMatrix[3][3] = {

{1.757, 0.04, -0.028},

{0.008, 1.767, -0.016},

{-0.018, 0.077, 1.782}

};

void setup()

{

// задание режимов пинов

//пин для кнопки

pinMode(buttonPin, INPUT_PULLUP);

// открываем последовательный порт

Serial.begin(115200);

Serial.println("Begin init...");

// инициализация акселерометра

accel.begin();

// инициализация гироскопа

gyro.begin();

// инициализация компаса

compass.begin();

// инициализация радиомодуля

// активировать модуль

radio.begin();

// режим подтверждения приёма, 1 вкл 0 выкл

radio.setAutoAck(1);

// (время между попыткой достучаться, число попыток)

radio.setRetries(0, 15);

// разрешить отсылку данных в ответ на входящий сигнал

radio.enableAckPayload();

// размер пакета, в байтах

radio.setPayloadSize(32);

// открываем канал для передачи данных для "трубы" 0

radio.openWritingPipe(address[0]);

// выбираем канал

radio.setChannel(0x60);

// уровень мощности передатчика

radio.setPALevel (RF24_PA_MAX);

// скорость обмена

radio.setDataRate (RF24_250KBPS); .

// начать работу

radio.powerUp();

// модуль не прослушивает эфир, а передает

radio.stopListening();

// калибровка компаса

compass.calibrateMatrix(compassCalibrationMatrix, compassCalibrationBias);

// выводим сообщение об удачной инициализации

Serial.println("Initialization completed");

}

void loop()

{

// опрос кнопок

button1S = digitalRead(buttonPin);

//отработка кнопок

buttons();

// отработка режимов кнопки

if (button1P)

{

// переключается режим готовности

check = !check;

Serial.println("pressed");

// центрируется рыскание

yawCenter = yaw;

button1P = 0;

}

// канал газа получает данные с дальномера

throttle = sonar.ping();

// данные фильтруются

throttle = kalman(throttle);

// запоминаем текущее время

unsigned long startMillis = millis();

// считываем данные с акселерометра в единицах G

accel.readGXYZ(&ax, &ay, &az);

// считываем данные с гироскопа в радианах в секунду

gyro.readRadPerSecXYZ(&gx, &gy, &gz);

// считываем данные с компаса в Гауссах

compass.readCalibrateGaussXYZ(&mx, &my, &mz);

// устанавливаем коэффициенты фильтра

filter.setKoeff(fps, BETA);

// обновляем входные данные в фильтр

filter.update(gx, gy, gz, ax, ay, az, mx, my, mz);

// получение углов yaw, pitch и roll из фильтра

yaw = filter.getYawDeg();

pitch = filter.getPitchDeg();

roll = filter.getRollDeg();

// преобразование данных

// рыскание

sendYaw = -3.175*(yaw - yawCenter);

if (sendYaw >= 127) sendYaw = 127;

if (sendYaw <= -127) sendYaw = -127;

// газ

sendThrottle = 0.159375 * throttle - 63.75;

if (throttle >= 2000) sendThrottle = 255;

if (throttle <= 400) sendThrottle = 0;

// тангаж

if (pitch >= -180 && pitch < -140) sendPitch = 3.175 * pitch + 571.5;

if (pitch > 140 && pitch <= 180) sendPitch = 3.175 * pitch - 571.5;

if (pitch >= -140 && pitch < 0) sendPitch = 127;

if (pitch <= 140 && pitch > 0) sendPitch = -127;

// крен

if (roll >= - 180 && roll < -140) sendRoll = -3.175 * roll - 571.5;

if (roll > 140 && roll <= 180) sendRoll = -3.175 * roll + 571.5;

if (roll <= 140 && roll > 0) sendRoll = 127;

if (roll >= -140 && roll < 0) sendRoll = -127;

// выводим полученные углы в serial-порт

if (check == false)

{

Serial.print("! ");

Serial.print("yaw: ");

Serial.print(yaw);

Serial.print("\t\t");

Serial.print("pitch: ");

Serial.print(pitch);

Serial.print("\t\t");

Serial.print("roll: ");

Serial.print(roll);

Serial.print("\t\t");

Serial.print("throttle: ");

Serial.println(throttle);

}

else

{

Serial.print("yaw: ");

Serial.print(sendYaw);

Serial.print("\t\t");

Serial.print("pitch: ");

Serial.print(sendPitch);

Serial.print("\t\t");

Serial.print("roll: ");

Serial.print(sendRoll);

Serial.print("\t\t");

Serial.print("throttle: ");

Serial.println(sendThrottle);

}

// принимаем данные из серийного порта

if (Serial.available())

{

getByte= (byte)Serial.read();

}

// "разбираем" байт на соответсвующие величины

if ((int)getByte == 127) {

mode1 = 255;

mode2 = 0;

}

else {

mode1 = 0;

mode2 = (int)getByte * 86;

if (mode2 > 255) {

mode2 = 255;

}

}

// вычисляем затраченное время на обработку данных

unsigned long deltaMillis = millis() - startMillis;

// вычисляем частоту обработки фильтра

fps = 1000 / deltaMillis;

// при готовности отправляем данные на приемник

if (check == true)

{

radio.write(&sendThrottle, sizeof(sendThrottle));

radio.write(&sendYaw, sizeof(sendYaw));

radio.write(&sendPitch, sizeof(sendPitch));

radio.write(&sendRoll, sizeof(sendRoll));

radio.write(&mode1, sizeof(mode1));

radio.write(&mode2, sizeof(mode2));

}

delay(5);

}

//функция фильтрации

float kalman (float val) {

Pc = P + varProcess;

G = Pc/(Pc + varVolt);

P = (1-G)*Pc;

Xp = Xe;

Zp = Xp;

// "фильтрованное" значение

Xe = G*(val-Zp)+Xp;

return(Xe);

}

void buttons() {

// нажатие (с антидребезгом)

if (button1S && !button1F && millis() - button1_timer > debounce) {

button1F = 1;

button1_timer = millis();

}

// если отпустили до hold, считать отпущенной

if (!button1S && button1F && !button1R && !button1DP && millis() - button1_timer < hold) {

button1R = 1;

button1F = 0;

button1_double = millis();

}

// если отпустили и прошло больше double_timer, считать 1 нажатием

if (button1R && !button1DP && millis() - button1_double > double_timer) {

button1R = 0;

button1P = 1;

}

// если отпустили и прошло меньше double_timer и нажата снова, считать что нажата 2 раз

if (button1F && !button1DP && button1R && millis() - button1_double < double_timer) {

button1F = 0;

button1R = 0;

button1DP = 1;

}

// если была нажата 2 раз и отпущена, считать что была нажата 2 раза

if (button1DP && millis() - button1_timer < hold) {

button1DP = 0;

button1D = 1;

}

// Если удерживается более hold, то считать удержанием

if (button1F && !button1D && !button1H && millis() - button1_timer > hold) {

button1H = 1;

}

// Если отпущена после hold, то считать, что была удержана

if (!button1S && button1F && millis() - button1_timer > hold) {

button1F = 0;

button1H = 0;

}

}

Arduino-приемник

// библиотеки для радиосвязи

#include <SPI.h> // библиотека для работы с шиной SPI

#include "nRF24L01.h" // библиотека радиомодуля

#include "RF24.h" // ещё библиотека радиомодуля

// создать объект на пинах 7 и 8

RF24 radio(7, 8);

//возможные номера труб

byte address[][6] = {"1Node", "2Node", "3Node", "4Node", "5Node", "6Node"};

// инициализация выходов

#define THROTTLE_PIN 3

#define YAW_PIN 5

#define PITCH_PIN 6

#define ROLL_PIN 9

#define MODE1_PIN 10

#define MODE2_PIN 11

void setup

{

// открываем последовательный порт

Serial.begin(115200);

// инициализация радиомодуля

// активировать модуль

radio.begin();

// режим подтверждения приёма, 1 вкл 0 выкл

radio.setAutoAck(1);

// время между попыткой достучаться, число попыток

radio.setRetries(0, 15);

// разрешить отсылку данных в ответ на входящий сигнал

radio.enableAckPayload();

// размер пакета, в байтах

radio.setPayloadSize(32);

// открываем канал для передачи данных для "трубы" 0

radio.openWritingPipe(address[0]);

// выбираем канал

radio.setChannel(0x60);

// уровень мощности передатчика

radio.setPALevel (RF24_PA_MAX);

// скорость обмена

radio.setDataRate (RF24_250KBPS); .

// начать работу

radio.powerUp();

// модуль не прослушивает эфир, а передает

radio.stopListening();

// калибровка компаса

compass.calibrateMatrix(compassCalibrationMatrix, compassCalibrationBias);

// выводим сообщение об удачной инициализации

Serial.println("Initialization completed");

// приемный модуль начинает прослушивать эфир

radio.startListening();

}

void loop() {

byte pipeNo, sendThrottle, sendYaw, sendPitch, sendRoll, mode1, mode2;

// слушаем эфир со всех "труб"

while( radio.available(&pipeNo)){

{

radio.read(&sendThrottle, sizeof(sendThrottle));

radio.read(&sendYaw, sizeof(sendYaw));

radio.read(&sendPitch, sizeof(sendPitch));

radio.read(&sendRoll, sizeof(sendRoll));

radio.read(&mode1, sizeof(mode1));

radio.read(&mode2, sizeof(mode2));

}

//выводим данные

analogWrite (3, sendThrottle);

analogWrite (5, sendYaw);

analogWrite (6, sendPitch);

analogWrite (9, sendRoll);

analogWrite (10, mode1);

analogWrite (11, mode2);

}

}

Размещено на Allbest.ru