#include <Wire.

h>
int gyro_x, gyro_y, gyro_z;
long gyro_x_cal, gyro_y_cal, gyro_z_cal;
boolean set_gyro_angles;

long acc_x, acc_y, acc_z, acc_total_vector;

float angle_roll_acc, angle_pitch_acc;

float angle_pitch, angle_roll;

int angle_pitch_buffer, angle_roll_buffer;
float angle_pitch_output, angle_roll_output;

long loop_timer;
int temp;

void setup_mpu_6050_registers(){
//Mengaktifkan MPU-6050
Wire.beginTransmission(0x68); //Memulai komunikasi MPU-6050
Wire.write(0x6B); //Mengirim register mulai yang diminta
Wire.write(0x00); //Mengatur register mulai yang diminta
Wire.endTransmission();
//Konfigurasi accelerometer (+/-8g)
Wire.beginTransmission(0x68); //Memulai komunikasi MPU-6050
Wire.write(0x1C); //Mengirim register mulai yang diminta
Wire.write(0x10); //Mengatur register mulai yang diminta
Wire.endTransmission();
//Konfigurasi gyro (500dps full scale)
Wire.beginTransmission(0x68); //Memulai komunikasi MPU-6050
Wire.write(0x1B); //Mengirim register mulai yang diminta
Wire.write(0x08); //Mengatur register mulai yang diminta
Wire.endTransmission();
}

void read_mpu_6050_data(){ //Subroutine pembacaan raw data gyro dan acc

Wire.beginTransmission(0x68); //Memulai komunikasi MPU-6050
Wire.write(0x3B); //Mengirim register mulai yang diminta
Wire.endTransmission(); //Mengakhiri Transmisi
Wire.requestFrom(0x68,14); //Request 14 bytes dari MPU-6050
while(Wire.available() < 14); //Menunggu hingga seluruh bytes diterima
acc_x = Wire.read()<<8|Wire.read();
acc_y = Wire.read()<<8|Wire.read();
acc_z = Wire.read()<<8|Wire.read();
temp = Wire.read()<<8|Wire.read();
gyro_x = Wire.read()<<8|Wire.read();
gyro_y = Wire.read()<<8|Wire.read();
gyro_z = Wire.read()<<8|Wire.read();
}
void setup() {
Wire.begin(); //Mulai Komunikasi I2C
setup_mpu_6050_registers(); //Atur komunikasi register
for (int cal_int = 0; cal_int < 1000 ; cal_int ++){
//Membaca data acc dan gyro dari MPU-6050 selama 1000x
read_mpu_6050_data();
//Menambahkan gyro x offset ke variabel gyro_x_cal
gyro_x_cal += gyro_x;
//Menambahkan gyro y offset ke variabel gyro_y_cal
gyro_y_cal += gyro_y;
//Menambahkan gyro z offset ke variabel gyro_z_cal
gyro_z_cal += gyro_z;
delay(3); //Delay 3 microseconds
}

//Bagi dengan 1000 untuk mendapatkan offset rata-rata

gyro_x_cal /= 1000;
gyro_y_cal /= 1000;
gyro_z_cal /= 1000;
Serial.begin(115200);
loop_timer = micros(); //Reset loop timer
}

void loop(){
read_mpu_6050_data();

//Kalibrasi gyro
//Mengurangi offset values dari raw gyro value
gyro_x -= gyro_x_cal;
gyro_y -= gyro_y_cal;
gyro_z -= gyro_z_cal;

//Menghitung sudut gyro dimana 0.0000611 = 1 / (250Hz x 65.5)

//Menghitung pitch angle dan menambahkannya ke variabel angle_pitch
angle_pitch += gyro_x * 0.0000611;
// Menghitung roll angle dan menambahkannya ke variabel angle_roll
angle_roll += gyro_y * 0.0000611;
//0.000001066 = 0.0000611 * (3.142(PI) / 180degr) Fungsi sin dalam radian
//Jika IMU memiliki nilai yaw, transfer roll angle ke pitch angle
angle_pitch += angle_roll * sin(gyro_z * 0.000001066);
// Jika IMU memiliki nilai yaw, transfer pitch angle ke roll angle
angle_roll -= angle_pitch * sin(gyro_z * 0.000001066);

//Perhitungan sudut Accelerometer

//Menghitung total accelerometer vector
acc_total_vector = sqrt((acc_x*acc_x)+(acc_y*acc_y)+(acc_z*acc_z));
//57.296 = 1 / (3.142 / 180) asin dalam radians
//Menghitung pitch & roll angle
angle_pitch_acc = asin((float)acc_y/acc_total_vector)* 57.296;
angle_roll_acc = asin((float)acc_x/acc_total_vector)* -57.296;
 angle_pitch_acc -= 0.0; //Kalibrasi nilai Accelerometer untuk pitch
angle_roll_acc -= 0.0; //Kalibrasi nilai Accelerometer untuk roll

if(set_gyro_angles){ //Jika IMU mulai aktif

//Koreksi drift dari gyro pitch angle dengan accelerometer pitch angle
angle_pitch = angle_pitch * 0.9996 + angle_pitch_acc * 0.0004;
//Koreksi drift dari gyro roll angle dengan accelerometer roll angle
angle_roll = angle_roll * 0.9996 + angle_roll_acc * 0.0004;
}
else{ //Pada awal start (Kalibrasi awal)
angle_pitch = angle_pitch_acc; //Atur sudut gyro pitch = acc pitch
angle_roll = angle_roll_acc; //Atur sudut gyro roll = acc roll
set_gyro_angles = true; //Atur keadaan awal flag IMU
}

//Fussion complementary filter

//90% gyro pitch value dan 10% accelerometer pitch value
angle_pitch_output = angle_pitch * 0.9 + angle_pitch_acc * 0.1;
//90% gyro roll value dan 10% accelerometer roll value
angle_roll_output = angle_roll * 0.9 + angle_roll_acc * 0.1;
Serial.print(" | Acc X = "); Serial.print(angle_pitch_acc);
Serial.print(" | Acc Y = "); Serial.print(angle_roll_acc);
Serial.print(" | Gyro X = "); Serial.print(angle_pitch);
Serial.print(" | Gyro Y = "); Serial.print(angle_roll);
Serial.print(" | X = "); Serial.print(angle_pitch_output);
Serial.print(" | Y = "); Serial.println(angle_roll_output);

//Menunggu sampai loop_timer mencapai 4000us (250Hz) sebelum memulai loop

//berikutnya
while(micros() - loop_timer < 4000);
loop_timer = micros();//Reset the loop timer
}

Variasi Nilai x
Data Bacaan Busur Bacaan Gyro (derajat)
1 -75 -75,30
2 -60 -59,50
3 -45 -45,20
4 -30 -29,99
5 0 -0,42
6 30 30,02
7 45 45,03
8 60 59,73
9 75 75,43
10 90 89,89
 Variasi Nilai y
Data Bacaan Busur Bacaan Gyro (derajat)
1 -75 -75,16
2 -60 -59,67
3 -45 -45,17
4 -30 -29,98
5 0 0,14
6 30 30,09
7 45 45,07
8 60 60,00
9 75 75,31
10 90 89,91
 Variasi Nilai x Bacaan Gyro sebanyak 30 data
Data -75 -60 -45 -30 0 30 45 60 75 90
x x x x x x x x x x
1 -75,35 -59,63 -45,16 -29,96 -0,40 29,94 45,01 59,68 75,37 89,96
2 -75,35 -59,63 -45,17 -29,96 -0,42 29,96 45,01 59,68 75,37 89,96
3 -75,35 -59,61 -45,17 -29,96 -0,43 29,96 45,01 59,68 75,37 89,95
4 -75,34 -59,61 -45,17 -29,97 -0,43 29,96 45,01 59,69 75,38 89,94
5 -75,34 -59,61 -45,17 -29,97 -0,44 29,96 45,01 59,69 75,38 89,95
6 -75,33 -59,60 -45,17 -29,97 -0,46 29,97 45,01 59,69 75,39 89,96
7 -75,33 -59,60 -45,18 -29,97 -0,45 29,97 45,01 59,69 75,39 89,96
8 -75,33 -59,60 -45,18 -29,97 -0,44 29,99 45,01 59,70 75,39 89,95
9 -75,33 -59,60 -45,19 -29,97 -0,42 29,99 45,01 59,71 75,40 89,95
10 -75,32 -59,60 -45,19 -29,97 -0,40 30,00 45,01 59,71 75,40 89,96
11 -75,31 -59,59 -45,19 -29,98 -0,41 30,00 45,02 59,71 75,41 89,93
12 -75,31 -59,58 -45,19 -29,98 -0,42 30,00 45,02 59,72 75,41 89,91
13 -75,31 -59,55 -45,19 -29,98 -0,43 30,01 45,02 59,72 75,42 89,91
14 -75,30 -59,53 -45,19 -29,98 -0,43 30,01 45,02 59,73 75,42 89,91
15 -75,30 -59,50 -45,20 -29,98 -0,44 30,01 45,02 59,73 75,42 89,90
16 -75,29 -59,46 -45,20 -29,98 -0,45 30,01 45,03 59,73 75,43 89,89
17 -75,29 -59,44 -45,20 -29,98 -0,44 30,02 45,03 59,73 75,43 89,90
18 -75,29 -59,43 -45,21 -29,98 -0,42 30,02 45,04 59,74 75,44 89,90
19 -75,28 -59,43 -45,21 -29,98 -0,41 30,03 45,04 59,75 75,44 89,90
20 -75,28 -59,43 -45,21 -29,99 -0,38 30,05 45,04 59,75 75,44 89,88
21 -75,27 -59,42 -45,22 -29,99 -0,38 30,06 45,04 59,75 75,45 89,88
22 -75,27 -59,42 -45,22 -29,99 -0,38 30,07 45,04 59,76 75,46 89,86
23 -75,26 -59,41 -45,22 -30,00 -0,38 30,08 45,04 59,76 75,46 89,84
24 -75,26 -59,41 -45,22 -30,00 -0,38 30,08 45,04 59,77 75,46 89,83
25 -75,26 -59,40 -45,22 -30,00 -0,39 30,08 45,05 59,77 75,46 89,81
26 -75,25 -59,40 -45,22 -30,01 -0,40 30,09 45,05 59,77 75,47 89,80
27 -75,25 -59,38 -45,22 -30,01 -0,41 30,09 45,05 59,77 75,47 89,80
28 -75,24 -59,37 -45,22 -30,02 -0,41 30,10 45,05 59,78 75,48 89,79
29 -75,24 -59,36 -45,23 -30,03 -0,41 30,11 45,05 59,79 75,48 89,78
30 -75,24 -59,36 -45,23 -30,03 -0,43 30,12 45,05 59,79 75,48 89,77

Means -75,30 -59,50 -45,20 -29,99 -0,42 30,02 45,03 59,73 75,43 89,89
 Variasi Nilai y Bacaan Gyro sebanyak 30 data
Data -75 -60 -45 -30 0 30 45 60 75 90
y y y y y y y y y y
1 -75,21 -59,80 -45,13 -29,95 0,08 30,01 45,11 59,95 75,25 89,98
2 -75,21 -59,80 -45,14 -29,96 0,09 30,01 45,11 59,95 75,25 89,98
3 -75,21 -59,78 -45,14 -29,96 0,10 30,01 45,10 59,95 75,25 89,97
4 -75,21 -59,78 -45,14 -29,96 0,11 30,01 45,10 59,96 75,26 89,96
5 -75,20 -59,78 -45,14 -29,97 0,12 30,02 45,10 59,96 75,26 89,97
6 -75,19 -59,77 -45,14 -29,97 0,12 30,03 45,10 59,96 75,27 89,98
7 -75,19 -59,77 -45,15 -29,97 0,12 30,03 45,09 59,96 75,27 89,98
8 -75,19 -59,77 -45,15 -29,97 0,13 30,03 45,09 59,97 75,27 89,97
9 -75,19 -59,77 -45,16 -29,97 0,14 30,03 45,09 59,98 75,28 89,97
10 -75,18 -59,77 -45,16 -29,98 0,15 30,04 45,09 59,98 75,28 89,98
11 -75,17 -59,76 -45,16 -29,98 0,15 30,05 45,09 59,98 75,29 89,95
12 -75,17 -59,75 -45,16 -29,98 0,16 30,06 45,08 59,99 75,29 89,93
13 -75,17 -59,72 -45,16 -29,98 0,16 30,06 45,08 59,99 75,30 89,93
14 -75,16 -59,70 -45,16 -29,98 0,16 30,08 45,08 60,00 75,30 89,93
15 -75,16 -59,67 -45,17 -29,98 0,16 30,08 45,07 60,00 75,30 89,92
16 -75,15 -59,63 -45,17 -29,98 0,16 30,08 45,07 60,00 75,31 89,91
17 -75,15 -59,61 -45,17 -29,98 0,14 30,09 45,07 60,00 75,31 89,92
18 -75,15 -59,60 -45,18 -29,98 0,14 30,09 45,06 60,01 75,32 89,92
19 -75,14 -59,60 -45,18 -29,99 0,15 30,10 45,06 60,02 75,32 89,92
20 -75,14 -59,60 -45,18 -29,99 0,15 30,12 45,06 60,02 75,32 89,90
21 -75,13 -59,59 -45,19 -29,99 0,16 30,13 45,06 60,02 75,33 89,90
22 -75,13 -59,59 -45,19 -29,99 0,16 30,14 45,06 60,03 75,34 89,88
23 -75,12 -59,58 -45,19 -30,00 0,16 30,15 45,05 60,03 75,34 89,86
24 -75,12 -59,58 -45,19 -30,00 0,17 30,15 45,05 60,04 75,34 89,85
25 -75,12 -59,57 -45,19 -30,00 0,17 30,15 45,04 60,04 75,34 89,83
26 -75,11 -59,57 -45,19 -30,01 0,17 30,16 45,04 60,04 75,35 89,82
27 -75,11 -59,55 -45,19 -30,01 0,14 30,16 45,04 60,04 75,35 89,82
28 -75,11 -59,54 -45,19 -30,01 0,14 30,17 45,03 60,05 75,36 89,81
29 -75,10 -59,53 -45,20 -30,01 0,14 30,18 45,01 60,06 75,36 89,80
30 -75,10 -59,53 -45,20 -30,02 0,13 30,19 45,01 60,06 75,36 89,79

Means -75,16 -59,67 -45,17 -29,98 0,14 30,09 45,07 60,00 75,31 89,91
 Variasi Nilai x
Data Bacaan Busur Complementary Filter (derajat)
1 -75 -75,26
2 -60 -59,53
3 -45 -45,19
4 -30 -30,01
5 0 -0,41
6 30 30,04
7 45 45,05
8 60 59,84
9 75 75,30
10 90 89,90

Variasi Nilai y
Data Bacaan Busur Complementary Filter (derajat)
1 -75 -75,17
2 -60 -59,75
3 -45 -45,26
4 -30 -30,00
5 0 -0,02
6 30 30,12
7 45 45,20
8 60 59,99
9 75 75,18
10 90 89,85
 Variasi Nilai x Complementary Filter 30 data
Data -75 -60 -45 -30 0 30 45 60 75 90
x x x x x x x x x x
1 -75,32 -59,69 -45,17 -29,96 -0,39 30,00 45,00 59,79 75,22 89,96
2 -75,26 -59,68 -45,18 -29,96 -0,41 29,99 45,08 59,78 75,22 89,95
3 -75,27 -59,66 -45,20 -29,97 -0,42 29,99 45,03 59,80 75,24 89,96
4 -75,26 -59,65 -45,19 -29,99 -0,42 29,94 45,04 59,84 75,29 89,94
5 -75,26 -59,66 -45,14 -30,00 -0,43 30,03 45,06 59,81 75,28 89,96
6 -75,32 -59,63 -45,13 -29,97 -0,45 30,00 45,08 59,79 75,28 89,96
7 -75,27 -59,63 -45,15 -29,97 -0,44 30,05 45,01 59,80 75,25 89,96
8 -75,27 -59,66 -45,20 -29,98 -0,43 29,97 45,02 59,83 75,26 89,92
9 -75,33 -59,58 -45,15 -30,02 -0,41 30,00 45,04 59,84 75,30 89,96
10 -75,25 -59,61 -45,20 -29,98 -0,39 30,01 45,04 59,85 75,26 89,96
11 -75,30 -59,60 -45,17 -30,03 -0,40 30,05 45,02 59,80 75,31 89,94
12 -75,25 -59,65 -45,21 -30,02 -0,41 29,99 45,01 59,87 75,25 89,92
13 -75,31 -59,62 -45,19 -29,98 -0,42 30,03 45,04 59,82 75,28 89,92
14 -75,29 -59,59 -45,18 -30,00 -0,42 30,02 45,02 59,86 75,31 89,89
15 -75,27 -59,53 -45,16 -29,98 -0,43 30,01 45,03 59,88 75,29 89,91
16 -75,27 -59,47 -45,17 -30,05 -0,44 30,02 45,08 59,84 75,30 89,90
17 -75,25 -59,46 -45,23 -30,06 -0,43 30,08 45,04 59,83 75,32 89,90
18 -75,28 -59,47 -45,19 -30,04 -0,41 30,01 45,04 59,85 75,30 89,91
19 -75,20 -59,43 -45,16 -30,03 -0,40 30,05 45,05 59,86 75,30 89,91
20 -75,26 -59,48 -45,20 -30,05 -0,38 30,03 45,11 59,89 75,32 89,89
21 -75,21 -59,41 -45,17 -30,03 -0,38 30,04 45,08 59,84 75,32 89,88
22 -75,29 -59,43 -45,24 -30,07 -0,38 30,09 45,07 59,82 75,33 89,87
23 -75,28 -59,43 -45,25 -30,04 -0,38 30,13 45,06 59,86 75,31 89,83
24 -75,26 -59,44 -45,25 -29,99 -0,38 30,11 45,05 59,84 75,32 89,84
25 -75,17 -59,44 -45,20 -30,02 -0,38 30,06 45,09 59,87 75,37 89,83
26 -75,23 -59,41 -45,23 -30,01 -0,39 30,11 45,07 59,85 75,34 89,81
27 -75,20 -59,42 -45,20 -30,04 -0,40 30,11 45,07 59,86 75,32 89,81
28 -75,22 -59,41 -45,18 -30,09 -0,40 30,10 45,11 59,91 75,32 89,81
29 -75,23 -59,35 -45,18 -30,05 -0,40 30,09 45,10 59,86 75,35 89,77
30 -75,20 -59,35 -45,26 -30,03 -0,42 30,12 45,06 59,93 75,31 89,78

Means -75,26 -59,53 -45,19 -30,01 -0,41 30,04 45,05 59,84 75,30 89,90
 Variasi Nilai y Complementary Filter 30 data
Data -75 -60 -45 -30 0 30 45 60 75 90
y y y y y y y y y y
1 -75,09 -59,92 -45,29 -29,98 -0,08 30,08 45,11 59,94 75,12 89,98
2 -75,16 -59,85 -45,28 -29,95 -0,07 30,04 45,24 59,91 75,14 89,98
3 -75,26 -59,85 -45,27 -29,97 -0,06 29,97 45,36 60,02 75,11 89,78
4 -75,25 -59,83 -45,24 -29,97 -0,05 29,99 45,36 59,91 75,17 89,87
5 -75,20 -59,82 -45,25 -30,03 -0,04 29,94 45,31 59,99 75,15 89,97
6 -75,24 -59,84 -45,21 -30,03 -0,04 29,92 45,36 59,94 75,12 89,98
7 -75,21 -59,88 -45,29 -30,04 -0,04 29,96 45,34 59,89 75,14 89,84
8 -75,19 -59,88 -45,21 -30,04 -0,03 29,91 45,45 60,01 75,17 89,97
9 -75,25 -59,85 -45,20 -29,98 -0,02 29,87 45,57 60,02 75,17 89,97
10 -75,21 -59,89 -45,19 -29,94 -0,01 29,97 45,44 59,97 75,13 89,98
11 -75,21 -59,84 -45,25 -30,02 -0,01 29,99 45,37 59,97 75,19 89,96
12 -75,15 -59,78 -45,27 -29,92 0,00 30,00 45,30 60,01 75,13 89,85
13 -75,21 -59,83 -45,25 -29,92 0,00 30,06 45,34 59,98 75,13 89,77
14 -75,15 -59,83 -45,24 -29,92 0,00 30,10 45,19 59,98 75,12 89,83
15 -75,12 -59,76 -45,29 -30,00 0,00 30,18 45,21 60,00 75,18 89,74
16 -75,13 -59,68 -45,27 -30,00 0,00 30,20 45,21 59,99 75,19 89,72
17 -75,16 -59,70 -45,27 -30,02 -0,02 30,23 45,16 60,02 75,23 89,75
18 -75,16 -59,64 -45,28 -30,00 -0,02 30,24 45,15 60,01 75,20 89,93
19 -75,19 -59,68 -45,24 -30,00 -0,01 30,24 45,10 60,02 75,17 89,93
20 -75,18 -59,68 -45,31 -30,00 -0,01 30,17 45,13 60,04 75,21 89,89
21 -75,13 -59,71 -45,27 -30,01 0,00 30,25 45,07 59,96 75,21 89,81
22 -75,13 -59,67 -45,28 -30,06 0,00 30,26 45,00 60,02 75,25 89,89
23 -75,14 -59,66 -45,30 -30,03 0,00 30,24 45,09 60,00 75,21 89,61
24 -75,18 -59,65 -45,27 -30,01 0,01 30,24 45,07 60,05 75,20 89,72
25 -75,17 -59,70 -45,25 -30,01 0,01 30,27 45,05 60,01 75,17 89,70
26 -75,15 -59,64 -45,23 -30,00 0,01 30,25 45,04 60,04 75,21 89,84
27 -75,14 -59,63 -45,26 -30,01 -0,02 30,25 45,02 60,00 75,19 89,84
28 -75,09 -59,68 -45,24 -30,01 -0,02 30,25 45,03 59,98 75,25 89,83
29 -75,18 -59,58 -45,27 -30,01 -0,02 30,23 45,02 60,03 75,22 89,82
30 -75,13 -59,63 -45,30 -30,02 -0,03 30,28 45,02 60,03 75,19 89,63

Means -75,17 -59,75 -45,26 -30,00 -0,02 30,12 45,20 59,99 75,18 89,85