Разработка блока для калибровки гироскопа MPU6050

[Sanbi4]

Всем привет! нужна помощь по коду, вернее как правильней сделать.
Имеем готовый код по калибровке гироскопа и акселерометра

СпойлерПоказать

#include "I2Cdev.h"
#include "MPU6050.h"


#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif


MPU6050 accelgyro;
//MPU6050 accelgyro(0x69); // <-- use for AD0 high


const char LBRACKET = '[';
const char RBRACKET = ']';
const char COMMA = ',';
const char BLANK = ' ';
const char PERIOD = '.';

const int iAx = 0;
const int iAy = 1;
const int iAz = 2;
const int iGx = 3;
const int iGy = 4;
const int iGz = 5;

const int usDelay = 3150; // empirical, to hold sampling to 200 Hz
const int NFast = 1000; // the bigger, the better (but slower)
const int NSlow = 10000; // ..
const int LinesBetweenHeaders = 5;
int LowValue[6];
int HighValue[6];
int Smoothed[6];
int LowOffset[6];
int HighOffset[6];
int Target[6];
int LinesOut;
int N;

void ForceHeader()
{ LinesOut = 99; }

void GetSmoothed()
{ int16_t RawValue[6];
int i;
long Sums[6];
for (i = iAx; i <= iGz; i++)
{ Sums = 0; }
// unsigned long Start = micros();

for (i = 1; i <= N; i++)
{ // get sums
accelgyro.getMotion6(&RawValue[iAx], &RawValue[iAy], &RawValue[iAz],
&RawValue[iGx], &RawValue[iGy], &RawValue[iGz]);
if ((i % 500) == 0)
Serial.print(PERIOD);
delayMicroseconds(usDelay);
for (int j = iAx; j <= iGz; j++)
Sums

= Sums

+ RawValue

;
} // get sums
// unsigned long usForN = micros() - Start;
// Serial.print(" reading at ");
// Serial.print(1000000/((usForN+N/2)/N));
// Serial.println(" Hz");
for (i = iAx; i <= iGz; i++)
{ Smoothed = (Sums + N/2) / N ; }
} // GetSmoothed

void Initialize()
{
// join I2C bus (I2Cdev library doesn't do this automatically)
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
Wire.begin();
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(400, true);
#endif

Serial.begin(9600);

// initialize device
Serial.println("Initializing I2C devices...");
accelgyro.initialize();

// verify connection
Serial.println("Testing device connections...");
Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
Serial.println("PID tuning Each Dot = 100 readings");

accelgyro.CalibrateAccel(6);
accelgyro.CalibrateGyro(6);
Serial.println("\nat 600 Readings");
accelgyro.PrintActiveOffsets();
Serial.println();
accelgyro.CalibrateAccel(1);
accelgyro.CalibrateGyro(1);
Serial.println("700 Total Readings");
accelgyro.PrintActiveOffsets();
Serial.println();
accelgyro.CalibrateAccel(1);
accelgyro.CalibrateGyro(1);
Serial.println("800 Total Readings");
accelgyro.PrintActiveOffsets();
Serial.println();
accelgyro.CalibrateAccel(1);
accelgyro.CalibrateGyro(1);
Serial.println("900 Total Readings");
accelgyro.PrintActiveOffsets();
Serial.println();
accelgyro.CalibrateAccel(1);
accelgyro.CalibrateGyro(1);
Serial.println("1000 Total Readings");
accelgyro.PrintActiveOffsets();
Serial.println("\n\n Any of the above offsets will work nice \n\n Lets proof the PID tuning using another method:");
} // Initialize

void SetOffsets(int TheOffsets[6])
{ accelgyro.setXAccelOffset(TheOffsets [iAx]);
accelgyro.setYAccelOffset(TheOffsets [iAy]);
accelgyro.setZAccelOffset(TheOffsets [iAz]);
accelgyro.setXGyroOffset (TheOffsets [iGx]);
accelgyro.setYGyroOffset (TheOffsets [iGy]);
accelgyro.setZGyroOffset (TheOffsets [iGz]);
} // SetOffsets

void ShowProgress()
{ if (LinesOut >= LinesBetweenHeaders)
{ // show header
Serial.println("\tXAccel\t\t\tYAccel\t\t\t\tZAccel\t\t\tXGyro\t\t\tYGyro\t\t\tZGyro");
LinesOut = 0;
} // show header
Serial.print(BLANK);
for (int i = iAx; i <= iGz; i++)
{ Serial.print(LBRACKET);
Serial.print(LowOffset),
Serial.print(COMMA);
Serial.print(HighOffset);
Serial.print("] --> [");
Serial.print(LowValue);
Serial.print(COMMA);
Serial.print(HighValue);
if (i == iGz)
{ Serial.println(RBRACKET); }
else
{ Serial.print("]\t"); }
}
LinesOut++;
} // ShowProgress

void PullBracketsIn()
{ boolean AllBracketsNarrow;
boolean StillWorking;
int NewOffset[6];

Serial.println("\nclosing in:");
AllBracketsNarrow = false;
ForceHeader();
StillWorking = true;
while (StillWorking)
{ StillWorking = false;
if (AllBracketsNarrow && (N == NFast))
{ SetAveraging(NSlow); }
else
{ AllBracketsNarrow = true; }// tentative
for (int i = iAx; i <= iGz; i++)
{ if (HighOffset <= (LowOffset+1))
{ NewOffset = LowOffset[i]; }
else
{ // binary search
StillWorking = true;
NewOffset[i] = (LowOffset[i] + HighOffset[i]) / 2;
if (HighOffset[i] > (LowOffset[i] + 10))
{ AllBracketsNarrow = false; }
} // binary search
}
SetOffsets(NewOffset);
GetSmoothed();
for (int i = iAx; i <= iGz; i++)
{ // closing in
if (Smoothed[i] > Target[i])
{ // use lower half
HighOffset[i] = NewOffset[i];
HighValue[i] = Smoothed[i];
} // use lower half
else
{ // use upper half
LowOffset[i] = NewOffset[i];
LowValue[i] = Smoothed[i];
} // use upper half
} // closing in
ShowProgress();
} // still working

} // PullBracketsIn

void PullBracketsOut()
{ boolean Done = false;
int NextLowOffset[6];
int NextHighOffset[6];

Serial.println("expanding:");
ForceHeader();

while (!Done)
{ Done = true;
SetOffsets(LowOffset);
GetSmoothed();
for (int i = iAx; i <= iGz; i++)
{ // got low values
LowValue[i] = Smoothed[i];
if (LowValue[i] >= Target[i])
{ Done = false;
NextLowOffset[i] = LowOffset[i] - 1000;
}
else
{ NextLowOffset[i] = LowOffset[i]; }
} // got low values

SetOffsets(HighOffset);
GetSmoothed();
for (int i = iAx; i <= iGz; i++)
{ // got high values
HighValue[i] = Smoothed[i];
if (HighValue[i] <= Target[i])
{ Done = false;
NextHighOffset[i] = HighOffset[i] + 1000;
}
else
{ NextHighOffset[i] = HighOffset[i]; }
} // got high values
ShowProgress();
for (int i = iAx; i <= iGz; i++)
{ LowOffset[i] = NextLowOffset[i]; // had to wait until ShowProgress done
HighOffset[i] = NextHighOffset[i]; // ..
}
} // keep going
} // PullBracketsOut

void SetAveraging(int NewN)
{ N = NewN;
Serial.print("averaging ");
Serial.print(N);
Serial.println(" readings each time");
} // SetAveraging

void setup()
{ Initialize();
for (int i = iAx; i <= iGz; i++)
{ // set targets and initial guesses
Target[i] = 0; // must fix for ZAccel
HighOffset[i] = 0;
LowOffset[i] = 0;
} // set targets and initial guesses
Target[iAz] = 16384;
SetAveraging(NFast);

PullBracketsOut();
PullBracketsIn();

Serial.println("-------------- done --------------");
} // setup

void loop()
{

} // loop

Создать блок на основе этого кода для меня не проблема. Но! меня интересует, как правильно сделать, чтобы данный блок запускался при старте программы, до старта основной программы, и старт данного кода по требованию ( в FBD я знаю, как это сделать). Проблема моя в том. что тут использованы функции, которые прописаны до void setup();, а функция петли пустая. Видно, что вызов предыдущей функции есть в других последующих функциях.

Почему именно этот код калибровки?! Потому что этот код более точно делает калибровку по все осям, и калибровка сохраняется только пока на датчике есть питание. Другие скрипты не дают должного результата калибровки. использование setOffset() не дает точности.

Подытожу : возможен ли перенос функций в секцию LOOP и там использовать условие запуска кода?

[Naladchik]

Проблема моя в том. что тут использованы функции, которые прописаны до void setup()

А не имеет значения где эти функции прописаны, в начале скетча или в конце. Главное чтоб они были, а где - не важно.

...возможен ли перенос функций в секцию LOOP ...

при создании блока типа CODE они уйдут за секцию loop

...секцию LOOP и там использовать условие запуска кода?...

На мой взгляд - ничего особого. Завернуть в if то что сейчас в секции setup и запускать по сигналу на входе в блок.
Ну и вывод в серийный порт повырезать.

[Ardudue]

А не имеет значения где эти функции прописаны, в начале скетча или в конце. Главное чтоб они были, а где - не важно.

мне взрослые программисты сказали: "Правильно описывать функции до их использования, а не где попало "

[Naladchik]

мне взрослые программисты сказали: "Правильно описывать функции до их использования, а не где попало "

и не поспоришь, осталось это Сергею рассказать, так как FLProg вставляет описания функций именно в конце.
да и мы не взрослые программисты, нам можно.

Имеем готовый код по калибровке гироскопа и акселерометра

что то Ваш готовый код даже с библиотеками не компилируется. ошибок море. исходник где лежит? а то интересно стало.

[Sanbi4]

Naladchik, исходник тут https://github.com/ElectronicCats/mpu60 ... U_Zero.ino
Все работает нормально.

Функции для теста переместил после лупа, зашил, и все работает также.

борюсь с одной вещью сейчас. Удалил все, что связано с print и serial, так как это есть в основном блоке, а блок калибровки делаю, как дополнение к основному.

собственно, я и спрашивал, как затащить функции в луп, чтобы их можно было вызывать по событию(условии) в любой момент времени, и есть ли в этом смысл.

[Naladchik]

Все работает нормально.

в приведенном Вами листинге вырезаны все i в квадратных скобках

слева Ваш, справа оригинал
Не Ваша вина, при вставке текста в окно сообщения такое вышло
скетч с оригинала компилируется без проблем.

Отправлено спустя 7 минут 1 секунду:

как затащить функции в луп, чтобы их можно было вызывать по событию(условии) в любой момент времени,

примерно так

СпойлерПоказать

ну а функции, само собой в FunctionSection.

[Ardudue]

и не поспоришь, осталось это Сергею рассказать, так как FLProg вставляет описания функций именно в конце.
да и мы не взрослые программисты, нам можно.

взрослые программисты мне сказали: "Это печально. Просто IDE делает за FLProg эту работу при компиляции. Странно что взявшись за создание такой проги автор не изучил азы программирования. "

Но в 7.0.1 ПБ уже могут раскладывать код по фэншую.
Нам то конечно можно всё.

[Naladchik]

"...Странно что взявшись за создание такой проги автор не изучил азы программирования."

Автор много раз повторял что он ну никак не программист ("я не волшебник, я только учусь") и при всем при этом создать такую ВЕЩЬ! и мне, лично, глубоко по барабану кто что и куда складывает, мне важно то, что я с помощью этого ИНСТРУМЕНТА могу гораздо больше чем без него!!!!!

"Создать блок на основе этого кода..."

Попробуйте, а вдруг заработает, мне проверить не на чем, но компиляция проходит.

MPU6050_Calibration.zip

(798.64 КБ) 65 скачиваний

В проект вложил библиотеки, которые нашел. Они или не они не знаю.

[Ardudue]

Проблема моя в том. что тут использованы функции, которые прописаны до void setup();

насколько я понял исследуя материалы по программированию, то эти функции задекларированные до void setup можно вызывать в любое время и везде в коде.

[Sanbi4]

Naladchik, Спасибо! блок я вчера создал сам, лишнее вырезал, но возникли вопросы:

1, Возможно ли сделать так, что я объявляю переменную в основном блоке, а в блоке калибровки использовалась та же переменная, чтобы руками в IDE не править?

2, в функции void SetOffsets использует аргумент (int TheOffsets[6]), по скольку мне нужно 2 блока на 2 датчика с разными адресами (0х68 и 0х69), имя аргумента не меняется, и получаем один и тот же аргумент на 2 блока.

Во вложении блоки, к которым делается калибровка, и сам блок калибровки,который получился у меня, компиляцию не проходит и я знаю почему, не допиливал, так как надо решить следующее: думаю, правильней поместить код калибровки в эти блоки, тогда 1 вопрос отпадает, а 2ой остается актуальным. И если, кто-то на форуме захочет использовать мой блок гироскопа, а калибровка ему не нужна, то просто будем занимать место в памяти ненужным кодом.

И разве функцию сетап можно принудительно вызывать по условию? я думал (да и как написано в учебниках) сетап вызывается один раз при старте программы.

[Naladchik]

Sanbi4,
Ответил в личку.