Pada kesempatan kali ini saya akan menjelaskan mengenai bagaimana cara membuat alat yang bisa menampilkan nilai dari sensor IMU MPU GY-80 dan juga mengambil data lokasi dari GPS Handphone Android. alat ini memang lumayan kompleks dan juga programnya agak panjang sehingga harus diperhatikan tentang register dan address i2cnya. alat ini digunakan untuk menentukan kemiringan suatu daerah dan juga arah suatu benda beserta lokasinya. maka dari itu dipakailah sensor IMU dan nilai lokasinya nya diambil dari GPS Handphone Android. untuk lebih jelasnya berikut adalah skema dan programnya.
a. Minimum System ATMega
b. MPU GY-80
c. Bluetooth HC-05
d. Handphone Android Dengan Fitur GPS
e. Program Android
f. Program Bascom AVR
'menggunakan Bascom AVR versi 2.0.7.5
$regfile = "m16def.dat"
$crystal = 12000000
$baud = 9600
$hwstack = 32
$swstack = 10
$framesize = 40
Config Lcdpin = Pin , Rs = Portd.7 , E = Portd.6 , Db4 = Portc.2
Config Lcdpin = Pin , Db5 = Portc.3 , Db6 = Portc.4 , Db7 = Portc.5
Config Lcd = 16 * 2
Dim Lop As Integer
Cls
Cursor Off
Dim Inchar As String * 50
Dim Count As Integer
Dim I As Byte
Dim Flagset As Bit '0 = Waktu, 1 = Tanggal
Dim Flagreset As Bit
Dim Line1 As String * 1
Dim Zx As Integer
Dim Datas As String * 50
Dim Id As String * 50
Dim Data1 As String * 50
Dim Data2 As String * 50
Dim Data3 As String * 50
Dim Cmd_pos1 As Byte
Dim Cmd_pos2 As Byte
Dim Datain2 As Single
Dim Datain3 As Single
Datas = ""
Zx = 0
Lop = 1
Cls
Cursor Off , Noblink
'=====[ I2C BUS INIT ]==========================================================
'configure the scl and sda pins
$lib "I2C_TWI.LBX"
Config Scl = Portc.0
Config Sda = Portc.1
Config Twi = 400000
I2cinit
'=====[ Setup Timer0 for Tick isr ]=============================================
Config Timer2 = Timer , Prescale = 1024
' crystal / Prescale / CountWanted = reload value
' 18432000 / 1024 / 100Hz = 180 (trigger every 100th sec (10ms))
Ocr2 = 180 - 1
On Oc2 Tickcounter_isr
Enable Oc2
'-------------------------------------------------------------------------------
'====[ ALAMAT ADXL345 ]=========================================================
Const Adxl_r = &HA7
Const Adxl_w = &HA6
Const Power_ctl = &H2D
Const Data_format = &H31
Const Data_xl = &H32
Const Data_xh = &H33
Const Data_yl = &H34
Const Data_yh = &H35
Const Data_zl = &H36
Const Data_zh = &H37
'-------------------------------------------------------------------------------
'====[ ALAMAT BMP085 ]==========================================================
Const Addr_schreiben = &B11101110
Const Addr_lesen = &B11101111
'-------------------------------------------------------------------------------
'====[ ALAMAT HMC5883L ]========================================================
Const Hmc_w = &H3C
Const Hmc_r = &H3D
Const Mode_register_compass = &H02
Const Continous_register_mode = &H00
'-------------------------------------------------------------------------------
'====[ ALAMAT L3G4200D ]========================================================
Const Who_am_i = &H0F
Const L3g4200d_r = &HD3
Const L3g4200d_w = &HD2
Const Ctrl_reg1 = &H20
Const Ctrl_reg2 = &H21
Const Ctrl_reg3 = &H22
Const Ctrl_reg4 = &H23
Const Ctrl_reg5 = &H24
'-------------------------------------------------------------------------------
'====[ Variabel ADXL345 ]=======================================================
Dim X As Integer
Dim Xl As Byte At X + 0 Overlay
Dim Xh As Byte At X + 1 Overlay
Dim Y As Integer
Dim Yl As Byte At Y + 0 Overlay
Dim Yh As Byte At Y + 1 Overlay
Dim Z As Integer
Dim Zl As Byte At Z + 0 Overlay
Dim Zh As Byte At Z + 1 Overlay
'-------------------------------------------------------------------------------
'====[ Variabel BMP085 ]========================================================
Dim Ac1 As Integer
Dim Ac1a As Long
Dim Ac2 As Integer
Dim Ac3 As Integer
Dim Ac4 As Word
Dim Ac5 As Word
Dim Ac5a As Single
Dim Ac6 As Word
Dim B1 As Integer
Dim B2 As Integer
Dim B3 As Long
Dim B3a As Long
Dim B4 As Long
Dim B5 As Long
Dim B6 As Integer
Dim B6a As Long
Dim B7 As Long
Dim Mb As Integer
Dim Mc As Integer
Dim Mc2 As Long
Dim Md As Integer
Dim Adres As Byte
Dim Wert As Byte
Dim Ut As Long
Dim X1 As Long
Dim X1t As Single
Dim X2 As Long
Dim X3 As Long
Dim T As Single
Dim T2 As Long
Dim Temperatur As String * 6
Dim Luftdruck As String * 8
Dim Druck As Single
Dim P As Long
Dim P2 As Long
Dim 2potenz15 As Long
Dim 2potenz11 As Integer
Dim 2potenz13 As Integer
Dim 2potenz8 As Integer
Dim 2potenz4 As Integer
Dim 2potenz2 As Integer
Dim 2potenz16 As Long
Dim 2potenz12 As Integer
Dim Potenz5255 As Single
Dim Altitude As Single
Dim Altitudes As String * 8
Dim Msb As Long
Dim Lsb As Long
Dim Up As Long
Dim Druckvar As Byte
Declare Sub Berechne_temp
Declare Sub Berechne_druck
Declare Sub Schreibe_bmp085(byval Adres As Byte , Byval Wert As Byte)
Declare Sub Lese_bmp085(byval Adres As Byte , Wert As Byte)
Declare Sub Lese_roh_daten
Declare Sub Messen
Declare Sub Lese_kalib_data
'====[ Variabel HMC5883L ]======================================================
Dim Hmc_x As Integer
Dim Hmc_xl As Byte At Hmc_x + 0 Overlay
Dim Hmc_xh As Byte At Hmc_x + 1 Overlay
Dim Hmc_y As Integer
Dim Hmc_yl As Byte At Hmc_y + 0 Overlay
Dim Hmc_yh As Byte At Hmc_y + 1 Overlay
Dim Hmc_z As Integer
Dim Hmc_zl As Byte At Hmc_z + 0 Overlay
Dim Hmc_zh As Byte At Hmc_z + 1 Overlay
Dim Hmc_status As Byte
Dim X_comp As Single
Dim Y_comp As Single
Dim Z_comp As Single
Dim Angle As Single
'-------------------------------------------------------------------------------
'=====[ Variabel L3G4200D ]=====================================================
Dim Tick As Word ' used for 100Hz tick count
Dim Tickn As Word ' used for next timer tick event
Dim N As Integer ' temp var for visual positioning
Dim Xg As Integer
Dim Xlg As Byte At Xg + 0 Overlay
Dim Xhg As Byte At Xg + 1 Overlay
Dim Yg As Integer
Dim Ylg As Byte At Yg + 0 Overlay
Dim Yhg As Byte At Yg + 1 Overlay
Dim Zg As Integer
Dim Zlg As Byte At Zg + 0 Overlay
Dim Zhg As Byte At Zg + 1 Overlay
Dim Temp As Byte
Dim Status As Byte
Dim Test_gyro As Byte
'-------------------------------------------------------------------------------
Cls
Locate 1 , 1
Lcd "Inisialisasi"
Locate 2 , 1
Lcd "ADXL345"
Wait 1
Gosub Initaccel
Cls
Locate 1 , 1
Lcd "Inisialisasi"
Locate 2 , 1
Lcd "BMP085"
Wait 1
Call Lese_kalib_data
Cls
Locate 1 , 1
Lcd "Inisialisasi"
Locate 2 , 1
Lcd "HMC5883L"
Wait 1
Gosub Hmc_initialize
Cls
Locate 1 , 1
Lcd "Inisialisasi"
Locate 2 , 1
Lcd "L3G4200D"
Wait 1
Gosub Initgyro
Main:
On Urxc Getchar
Enable Interrupts
Enable Urxc
Utama:
Cls
Do
Gosub Read_accel
Gosub Hmc_readdata
Gosub Hmc_readstatus
Locate 2 , 1
Lcd X ; " " ; Y ; " " ; Z ; " "
Call Messen
T = T / 10
Temperatur = Fusing(t , "###.##")
Potenz5255 = 1 / 5.255
Altitude = P / 101325
Altitude = Altitude ^ Potenz5255
Altitude = 1 - Altitude
Altitude = Altitude * 44330
Druck = P
Druck = Druck / 100
Luftdruck = Fusing(druck , "####.##")
Altitudes = Fusing(altitude , "#####.##")
'Convert integer to float
X_comp = Hmc_x
Y_comp = Hmc_y
Z_comp = Hmc_z
'Calculate angle:
Angle = Atn2(y_comp , X_comp)
Angle = Angle * 57.29577951
Angle = Angle + 180
If Tick >= Tickn Then
Gosub Read_gyro
Xg = Xg / 256
Yg = Yg / 256
Zg = Zg / 256
Locate 1 , 1
'Lcd "X=" ; Xg ; " " ; "Y=" ; Yg ; " " ; "Z=" ; Zg
Lcd "Y=" ; Yg
Tickn = Tickn + 20
End If
'Config Powermode = Idle
Locate 1 , 8
'Lcd "A=" ; Fusing(angle , "#.#")
Lcd "x=" ; X_comp
Zx = 0
Incr Count
Waitms 10
Loop Until Count > 300
Count = 0
Wait 1
Cls
Do
Upperline
Lcd "LAT= " ; Datain3
Lowerline
Lcd "Long= " ; Datain2
Gosub Read_accel
Gosub Hmc_readdata
Gosub Hmc_readstatus
Call Messen
T = T / 10
Temperatur = Fusing(t , "###.##")
Potenz5255 = 1 / 5.255
Altitude = P / 101325
Altitude = Altitude ^ Potenz5255
Altitude = 1 - Altitude
Altitude = Altitude * 44330
Druck = P
Druck = Druck / 100
Luftdruck = Fusing(druck , "####.##")
Altitudes = Fusing(altitude , "#####.##")
'Convert integer to float
X_comp = Hmc_x
Y_comp = Hmc_y
Z_comp = Hmc_z
'Calculate angle:
Angle = Atn2(y_comp , X_comp)
Angle = Angle * 57.29577951
Angle = Angle + 180
If Tick >= Tickn Then
Gosub Read_gyro
Xg = Xg / 256
Yg = Yg / 256
Zg = Zg / 256
Tickn = Tickn + 20
End If
Zx = 0
Incr Count
Waitms 10
Loop Until Count > 300
Count = 0
Goto Utama
'=====[ Inisialisasi BMP085]=========================================================
'=====[ Read the raw temp from the BMP085 ]=====================================
Sub Messen
'Temparatur messen
Call Schreibe_bmp085(&Hf4 , &H2E)
Adres = &HF6
Call Lese_roh_daten
'Print "msb " ; Msb
'Print "lsb " ; Lsb
Shift Msb , Left , 8
'Print "msb shift " ; Msb
Ut = Msb + Lsb
Call Berechne_temp
'Druck messen
Call Schreibe_bmp085(&Hf4 , &H74 )
Adres = &HF6
Call Lese_roh_daten
Shift Msb , Left , 16
Shift Lsb , Left , 8
Up = Msb + Lsb
Shift Up , Right , 6
'Print "msb " ; Msb ; "lsb " ; Lsb ; "up " ; Up
Call Berechne_druck
End Sub
Sub Berechne_temp
'X1
Ut = Ut - Ac6
2potenz15 = 2 ^ 15
Ac5a = Ac5 / 2potenz15
X1t = Ut * Ac5a
'Print "X1 " ; X1t
'X2
2potenz11 = 2 ^ 11
Mc2 = Mc * 2potenz11
X3 = X1t + Md
X2 = Mc2 / X3
'Print "X2 " ; X2
'B5
B5 = X1t + X2
'Print "B5 " ; B5
'T
2potenz4 = 2 ^ 4
T2 = B5 + 8
T = T2 / 2potenz4
End Sub
Sub Berechne_druck
'B6
B6 = B5 - 4000
'X1
2potenz12 = 2 ^ 12
B6a = B6 * B6
B6a = B6a / 2potenz12
X1 = B2 * B6a
X1 = X1 / 2potenz11
'X2
X2 = Ac2 * B6
X2 = X2 / 2potenz11
'X3
X3 = X1 + X2
'B3
Ac1a = Ac1 * 4
Ac1a = Ac1a + X3
Shift Ac1a , Left , 2
Ac1a = Ac1a + 2
B3 = Ac1a / 4
'X1 die zweite
2potenz13 = 2 ^ 13
B6a = B6 / 2potenz13
X1 = Ac3 * B6a
'X2 die zweite
B6a = B6 * B6
B6a = B6a / 2potenz12
X2 = B1 * B6a
2potenz16 = 2 ^ 16
X2 = X2 / 2potenz16
'X3 die zweite
X3 = X1 + X2
X3 = X3 + 2
2potenz2 = 2 ^ 2
X3 = X3 / 2potenz2
'B4
B4 = X3 + 32768
B4 = B4 * Ac4
B4 = B4 / 2potenz15
If B4 <= 0 Then B4 = B4 * -1
'Print B4
'B7
B7 = Up - B3
B3a = 50000
Shift B3a , Right , 2
B7 = B7 * B3a
If B7 <= 0 Then B7 = B7 * -1
'Print B7
If B7 < &H80000000 Then
P = B7 * 2
'Print "p1 " ; P
P = P / B4
'Print "p1 " ; P
Else
P = B7 / B4
'Print "p2 " ; P
P = P * 2
'Print "p2 " ; P
End If
'Print P
'nochmal X1
2potenz8 = 2 ^ 8
P2 = P / 2potenz8
X1 = P2 * P2
'Print X1
' schon wieder x1 O.รณ
X1 = X1 * 3038
X1 = X1 / 2potenz16
'Print X1
' nochmal X2
X2 = -7357 * P
X2 = X2 / 2potenz16
' endlich das finale!!! Der Druck
'Print X2
P2 = P
P = X2 + 3791
P = P + X1
P = P / 2potenz4
P = P + P2
End Sub
Sub Lese_kalib_data
Adres = &HAA
'Ac1 lesen
Call Lese_bmp085(adres , Wert)
Msb = Wert
Adres = Adres + 1
Call Lese_bmp085(adres , Wert)
Lsb = Wert
Shift Msb , Left , 8
Ac1 = Msb + Lsb
Adres = Adres + 1
'Ac2 lesen
Call Lese_bmp085(adres , Wert)
Msb = Wert
Adres = Adres + 1
Call Lese_bmp085(adres , Wert)
Lsb = Wert
Shift Msb , Left , 8
Ac2 = Msb + Lsb
Adres = Adres + 1
'Ac3 lesen
Call Lese_bmp085(adres , Wert)
Msb = Wert
Adres = Adres + 1
Call Lese_bmp085(adres , Wert)
Lsb = Wert
Shift Msb , Left , 8
Ac3 = Msb + Lsb
Adres = Adres + 1
'Ac4 lesen
Call Lese_bmp085(adres , Wert)
Msb = Wert
Adres = Adres + 1
Call Lese_bmp085(adres , Wert)
Lsb = Wert
Shift Msb , Left , 8
Ac4 = Msb + Lsb
Adres = Adres + 1
'Ac5 lesen
Call Lese_bmp085(adres , Wert)
Msb = Wert
Adres = Adres + 1
Call Lese_bmp085(adres , Wert)
Lsb = Wert
Shift Msb , Left , 8
Ac5 = Msb + Lsb
Adres = Adres + 1
'Ac6 lesen
Call Lese_bmp085(adres , Wert)
Msb = Wert
Adres = Adres + 1
Call Lese_bmp085(adres , Wert)
Lsb = Wert
Shift Msb , Left , 8
Ac6 = Msb + Lsb
Adres = Adres + 1
'B1 lesen
Call Lese_bmp085(adres , Wert)
Msb = Wert
Adres = Adres + 1
Call Lese_bmp085(adres , Wert)
Lsb = Wert
Shift Msb , Left , 8
B1 = Msb + Lsb
Adres = Adres + 1
'B2 lesen
Call Lese_bmp085(adres , Wert)
Msb = Wert
Adres = Adres + 1
Call Lese_bmp085(adres , Wert)
Lsb = Wert
Shift Msb , Left , 8
B2 = Msb + Lsb
Adres = Adres + 1
'MB lesen
Call Lese_bmp085(adres , Wert)
Msb = Wert
Adres = Adres + 1
Call Lese_bmp085(adres , Wert)
Lsb = Wert
Shift Msb , Left , 8
Mb = Msb + Lsb
Adres = Adres + 1
'MB lesen
Call Lese_bmp085(adres , Wert)
Msb = Wert
Adres = Adres + 1
Call Lese_bmp085(adres , Wert)
Lsb = Wert
Shift Msb , Left , 8
Mc = Msb + Lsb
Adres = Adres + 1
'Md lesen
Call Lese_bmp085(adres , Wert)
Msb = Wert
Adres = Adres + 1
Call Lese_bmp085(adres , Wert)
Lsb = Wert
Shift Msb , Left , 8
Md = Msb + Lsb
Adres = Adres + 1
End Sub
Sub Lese_roh_daten
Call Lese_bmp085(adres , Wert)
Msb = Wert
Adres = Adres + 1
Call Lese_bmp085(adres , Wert)
Lsb = Wert
End Sub
Sub Schreibe_bmp085(byval Adres As Byte , Byval Wert As Byte)
I2cstart
I2cwbyte Addr_schreiben
I2cwbyte Adres
I2cwbyte Wert
I2cstop
Waitms 10
End Sub
Sub Lese_bmp085(byval Adres As Byte , Wert As Byte)
I2cstart
I2cwbyte Addr_schreiben
I2cwbyte Adres
I2cstart
I2cwbyte Addr_lesen
I2crbyte Wert , Nack
I2cstop
Waitms 10
End Sub
Return
'--------------------------------------------------------------------------------
'=====[ Inisisalisasi Accelero ]================================================
Initaccel:
I2cstart
I2cwbyte Adxl_w
I2cwbyte Data_format
I2cwbyte &H03
I2cstop
I2cstart
I2cwbyte Adxl_w
I2cwbyte Power_ctl
I2cwbyte &H08
I2cstop
Return
'-------------------------------------------------------------------------------
'=====[ Read Accel Data ]========================================================
Read_accel:
I2cstart
I2cwbyte Adxl_w
I2cwbyte Data_xl
I2cstart
I2cwbyte Adxl_r
I2crbyte Xl , Ack
I2crbyte Xh , Ack
I2crbyte Yl , Ack
I2crbyte Yh , Ack
I2crbyte Zl , Ack
I2crbyte Zh , Nack
I2cstop
Return
'-------------------------------------------------------------------------------
'=====[ Initialize HMC Compass ]================================================
Hmc_initialize:
' Initialize free running mode for compass
I2cstart
I2cwbyte Hmc_w
I2cwbyte &H02
I2cwbyte &H00 ' set register possition to 0
I2cwbyte &B0101_1000 ' set ConfRegA [4samples, 75Hz, normal]
I2cwbyte &H0010_0000 ' set ConfRegC [gain to default (1090)]
I2cwbyte &H0000_0000 ' set free running mode
I2cstop
Return
'-------------------------------------------------------------------------------
'=====[ Read HMC Compass ]======================================================
Hmc_readstatus:
' Read HMC Compass status
I2cstart
I2cwbyte Hmc_w
I2cwbyte &H09 ' set read position
I2cstart
I2cwbyte Hmc_r
I2crbyte Hmc_status , Nack ' read in status
I2cstop
Return
'-------------------------------------------------------------------------------
'=====[ Read HMC Compass ]======================================================
Hmc_readdata:
' Read HMC Compass X, Y, Z integer values
I2cstart
I2cwbyte Hmc_w
I2cwbyte &H03 ' set read position
I2cstart
I2cwbyte Hmc_r
I2crbyte Hmc_xh , Ack ' read in high order x
I2crbyte Hmc_xl , Ack ' read in low order x
I2crbyte Hmc_zh , Ack ' read in high order z
I2crbyte Hmc_zl , Ack ' read in low order z
I2crbyte Hmc_yh , Ack ' read in high order y
I2crbyte Hmc_yl , Nack ' read in low order y
I2cstop
Return
'-------------------------------------------------------------------------------
'=====[ Tick timer ISR ]========================================================
Tickcounter_isr:
Incr Tick
Return
'-------------------------------------------------------------------------------
'=====[ Initilize Gyro]=========================================================
Initgyro:
I2cstart
I2cwbyte L3g4200d_w
I2cwbyte Who_am_i
I2cstart
I2cwbyte L3g4200d_r
I2crbyte Test_gyro , Nack
I2cstop
I2cstart
I2cwbyte L3g4200d_w
I2cwbyte Ctrl_reg1
I2cwbyte &B00000_1111
I2cstop
I2cstart
I2cwbyte L3g4200d_w
I2cwbyte Ctrl_reg2
I2cwbyte &B0010_1001 'adjust/use the HPF
I2cstop
I2cstart
I2cwbyte L3g4200d_w
I2cwbyte Ctrl_reg3
I2cwbyte &B0000_1000
I2cstop
I2cstart
I2cwbyte L3g4200d_w
I2cwbyte Ctrl_reg4
' I2cwbyte &B0000_0000 'scale 250
' I2cwbyte &B0001_0000 'scale 500
I2cwbyte &B1011_0000 'scale 2000
I2cstop
I2cstart
I2cwbyte L3g4200d_w
I2cwbyte Ctrl_reg5
I2cwbyte &B0000_0000 'high-pass filtering of outputs
I2cstop
Return
'-------------------------------------------------------------------------------
'=====[ Read Gyro Data ]========================================================
Read_gyro:
I2cstart
I2cwbyte L3g4200d_w
I2cwbyte &B1010_0110
I2cstart
I2cwbyte L3g4200d_r
I2crbyte Temp , Ack
I2crbyte Status , Ack
I2crbyte Xlg , Ack
I2crbyte Xhg , Ack
I2crbyte Ylg , Ack
I2crbyte Yhg , Ack
I2crbyte Zlg , Ack
I2crbyte Zhg , Nack
I2cstop
Return
'-----------------------------------------------------------------------------
Getchar:
Cls
Zx = 1
Datas = ""
Do
Inchar = Inkey()
Datas = Datas + Inchar
Id = Left(datas , 50 )
Incr Zx
Loop Until Zx > 1000
Cmd_pos1 = Instr(id , "A")
If Cmd_pos1 <> 0 Then
Incr Cmd_pos1
Data2 = Mid(id , Cmd_pos1 , 9)
End If
Cmd_pos2 = Instr(id , "B")
If Cmd_pos2 <> 0 Then
Incr Cmd_pos2
Data3 = Mid(id , Cmd_pos2 , 10)
End If
Datain2 = Val(data2)
Datain3 = Val(data3)
Goto Main
Return
$regfile = "m16def.dat"
$crystal = 12000000
$baud = 9600
$hwstack = 32
$swstack = 10
$framesize = 40
Config Lcdpin = Pin , Rs = Portd.7 , E = Portd.6 , Db4 = Portc.2
Config Lcdpin = Pin , Db5 = Portc.3 , Db6 = Portc.4 , Db7 = Portc.5
Config Lcd = 16 * 2
Dim Lop As Integer
Cls
Cursor Off
Dim Inchar As String * 50
Dim Count As Integer
Dim I As Byte
Dim Flagset As Bit '0 = Waktu, 1 = Tanggal
Dim Flagreset As Bit
Dim Line1 As String * 1
Dim Zx As Integer
Dim Datas As String * 50
Dim Id As String * 50
Dim Data1 As String * 50
Dim Data2 As String * 50
Dim Data3 As String * 50
Dim Cmd_pos1 As Byte
Dim Cmd_pos2 As Byte
Dim Datain2 As Single
Dim Datain3 As Single
Datas = ""
Zx = 0
Lop = 1
Cls
Cursor Off , Noblink
'=====[ I2C BUS INIT ]==========================================================
'configure the scl and sda pins
$lib "I2C_TWI.LBX"
Config Scl = Portc.0
Config Sda = Portc.1
Config Twi = 400000
I2cinit
'=====[ Setup Timer0 for Tick isr ]=============================================
Config Timer2 = Timer , Prescale = 1024
' crystal / Prescale / CountWanted = reload value
' 18432000 / 1024 / 100Hz = 180 (trigger every 100th sec (10ms))
Ocr2 = 180 - 1
On Oc2 Tickcounter_isr
Enable Oc2
'-------------------------------------------------------------------------------
'====[ ALAMAT ADXL345 ]=========================================================
Const Adxl_r = &HA7
Const Adxl_w = &HA6
Const Power_ctl = &H2D
Const Data_format = &H31
Const Data_xl = &H32
Const Data_xh = &H33
Const Data_yl = &H34
Const Data_yh = &H35
Const Data_zl = &H36
Const Data_zh = &H37
'-------------------------------------------------------------------------------
'====[ ALAMAT BMP085 ]==========================================================
Const Addr_schreiben = &B11101110
Const Addr_lesen = &B11101111
'-------------------------------------------------------------------------------
'====[ ALAMAT HMC5883L ]========================================================
Const Hmc_w = &H3C
Const Hmc_r = &H3D
Const Mode_register_compass = &H02
Const Continous_register_mode = &H00
'-------------------------------------------------------------------------------
'====[ ALAMAT L3G4200D ]========================================================
Const Who_am_i = &H0F
Const L3g4200d_r = &HD3
Const L3g4200d_w = &HD2
Const Ctrl_reg1 = &H20
Const Ctrl_reg2 = &H21
Const Ctrl_reg3 = &H22
Const Ctrl_reg4 = &H23
Const Ctrl_reg5 = &H24
'-------------------------------------------------------------------------------
'====[ Variabel ADXL345 ]=======================================================
Dim X As Integer
Dim Xl As Byte At X + 0 Overlay
Dim Xh As Byte At X + 1 Overlay
Dim Y As Integer
Dim Yl As Byte At Y + 0 Overlay
Dim Yh As Byte At Y + 1 Overlay
Dim Z As Integer
Dim Zl As Byte At Z + 0 Overlay
Dim Zh As Byte At Z + 1 Overlay
'-------------------------------------------------------------------------------
'====[ Variabel BMP085 ]========================================================
Dim Ac1 As Integer
Dim Ac1a As Long
Dim Ac2 As Integer
Dim Ac3 As Integer
Dim Ac4 As Word
Dim Ac5 As Word
Dim Ac5a As Single
Dim Ac6 As Word
Dim B1 As Integer
Dim B2 As Integer
Dim B3 As Long
Dim B3a As Long
Dim B4 As Long
Dim B5 As Long
Dim B6 As Integer
Dim B6a As Long
Dim B7 As Long
Dim Mb As Integer
Dim Mc As Integer
Dim Mc2 As Long
Dim Md As Integer
Dim Adres As Byte
Dim Wert As Byte
Dim Ut As Long
Dim X1 As Long
Dim X1t As Single
Dim X2 As Long
Dim X3 As Long
Dim T As Single
Dim T2 As Long
Dim Temperatur As String * 6
Dim Luftdruck As String * 8
Dim Druck As Single
Dim P As Long
Dim P2 As Long
Dim 2potenz15 As Long
Dim 2potenz11 As Integer
Dim 2potenz13 As Integer
Dim 2potenz8 As Integer
Dim 2potenz4 As Integer
Dim 2potenz2 As Integer
Dim 2potenz16 As Long
Dim 2potenz12 As Integer
Dim Potenz5255 As Single
Dim Altitude As Single
Dim Altitudes As String * 8
Dim Msb As Long
Dim Lsb As Long
Dim Up As Long
Dim Druckvar As Byte
Declare Sub Berechne_temp
Declare Sub Berechne_druck
Declare Sub Schreibe_bmp085(byval Adres As Byte , Byval Wert As Byte)
Declare Sub Lese_bmp085(byval Adres As Byte , Wert As Byte)
Declare Sub Lese_roh_daten
Declare Sub Messen
Declare Sub Lese_kalib_data
'====[ Variabel HMC5883L ]======================================================
Dim Hmc_x As Integer
Dim Hmc_xl As Byte At Hmc_x + 0 Overlay
Dim Hmc_xh As Byte At Hmc_x + 1 Overlay
Dim Hmc_y As Integer
Dim Hmc_yl As Byte At Hmc_y + 0 Overlay
Dim Hmc_yh As Byte At Hmc_y + 1 Overlay
Dim Hmc_z As Integer
Dim Hmc_zl As Byte At Hmc_z + 0 Overlay
Dim Hmc_zh As Byte At Hmc_z + 1 Overlay
Dim Hmc_status As Byte
Dim X_comp As Single
Dim Y_comp As Single
Dim Z_comp As Single
Dim Angle As Single
'-------------------------------------------------------------------------------
'=====[ Variabel L3G4200D ]=====================================================
Dim Tick As Word ' used for 100Hz tick count
Dim Tickn As Word ' used for next timer tick event
Dim N As Integer ' temp var for visual positioning
Dim Xg As Integer
Dim Xlg As Byte At Xg + 0 Overlay
Dim Xhg As Byte At Xg + 1 Overlay
Dim Yg As Integer
Dim Ylg As Byte At Yg + 0 Overlay
Dim Yhg As Byte At Yg + 1 Overlay
Dim Zg As Integer
Dim Zlg As Byte At Zg + 0 Overlay
Dim Zhg As Byte At Zg + 1 Overlay
Dim Temp As Byte
Dim Status As Byte
Dim Test_gyro As Byte
'-------------------------------------------------------------------------------
Cls
Locate 1 , 1
Lcd "Inisialisasi"
Locate 2 , 1
Lcd "ADXL345"
Wait 1
Gosub Initaccel
Cls
Locate 1 , 1
Lcd "Inisialisasi"
Locate 2 , 1
Lcd "BMP085"
Wait 1
Call Lese_kalib_data
Cls
Locate 1 , 1
Lcd "Inisialisasi"
Locate 2 , 1
Lcd "HMC5883L"
Wait 1
Gosub Hmc_initialize
Cls
Locate 1 , 1
Lcd "Inisialisasi"
Locate 2 , 1
Lcd "L3G4200D"
Wait 1
Gosub Initgyro
Main:
On Urxc Getchar
Enable Interrupts
Enable Urxc
Utama:
Cls
Do
Gosub Read_accel
Gosub Hmc_readdata
Gosub Hmc_readstatus
Locate 2 , 1
Lcd X ; " " ; Y ; " " ; Z ; " "
Call Messen
T = T / 10
Temperatur = Fusing(t , "###.##")
Potenz5255 = 1 / 5.255
Altitude = P / 101325
Altitude = Altitude ^ Potenz5255
Altitude = 1 - Altitude
Altitude = Altitude * 44330
Druck = P
Druck = Druck / 100
Luftdruck = Fusing(druck , "####.##")
Altitudes = Fusing(altitude , "#####.##")
'Convert integer to float
X_comp = Hmc_x
Y_comp = Hmc_y
Z_comp = Hmc_z
'Calculate angle:
Angle = Atn2(y_comp , X_comp)
Angle = Angle * 57.29577951
Angle = Angle + 180
If Tick >= Tickn Then
Gosub Read_gyro
Xg = Xg / 256
Yg = Yg / 256
Zg = Zg / 256
Locate 1 , 1
'Lcd "X=" ; Xg ; " " ; "Y=" ; Yg ; " " ; "Z=" ; Zg
Lcd "Y=" ; Yg
Tickn = Tickn + 20
End If
'Config Powermode = Idle
Locate 1 , 8
'Lcd "A=" ; Fusing(angle , "#.#")
Lcd "x=" ; X_comp
Zx = 0
Incr Count
Waitms 10
Loop Until Count > 300
Count = 0
Wait 1
Cls
Do
Upperline
Lcd "LAT= " ; Datain3
Lowerline
Lcd "Long= " ; Datain2
Gosub Read_accel
Gosub Hmc_readdata
Gosub Hmc_readstatus
Call Messen
T = T / 10
Temperatur = Fusing(t , "###.##")
Potenz5255 = 1 / 5.255
Altitude = P / 101325
Altitude = Altitude ^ Potenz5255
Altitude = 1 - Altitude
Altitude = Altitude * 44330
Druck = P
Druck = Druck / 100
Luftdruck = Fusing(druck , "####.##")
Altitudes = Fusing(altitude , "#####.##")
'Convert integer to float
X_comp = Hmc_x
Y_comp = Hmc_y
Z_comp = Hmc_z
'Calculate angle:
Angle = Atn2(y_comp , X_comp)
Angle = Angle * 57.29577951
Angle = Angle + 180
If Tick >= Tickn Then
Gosub Read_gyro
Xg = Xg / 256
Yg = Yg / 256
Zg = Zg / 256
Tickn = Tickn + 20
End If
Zx = 0
Incr Count
Waitms 10
Loop Until Count > 300
Count = 0
Goto Utama
'=====[ Inisialisasi BMP085]=========================================================
'=====[ Read the raw temp from the BMP085 ]=====================================
Sub Messen
'Temparatur messen
Call Schreibe_bmp085(&Hf4 , &H2E)
Adres = &HF6
Call Lese_roh_daten
'Print "msb " ; Msb
'Print "lsb " ; Lsb
Shift Msb , Left , 8
'Print "msb shift " ; Msb
Ut = Msb + Lsb
Call Berechne_temp
'Druck messen
Call Schreibe_bmp085(&Hf4 , &H74 )
Adres = &HF6
Call Lese_roh_daten
Shift Msb , Left , 16
Shift Lsb , Left , 8
Up = Msb + Lsb
Shift Up , Right , 6
'Print "msb " ; Msb ; "lsb " ; Lsb ; "up " ; Up
Call Berechne_druck
End Sub
Sub Berechne_temp
'X1
Ut = Ut - Ac6
2potenz15 = 2 ^ 15
Ac5a = Ac5 / 2potenz15
X1t = Ut * Ac5a
'Print "X1 " ; X1t
'X2
2potenz11 = 2 ^ 11
Mc2 = Mc * 2potenz11
X3 = X1t + Md
X2 = Mc2 / X3
'Print "X2 " ; X2
'B5
B5 = X1t + X2
'Print "B5 " ; B5
'T
2potenz4 = 2 ^ 4
T2 = B5 + 8
T = T2 / 2potenz4
End Sub
Sub Berechne_druck
'B6
B6 = B5 - 4000
'X1
2potenz12 = 2 ^ 12
B6a = B6 * B6
B6a = B6a / 2potenz12
X1 = B2 * B6a
X1 = X1 / 2potenz11
'X2
X2 = Ac2 * B6
X2 = X2 / 2potenz11
'X3
X3 = X1 + X2
'B3
Ac1a = Ac1 * 4
Ac1a = Ac1a + X3
Shift Ac1a , Left , 2
Ac1a = Ac1a + 2
B3 = Ac1a / 4
'X1 die zweite
2potenz13 = 2 ^ 13
B6a = B6 / 2potenz13
X1 = Ac3 * B6a
'X2 die zweite
B6a = B6 * B6
B6a = B6a / 2potenz12
X2 = B1 * B6a
2potenz16 = 2 ^ 16
X2 = X2 / 2potenz16
'X3 die zweite
X3 = X1 + X2
X3 = X3 + 2
2potenz2 = 2 ^ 2
X3 = X3 / 2potenz2
'B4
B4 = X3 + 32768
B4 = B4 * Ac4
B4 = B4 / 2potenz15
If B4 <= 0 Then B4 = B4 * -1
'Print B4
'B7
B7 = Up - B3
B3a = 50000
Shift B3a , Right , 2
B7 = B7 * B3a
If B7 <= 0 Then B7 = B7 * -1
'Print B7
If B7 < &H80000000 Then
P = B7 * 2
'Print "p1 " ; P
P = P / B4
'Print "p1 " ; P
Else
P = B7 / B4
'Print "p2 " ; P
P = P * 2
'Print "p2 " ; P
End If
'Print P
'nochmal X1
2potenz8 = 2 ^ 8
P2 = P / 2potenz8
X1 = P2 * P2
'Print X1
' schon wieder x1 O.รณ
X1 = X1 * 3038
X1 = X1 / 2potenz16
'Print X1
' nochmal X2
X2 = -7357 * P
X2 = X2 / 2potenz16
' endlich das finale!!! Der Druck
'Print X2
P2 = P
P = X2 + 3791
P = P + X1
P = P / 2potenz4
P = P + P2
End Sub
Sub Lese_kalib_data
Adres = &HAA
'Ac1 lesen
Call Lese_bmp085(adres , Wert)
Msb = Wert
Adres = Adres + 1
Call Lese_bmp085(adres , Wert)
Lsb = Wert
Shift Msb , Left , 8
Ac1 = Msb + Lsb
Adres = Adres + 1
'Ac2 lesen
Call Lese_bmp085(adres , Wert)
Msb = Wert
Adres = Adres + 1
Call Lese_bmp085(adres , Wert)
Lsb = Wert
Shift Msb , Left , 8
Ac2 = Msb + Lsb
Adres = Adres + 1
'Ac3 lesen
Call Lese_bmp085(adres , Wert)
Msb = Wert
Adres = Adres + 1
Call Lese_bmp085(adres , Wert)
Lsb = Wert
Shift Msb , Left , 8
Ac3 = Msb + Lsb
Adres = Adres + 1
'Ac4 lesen
Call Lese_bmp085(adres , Wert)
Msb = Wert
Adres = Adres + 1
Call Lese_bmp085(adres , Wert)
Lsb = Wert
Shift Msb , Left , 8
Ac4 = Msb + Lsb
Adres = Adres + 1
'Ac5 lesen
Call Lese_bmp085(adres , Wert)
Msb = Wert
Adres = Adres + 1
Call Lese_bmp085(adres , Wert)
Lsb = Wert
Shift Msb , Left , 8
Ac5 = Msb + Lsb
Adres = Adres + 1
'Ac6 lesen
Call Lese_bmp085(adres , Wert)
Msb = Wert
Adres = Adres + 1
Call Lese_bmp085(adres , Wert)
Lsb = Wert
Shift Msb , Left , 8
Ac6 = Msb + Lsb
Adres = Adres + 1
'B1 lesen
Call Lese_bmp085(adres , Wert)
Msb = Wert
Adres = Adres + 1
Call Lese_bmp085(adres , Wert)
Lsb = Wert
Shift Msb , Left , 8
B1 = Msb + Lsb
Adres = Adres + 1
'B2 lesen
Call Lese_bmp085(adres , Wert)
Msb = Wert
Adres = Adres + 1
Call Lese_bmp085(adres , Wert)
Lsb = Wert
Shift Msb , Left , 8
B2 = Msb + Lsb
Adres = Adres + 1
'MB lesen
Call Lese_bmp085(adres , Wert)
Msb = Wert
Adres = Adres + 1
Call Lese_bmp085(adres , Wert)
Lsb = Wert
Shift Msb , Left , 8
Mb = Msb + Lsb
Adres = Adres + 1
'MB lesen
Call Lese_bmp085(adres , Wert)
Msb = Wert
Adres = Adres + 1
Call Lese_bmp085(adres , Wert)
Lsb = Wert
Shift Msb , Left , 8
Mc = Msb + Lsb
Adres = Adres + 1
'Md lesen
Call Lese_bmp085(adres , Wert)
Msb = Wert
Adres = Adres + 1
Call Lese_bmp085(adres , Wert)
Lsb = Wert
Shift Msb , Left , 8
Md = Msb + Lsb
Adres = Adres + 1
End Sub
Sub Lese_roh_daten
Call Lese_bmp085(adres , Wert)
Msb = Wert
Adres = Adres + 1
Call Lese_bmp085(adres , Wert)
Lsb = Wert
End Sub
Sub Schreibe_bmp085(byval Adres As Byte , Byval Wert As Byte)
I2cstart
I2cwbyte Addr_schreiben
I2cwbyte Adres
I2cwbyte Wert
I2cstop
Waitms 10
End Sub
Sub Lese_bmp085(byval Adres As Byte , Wert As Byte)
I2cstart
I2cwbyte Addr_schreiben
I2cwbyte Adres
I2cstart
I2cwbyte Addr_lesen
I2crbyte Wert , Nack
I2cstop
Waitms 10
End Sub
Return
'--------------------------------------------------------------------------------
'=====[ Inisisalisasi Accelero ]================================================
Initaccel:
I2cstart
I2cwbyte Adxl_w
I2cwbyte Data_format
I2cwbyte &H03
I2cstop
I2cstart
I2cwbyte Adxl_w
I2cwbyte Power_ctl
I2cwbyte &H08
I2cstop
Return
'-------------------------------------------------------------------------------
'=====[ Read Accel Data ]========================================================
Read_accel:
I2cstart
I2cwbyte Adxl_w
I2cwbyte Data_xl
I2cstart
I2cwbyte Adxl_r
I2crbyte Xl , Ack
I2crbyte Xh , Ack
I2crbyte Yl , Ack
I2crbyte Yh , Ack
I2crbyte Zl , Ack
I2crbyte Zh , Nack
I2cstop
Return
'-------------------------------------------------------------------------------
'=====[ Initialize HMC Compass ]================================================
Hmc_initialize:
' Initialize free running mode for compass
I2cstart
I2cwbyte Hmc_w
I2cwbyte &H02
I2cwbyte &H00 ' set register possition to 0
I2cwbyte &B0101_1000 ' set ConfRegA [4samples, 75Hz, normal]
I2cwbyte &H0010_0000 ' set ConfRegC [gain to default (1090)]
I2cwbyte &H0000_0000 ' set free running mode
I2cstop
Return
'-------------------------------------------------------------------------------
'=====[ Read HMC Compass ]======================================================
Hmc_readstatus:
' Read HMC Compass status
I2cstart
I2cwbyte Hmc_w
I2cwbyte &H09 ' set read position
I2cstart
I2cwbyte Hmc_r
I2crbyte Hmc_status , Nack ' read in status
I2cstop
Return
'-------------------------------------------------------------------------------
'=====[ Read HMC Compass ]======================================================
Hmc_readdata:
' Read HMC Compass X, Y, Z integer values
I2cstart
I2cwbyte Hmc_w
I2cwbyte &H03 ' set read position
I2cstart
I2cwbyte Hmc_r
I2crbyte Hmc_xh , Ack ' read in high order x
I2crbyte Hmc_xl , Ack ' read in low order x
I2crbyte Hmc_zh , Ack ' read in high order z
I2crbyte Hmc_zl , Ack ' read in low order z
I2crbyte Hmc_yh , Ack ' read in high order y
I2crbyte Hmc_yl , Nack ' read in low order y
I2cstop
Return
'-------------------------------------------------------------------------------
'=====[ Tick timer ISR ]========================================================
Tickcounter_isr:
Incr Tick
Return
'-------------------------------------------------------------------------------
'=====[ Initilize Gyro]=========================================================
Initgyro:
I2cstart
I2cwbyte L3g4200d_w
I2cwbyte Who_am_i
I2cstart
I2cwbyte L3g4200d_r
I2crbyte Test_gyro , Nack
I2cstop
I2cstart
I2cwbyte L3g4200d_w
I2cwbyte Ctrl_reg1
I2cwbyte &B00000_1111
I2cstop
I2cstart
I2cwbyte L3g4200d_w
I2cwbyte Ctrl_reg2
I2cwbyte &B0010_1001 'adjust/use the HPF
I2cstop
I2cstart
I2cwbyte L3g4200d_w
I2cwbyte Ctrl_reg3
I2cwbyte &B0000_1000
I2cstop
I2cstart
I2cwbyte L3g4200d_w
I2cwbyte Ctrl_reg4
' I2cwbyte &B0000_0000 'scale 250
' I2cwbyte &B0001_0000 'scale 500
I2cwbyte &B1011_0000 'scale 2000
I2cstop
I2cstart
I2cwbyte L3g4200d_w
I2cwbyte Ctrl_reg5
I2cwbyte &B0000_0000 'high-pass filtering of outputs
I2cstop
Return
'-------------------------------------------------------------------------------
'=====[ Read Gyro Data ]========================================================
Read_gyro:
I2cstart
I2cwbyte L3g4200d_w
I2cwbyte &B1010_0110
I2cstart
I2cwbyte L3g4200d_r
I2crbyte Temp , Ack
I2crbyte Status , Ack
I2crbyte Xlg , Ack
I2crbyte Xhg , Ack
I2crbyte Ylg , Ack
I2crbyte Yhg , Ack
I2crbyte Zlg , Ack
I2crbyte Zhg , Nack
I2cstop
Return
'-----------------------------------------------------------------------------
Getchar:
Cls
Zx = 1
Datas = ""
Do
Inchar = Inkey()
Datas = Datas + Inchar
Id = Left(datas , 50 )
Incr Zx
Loop Until Zx > 1000
Cmd_pos1 = Instr(id , "A")
If Cmd_pos1 <> 0 Then
Incr Cmd_pos1
Data2 = Mid(id , Cmd_pos1 , 9)
End If
Cmd_pos2 = Instr(id , "B")
If Cmd_pos2 <> 0 Then
Incr Cmd_pos2
Data3 = Mid(id , Cmd_pos2 , 10)
End If
Datain2 = Val(data2)
Datain3 = Val(data3)
Goto Main
Return
g. Gambar Hasilnya
h. Video Hasilnya
No comments:
Post a Comment