// служит количеством отфильтрованных контуров++;= angleCount + angle;
}
}
//выводим количество стартовnumberOfFingers = Integer.toString(defectFilteredCounter);textPoint = new Point (20, 80);
// среднее арифметическое углов
double angleThresh = angleCount/defectFilteredCounter;
//переводим в цвет.cvtColor(frame, frame, Imgproc.COLOR_GRAY2BGR);
// если среднее арифметическое угла больше определенного значения, цвет меняется
if (angleThresh > 0.75 || defectFilteredCounter <= 5)
{.putText(frame, numberOfFingers, textPoint, 1, 6, new Scalar(0, 255, 0), 4);.drawContours(frame, finalContours, -1, new Scalar(255, 0, 0), 5);
}
else
{.drawContours(frame, finalContours, -1, new Scalar(0, 0, 255), 5);
}
// обводящий контур.drawContours(frame, hullmop, -1, new Scalar(0, 0, 255), 2);
// точки старт и дефект обозначаются кругами разных цветов
for (int i = 0; i < defectFilteredCounter; i++)
{.circle(frame, defectFiltered [i], 5, new Scalar(0, 255, 0), 5);
}
for (int i = 0; i < defectFilteredCounter; i++)
{.circle(frame, startFiltered [i], 5, new Scalar(0, 255, 255), 5);
}
//делаем байт
if (angleThresh > 0.75 || defectFilteredCounter <= 5)
{= (byte)127;
}
if (angleThresh > 0.75)
{= (byte)defectFilteredCounter;
}
}
}.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)
{.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)
{.onFXThread(view.imageProperty(), image);
}
// При закрытии приложения останавливаем захват:
protected void setClosed()
{
this.stopAcquisition();.close();
}
}.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;
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() {portId = null;portEnum = CommPortIdentifier.getPortIdentifiers();
while (portEnum.hasMoreElements()) {currPortId = (CommPortIdentifier) portEnum.nextElement();
for (String portName : PORT_NAMES) {
if (currPortId.getName().equals(portName)) {= currPortId;
break;
}
}
}
try {
// открываем серийный порт= (SerialPort) portId.open(this.getClass().getName(),TIME_OUT);
// устанавливаем параметры порта.setSerialPortParams(DATA_RATE,.DATABITS_8,.STOPBITS_1,.PARITY_NONE);
// открываем входные и выходные потоки= serialPort.getInputStream();= serialPort.getOutputStream();
// добавляем слушаетелей событий
serialPort.addEventListener(this);.notifyOnDataAvailable(true);
} catch (Exception e) {.err.println(e.toString());
}
}
// метод для закрытия порта
public synchronized void close() {
if (serialPort != null) {.removeEventListener();
serialPort.close();
}
}
// метод для отправки одного байта
public void sendSingleByte(byte myByte){
try {.write(myByte);.flush();
} catch (Exception e) {.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){.out.println(value);
}
} catch (Exception e) {.err.println(e.toString());
}
}
}
}
Приложение 2
передатчик
#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 и 10radio(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)button1F; // флажки кнопок (F - Flag)button1R; // флажки кнопок на отпускание (R - Release)button1P; // флажки кнопок на нажатие (P - Press)button1H; // флажки кнопок на удержание (H - Hold)button1D; // флажки кнопок на двойное нажатие (D - Double)button1DP; // флажки кнопок на двойное нажатие и отпускание (D - Double Pressed)
#define double_timer 100 // время (мс), отведённое на второе нажатие
#define hold 500 // время (мс), после которого кнопка считается зажатой
#define debounce 80 // (мс), антидребезгlong button1_timer; // таймер последнего нажатия кнопки
unsigned long button1_double; // таймер двойного нажатия кнопки
// библиотека для работы I²C
#include <Wire.h>
// библиотека для работы с модулями IMU
#include <TroykaIMU.h>
// множитель фильтра Магвика
#define BETA 0.22
// создаём объект для работы с акселерометромaccel;
// создаём объект для работы с гироскопомgyro;
// создаём объект для работы с компасомcompass;
// создаём объект для фильтра Магвика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] = {
.21,
.214,
.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);.println("Begin init...");
// инициализация акселерометра.begin();
// инициализация гироскопа.begin();
// инициализация компаса.begin();
// инициализация радиомодуля
// активировать модуль.begin();
// режим подтверждения приёма, 1 вкл 0 выкл.setAutoAck(1);
// (время между попыткой достучаться, число попыток).setRetries(0, 15);
// разрешить отсылку данных в ответ на входящий сигнал.enableAckPayload();
// размер пакета, в байтах.setPayloadSize(32);
// открываем канал для передачи данных для "трубы" 0
radio.openWritingPipe(address[0]);
// выбираем канал.setChannel(0x60);
// уровень мощности передатчика
radio.setPALevel (RF24_PA_MAX);
// скорость обмена.setDataRate (RF24_250KBPS); .
// начать работу.powerUp();
// модуль не прослушивает эфир, а передает
radio.stopListening();
// калибровка компаса.calibrateMatrix(compassCalibrationMatrix, compassCalibrationBias);
// выводим сообщение об удачной инициализации.println("Initialization completed");
}
void loop()
{
// опрос кнопокS = digitalRead(buttonPin);
//отработка кнопок();
// отработка режимов кнопки
if (button1P)
{
// переключается режим готовности= !check;.println("pressed");
// центрируется рыскание= yaw;P = 0;
}
// канал газа получает данные с дальномера
throttle = sonar.ping();
// данные фильтруются= kalman(throttle);
// запоминаем текущее времяlong startMillis = millis();
// считываем данные с акселерометра в единицах G
accel.readGXYZ(&ax, &ay, &az);
// считываем данные с гироскопа в радианах в секунду.readRadPerSecXYZ(&gx, &gy, &gz);
// считываем данные с компаса в Гауссах
compass.readCalibrateGaussXYZ(&mx, &my, &mz);
// устанавливаем коэффициенты фильтра.setKoeff(fps, BETA);
// обновляем входные данные в фильтр.update(gx, gy, gz, ax, ay, az, mx, my, mz);
// получение углов yaw, pitch и roll из фильтра
yaw = filter.getYawDeg();= filter.getPitchDeg();
roll = filter.getRollDeg();
// преобразование данных
// рыскание
sendYaw = -3.175*(yaw - yawCenter);
if (sendYaw >= 127) sendYaw = 127;
if (sendYaw <= -127) sendYaw = -127;
// газ= 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)
{.print("! ");.print("yaw: ");.print(yaw);.print("\t\t");.print("pitch: ");.print(pitch);.print("\t\t");.print("roll: ");.print(roll);.print("\t\t");.print("throttle: ");.println(throttle);
}
else
{.print("yaw: ");.print(sendYaw);.print("\t\t");.print("pitch: ");.print(sendPitch);.print("\t\t");.print("roll: ");.print(sendRoll);.print("\t\t");.print("throttle: ");.println(sendThrottle);
}
// принимаем данные из серийного порта
if (Serial.available())
{= (byte)Serial.read();
}
// "разбираем" байт на соответсвующие величины
if ((int)getByte == 127) {= 255;= 0;
}
else {= 0;= (int)getByte * 86;
if (mode2 > 255) {= 255;
}
}
// вычисляем затраченное время на обработку данных
unsigned long deltaMillis = millis() - startMillis;
// вычисляем частоту обработки фильтра= 1000 / deltaMillis;
// при готовности отправляем данные на приемник
if (check == true)
{.write(&sendThrottle, sizeof(sendThrottle));.write(&sendYaw, sizeof(sendYaw));.write(&sendPitch, sizeof(sendPitch));.write(&sendRoll, sizeof(sendRoll));.write(&mode1, sizeof(mode1));.write(&mode2, sizeof(mode2));
}(5);
}
//функция фильтрации
float kalman (float val) {= P + varProcess;= Pc/(Pc + varVolt);= (1-G)*Pc;= Xe;= Xp;
// "фильтрованное" значение
Xe = G*(val-Zp)+Xp;
return(Xe);
}
void buttons() {
// нажатие (с антидребезгом)
if (button1S && !button1F && millis() - button1_timer > debounce) {
button1F = 1;_timer = millis();
}
// если отпустили до hold, считать отпущенной
if (!button1S && button1F && !button1R && !button1DP && millis() - button1_timer < hold) {R = 1;F = 0;_double = millis();
}
// если отпустили и прошло больше double_timer, считать 1 нажатием
if (button1R && !button1DP && millis() - button1_double > double_timer) {
button1R = 0;P = 1;
}
// если отпустили и прошло меньше double_timer и нажата снова, считать что нажата 2 раз
if (button1F && !button1DP && button1R && millis() - button1_double < double_timer) {
button1F = 0;R = 0;DP = 1;
}
// если была нажата 2 раз и отпущена, считать что была нажата 2 раза
if (button1DP && millis() - button1_timer < hold) {
button1DP = 0;D = 1;
}
// Если удерживается более hold, то считать удержанием
if (button1F && !button1D && !button1H && millis() - button1_timer > hold) {
button1H = 1;
}
// Если отпущена после hold, то считать, что была удержана
if (!button1S && button1F && millis() - button1_timer > hold) {F = 0;H = 0;
}
}приемник
// библиотеки для радиосвязи
#include <SPI.h> // библиотека для работы с шиной SPI
#include "nRF24L01.h" // библиотека радиомодуля
#include "RF24.h" // ещё библиотека радиомодуля
// создать объект на пинах 7 и 8radio(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
{
// открываем последовательный порт.begin(115200);
// инициализация радиомодуля
// активировать модуль.begin();
// режим подтверждения приёма, 1 вкл 0 выкл.setAutoAck(1);
// время между попыткой достучаться, число попыток.setRetries(0, 15);