На закінчення 2020 року реалізовано програмне приведення сигналу в каналі "У" до "нуля" шляхом повороту вектора. Так як всі роботи "на столі", в даній модифікації функція запускається при старті системи; в реальному приладі вона повинна викликатись при натисканні "спеціально навченої кнопки".
Алгоритм досить простий:
-
знаходимо за таблицею Брадіса початковий кут зсуву сигналу розбалансу;
- за отриманим кутом з таблиці
intSinCos отримуємо значення синуса та косинуса для "повороту"
- здійснюємо сам поворот, за наведеною вище формулою.
Частини коду для виконання даного алгоритму:
Масив з таблицею
intSinCos:
Код: Виділити все
// Таблиця Брадіса із значеннями sin / cos домножених на 16384 (2 в 15 ступені)для цілочисельних розрахунків
int intSinCos[intBradisTableLength] = {
286, 572, 857, 1143, 1428, 1713, 1997, 2280, 2563, 2845, 3126, 3406, 3686, 3964, 4240,
4516, 4790, 5063, 5334, 5604, 5872, 6138, 6402, 6664, 6924, 7182, 7438, 7692, 7943, 8192,
8438, 8682, 8923, 9162, 9397, 9630, 9860, 10087, 10311, 10531, 10749, 10963, 11174, 11381, 11585,
11786, 11983, 12176, 12365, 12551, 12733, 12911, 13085, 13255, 13421, 13583, 13741, 13894, 14044, 14189,
14330, 14466, 14598, 14726, 14849, 14968, 15082, 15191, 15296, 15396, 15491, 15582, 15668, 15749, 15826,
15897, 15964, 16026, 16083, 16135, 16182, 16225, 16262, 16294, 16322, 16344, 16362, 16374, 16382, 16384
};
Змінні для здійснення повороту:
Код: Виділити все
// Корекція фази (поворот вектора)
volatile boolean bolAir = true; // змінна для "дозволу" балансування фазового зсуву аналогової частини
volatile int intSin; // значення sin домножене на 16384 (2 в 15 ступені)
volatile int intCos; // значення cos домножене на 16384 (2 в 15 ступені)
long lngSXrev; // сума даних по X з "поворотом" (приведення "X" до нуля)
long lngSYrev; // сума даних по У з "поворотом" (приведення "У" до нуля)
Функція повороту:
Код: Виділити все
/*функція повороту вектору розбалансу до нуля*/
long fAir (long x, long y)
{
if (x < 0) x = -1 * x; // якщо x< 0 інвертуємо знак
if (y > 0) { // "У" завжди повинен бути позитивним
long tmpX = x;
tmpX = tmpX << 10; // домножимо "Х" на 1024 (2 в 10 ступені, так як у таблиці теж домножено) - зсув виконується швидше
x = tmpX / y; // розрахунок тангенса кута "помноженого" на 1024
for ( int ii = 0; ii < intBradisTableLength; ii++) // "перебігаємо" по таблиці від меншого кута до більшого
{
if (x > (*(BradisTable + ii))) // знаходимо кут за значенням арктангенсу
{
intCos = 1 * intSinCos[ii]; // отримуємо з таблиці значення cos для повороту
intSin = -1 * intSinCos[88 - ii]; // отримуємо з таблиці значення sin для повороту
}
bolAir = false; // балансування (поворот вектора для приведення "У" до нуля) виконано
}
}
}
І, на завершення, сам розрахунок нових значень "Х" та "У":
Код: Виділити все
if (bolAir) // якщо натиснута кнопка "балансування" (ще не реалізовано, виконується при старті)
{
fAir (lngSX, lngSY); // проводимо корекцію "Х" та "У"
}
lngSX = lngSX >> 2; // зсув вправо на 2 розряди (ділимо на 4) для уникнення перевищення озміру змінної
lngSY = lngSY >> 2; // зсув вправо на 2 розряди (ділимо на 4) для уникнення перевищення озміру змінної
lngSXrev = (lngSX * intCos + lngSY * intSin) / 16384; // здійснюємо поворот вектора для отримання коригованого значення "Х"
lngSYrev = (lngSY * intCos - lngSX * intSin) / 16384; // здійснюємо поворот вектора для отримання коригованого значення "Х"
В архіві повний скетч; для тих хто не користується
ARDUINO DUE - лістинг коду у форматі doc