COMPAS HMC5883L

 '===============================================================================
$regfile = "m128def.dat"                                    'specify the used micro
$crystal = 16000000                                         'used crystal frequency
$baud = 19200                                               'dfs
$hwstack = 100
$swstack = 100
$framesize = 64
'===============================================================================
'     Deklarasi
'===============================================================================
'------- Kompas ----------------------------
Config Scl = Portd.0                                        ' needed if using soft I2C -- portc.5
Config Sda = Portd.1                                        ' needed if using soft I2C -- portc.4
Config Twi = 400000                                         ' set the I2C bus to 'fast' speed
I2cinit

Const Hmc_w = &H3C                                          ' HMC5883L Write address
Const Hmc_r = &H3D                                          ' HMC5883L Read  address

'------- Variabel Kompas -------------------
Dim Sudutmeja(4) As Integer
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 Angle As Single
Dim X As Single
Dim Y As Single
Dim Z As Single
Dim Tempsingle1 As Single

'===============================================================================
'     Program Utama
'===============================================================================
Waitms 500
Print "mulaiiii"
Gosub Hmc_initialize
Do
   Gosub Getangle
   Angle = Int(angle)
   Print Angle
Loop

End                                                         'end program
'===============================================================================

'------- HMC inisialisasi ------------------
Hmc_initialize:
   Print "hmcinitial"
   I2cstart
   I2cwbyte Hmc_w
   Print "end"
   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

'------- Get Angle --------------------------
Getangle:
   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

   X = Hmc_x
   Y = Hmc_y
   Z = Hmc_z

' Calculate angle:

   Angle = Atn2(y , X)
   Angle = Angle * 57.29577951                              ' (180 / 3.14159265)
   Angle = Angle + 180
   Print "angl " ; Angle

   If Angle > 65 And Angle <= 82 Then
      Tempsingle1 = Angle + 7
      Angle = Tempsingle1
   Elseif Angle > 82 And Angle <= 92 Then
      Tempsingle1 = Angle + 12
      Angle = Tempsingle1
   Elseif Angle > 92 And Angle <= 112 Then
      Tempsingle1 = Angle + 18
      Angle = Tempsingle1
   Elseif Angle > 112 And Angle <= 200 Then
      Tempsingle1 = Angle + 23
      Angle = Tempsingle1
   Elseif Angle > 200 And Angle <= 298 Then
      Tempsingle1 = Angle + 15
      Angle = Tempsingle1
   Elseif Angle > 298 And Angle <= 335 Then
      Tempsingle1 = Angle + 8
      Angle = Tempsingle1
  End If
Return