Від Гвинта! Або РПО ВІШ на Arduino



Запропонували мені зробити прототип регулятора постійних оборотів (РПЗ) для прототипу гвинт змінюваного кроку (ВІШ) невеликого літального апарату (ЛА).

Що маємо:
ЛА — Аэропоракт А-27М, двигун Rotax 912, прототип ВІШ з електричним приводом.

Що потрібно:
Виготовити прототип РПЗ, який повинен вміти підтримувати заздалегідь задані оберти — 3 режими, мати можливість в ручну управляти кроком гвинта, мати індикатори збільшення\зменшення кроку гвинта і індикатори спрацьовування кінцевих вимикачів.



Завдання РПО зводиться до зміни кута установки лопатей (крок гвинта) в залежності від режиму польоту, таким чином, щоб оберти гвинта завжди залишалися постійними. В залежності від швидкості горизонтального польоту крок гвинта змінюється від ᵠ хв. до ᵠ макс. На злітному режимі при нульовій горизонтальній швидкості крок гвинта мінімальний. По мірі збільшення горизонтальної швидкості кут атаки лопаті зменшується (кут атаки на малюнку не показаний, але він приблизно дорівнює ᵠ хв.). Зменшується опір обертанню — гвинт «полегшується». Обороти двигун починають зростати, РПЗ збільшує крок гвинта, при цьому збільшується опір обертанню гвинта — гвинт «затяжеляется». Обороти двигун падають. Так РПО підтримує задані оберти.



Зміна кута установки лопатей, проводиться за допомогою електричного приводу. Для прийняття рішення про те, в яку сторону, і з якою швидкістю обертати ел. двигун приводу, потрібно знати тільки дельту, між поточною швидкістю і заданої. РПО буде прагнути утримувати дельту в діапазоні від -60 до 60. При негативній дельті — зменшується кут установки лопаті, при позитивної — збільшується. В ідеалі РПО повинен прагнути тримати нульове значення, але на практиці це неможливо з багатьох причин.

Для більшої ефективності, швидкість зміни кута установки, змінюється:

якщо дельта більше |150| швидкість MAX — максимальна
якщо дельта більше |60| швидкість MIN — мінімальна



За основу регулятора, було взято Arduino. Я не став вдаватися до технології Shieldsbuilding, а просто взяв голий мікроконтролер, і залив туди завантажувач від Arduino. Використовував альтернативний завантажувач з пакету optiboot. Стандартний завантажувач некоректно працює з watchdog.

В якості силової частини була використана пара напівмостів BTS7960. Це дуже сильно з запасом, але оскільки я прагнув зробити по максимуму надійну схему, вибір припав на них. Сигнал тахометра береться штатний з двигуна, підключив через оптрон 6N137, світлодіод D6, гасить імпульси зворотної полярності. Датчик струму ACS712-05A, використовується тільки для визначення спрацьовування кінцевих вимикачів. Ручне керування установки кроку гвинта, реалізовано програмно, це не дуже добре, але для першого прототипу цілком згодиться. Перемикач вибору режиму роботи, на рівні електричної схеми реалізований так, щоб при всіх невизначених положеннях (інтервал між сусідніми секціями контактів), інтерпретувався як ручний режим.

Плата була виготовлена по технології Oracal, двостороння, мені так легше розводити. Елементи з монтажем в отвори, активно використовувалися для переходу на іншу сторону плати. Оскільки плата не складна, розведення робив прямо в CorelDRAW. Малював лініями потрібної товщини, потім перетворював їх у контур. Дуже зручно малювати по сітці, в режимі точок, з кроком 1.27 мм, з включеною прив'язкою до сітки. Підготовлені контури вирізав за допомогою плоттера, на плівці Oracal. Монтажної плівкою переніс на заздалегідь підготовлений текстоліт. Трохи кропіткої роботи з видалення непотрібних частин плівки, і плата готова до травленню. Далі все стандартно.

Корпус був виготовлений власноруч власником ЛА, з листового полістиролу товщиною 2мм. Для екранування від перешкод, був обклеєний алюмінієвою фольгою і підключений до загального проводу.

Для підбору оптимальних значень параметрів РПЗ, була успішно проведена серія наземних і льотних випробувань.

Скетч:
#include <avr/wdt.h> // Підключаємо бібліотеки wdt

volatile unsigned long micros_prev = 0; // Предидущее значення мікросекунд
long rpm_sum = 0; // Сума буде приділятися розміру накопичених N значень оборотів
int rpm_val = 0; // Лічильник N
int a = 0; // Тахометр
int rpm_go = 0; // Задані оберти
int rpm_select = 0; // Перемикач режимів
int rotor_left = 0; // Прапор обертання в ліво
int rotor_right = 0; // Прапор обертання право
int current = 0; // Датчик струму
int current_left = 0; // Прапор факту наявності обертання в ліво
int current_right = 0; // Прапор факту наявності обертання право

void setup() {
wdt_disable(); // Відключаємо wdt
pinMode(2, INPUT); digitalWrite(2, HIGH); // RPM Сигнал з тахометра і підтяжка до +
pinMode(4, INPUT); // IS Помилка драйвера (обробка сигналу не реалізована)
pinMode(7, INPUT); digitalWrite(7, HIGH); // Step_down Малий крок і підтяжка до +
pinMode(8, INPUT); digitalWrite(8, HIGH); // Step_up Великий кроки підтяжка до +
pinMode(13, OUTPUT); // Світлодіод - Ручний режим "червоний"
pinMode(3, OUTPUT); // INH Дозволити крутити ел.двигун
pinMode(5, OUTPUT); // IN 1-ий напівміст
pinMode(6, OUTPUT); // IN 2-ой напівміст
pinMode(12, OUTPUT); // Світлодіод - Великий крок "зелений"
pinMode(11, OUTPUT); // Світлодіод - Малий крок "зелений"
pinMode(10, OUTPUT); // Світлодіод - Великий крок відбійник "червоний" упор
pinMode(9, OUTPUT); // Світлодіод - Малий крок відбійник "червоний" упор
attachInterrupt(0, RPM_IN, FALLING); // Активуємо зовнішнє переривання
wdt_enable (WDTO_2S); // Включаємо wdt з інтервалом 2 секунди
}

void loop() {
wdt_reset(); // Сбрасывам лічильник wdt
MODE();
CURR();
LED();
}

// Функція обертання в ліво
void LEFT() {
digitalWrite(5, HIGH);
digitalWrite(6, LOW);
digitalWrite(3, HIGH);
}

// Функція обертання в ліво - швидкість "MAX" (ШІМ)
void LEFT_MAX() {
digitalWrite(5, HIGH);
digitalWrite(6, LOW);
analogWrite(3, 200);
}

// Функція обертання в ліво - швидкість "MIN" (ШІМ)
void LEFT_MIN() {
digitalWrite(5, HIGH);
digitalWrite(6, LOW);
analogWrite(3, 120);
}

// Функція обертання право
void RIGHT() {
digitalWrite(5, LOW);
digitalWrite(6, HIGH);
digitalWrite(3, HIGH);
}

// Функція обертання право - швидкість "MAX" (ШІМ)
void RIGHT_MAX() {
digitalWrite(5, LOW);
digitalWrite(6, HIGH);
analogWrite(3, 200);
}

// Функція обертання право - швидкість "MIN" (ШІМ)
void RIGHT_MIN() {
digitalWrite(5, LOW);
digitalWrite(6, HIGH);
analogWrite(3, 120);
}

// Функція зупинки ел.двигуна з гальмуванням
void STOP() {
digitalWrite(5, LOW);
digitalWrite(6, LOW);
digitalWrite(3, HIGH);
}

// Функція ручного режиму
void HAND() {
if (digitalRead(7) == LOW) {
LEFT(); // Крутимо ел.двигун в ліво якщо натиснута кнопка в ліво
rotor_left = 1;
} else {
rotor_left = 0;
}
if (digitalRead(8) == LOW) {
RIGHT(); // Крутимо ел.двигун право якщо натиснута кнопка в право
rotor_right = 1;
} else {
rotor_right = 0;
}
if (rotor_left == 0 && rotor_right == 0) {
STOP(); // Зупинка ел.двигуна якщо не натиснута жодна кнопка
}
}

// Функція автоматичного режиму
void AUTO() {
if ((micros() - micros_prev) > 1000000) {
rpm = 0;
}
// Відпрацювання обертання в ліво
if (a < rpm_go - 60) {
if (a < rpm_go - 150) {
LEFT_MAX ();
} else {
LEFT_MIN ();
};
rotor_left = 1;
} else {
rotor_left = 0;
}
// Відпрацювання обертання право
if (rpm > rpm_go + 60) {
if (rpm > rpm_go + 150) {
RIGHT_MAX ();
} else {
RIGHT_MIN ();
};
rotor_right = 1;
} else {
rotor_right = 0;
}
// Відпрацювання діапазону "stop"
if (rotor_left == 0 && rotor_right == 0) {
STOP();
}
}

// Функція обробки перемикача режимів роботи
void MODE() {
rpm_select = analogRead(0);
if (rpm_select < 100) {
rpm_go = 0; // Ручний режим
} else {
if (rpm_select > 800) {
rpm_go = 5700; // Автоматичний режим "Зліт/Посадка"
} else {
if (rpm_select > 450) {
rpm_go = 5300; // Автоматичний режим "Набір висоти"
} else {
rpm_go = 4600; // Автоматичний режим "Круїз"
}
}
}
if (rpm_go == 0){
HAND ();
} else {
AUTO ();
}
}

// Функція розпізнавання напрямку обертання ел.двигуна
void CURR() {
for (int i = 0; i < 4; i++) {
current = current + analogRead(1);
}
current = current / 5;
if (current < 496) {
current_left = 1;
} else {
current_left = 0;
}
if (current > 516) {
current_right = 1;
} else {
current_right = 0;
}
}

// Функція відпрацювання світлодіодів
void LED() {
if (rpm_go == 0) {
digitalWrite(13, HIGH); // Ручний режим
} else {
digitalWrite(13, LOW);
}
// Якщо ел.двигун крутимо в ліво а обертання немає, то включаємо червоний світлодіод "Малий крок" упор
if (rotor_left == 1 && current_left == 0) {
digitalWrite(11, LOW);
digitalWrite(9, HIGH);
}
// Якщо ел.двигун крутимо в ліво, то включаємо зелений світлодіод "Малий крок"
else {
if (rotor_left == 1) {
digitalWrite(11, HIGH);
digitalWrite(9, LOW);
}
else {
digitalWrite(11, LOW);
digitalWrite(9, LOW);
}
}
// Якщо ел.двигун крутимо в право а обертання немає, то включаємо червоний світлодіод "крок" упор
if (rotor_right == 1 && current_right == 0) {
digitalWrite(12, LOW);
digitalWrite(10, HIGH);
}
// Якщо ел.двигун крутимо в право, то включаємо зелений світлодіод "крок"
else {
if (rotor_right == 1) {
digitalWrite(12, HIGH);
digitalWrite(10, LOW);
}
else {
digitalWrite(12, LOW);
digitalWrite(10, LOW);
}
}
}

// Функція обробки сигналу з тахометра з усередненням даних
void RPM_IN() {
rpm_sum = rpm_sum + (60000000.0 / (micros() - micros_prev));
micros_prev = micros(); rpm_val++;
if (rpm_val >= 10) {
rpm = rpm_sum / 10;
rpm_sum = 0;
rpm_val = 0;
}
}


Джерело: Хабрахабр

0 коментарів

Тільки зареєстровані та авторизовані користувачі можуть залишати коментарі.