Lompat ke konten Lompat ke sidebar Lompat ke footer

Cara Pakai MPU6050 Arduino Accelerometer Gyroscope

Akses MPU6050 Arduino

A. Penjelasan MPU6050 Arduino

Cara Menggunakan MPU6050 Arduino Accelerometer & Gyroscope - MPU050 adalah sebuah chip yang didalamnya telah ada Accelerometer, Gyroscope, dan Sensor Temperature.
 
Dipasaran telah tersedia  MPU6050 Arduino module yang dengan mudah diakses menggunakan Arduino. Untuk harga dari modul mpu ini terbilang cukup murah, berkisar Rp 12.000 (saat artikel ini dibuat). 
 
MPU6050 arduino ini termasuk kedalam perangkat IMU (Inertial Measurement Unit) . IMU biasa disebut juga dengan Magnetometer, yaitu penggabungan perangkat yang mampu mengukur dan mendapatkan nilai dari kecepatan, oritentasi dan gaya gravitasi.
 
Contoh umum Penggunaan / Aplikasi perangkat IMU ini adalah seperti:
  • Pesawat Tanpa Awak
  • QuadCopter
  • Robot Cerdas
  • Smarthphone
  • Game konsole 
  • Gimbal untuk Video Image Stabilization
  • Headset Gaming
  • Virtual Reality
  • dll
 
Nah teman-teman semua, sebelum kita memasuki program MPU6050 Arduino, mari kita memahami dasar dari Accelerometer, Gyroscope dan Sensor temperatur. 
 

1. Dasar Teori Accelerometer MPU6050

Accelerometer adalah perangkat yang berfungsi untuk menentukan orientasi (arah) berdasarkan percepatan yang diukur. Contoh penggunaan Accelerometer adalah pada smartphone seperti mengganti wallpaper hanya dengan gerakan.
 
Nah, Percepatan adalah laju perubahan kecepatan dari suatu benda. Ketika suatu benda digerakkan dengan kocepatan konstan, maka benda tersebut dikatakan tidak memiliki percepatan. Namun, ketika suatu benda digerakkan dengan kecepatan tidak konstan, maka benda tersebut dikatakan memiliki percepatan.
 
Accelerometer tidak menggunakan gravitasi bumi dalam pengoperasiannya, namun accelerometer perlu memperhitungkan percepatan statis seperti gaya gravitasi saat proses kerjanya.
 
Accelerometer Features MPU6050:
  • 3 Axis
  • Programmable full scale range of ±2g, ±4g, ±8g and ±16g
  • 16-bit ADC
  • Arus Low power mode: 10µA at 1.25Hz, 20µA at 5Hz, 60µA at 20Hz, 110µA at 40Hz
  • Arus Normal mode: 500µA 
  • Untuk lebih lengkap, silahkan lihat datasheet pada halaman ke 10.
 

2. Dasar Teori Gyroscope MPU6050

Gyroscope adalah perangkat yang  berfungsi untuk menentukan rotasi (perputaran). Contoh penggunaan pada smartphone adalah kompas.  Gyroscope menggunakan gravitasi bumi dalam pengoperasiannya. 
 
Gyroscope Features MPU6050:
  • 3 Axis
  • Digital-output X-, Y-, and Z-Axis angular rate sensors (gyroscopes) with a user-programmable fullscale range of ±250, ±500, ±1000, and ±2000°/sec untuk pelacakan presisi dari gerakan cepat dan lambat.
  • 16-bit ADC
  • Standby current: 5µA
  • Operating current: 3.6mA
  • Untuk lebih lengkap, silahkan lihat datasheet pada halaman ke 10.
 

3. Temperature Sensore MPU6050

Didalam chip ini juga terdapat sensor temperatur yang dapat kita gunakan dengan spesifikasi:
  • - 40 hingga + 85 °C
  • Sensitifitas 340 LSB/°C (tanpa di trim)
  • Akurasi +1°C
 

B. Bagaimana MPU6070 Bekerja?

Sebagaimana telah dijelaskan pada dasar teori diatas, chip mpu ini memiliki akselerometer, giroskop dan sensor suhu. Ketika mpu ini melakukan operasi, maka data nilai dari masing-masing sensor akan masuk ke Digital Motion Processor (DPM).  Tidak hanya itu, DMP juga dapat digunakan untuk mengambil data sensor eksternal (jika ada) yang dihubungkan ke modul mpu ini.
 
DMP akan mengirimkan data tersebut menggunakan komunikasi I2C (SDA & SCL). Data yang dikirimkan akan di terima oleh arduino.
 

C. MPU6050 Pinout & Rangkaian Arduino

Untuk dapat menghubungkan modul MPU6050 Arduino sensor ini, kita perlu mengetahui pinout yang ada pada modul. Adapun pinout dapat anda lihat pada gambar berikut:
 
Cara Menggunakan MPU6050 Arduino Accelerometer & Gyroscope

 

Penjelasan PinOut:
  • Pada dasarnya, chip ini bekerja dengan tengangan 3.3V. Menggunakan ic regulator seperti “4B2X” dengan output 3.3V, kita dapat menggunakan VCC dengan nilai +5V.
  • I2C Clock dan Data digunakan untuk komunikasi dengan mikrokontroller seperti Arduno Uno.
  • Eksternal I2C Data dan Clock digunakan untuk berkomunikasi  ke sensor eksternal.
  • Address Bit I2C digunakan untuk mengubah alamat I2C internal dari modul jika alamat default dari modul ini bentrok dengan perangkat I2C lain.
    Jika pin AD0 LOW, maka alamatnya adalah 0x68 (default).
    Jika pin AD0 HIGH, maka alamatnya adalah 0x69.
  • Interrupt adalah output interupsi.
Adapun rangkaian mpu6050 ke arduino adalah sebagai berikut:
Cara Menggunakan MPU6050 Arduino Accelerometer & Gyroscope


 

D. Library MPU6050 Arduino

Sekarang kita telah masuk ke inti dari artikel ini yaitu bagaimana cara kita menggunakan MPU6050 menggunakan Arduno. 

Untuk data-data yang dikirimkan dari modul ini dapat kita ambil dengan mudah, dibantu sebuah library yang telah dibuat oleh ElectronicCats.
 
Untuk instalasi library silahkan menuju Library Manager dengan cara:
  • Klik menu Sketch
  • klik Include Library -> Manage Libraries
  • Cari MPU6050 -> pilih by Electronic Cats
  • Lalu klik Install

Setelah library terinstall, maka selanjutnya kita akan program arduino dengan program yang saya sediakan dibawah. Tedapat 6 mode program, silahkan diupload ke Arduino teman-teman,

 

E. MPU6050 Kode Program Arduino

1. Mode Yaw Pitch Roll (3D)

Mode Yaw Pitch Roll adalah  teknik pergerakan dalam 3 dimensi. Mode ini mengikuti prinsip pesawat terbang, dimana perputaran/perpindahan terjadi dalam 3 axis atau 3 sumbu, yaitu Yaw, Pitch dan Roll.
  • Yaw adalah perputaran objek pada sumbu vertikal.
  • Pitch adalah perputaran objek pada sumbu lateral.
  • Roll adalah perputaran objek pada sumbu longitudinal.
 
Untuk jelasnya perhatikan pada gambar berikut:
Cara Menggunakan MPU6050 Arduino Accelerometer & Gyroscope
Yaw Pitch Roll (sumber gambar : wikipedia)
 

 
Silahkan upload program dibawah ini, kemudian buka serial monitor Arduino teman-teman.
#include "MPU6050_6Axis_MotionApps20.h"
#include "Wire.h"

MPU6050 mpu;

Quaternion q;
VectorFloat gravity;
uint8_t fifoBuffer[64];

uint16_t packetSize;
uint16_t fifoCount;
float ypr[3];

void setup() {
Serial.begin(115200);
Wire.begin();
TWBR = 24;
mpu.initialize();
mpu.dmpInitialize();

//Nilai offset ini dapat diubah sesuai kebutuhan
mpu.setXAccelOffset(-1343);
mpu.setYAccelOffset(-1155);
mpu.setZAccelOffset(1033);
mpu.setXGyroOffset(19);
mpu.setYGyroOffset(-27);
mpu.setZGyroOffset(16);
mpu.setDMPEnabled(true);
packetSize = mpu.dmpGetFIFOPacketSize();
fifoCount = mpu.getFIFOCount();

Serial.println("Proses penstabilan sensor... Mohon tunggu...");
delay(18000);
mpu.resetFIFO();
}

void loop() {
while (fifoCount < packetSize) {
fifoCount = mpu.getFIFOCount();
}

if (fifoCount == 1024) {

mpu.resetFIFO();
Serial.println(F("FIFO overflow!"));
}
else {

if (fifoCount % packetSize != 0) {

mpu.resetFIFO();
}
else {

while (fifoCount >= packetSize) {
mpu.getFIFOBytes(fifoBuffer, packetSize);
fifoCount -= packetSize;
}
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

Serial.print("ypr\t");
Serial.print(ypr[0] * 180 / PI);
Serial.print("\t");
Serial.print(ypr[1] * 180 / PI);
Serial.print("\t");
Serial.print(ypr[2] * 180 / PI);
Serial.println();
}
}
}
 
Maka hasil yang tampil adalah seperti berikut:
Cara Menggunakan MPU6050 Arduino Accelerometer & Gyroscope

 
 

2. Mode Quaternion (4 Dimensi)

Mode Quaternion adalah teknik pergerakan objek kedalam 4 dimensi. Sekarang, kita akan mencoba program dibawah ini untuk menampilkan nilai-nilai dalam teknik Quartenion:
#include "MPU6050_6Axis_MotionApps20.h"
#include "Wire.h"

MPU6050 mpu;

Quaternion q;
uint8_t fifoBuffer[64];
uint16_t packetSize;
uint16_t fifoCount;

void setup() {
Serial.begin(115200);
Wire.begin();
TWBR = 24;
mpu.initialize();
mpu.dmpInitialize();

//Nilai offset ini dapat diubah sesuai kebutuhan
mpu.setXAccelOffset(-1343);
mpu.setYAccelOffset(-1155);
mpu.setZAccelOffset(1033);
mpu.setXGyroOffset(19);
mpu.setYGyroOffset(-27);
mpu.setZGyroOffset(16);
mpu.setDMPEnabled(true);
packetSize = mpu.dmpGetFIFOPacketSize();
fifoCount = mpu.getFIFOCount();

Serial.println("Proses penstabilan sensor... Mohon tunggu...");
delay(18000);
mpu.resetFIFO();
}

void loop() {
while (fifoCount < packetSize) {
fifoCount = mpu.getFIFOCount();
}

if (fifoCount == 1024) {

mpu.resetFIFO();
Serial.println(F("FIFO overflow!"));
}
else {

if (fifoCount % packetSize != 0) {

mpu.resetFIFO();
}
else {

while (fifoCount >= packetSize) {
mpu.getFIFOBytes(fifoBuffer, packetSize);
fifoCount -= packetSize;
}
mpu.dmpGetQuaternion(&q, fifoBuffer);
Serial.print("quat\t");
Serial.print(q.w);
Serial.print("\t");
Serial.print(q.x);
Serial.print("\t");
Serial.print(q.y);
Serial.print("\t");
Serial.println(q.z);
}
}
}
 
Maka hasilnya adalah sebagai berikut:
Cara Menggunakan MPU6050 Arduino Accelerometer & Gyroscope

 

3. Mode Euler (Derajat)

Mode Euler adalah menampilkan hasil pergerakan sudut dari 3 dimensi kedalam besaran derajat. Berikut adalah programnya:
#include "MPU6050_6Axis_MotionApps20.h"
#include "Wire.h"

MPU6050 mpu;

Quaternion q;
uint8_t fifoBuffer[64];
uint16_t packetSize;
uint16_t fifoCount;

float euler[3];

void setup() {
Serial.begin(115200);
Wire.begin();
TWBR = 24;
mpu.initialize();
mpu.dmpInitialize();

//Nilai offset ini dapat diubah sesuai kebutuhan
mpu.setXAccelOffset(-1343);
mpu.setYAccelOffset(-1155);
mpu.setZAccelOffset(1033);
mpu.setXGyroOffset(19);
mpu.setYGyroOffset(-27);
mpu.setZGyroOffset(16);
mpu.setDMPEnabled(true);
packetSize = mpu.dmpGetFIFOPacketSize();
fifoCount = mpu.getFIFOCount();

Serial.println("Proses penstabilan sensor... Mohon tunggu...");
delay(18000);
mpu.resetFIFO();
}

void loop() {
while (fifoCount < packetSize) {
fifoCount = mpu.getFIFOCount();
}

if (fifoCount == 1024) {

mpu.resetFIFO();
Serial.println(F("FIFO overflow!"));
}
else {

if (fifoCount % packetSize != 0) {

mpu.resetFIFO();
}
else {

while (fifoCount >= packetSize) {
mpu.getFIFOBytes(fifoBuffer, packetSize);
fifoCount -= packetSize;
}

mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetEuler(euler, &q);
Serial.print("euler\t");
Serial.print(euler[0] * 180 / M_PI);
Serial.print("\t");
Serial.print(euler[1] * 180 / M_PI);
Serial.print("\t");
Serial.println(euler[2] * 180 / M_PI);
}
}
}
 
Maka hasilnya yang ditampilkan dari serial monitor adalah sebagai berikut:
Cara Menggunakan MPU6050 Arduino Accelerometer & Gyroscope

 

4. Mode RealAccel (Real Acceleration)

Mode RealAccel adalah menampilkan data real dari akselerasi tanpa mempertimbangkan gravitasi. Adapun programnya adalah sebagai berikut:
#include "MPU6050_6Axis_MotionApps20.h"
#include "Wire.h"

MPU6050 mpu;

Quaternion q;
uint8_t fifoBuffer[64];
uint16_t packetSize;
uint16_t fifoCount;
VectorInt16 aa;
VectorInt16 aaReal;
VectorFloat gravity;

void setup() {
Serial.begin(115200);
Wire.begin();
TWBR = 24;
mpu.initialize();
mpu.dmpInitialize();

//Nilai offset ini dapat diubah sesuai kebutuhan
mpu.setXAccelOffset(-1343);
mpu.setYAccelOffset(-1155);
mpu.setZAccelOffset(1033);
mpu.setXGyroOffset(19);
mpu.setYGyroOffset(-27);
mpu.setZGyroOffset(16);
mpu.setDMPEnabled(true);
packetSize = mpu.dmpGetFIFOPacketSize();
fifoCount = mpu.getFIFOCount();

Serial.println("Proses penstabilan sensor... Mohon tunggu...");
delay(18000);
mpu.resetFIFO();
}

void loop() {
while (fifoCount < packetSize) {
fifoCount = mpu.getFIFOCount();
}

if (fifoCount == 1024) {

mpu.resetFIFO();
Serial.println(F("FIFO overflow!"));
}
else {

if (fifoCount % packetSize != 0) {

mpu.resetFIFO();
}
else {

while (fifoCount >= packetSize) {
mpu.getFIFOBytes(fifoBuffer, packetSize);
fifoCount -= packetSize;
}

mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetAccel(&aa, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
Serial.print("areal\t");
Serial.print(aaReal.x);
Serial.print("\t");
Serial.print(aaReal.y);
Serial.print("\t");
Serial.println(aaReal.z);
}
}
}
 
Adapun hasilnya adalah sebagai berikut:
Cara Menggunakan MPU6050 Arduino Accelerometer & Gyroscope

 

5. Mode WorldAccel (World-Frame Acceleration)

Mode WorldAccel adalah mode dimana perputaran berdasarkan orientasi yang diketahui dari quaternion  dan gravitasi tidak diperhitungkan. Adapun programnya adalah:
#include "MPU6050_6Axis_MotionApps20.h"
#include "Wire.h"

MPU6050 mpu;

Quaternion q;
uint8_t fifoBuffer[64];
uint16_t packetSize;
uint16_t fifoCount;
VectorInt16 aa;
VectorInt16 aaReal;
VectorFloat gravity;
VectorInt16 aaWorld;

void setup() {
Serial.begin(115200);
Wire.begin();
TWBR = 24;
mpu.initialize();
mpu.dmpInitialize();

//Nilai offset ini dapat diubah sesuai kebutuhan
mpu.setXAccelOffset(-1343);
mpu.setYAccelOffset(-1155);
mpu.setZAccelOffset(1033);
mpu.setXGyroOffset(19);
mpu.setYGyroOffset(-27);
mpu.setZGyroOffset(16);
mpu.setDMPEnabled(true);
packetSize = mpu.dmpGetFIFOPacketSize();
fifoCount = mpu.getFIFOCount();

Serial.println("Proses penstabilan sensor... Mohon tunggu...");
delay(18000);
mpu.resetFIFO();
}

void loop() {
while (fifoCount < packetSize) {
fifoCount = mpu.getFIFOCount();
}

if (fifoCount == 1024) {

mpu.resetFIFO();
Serial.println(F("FIFO overflow!"));
}
else {

if (fifoCount % packetSize != 0) {

mpu.resetFIFO();
}
else {

while (fifoCount >= packetSize) {
mpu.getFIFOBytes(fifoBuffer, packetSize);
fifoCount -= packetSize;
}

mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetAccel(&aa, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
Serial.print("aworld\t");
Serial.print(aaWorld.x);
Serial.print("\t");
Serial.print(aaWorld.y);
Serial.print("\t");
Serial.println(aaWorld.z);
}
}
}
Adapun hasilnya adalah sebagai berikut:
Cara Menggunakan MPU6050 Arduino Accelerometer & Gyroscope

 

6. Mode Teapot (Visualisasi Processing)

Mode Teapot  adalah menampilkan nilai quanternion yang digunakan untuk memvisualisasikan modul mpu6050 di processing. Adapun program Arduino adalah sebagai berikut:
#include "MPU6050_6Axis_MotionApps20.h"
#include "Wire.h"

MPU6050 mpu;

Quaternion q;
uint8_t fifoBuffer[64];
uint16_t packetSize;
uint16_t fifoCount;
uint8_t teapotPacket[14] = { '$', 0x02, 0, 0, 0, 0, 0, 0, 0, 0, 0x00, 0x00, '\r', '\n' };


void setup() {
Serial.begin(115200);
Wire.begin();
TWBR = 24;
mpu.initialize();
mpu.dmpInitialize();

//Nilai offset ini dapat diubah sesuai kebutuhan
mpu.setXAccelOffset(-1343);
mpu.setYAccelOffset(-1155);
mpu.setZAccelOffset(1033);
mpu.setXGyroOffset(19);
mpu.setYGyroOffset(-27);
mpu.setZGyroOffset(16);
mpu.setDMPEnabled(true);
packetSize = mpu.dmpGetFIFOPacketSize();
fifoCount = mpu.getFIFOCount();

Serial.println("Proses penstabilan sensor... Mohon tunggu...");
delay(18000);
mpu.resetFIFO();
}

void loop() {
while (fifoCount < packetSize) {
fifoCount = mpu.getFIFOCount();
}

if (fifoCount == 1024) {

mpu.resetFIFO();
Serial.println(F("FIFO overflow!"));
}
else {

if (fifoCount % packetSize != 0) {

mpu.resetFIFO();
}
else {

while (fifoCount >= packetSize) {
mpu.getFIFOBytes(fifoBuffer, packetSize);
fifoCount -= packetSize;
}

teapotPacket[2] = fifoBuffer[0];
teapotPacket[3] = fifoBuffer[1];
teapotPacket[4] = fifoBuffer[4];
teapotPacket[5] = fifoBuffer[5];
teapotPacket[6] = fifoBuffer[8];
teapotPacket[7] = fifoBuffer[9];
teapotPacket[8] = fifoBuffer[12];
teapotPacket[9] = fifoBuffer[13];
Serial.write(teapotPacket, 14);
teapotPacket[11]++; // packetCount, loops at 0xFF on purpose
}
}
}
 
Adapun program Processing adalah sebagai berikut:
import processing.serial.*;
import processing.opengl.*;
import toxi.geom.*;
import toxi.processing.*;

// NOTE: requires ToxicLibs to be installed in order to run properly.
// 1. Download from http://toxiclibs.org/downloads
// 2. Extract into [userdir]/Processing/libraries
// (location may be different on Mac/Linux)
// 3. Run and bask in awesomeness

ToxiclibsSupport gfx;

Serial port; // The serial port
char[] teapotPacket = new char[14]; // InvenSense Teapot packet
int serialCount = 0; // current packet byte position
int synced = 0;
int interval = 0;

float[] q = new float[4];
Quaternion quat = new Quaternion(1, 0, 0, 0);

float[] gravity = new float[3];
float[] euler = new float[3];
float[] ypr = new float[3];

void setup() {
// 300px square viewport using OpenGL rendering
size(300, 300, OPENGL);
gfx = new ToxiclibsSupport(this);

// setup lights and antialiasing
lights();
smooth();

// display serial port list for debugging/clarity
println(Serial.list());

// get the first available port (use EITHER this OR the specific port code below)
String portName = Serial.list()[0];

// get a specific serial port (use EITHER this OR the first-available code above)
//String portName = "COM4";

// open the serial port
port = new Serial(this, portName, 115200);

// send single character to trigger DMP init/start
// (expected by MPU6050_DMP6 example Arduino sketch)
port.write('r');
}

void draw() {
if (millis() - interval > 1000) {
// resend single character to trigger DMP init/start
// in case the MPU is halted/reset while applet is running
port.write('r');
interval = millis();
}

// black background
background(0);

// translate everything to the middle of the viewport
pushMatrix();
translate(width / 2, height / 2);

// 3-step rotation from yaw/pitch/roll angles (gimbal lock!)
// ...and other weirdness I haven't figured out yet
//rotateY(-ypr[0]);
//rotateZ(-ypr[1]);
//rotateX(-ypr[2]);

// toxiclibs direct angle/axis rotation from quaternion (NO gimbal lock!)
// (axis order [1, 3, 2] and inversion [-1, +1, +1] is a consequence of
// different coordinate system orientation assumptions between Processing
// and InvenSense DMP)
float[] axis = quat.toAxisAngle();
rotate(axis[0], -axis[1], axis[3], axis[2]);

// draw main body in red
fill(255, 0, 0, 200);
box(10, 10, 200);

// draw front-facing tip in blue
fill(0, 0, 255, 200);
pushMatrix();
translate(0, 0, -120);
rotateX(PI/2);
drawCylinder(0, 20, 20, 8);
popMatrix();

// draw wings and tail fin in green
fill(0, 255, 0, 200);
beginShape(TRIANGLES);
vertex(-100, 2, 30); vertex(0, 2, -80); vertex(100, 2, 30); // wing top layer
vertex(-100, -2, 30); vertex(0, -2, -80); vertex(100, -2, 30); // wing bottom layer
vertex(-2, 0, 98); vertex(-2, -30, 98); vertex(-2, 0, 70); // tail left layer
vertex( 2, 0, 98); vertex( 2, -30, 98); vertex( 2, 0, 70); // tail right layer
endShape();
beginShape(QUADS);
vertex(-100, 2, 30); vertex(-100, -2, 30); vertex( 0, -2, -80); vertex( 0, 2, -80);
vertex( 100, 2, 30); vertex( 100, -2, 30); vertex( 0, -2, -80); vertex( 0, 2, -80);
vertex(-100, 2, 30); vertex(-100, -2, 30); vertex(100, -2, 30); vertex(100, 2, 30);
vertex(-2, 0, 98); vertex(2, 0, 98); vertex(2, -30, 98); vertex(-2, -30, 98);
vertex(-2, 0, 98); vertex(2, 0, 98); vertex(2, 0, 70); vertex(-2, 0, 70);
vertex(-2, -30, 98); vertex(2, -30, 98); vertex(2, 0, 70); vertex(-2, 0, 70);
endShape();

popMatrix();
}

void serialEvent(Serial port) {
interval = millis();
while (port.available() > 0) {
int ch = port.read();

if (synced == 0 && ch != '$') return; // initial synchronization - also used to resync/realign if needed
synced = 1;
print ((char)ch);

if ((serialCount == 1 && ch != 2)
|| (serialCount == 12 && ch != '\r')
|| (serialCount == 13 && ch != '\n')) {
serialCount = 0;
synced = 0;
return;
}

if (serialCount > 0 || ch == '$') {
teapotPacket[serialCount++] = (char)ch;
if (serialCount == 14) {
serialCount = 0; // restart packet byte position

// get quaternion from data packet
q[0] = ((teapotPacket[2] << 8) | teapotPacket[3]) / 16384.0f;
q[1] = ((teapotPacket[4] << 8) | teapotPacket[5]) / 16384.0f;
q[2] = ((teapotPacket[6] << 8) | teapotPacket[7]) / 16384.0f;
q[3] = ((teapotPacket[8] << 8) | teapotPacket[9]) / 16384.0f;
for (int i = 0; i < 4; i++) if (q[i] >= 2) q[i] = -4 + q[i];

// set our toxilibs quaternion to new data
quat.set(q[0], q[1], q[2], q[3]);

/*
// below calculations unnecessary for orientation only using toxilibs

// calculate gravity vector
gravity[0] = 2 * (q[1]*q[3] - q[0]*q[2]);
gravity[1] = 2 * (q[0]*q[1] + q[2]*q[3]);
gravity[2] = q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3];

// calculate Euler angles
euler[0] = atan2(2*q[1]*q[2] - 2*q[0]*q[3], 2*q[0]*q[0] + 2*q[1]*q[1] - 1);
euler[1] = -asin(2*q[1]*q[3] + 2*q[0]*q[2]);
euler[2] = atan2(2*q[2]*q[3] - 2*q[0]*q[1], 2*q[0]*q[0] + 2*q[3]*q[3] - 1);

// calculate yaw/pitch/roll angles
ypr[0] = atan2(2*q[1]*q[2] - 2*q[0]*q[3], 2*q[0]*q[0] + 2*q[1]*q[1] - 1);
ypr[1] = atan(gravity[0] / sqrt(gravity[1]*gravity[1] + gravity[2]*gravity[2]));
ypr[2] = atan(gravity[1] / sqrt(gravity[0]*gravity[0] + gravity[2]*gravity[2]));

// output various components for debugging
//println("q:\t" + round(q[0]*100.0f)/100.0f + "\t" + round(q[1]*100.0f)/100.0f + "\t" + round(q[2]*100.0f)/100.0f + "\t" + round(q[3]*100.0f)/100.0f);
//println("euler:\t" + euler[0]*180.0f/PI + "\t" + euler[1]*180.0f/PI + "\t" + euler[2]*180.0f/PI);
//println("ypr:\t" + ypr[0]*180.0f/PI + "\t" + ypr[1]*180.0f/PI + "\t" + ypr[2]*180.0f/PI);
*/
}
}
}
}

void drawCylinder(float topRadius, float bottomRadius, float tall, int sides) {
float angle = 0;
float angleIncrement = TWO_PI / sides;
beginShape(QUAD_STRIP);
for (int i = 0; i < sides + 1; ++i) {
vertex(topRadius*cos(angle), 0, topRadius*sin(angle));
vertex(bottomRadius*cos(angle), tall, bottomRadius*sin(angle));
angle += angleIncrement;
}
endShape();

// If it is not a cone, draw the circular top cap
if (topRadius != 0) {
angle = 0;
beginShape(TRIANGLE_FAN);

// Center point
vertex(0, 0, 0);
for (int i = 0; i < sides + 1; i++) {
vertex(topRadius * cos(angle), 0, topRadius * sin(angle));
angle += angleIncrement;
}
endShape();
}

// If it is not a cone, draw the circular bottom cap
if (bottomRadius != 0) {
angle = 0;
beginShape(TRIANGLE_FAN);

// Center point
vertex(0, tall, 0);
for (int i = 0; i < sides + 1; i++) {
vertex(bottomRadius * cos(angle), tall, bottomRadius * sin(angle));
angle += angleIncrement;
}
endShape();
}
}
 
Hasilnya adalah:
 

Sekian. Semoga artikel MPU6050 Arduino ini bermanfaat.

Jika terdapat kesalahan pada artikel ini, mohon kirimkan koreksinya kepada saya. Alamat email saya telah tercantum pada halaman Contact.

Posting Komentar untuk "Cara Pakai MPU6050 Arduino Accelerometer Gyroscope"