Gyorsulás mérő és giroszkóp MPU6050-el
Ebben a projektben kiegészítettem a Gyro szenzort egy accelerometer-el, így már nagyon pontos szöget lehet mérni.
Viszont csak két írányban(előre, hátra és jobbra balra dőlés), A teljes lefedettséghez ez még mindíg nem elég. Egy geometer is szükséges lenne ahoz, hogy az elfordulást is meg lehessen mérni. Az itt bemutatott példában ±180°-os dőlést érzékel az eszköz. A spectrum analyzer cikkben bemutatott kijelzőn megjeleníti az elfordulást, valamint bluetooth modulon keresztül a PC-nek is elküldi, és ott pedig egy programmal megjeleníti azt.
Az MPU6050
Az eszköz A giroszkóp része ugyan az, mint a másik cikkben bemutatott IC. A gyorsulás mérő szenzor 4 féle méréshatárral rendelkezik, ±2g, ±4g, ±8g, ±16g. A szögméréshez önmagában azért nem elég a gyorsulás mérő, mert nagyon érzékeny az apró ütésekre, egy kis rázkódástól már hatalmas elfordulásokat kapnánk. A giroszkóp nem érzékeny az ütésekre, viszont a „drift” miatt pontatlan. A két szenzort egy úgynevezett complementary filter vagy Kalman-filter segítségével tökéletes mérésre használhatjuk. Én az egyszerűsége miatt a complementary filtert használtam fel a projektben.
Számolási elmélet
A földön minden álló testre 1g erő hat lefele irányban.
A Trigonometrikus függvények inverzei közül az arkusz-tangensfüggvényre lesz szükségünk:
Viszont így csak ±90°-ban tudunk szöget mérni, és ha túlhaladunk a 90°-on akkor össze vissza fog forogni a testünk a monitoron. Ahoz hogy ±180°-ot tudjunk mérni a következő képletre lesz szükségünk:
A complementary filter
A gyro szenzort integráljuk az idővel, a gyorsulásmérővel megkapott adatokból kiszámoljuk a szöget és behelyettesítjük a képletbe. A 0.98 és 0.02 értékekkel lehet hangolni a filtert. Megvalósítása:
#define ACCELEROMETER_SENSITIVITY 16384.0 //Az adatlapból
#define x 0 //A kapott tömbben az irányok sorszámai
#define y 1
#define z 2
#define M_PI 3.14159265359f
#define dt 0.005 // 5 ms sample rate, amit az MPU6050-ben beállítottunk
void ComplementaryFilter(short accData[3], short gyrData[3], float *pitch, float *roll)
{
float pitchAcc, rollAcc;
// Gyroszkóp adatok integrálása az idővel
*pitch += ((float)gyrData[0] / GYROSCOPE_SENSITIVITY) * dt; // X-axis
*roll -= ((float)gyrData[1] / GYROSCOPE_SENSITIVITY) * dt; // Y-axis
// Drift kompenzálás
// Sensitivity = -2 to 2 G at 16Bit -> 2G = 32768 && 0.5G = 8192
int forceMagnitudeApprox = abs(accData[x]) + abs(accData[y]) + abs(accData[z]);
if (forceMagnitudeApprox > 8192 && forceMagnitudeApprox < 32767)
{
// A filter megvalósítása X tengelyen
pitchAcc = atan2f((float)accData[y], sqrtf((float)accData[z] *
(float)accData[z] +(float)accData[x] *
(float)accData[x])) * 180 / M_PI;
*pitch = *pitch * 0.98f + pitchAcc * 0.02f;
// A filter megvalósítása Y tengelyen
rollAcc = atan2f((float)accData[x], sqrtf((float)accData[z] *
(float)accData[z] +(float)accData[y] *
(float)accData[y])) * 180 / M_PI;
*roll = *roll * 0.98f + rollAcc * 0.02f;
}
}
A bluetooth
A bluetooth átvitel egy RN42 modullal történik. A bluetooth – PIC kapcsolatot UART-on keresztül történik. A bluetooth parancsait az adatlapban találjuk meg. A sebességen kívűl mást nem szükséges állítani, az alapértelmezett beállítások teljesen megfelelőek. A PC-n ha párosítottuk az eszközt, szintén soros portként érjük el. Csatlakozás után mehet egyből az adatküldés.
Az UART és bluetooth init kódja:
unsigned char BTCMDMode[] = {"$$$"}; //Command mode kódja
unsigned char BTCMDModeOff[] = {"---\n"}; //Command mód kilépés
unsigned char BTSpeed [] = {"SU,11\n"}; //Baudrate Beállítás, 110k
unsigned char BTReset[] = {"R,1\n"}; //Reset kód
unsigned char *ptr;
void UARTInit() //UART init
{
U1BRG = 416; //Baudrate 9600
U1MODEbits.UARTEN = 1; //UART Enable
U1STAbits.UTXEN = 1; //UART TX Enable
RPOR1bits.RP36R = 0b000001; //UART TX on RP36/RB4 pin
}
void BTInit()
{
__delay_ms(100);
BTRESET = 1; //Engedélyezzük a bluetooth-ot
__delay_ms(1000); //Megvárjuk,míg feléled a BT
ptr = BTCMDMode;
while(*ptr != 0) //Command módba rakjuk a Bluetoothot
{
while(U1STAbits.UTXBF);
U1TXREG = *ptr;
ptr++;
}
__delay_ms(100);
ptr = BTSpeed;
while(*ptr != 0) //Beállítjuk a Bluetooth sebességét 110k-ra
{
while(U1STAbits.UTXBF);
U1TXREG = *ptr;
ptr++;
}
__delay_ms(100);
BTRESET = 0; //BT SPEED aktiválás(Reset után lép életbe)
__delay_ms(100);
BTRESET = 1;
U1BRG = 35; //Beállítjuk a PIC-et is az új sebességre
}
A kapcsolási rajz
Nyákot nem készítettem, csak a videón látható próbapanelen lett összeállítva. Egy elvi kapcsolás a következő ábrán látható. Az MPU6050-et modulként vásároltam meg, így annak csak a táp és I2C kivezetései találhatóak meg a rajzon: