Lampiran 1.

Source Code

#include <mega16.h>
#include <stdlib.h>
// I2C Bus functions

#include <i2c.h>
#include <stdint.h>
#include <stdio.h>
#include <delay.h>
#include <math.h>

#asm
.equ __lcd_port=0x18 //port b
#endasm
#include <lcd.h>

#define ADC_VREF_TYPE 0x40

//-------------------
// deklarasi port I2C
//-------------------
#asm
//.equ __i2c_port=0x18 ;PORTB
.equ __i2c_port=0x15 ;PORTC
//.equ __sda_bit=0
//.equ __scl_bit=1
.equ __sda_bit=1
.equ __scl_bit=0
#endasm

//-------------------
// deklarasi prosedur
//-------------------
void akusisi_accel_1();
void akusisi_gyro_1();

53
void setup_mpu6050_1();
void kalibrasi_gyro_1();

void akusisi_accel_2();
void akusisi_gyro_2();
void setup_mpu6050_2();
void kalibrasi_gyro_2();

//-------------------
// deklarasi variabel
//-------------------
float atas0=1100;
float atas1=2000;
float atas2=1800;
float atas3=700;
float atas4=1000;

float bawah0=500;
float bawah1=1100;
float bawah2=600;
float bawah3=500;
float bawah4=500;

int s,t_x1;
float dt = 0.01;
char buff_x1[20], buff_y1[20], buff_x2[20], buffer_tmp[9];

char buff_xx[20], buff_yy[20], buff_adc[20];

float adc_xx, adc_yy;

char sudut1[20],sudut2[20],sudut3[20],sudut4[20],sudut5[20];
float sudut1_f,sudut2_f,sudut3_f,sudut4_f,sudut5_f;

int adc0,adc1,adc2;
int adc3,adc4;

signed int compass_high, compass_low;

54
unsigned char temp_h,temp_l;
signed int temp;
float temperature;
float a=0.99;

//-------------------
// deklarasi variabel
//-------------------
float filter_angle_x,filter_angle_y,filter_angle_z;
unsigned char giro_xh,giro_xl;
unsigned char giro_yh,giro_yl;
unsigned char giro_zh,giro_zl;
float sudut_giro_x;
float sudut_giro_z;
float sudut_giro_y;
signed long giro_z100;
signed long giro_x100;
signed long giro_y100;
signed int giro_x_ofset;
signed int giro_y_ofset;
signed int giro_z_ofset;
float rate_giro_x;
float rate_giro_y;
float rate_giro_z;
float sudut_giro_x;
float sudut_giro_z;
float sudut_giro_y;
signed int giro_x;
signed int giro_y;
signed int giro_z;

unsigned char acc_xh,acc_xl;

signed int acc_x;
unsigned char acc_yh,acc_yl;
signed int acc_y;
unsigned char acc_zh,acc_zl;

55
signed int acc_z;
float acc_sudut_x,acc_sudut_y,acc_sudut_z;///

//-------------------
// deklarasi variabel
//-------------------
float filter_angle_x_2,filter_angle_y_2,filter_angle_z_2;
unsigned char giro_xh_2,giro_xl_2;
unsigned char giro_yh_2,giro_yl_2;
unsigned char giro_zh_2,giro_zl_2;
float sudut_giro_x_2;
float sudut_giro_z_2;
float sudut_giro_y_2;
signed long giro_z100_2;
signed long giro_x100_2;
signed long giro_y100_2;
signed int giro_x_ofset_2;
signed int giro_y_ofset_2;
signed int giro_z_ofset_2;
float rate_giro_x_2;
float rate_giro_y_2;
float rate_giro_z_2;
float sudut_giro_x_2;
float sudut_giro_z_2;
float sudut_giro_y_2;
signed int giro_x_2;
signed int giro_y_2;
signed int giro_z_2;

unsigned char acc_xh_2,acc_xl_2;

signed int acc_x_2;
unsigned char acc_yh_2,acc_yl_2;
signed int acc_y_2;
unsigned char acc_zh_2,acc_zl_2;
signed int acc_z_2;
float acc_sudut_x_2,acc_sudut_y_2,acc_sudut_z_2;///

56
float tmp_x1, tmp_y1, tmp_x2;

int giro_xh100,giro_xl100;
uint16_t giro_yh100,giro_yl100;
uint16_t giro_zh100,giro_zl100;

//---------------------------
// deklarasi variabel MPU6050
//---------------------------
#define giro_x_sens=131;
#define giro_y_sens=131;
#define giro_z_sens=131;
#define compass_address 0xC0 // address compas
#define compass_8bit 0x01
#define compass_16bit1 0x02
#define compass_16bit2 0x03
#define reg_roll 0x05
#define reg_pitch 0x04

// address I2C MPU6050 yang pertama

#define MPU6050_ADDRESS 0xD0 // Address with end write bit 0x68
// address I2C MPU6050 yang kedua
#define MPU6050_ADDRESS_2 0xD2 // Address with end write bit 0x69

#define MPU6050_RA_XG_OFFS_TC 0x00 //[7] PWR_MODE, [6:1]

XG_OFFS_TC, [0] OTP_BNK_VLD
#define MPU6050_RA_YG_OFFS_TC 0x01 //[7] PWR_MODE, [6:1]
YG_OFFS_TC, [0] OTP_BNK_VLD
#define MPU6050_RA_ZG_OFFS_TC 0x02 //[7] PWR_MODE, [6:1]
ZG_OFFS_TC, [0] OTP_BNK_VLD
#define MPU6050_RA_X_FINE_GAIN 0x03 //[7:0] X_FINE_GAIN
#define MPU6050_RA_Y_FINE_GAIN 0x04 //[7:0] Y_FINE_GAIN
#define MPU6050_RA_Z_FINE_GAIN 0x05 //[7:0] Z_FINE_GAIN
#define MPU6050_RA_XA_OFFS_H 0x06 //[15:0] XA_OFFS
#define MPU6050_RA_XA_OFFS_L_TC 0x07
#define MPU6050_RA_YA_OFFS_H 0x08 //[15:0] YA_OFFS
#define MPU6050_RA_YA_OFFS_L_TC 0x09

57
#define MPU6050_RA_ZA_OFFS_H 0x0A //[15:0] ZA_OFFS
#define MPU6050_RA_ZA_OFFS_L_TC 0x0B
#define MPU6050_RA_XG_OFFS_USRH 0x13 //[15:0] XG_OFFS_USR
#define MPU6050_RA_XG_OFFS_USRL 0x14
#define MPU6050_RA_YG_OFFS_USRH 0x15 //[15:0] YG_OFFS_USR
#define MPU6050_RA_YG_OFFS_USRL 0x16
#define MPU6050_RA_ZG_OFFS_USRH 0x17 //[15:0] ZG_OFFS_USR
#define MPU6050_RA_ZG_OFFS_USRL 0x18
#define MPU6050_RA_SMPLRT_DIV 0x19
#define MPU6050_RA_CONFIG 0x1A
#define MPU6050_RA_GYRO_CONFIG 0x1B
#define MPU6050_RA_ACCEL_CONFIG 0x1C
#define MPU6050_RA_FF_THR 0x1D
#define MPU6050_RA_FF_DUR 0x1E
#define MPU6050_RA_MOT_THR 0x1F
#define MPU6050_RA_MOT_DUR 0x20
#define MPU6050_RA_ZRMOT_THR 0x21
#define MPU6050_RA_ZRMOT_DUR 0x22
#define MPU6050_RA_FIFO_EN 0x23
#define MPU6050_RA_I2C_MST_CTRL 0x24
#define MPU6050_RA_I2C_SLV0_ADDR 0x25
#define MPU6050_RA_I2C_SLV0_REG 0x26
#define MPU6050_RA_I2C_SLV0_CTRL 0x27
#define MPU6050_RA_I2C_SLV1_ADDR 0x28
#define MPU6050_RA_I2C_SLV1_REG 0x29
#define MPU6050_RA_I2C_SLV1_CTRL 0x2A
#define MPU6050_RA_I2C_SLV2_ADDR 0x2B
#define MPU6050_RA_I2C_SLV2_REG 0x2C
#define MPU6050_RA_I2C_SLV2_CTRL 0x2D
#define MPU6050_RA_I2C_SLV3_ADDR 0x2E
#define MPU6050_RA_I2C_SLV3_REG 0x2F
#define MPU6050_RA_I2C_SLV3_CTRL 0x30
#define MPU6050_RA_I2C_SLV4_ADDR 0x31
#define MPU6050_RA_I2C_SLV4_REG 0x32
#define MPU6050_RA_I2C_SLV4_DO 0x33
#define MPU6050_RA_I2C_SLV4_CTRL 0x34
#define MPU6050_RA_I2C_SLV4_DI 0x35

58
#define MPU6050_RA_I2C_MST_STATUS 0x36
#define MPU6050_RA_INT_PIN_CFG 0x37
#define MPU6050_RA_INT_ENABLE 0x38
#define MPU6050_RA_DMP_INT_STATUS 0x39
#define MPU6050_RA_INT_STATUS 0x3A
#define MPU6050_RA_ACCEL_XOUT_H 0x3B
#define MPU6050_RA_ACCEL_XOUT_L 0x3C
#define MPU6050_RA_ACCEL_YOUT_H 0x3D
#define MPU6050_RA_ACCEL_YOUT_L 0x3E
#define MPU6050_RA_ACCEL_ZOUT_H 0x3F
#define MPU6050_RA_ACCEL_ZOUT_L 0x40
#define MPU6050_RA_TEMP_OUT_H 0x41
#define MPU6050_RA_TEMP_OUT_L 0x42
#define MPU6050_RA_GYRO_XOUT_H 0x43
#define MPU6050_RA_GYRO_XOUT_L 0x44
#define MPU6050_RA_GYRO_YOUT_H 0x45
#define MPU6050_RA_GYRO_YOUT_L 0x46
#define MPU6050_RA_GYRO_ZOUT_H 0x47
#define MPU6050_RA_GYRO_ZOUT_L 0x48
#define MPU6050_RA_EXT_SENS_DATA_00 0x49
#define MPU6050_RA_EXT_SENS_DATA_01 0x4A
#define MPU6050_RA_EXT_SENS_DATA_02 0x4B
#define MPU6050_RA_EXT_SENS_DATA_03 0x4C
#define MPU6050_RA_EXT_SENS_DATA_04 0x4D
#define MPU6050_RA_EXT_SENS_DATA_05 0x4E
#define MPU6050_RA_EXT_SENS_DATA_06 0x4F
#define MPU6050_RA_EXT_SENS_DATA_07 0x50
#define MPU6050_RA_EXT_SENS_DATA_08 0x51
#define MPU6050_RA_EXT_SENS_DATA_09 0x52
#define MPU6050_RA_EXT_SENS_DATA_10 0x53
#define MPU6050_RA_EXT_SENS_DATA_11 0x54
#define MPU6050_RA_EXT_SENS_DATA_12 0x55
#define MPU6050_RA_EXT_SENS_DATA_13 0x56
#define MPU6050_RA_EXT_SENS_DATA_14 0x57
#define MPU6050_RA_EXT_SENS_DATA_15 0x58
#define MPU6050_RA_EXT_SENS_DATA_16 0x59
#define MPU6050_RA_EXT_SENS_DATA_17 0x5A

59
#define MPU6050_RA_EXT_SENS_DATA_18 0x5B
#define MPU6050_RA_EXT_SENS_DATA_19 0x5C
#define MPU6050_RA_EXT_SENS_DATA_20 0x5D
#define MPU6050_RA_EXT_SENS_DATA_21 0x5E
#define MPU6050_RA_EXT_SENS_DATA_22 0x5F
#define MPU6050_RA_EXT_SENS_DATA_23 0x60
#define MPU6050_RA_MOT_DETECT_STATUS 0x61
#define MPU6050_RA_I2C_SLV0_DO 0x63
#define MPU6050_RA_I2C_SLV1_DO 0x64
#define MPU6050_RA_I2C_SLV2_DO 0x65
#define MPU6050_RA_I2C_SLV3_DO 0x66
#define MPU6050_RA_I2C_MST_DELAY_CTRL 0x67
#define MPU6050_RA_SIGNAL_PATH_RESET 0x68
#define MPU6050_RA_MOT_DETECT_CTRL 0x69
#define MPU6050_RA_USER_CTRL 0x6A
#define MPU6050_RA_PWR_MGMT_1 0x6B
#define MPU6050_RA_PWR_MGMT_2 0x6C
#define MPU6050_RA_BANK_SEL 0x6D
#define MPU6050_RA_MEM_START_ADDR 0x6E
#define MPU6050_RA_MEM_R_W 0x6F
#define MPU6050_RA_DMP_CFG_1 0x70
#define MPU6050_RA_DMP_CFG_2 0x71
#define MPU6050_RA_FIFO_COUNTH 0x72
#define MPU6050_RA_FIFO_COUNTL 0x73
#define MPU6050_RA_FIFO_R_W 0x74
#define MPU6050_RA_WHO_AM_I 0x75

//-------------------
// deklarasi variabel
//-------------------
char lcdbuff[16];
int adc0,adc1,adc2;
float tmp0,tmp1,tmp2;

//-------------------
// prosedur baca ADC

60
//-------------------
unsigned int read_adc(unsigned char adc_input)
{
ADMUX=adc_input | (ADC_VREF_TYPE & 0xff);
delay_us(10);
// Start the AD conversion
ADCSRA|=0x40;
// Wait for the AD conversion to complete
while ((ADCSRA & 0x10)==0);
ADCSRA|=0x10;
return ADCW;
}

//-------------------
// prosedur utama
//-------------------
void main(void)
{
// Declare your local variables here

// Input/Output Ports initialization

// Port A initialization
// Func7=In Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In
Func0=In
// State7=T State6=T State5=T State4=T State3=T State2=T State1=T
State0=T
PORTA=0x00;
DDRA=0x00;

// Port B initialization
// Func7=In Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In
Func0=In
// State7=T State6=T State5=T State4=T State3=T State2=T State1=T
State0=T
PORTB=0x00;
DDRB=0x00;

61
// Port C initialization
// Func7=In Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In
Func0=In
// State7=T State6=T State5=T State4=T State3=T State2=T State1=T
State0=T
PORTC=0x00;
DDRC=0x00;

// Port D initialization
// Func7=In Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In
Func0=In
// State7=T State6=T State5=T State4=T State3=T State2=T State1=T
State0=T
PORTD=0x00;
DDRD=0x18;

// Timer/Counter 0 initialization
// Clock source: System Clock
// Clock value: Timer 0 Stopped
// Mode: Normal top=0xFF
// OC0 output: Disconnected
TCCR0=0x00;
TCNT0=0x00;
OCR0=0x00;

// Timer/Counter 1 initialization
// Clock source: System Clock
// Clock value: Timer1 Stopped
// Mode: Normal top=0xFFFF
// OC1A output: Discon.
// OC1B output: Discon.
// Noise Canceler: Off
// Input Capture on Falling Edge
// Timer1 Overflow Interrupt: Off
// Input Capture Interrupt: Off
// Compare A Match Interrupt: Off

62
// Compare B Match Interrupt: Off
TCCR1A=0x00;
TCCR1B=0x00;
TCNT1H=0x00;
TCNT1L=0x00;
ICR1H=0x00;
ICR1L=0x00;
OCR1AH=0x00;
OCR1AL=0x00;
OCR1BH=0x00;
OCR1BL=0x00;

// Timer/Counter 2 initialization
// Clock source: System Clock
// Clock value: Timer2 Stopped
// Mode: Normal top=0xFF
// OC2 output: Disconnected
ASSR=0x00;
TCCR2=0x00;
TCNT2=0x00;
OCR2=0x00;

// External Interrupt(s) initialization

// INT0: Off
// INT1: Off
// INT2: Off
MCUCR=0x00;
MCUCSR=0x00;

// Timer(s)/Counter(s) Interrupt(s) initialization

TIMSK=0x00;

// USART initialization
// Communication Parameters: 8 Data, 1 Stop, No Parity
// USART Receiver: On
// USART Transmitter: On
// USART Mode: Asynchronous

63
// USART Baud Rate: 9600
//-------------------------
// mode USART / data serial
//-------------------------
UCSRA=0x00;
UCSRB=0x18;
UCSRC=0x86;
UBRRH=0x00;
UBRRL=0x33;

// Analog Comparator initialization

// Analog Comparator: Off
// Analog Comparator Input Capture by Timer/Counter 1: Off
ACSR=0x80;
SFIOR=0x00;

// ADC initialization
// ADC Clock frequency: 1000,000 kHz
// ADC Voltage Reference: AREF pin
// ADC Auto Trigger Source: ADC Stopped
ADMUX=ADC_VREF_TYPE & 0xff;
ADCSRA=0x83;

// SPI initialization
// SPI disabled
SPCR=0x00;

// TWI initialization
// TWI disabled
TWCR=0x00;

lcd_init(16);

// jalankan prosedur inisialisasi MPU6050

setup_mpu6050_1();

64
// lakukan kalibrasi awal
kalibrasi_gyro_1();

while (1)
{

{
// nilai ADC potensio utk gerakan jepit
adc0 = read_adc(0);
// nilai ADC GY61 utk gerak x dan y pada pergelangan robot
adc3 = read_adc(3);
adc4 = read_adc(4);

// batasan hasil bacaan potensio

if(adc0<772){
adc0=772;
}//else

if(adc0>952){
adc0=952;
}

//////////////////////
// bacaan hasil GY61
if(adc3>612){
adc3=612;
}/*else*/
if(adc3<408){
adc3=408;
}
if(adc4>612){
adc4=612;
}/*else*/
if(adc4<408){
adc4=408;
}

65
 // penyesuaian nilai bacaan ADC GY61 utk x dan y, untuk
dikirimkan ke data serial
adc_xx = 2500 - ( ( (float)((float)(adc3-408)/(204)) * 2000 ) + 400);
adc_yy = ( (float)((float)(adc4-408)/(204)) * 2000 ) + 500;

//////////////////////

// penyesuaian nilai bacaan ADC potensio untuk dikirimkan ke

data serial
adc0 = (adc0-772);
adc0 = (( (adc0 / 0.09) + 500)-850); // 0.09 itu resolusi konversi
dari 0-180 ke 0-2000

// dapatkan nilai sensor MPU6050

akusisi_accel_1();
akusisi_gyro_1();

// dapatkan nilai sensor MPU6050

akusisi_accel_2();
akusisi_gyro_2();

tmp_x1=0;
tmp_x2=0;

filter_angle_x =(float) (a) * (filter_angle_x + (rate_giro_x * dt)) +

(float) ((1-a) * (acc_sudut_x));
tmp_x1 = ((float)filter_angle_x * 10);
filter_angle_x_2 =(float) (a) * (filter_angle_x_2 + (rate_giro_x_2 *
dt)) + (float) ((1-a) * (acc_sudut_x_2));t_x1=adc4;
tmp_x2 = ((float)filter_angle_x_2 * 10);

tmp_x2 = 350 + ( (tmp_x2 / 0.09) + 500); // itu resolusi konversi

dari 0-180 ke 500-2500

// batasan nilai yang akan dikirimkan ke lengan robot yang sesuai

dengan posisi maksimal dan minimal lengan (servo 5)

66
 if (tmp_x2>atas4)
{
tmp_x2 = atas4;
}
if (tmp_x2<bawah4)
{
tmp_x2 = bawah4;
}

//ftoa( (float) ((float)( (float)((float)(2000-adc0) -

bawah0)/(float)(atas0-bawah0) ) + 500), 0 , buff_adc);

// batasan nilai yang akan dikirimkan ke lengan robot yang sesuai

dengan posisi maksimal dan minimal penjepit (servo 1)
if (adc0 < bawah0)
{
adc0 = bawah0;
}

// batasan nilai yang akan dikirimkan ke lengan robot yang sesuai

dengan posisi maksimal dan minimal pergelangan (servo 3)
if (adc_yy < bawah1)
{
adc_yy = bawah1;
}
if (adc_yy > atas1)
{
adc_yy = atas1;
}

// batasan nilai yang akan dikirimkan ke lengan robot yang sesuai

dengan posisi maksimal dan minimal pergelangan (servo 2)
if (adc_xx < bawah2)
{
adc_xx = bawah2;
}
if (adc_xx > atas2)

67
 {
adc_xx = atas2;
}

// batasan nilai yang akan dikirimkan ke lengan robot yang sesuai

dengan posisi maksimal dan minimal lengan (servo 4)
tmp_x1 = (200 - ((float) ( ((( ( (float)((float)(t_x1-408)/(204)) * 2000
)))) / 4)) ) + 600;
if (tmp_x1>atas3)
{
tmp_x1 = atas3;
}
if (tmp_x1<bawah3)
{
tmp_x1 = bawah3;
}

// konversi tipe data float ke string

ftoa((float)adc0, 0, buff_adc);

ftoa((float)adc_xx, 0, buff_xx);
ftoa((float)adc_yy, 0, buff_yy);

ftoa((float)tmp_x1, 0, buff_x1);
ftoa((float)tmp_x2, 0, buff_x2);

// lcd_clear();

delay_ms(30);
/**/
/* buat kalau lihat di serial monitor utk nilai 500-2500 nya
*/
// printf("P0:%s x1:%s x2:%s x:%s y:%s\n", buff_adc, buff_x1,
buff_x2, buff_xx, buff_yy);

// kirimkan data serial untuk servo 1

if(adc0<1000){

68
 printf("#00P0%s!",buff_adc);
}
if(adc0>=1000){
printf("#00P%s!",buff_adc);
}

// kirimkan data serial untuk servo 2

if(adc_yy<1000){
printf("#01P0%s!",buff_yy);
}
if(adc_yy>=1000){
printf("#01P%s!",buff_yy);
}

// kirimkan data serial untuk servo 3

if(adc_xx<1000){
printf("#02P0%s!",buff_xx);
}
if(adc_xx>=1000){
printf("#02P%s!",buff_xx);
}

// kirimkan data serial untuk servo 4

if(tmp_x1<1000){
printf("#03P0%s!",buff_x1);
}
if(tmp_x1>=1000){
printf("#03P%s!",buff_x1);
}

// kirimkan data serial untuk servo 5

if(tmp_x2<1000){
//printf("#04P0%s!\n",buff_x2);
printf("#04P0%s!",buff_x2);
}
if(tmp_x2>=1000){
//printf("#04P%s!\n",buff_x2);

69
 printf("#04P%s!",buff_x2);
}
/**/
sudut1_f = (float)(((float)((atof(buff_adc)-500)/2000)) * 180);
sudut2_f = (float)(((float)((atof(buff_yy)-500)/2000)) * 180);
sudut3_f = (float)(((float)((atof(buff_xx)-500)/2000)) * 180);
sudut4_f = (float)(((float)((atof(buff_x1)-500)/2000)) * 180);
sudut5_f = (float)(((float)((atof(buff_x2)-500)/2000)) * 180);

ftoa(sudut1_f, 0, sudut1);
lcd_gotoxy(0,0);
lcd_puts("a") ;
lcd_puts(sudut1);

lcd_gotoxy(5,0);
lcd_puts("b") ;
lcd_puts(sudut2);

lcd_gotoxy(10,0);
lcd_puts("c") ;
lcd_puts(sudut3);

lcd_gotoxy(0,1);
lcd_puts("d") ;
lcd_puts(sudut4);

lcd_gotoxy(5,1);
lcd_puts("e") ;
lcd_puts(sudut5);
/**/

}
}
}

//prosedur untuk membaca data mentah I2C

70
unsigned char read_i2c(unsigned char address, unsigned char reg)
{
unsigned char data;
i2c_start();
i2c_write(address);
i2c_write(reg);
i2c_start();
i2c_write(address|1);
data=i2c_read(0);
i2c_stop();
return data;
}

//prosedur untuk mengirim perintah/request ke I2C

void write_i2c(unsigned char address,unsigned char reg, unsigned char
data)
{
i2c_start();
i2c_write(address);
i2c_write(reg);
i2c_write(data);
i2c_stop();
delay_ms(10);
}

//prosedur untuk pengaturan awal MPU6050 yang pertama

void setup_mpu6050_1()
{ // config smple rate divider
write_i2c(MPU6050_ADDRESS,MPU6050_RA_SMPLRT_DIV,0x01);

// config fsync dan digital low pass filter(DLPF)

write_i2c(MPU6050_ADDRESS,MPU6050_RA_CONFIG,0x00);

// config gyro outputs

write_i2c(MPU6050_ADDRESS, MPU6050_RA_GYRO_CONFIG,
0b00000000);

71
 // config output aceel
write_i2c(MPU6050_ADDRESS, MPU6050_RA_ACCEL_CONFIG,
0b00000000);

write_i2c(MPU6050_ADDRESS, MPU6050_RA_FF_THR, 0x00);

//Freefall duration limit of 0
write_i2c(MPU6050_ADDRESS, MPU6050_RA_FF_DUR, 0x00);
//Motion threshold of 0mg
write_i2c(MPU6050_ADDRESS, MPU6050_RA_MOT_THR, 0x00);
//Motion duration of 0s
write_i2c(MPU6050_ADDRESS, MPU6050_RA_MOT_DUR, 0x00);
//Zero motion threshold
write_i2c(MPU6050_ADDRESS, MPU6050_RA_ZRMOT_THR, 0x00);
//Zero motion duration threshold
write_i2c(MPU6050_ADDRESS, MPU6050_RA_ZRMOT_DUR, 0x00);
//Disable sensor output to FIFO buffer
write_i2c(MPU6050_ADDRESS, MPU6050_RA_FIFO_EN, 0x00);
//i2c MST CLOCK 348 kHz divider 23 dari 8Mhz mpu6050 internal clk
write_i2c(MPU6050_ADDRESS, MPU6050_RA_I2C_MST_CTRL,
0x00);
//Setup AUX I2C slaves
write_i2c(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_ADDR,
0x00);
write_i2c(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_REG, 0x00);
write_i2c(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_CTRL,
0x00);
write_i2c(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV1_ADDR,
0x00);
write_i2c(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV1_REG, 0x00);
write_i2c(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV1_CTRL,
0x00);
write_i2c(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_ADDR,
0x00);
write_i2c(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_REG, 0x00);
write_i2c(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_CTRL,
0x00);

72
 write_i2c(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV3_ADDR,
0x00);
write_i2c(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV3_REG, 0x00);
write_i2c(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV3_CTRL,
0x00);
write_i2c(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_ADDR,
0x00);
write_i2c(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_REG, 0x00);
write_i2c(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_DO, 0x00);
write_i2c(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_CTRL,
0x00);
write_i2c(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_DI, 0x00);
//MPU6050_RA_I2C_MST_STATUS //Read-only
//Setup INT pin and AUX I2C pass through
write_i2c(MPU6050_ADDRESS, MPU6050_RA_INT_PIN_CFG, 0x00);
//Enable data ready interrupt
write_i2c(MPU6050_ADDRESS, MPU6050_RA_INT_ENABLE, 0x00);
//Slave out, dont care
write_i2c(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_DO, 0x00);
write_i2c(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV1_DO, 0x00);
write_i2c(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_DO, 0x00);
write_i2c(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV3_DO, 0x00);
//More slave config
write_i2c(MPU6050_ADDRESS,
MPU6050_RA_I2C_MST_DELAY_CTRL, 0x00);
//Reset sensor signal paths
write_i2c(MPU6050_ADDRESS, MPU6050_RA_SIGNAL_PATH_RESET,
0x00);
//Motion detection control
write_i2c(MPU6050_ADDRESS, MPU6050_RA_MOT_DETECT_CTRL,
0x00);
//Disables FIFO, AUX I2C, FIFO and I2C reset bits to 0
write_i2c(MPU6050_ADDRESS, MPU6050_RA_USER_CTRL, 0x00);
//Sets clock source to gyro reference w/ PLL
write_i2c(MPU6050_ADDRESS, MPU6050_RA_PWR_MGMT_1,
0b00000010);

73
 //Controls frequency of wakeups in accel low power mode plus the
sensor standby modes
write_i2c(MPU6050_ADDRESS, MPU6050_RA_PWR_MGMT_2,
0x00);
write_i2c(MPU6050_ADDRESS, MPU6050_RA_FIFO_R_W, 0x00);

//lcd_gotoxy(0,0);
//lcd_putsf("setup done");
printf("Setup selesai...%i\n",1);
delay_ms(1000);

setup_mpu6050_2();
}

//prosedur untuk pengaturan awal MPU6050 yang kedua

void setup_mpu6050_2()
{ // config smple rate divider
write_i2c(MPU6050_ADDRESS_2, MPU6050_RA_SMPLRT_DIV,0x01);

// config fsync dan digital low pass filter(DLPF)

write_i2c(MPU6050_ADDRESS_2, MPU6050_RA_CONFIG,0x00);

// config gyro outputs

write_i2c(MPU6050_ADDRESS_2, MPU6050_RA_GYRO_CONFIG,
0b00000000);

// config output aceel

write_i2c(MPU6050_ADDRESS_2, MPU6050_RA_ACCEL_CONFIG,
0b00000000);

write_i2c(MPU6050_ADDRESS_2, MPU6050_RA_FF_THR, 0x00);

//Freefall duration limit of 0
write_i2c(MPU6050_ADDRESS_2, MPU6050_RA_FF_DUR, 0x00);
//Motion threshold of 0mg
write_i2c(MPU6050_ADDRESS_2, MPU6050_RA_MOT_THR, 0x00);
//Motion duration of 0s
write_i2c(MPU6050_ADDRESS_2, MPU6050_RA_MOT_DUR, 0x00);

74
 //Zero motion threshold
write_i2c(MPU6050_ADDRESS_2, MPU6050_RA_ZRMOT_THR, 0x00);
//Zero motion duration threshold
write_i2c(MPU6050_ADDRESS_2, MPU6050_RA_ZRMOT_DUR,
0x00);
//Disable sensor output to FIFO buffer
write_i2c(MPU6050_ADDRESS_2, MPU6050_RA_FIFO_EN, 0x00);
//i2c MST CLOCK 348 kHz divider 23 dari 8Mhz mpu6050 internal clk
write_i2c(MPU6050_ADDRESS_2, MPU6050_RA_I2C_MST_CTRL,
0x00);
//Setup AUX I2C slaves
write_i2c(MPU6050_ADDRESS_2, MPU6050_RA_I2C_SLV0_ADDR,
0x00);
write_i2c(MPU6050_ADDRESS_2, MPU6050_RA_I2C_SLV0_REG,
0x00);
write_i2c(MPU6050_ADDRESS_2, MPU6050_RA_I2C_SLV0_CTRL,
0x00);
write_i2c(MPU6050_ADDRESS_2, MPU6050_RA_I2C_SLV1_ADDR,
0x00);
write_i2c(MPU6050_ADDRESS_2, MPU6050_RA_I2C_SLV1_REG,
0x00);
write_i2c(MPU6050_ADDRESS_2, MPU6050_RA_I2C_SLV1_CTRL,
0x00);
write_i2c(MPU6050_ADDRESS_2, MPU6050_RA_I2C_SLV2_ADDR,
0x00);
write_i2c(MPU6050_ADDRESS_2, MPU6050_RA_I2C_SLV2_REG,
0x00);
write_i2c(MPU6050_ADDRESS_2, MPU6050_RA_I2C_SLV2_CTRL,
0x00);
write_i2c(MPU6050_ADDRESS_2, MPU6050_RA_I2C_SLV3_ADDR,
0x00);
write_i2c(MPU6050_ADDRESS_2, MPU6050_RA_I2C_SLV3_REG,
0x00);
write_i2c(MPU6050_ADDRESS_2, MPU6050_RA_I2C_SLV3_CTRL,
0x00);
write_i2c(MPU6050_ADDRESS_2, MPU6050_RA_I2C_SLV4_ADDR,
0x00);

75
 write_i2c(MPU6050_ADDRESS_2, MPU6050_RA_I2C_SLV4_REG,
0x00);
write_i2c(MPU6050_ADDRESS_2, MPU6050_RA_I2C_SLV4_DO,
0x00);
write_i2c(MPU6050_ADDRESS_2, MPU6050_RA_I2C_SLV4_CTRL,
0x00);
write_i2c(MPU6050_ADDRESS_2, MPU6050_RA_I2C_SLV4_DI,
0x00);
//MPU6050_RA_I2C_MST_STATUS //Read-only
//Setup INT pin and AUX I2C pass through
write_i2c(MPU6050_ADDRESS_2, MPU6050_RA_INT_PIN_CFG,
0x00);
//Enable data ready interrupt
write_i2c(MPU6050_ADDRESS_2, MPU6050_RA_INT_ENABLE,
0x00);
//Slave out, dont care
write_i2c(MPU6050_ADDRESS_2, MPU6050_RA_I2C_SLV0_DO,
0x00);
write_i2c(MPU6050_ADDRESS_2, MPU6050_RA_I2C_SLV1_DO,
0x00);
write_i2c(MPU6050_ADDRESS_2, MPU6050_RA_I2C_SLV2_DO,
0x00);
write_i2c(MPU6050_ADDRESS_2, MPU6050_RA_I2C_SLV3_DO,
0x00);
//More slave config
write_i2c(MPU6050_ADDRESS_2,
MPU6050_RA_I2C_MST_DELAY_CTRL, 0x00);
//Reset sensor signal paths
write_i2c(MPU6050_ADDRESS_2,
MPU6050_RA_SIGNAL_PATH_RESET, 0x00);
//Motion detection control
write_i2c(MPU6050_ADDRESS_2,
MPU6050_RA_MOT_DETECT_CTRL, 0x00);
//Disables FIFO, AUX I2C, FIFO and I2C reset bits to 0
write_i2c(MPU6050_ADDRESS_2, MPU6050_RA_USER_CTRL, 0x00);
//Sets clock source to gyro reference w/ PLL

76
 write_i2c(MPU6050_ADDRESS_2, MPU6050_RA_PWR_MGMT_1,
0b00000010);
//Controls frequency of wakeups in accel low power mode plus the
sensor standby modes
write_i2c(MPU6050_ADDRESS_2, MPU6050_RA_PWR_MGMT_2,
0x00);
write_i2c(MPU6050_ADDRESS_2, MPU6050_RA_FIFO_R_W, 0x00);

//lcd_gotoxy(0,0);
//lcd_putsf("setup done");
printf("Setup selesai...%i\n",2);
delay_ms(1000);

//prosedur untuk mendapatkan data akselerasi dari MPU6050 yang

pertama
void akusisi_accel_1()
{
acc_xh =
read_i2c(MPU6050_ADDRESS,MPU6050_RA_ACCEL_XOUT_H);
acc_xl =
read_i2c(MPU6050_ADDRESS,MPU6050_RA_ACCEL_XOUT_L);
acc_yh =
read_i2c(MPU6050_ADDRESS,MPU6050_RA_ACCEL_YOUT_H);
acc_yl =
read_i2c(MPU6050_ADDRESS,MPU6050_RA_ACCEL_YOUT_L);
acc_zh =
read_i2c(MPU6050_ADDRESS,MPU6050_RA_ACCEL_ZOUT_H);
acc_zl =
read_i2c(MPU6050_ADDRESS,MPU6050_RA_ACCEL_ZOUT_L);
acc_x = (float)(((int)acc_xh<<8)|(int)acc_xl);
acc_y = (float)(((int)acc_yh<<8)|(int)acc_yl);
acc_z = (float)(((int)acc_zh<<8)|(int)acc_zl);

/*

77
 acc_sudut_x =(float) 57.295*atan((float)acc_y/
sqrt(pow((float)acc_z,2)+pow((float)acc_x,2)));
acc_sudut_y =(float) 57.295*atan((float)-acc_x/
sqrt(pow((float)acc_z,2)+pow((float)acc_y,2)));
acc_sudut_z =(float)
57.295*atan((float)(sqrt(pow(acc_x,2)+pow((float)acc_y,2))/(float)acc_
z)); */

acc_sudut_x = acc_x/16384.0;
acc_sudut_y = acc_y/16384.0;
acc_sudut_z = acc_z/16384.0;
}

//prosedur untuk mendapatkan data akselerasi dari MPU6050 yang

kedua
void akusisi_accel_2()
{
acc_xh_2 =
read_i2c(MPU6050_ADDRESS_2,MPU6050_RA_ACCEL_XOUT_H);
acc_xl_2 =
read_i2c(MPU6050_ADDRESS_2,MPU6050_RA_ACCEL_XOUT_L);
acc_yh_2 =
read_i2c(MPU6050_ADDRESS_2,MPU6050_RA_ACCEL_YOUT_H);
acc_yl_2 =
read_i2c(MPU6050_ADDRESS_2,MPU6050_RA_ACCEL_YOUT_L);
acc_zh_2 =
read_i2c(MPU6050_ADDRESS_2,MPU6050_RA_ACCEL_ZOUT_H);
acc_zl_2 =
read_i2c(MPU6050_ADDRESS_2,MPU6050_RA_ACCEL_ZOUT_L);
acc_x_2 = (float)(((int)acc_xh_2<<8)|(int)acc_xl_2);
acc_y_2 = (float)(((int)acc_yh_2<<8)|(int)acc_yl_2);
acc_z_2 = (float)(((int)acc_zh_2<<8)|(int)acc_zl_2);

acc_sudut_x_2 = acc_x_2/16384.0;
acc_sudut_y_2 = acc_y_2/16384.0;
acc_sudut_z_2 = acc_z_2/16384.0;

78
}

//prosedur untuk melakukan kalibrasi sensor gyro MPU6050 yang

pertama
void kalibrasi_gyro_1()
{
printf("Kalibrasi Gyro...%i\n",1);

for(s = 0;s<1000;s++)
{
giro_xh =
read_i2c(MPU6050_ADDRESS,MPU6050_RA_GYRO_XOUT_H);
giro_xl =
read_i2c(MPU6050_ADDRESS,MPU6050_RA_GYRO_XOUT_L);
giro_yh =
read_i2c(MPU6050_ADDRESS,MPU6050_RA_GYRO_YOUT_H);
giro_yl =
read_i2c(MPU6050_ADDRESS,MPU6050_RA_GYRO_YOUT_L);
giro_zh =
read_i2c(MPU6050_ADDRESS,MPU6050_RA_GYRO_ZOUT_H);
giro_zl =
read_i2c(MPU6050_ADDRESS,MPU6050_RA_GYRO_ZOUT_L);

giro_x100 += ((giro_xh<<8)|(giro_xl));
giro_y100 += ((giro_yh<<8)|(giro_yl));
giro_z100 += ((giro_zh<<8)|(giro_zl));

delay_ms(1);
}

giro_x_ofset = giro_x100/1000;
giro_y_ofset = giro_y100/1000;
giro_z_ofset = giro_z100/1000;

printf("Kalibrasi Gyro Selesai...%i\n",1);

79
 kalibrasi_gyro_2();
}

//prosedur untuk melakukan kalibrasi sensor gyro MPU6050 yang

kedua
void kalibrasi_gyro_2()
{
printf("Kalibrasi Gyro...%i\n",2);

for(s = 0;s<1000;s++)
{
giro_xh_2 =
read_i2c(MPU6050_ADDRESS_2,MPU6050_RA_GYRO_XOUT_H);
giro_xl_2 =
read_i2c(MPU6050_ADDRESS_2,MPU6050_RA_GYRO_XOUT_L);
giro_yh_2 =
read_i2c(MPU6050_ADDRESS_2,MPU6050_RA_GYRO_YOUT_H);
giro_yl_2 =
read_i2c(MPU6050_ADDRESS_2,MPU6050_RA_GYRO_YOUT_L);
giro_zh_2 =
read_i2c(MPU6050_ADDRESS_2,MPU6050_RA_GYRO_ZOUT_H);
giro_zl_2 =
read_i2c(MPU6050_ADDRESS_2,MPU6050_RA_GYRO_ZOUT_L);

giro_x100_2 += ((giro_xh_2<<8)|(giro_xl_2));
giro_y100_2 += ((giro_yh_2<<8)|(giro_yl_2));
giro_z100_2 += ((giro_zh_2<<8)|(giro_zl_2));

delay_ms(1);
}

giro_x_ofset_2 = giro_x100_2/1000;
giro_y_ofset_2 = giro_y100_2/1000;
giro_z_ofset_2 = giro_z100_2/1000;

printf("Kalibrasi Gyro Selesai...%i\n",2);

}

80
//prosedur untuk mendapatkan data sensor gyro MPU6050 yang
pertama
void akusisi_gyro_1()
{
giro_xh =
read_i2c(MPU6050_ADDRESS,MPU6050_RA_GYRO_XOUT_H);
giro_xl =
read_i2c(MPU6050_ADDRESS,MPU6050_RA_GYRO_XOUT_L);
giro_yh =
read_i2c(MPU6050_ADDRESS,MPU6050_RA_GYRO_YOUT_H);
giro_yl =
read_i2c(MPU6050_ADDRESS,MPU6050_RA_GYRO_YOUT_L);
giro_zh =
read_i2c(MPU6050_ADDRESS,MPU6050_RA_GYRO_ZOUT_H);
giro_zl =
read_i2c(MPU6050_ADDRESS,MPU6050_RA_GYRO_ZOUT_L);

giro_x = ((giro_xh<<8)|giro_xl)- giro_x_ofset;

giro_y = ((giro_yh<<8)|giro_yl)- giro_y_ofset;
giro_z = ((giro_zh<<8)|giro_zl)- giro_z_ofset;

sudut_giro_x = giro_x/16.4;
sudut_giro_y = giro_y/16.4;
sudut_giro_z = giro_z/16.4;

rate_giro_x = giro_x/giro_x_sens;
sudut_giro_x +=(float)(rate_giro_x*dt);
}

//prosedur untuk mendapatkan data sensor gyro MPU6050 yang kedua

void akusisi_gyro_2()
{
giro_xh_2 =
read_i2c(MPU6050_ADDRESS_2,MPU6050_RA_GYRO_XOUT_H);

81
 giro_xl_2 =
read_i2c(MPU6050_ADDRESS_2,MPU6050_RA_GYRO_XOUT_L);
giro_yh_2 =
read_i2c(MPU6050_ADDRESS_2,MPU6050_RA_GYRO_YOUT_H);
giro_yl_2 =
read_i2c(MPU6050_ADDRESS_2,MPU6050_RA_GYRO_YOUT_L);
giro_zh_2 =
read_i2c(MPU6050_ADDRESS_2,MPU6050_RA_GYRO_ZOUT_H);
giro_zl_2 =
read_i2c(MPU6050_ADDRESS_2,MPU6050_RA_GYRO_ZOUT_L);

giro_x_2 = ((giro_xh_2<<8)|giro_xl_2)- giro_x_ofset_2;

giro_y_2 = ((giro_yh_2<<8)|giro_yl_2)- giro_y_ofset_2;
giro_z_2 = ((giro_zh_2<<8)|giro_zl_2)- giro_z_ofset_2;

sudut_giro_x_2 = giro_x_2/16.4;
sudut_giro_y_2 = giro_y_2/16.4;
sudut_giro_z_2 = giro_z_2/16.4;

rate_giro_x_2 = giro_x_2/giro_x_sens;
sudut_giro_x_2 +=(float)(rate_giro_x_2*dt);
}

82
 BIODATA PENULIS

Nama : Ainur Rizky Amirullah

Tempat, Tanggal Lahir : Bangkalan, 12 Agustus 1995
Alamat : Perum Batara Regency Kav. 02
Jl. Nusa Indah Perumda
Kewarganegaraan : Indonesia
Agama : Islam
Telepon : 081937270008
Email : ainur120895@gmail.com

RIWAYAT PENDIDIKAN
2001-2007 : SD Negeri Batah Barat 2
2007-2010 : SMP Negeri 3 Bangkalan
2010-2013 : SMA Negeri 2 Bangkalan
2013-2019 : Teknik Elektro Universitas Trunojoyo
Madura

83