MPU6050

$regfile = "m328pdef.dat"
$crystal = 16000000
$baud = 9600

Config Sda = Portc.1
Config Scl = Portc.0
Config Twi = 100000

Declare Sub Read_gyro
Declare Sub Read_acc




Dim Gyrox As Integer
Dim Tmp_gyrox(2) As Byte At Gyrox Overlay
Dim Gyroy As Integer
Dim Gyroz As Integer

Dim Accx As Integer
Dim Accy As Integer
Dim Accz As Integer
Dim Xc As Integer
Dim Xv As Integer
Dim Yc As Integer
Dim Yv As Integer
Dim Zc As Integer
Dim Zv As Single
Dim Servos_c As Single
Dim Servos_v As Integer

Dim Xcerr As Integer
Dim Ycerr As Integer
Dim Zcerr As Integer

Dim Tmp_gyroy(2) As Byte
Dim Tmp_gyroz(2) As Byte

Dim Tmp_accx(2) As Byte
Dim Tmp_accy(2) As Byte
Dim Tmp_accz(2) As Byte

I2cinit


'--- (25) Sample Rate Divider = 1 ---
   I2cstart                                                 'start condition
   I2cwbyte &HD0                                            'write adress of MPU-6050
   I2cwbyte 25                                              'Register 25 Sample Rate Divider (1..8 kHz)
   I2cwbyte &B00000000                                      'Divider set to 1   (soll)
   I2cstop                                                  'stop condition

   '--- (26) DLPF = 42/44 Hz ---
   I2cstart                                                 'start condition
   I2cwbyte &HD0                                            'write adress of MPU-6050
   I2cwbyte 26                                              'Register 26 DLPF_CFG (digital lowpass filter) Configuration
   I2cwbyte &B00000011                                      'Bits 0..2 = 011 (3) - ACC:44Hz, 4.9ms; Gyro:42Hz, 4.8ms
   I2cstop                                                  'stop condition

   '--- (27) Gyro Full Range = +-2000°/s ---
   I2cstart                                                 'start condition
   I2cwbyte &HD0                                            'write adress of MPU-6050
   I2cwbyte 27                                              'Register 27 Gyro Configuration
   I2cwbyte &B00011000                                      'Bits 3+4 = 11 - Full Scale Range: +/-2000°/ss
   I2cstop                                                  'stop condition

   '--- (28) ACC Full Range = +-2g ---
   I2cstart                                                 'start condition
   I2cwbyte &HD0                                            'write adress of MPU-6050
   I2cwbyte 28                                              'Register 28 ACC Configuration
   I2cwbyte &B00000000                                      'Bits 3+4 = 00 - Full Scale Range: +/-2g / No High Pass Filter
   I2cstop                                                  'stop condition

   '--- (107) Power Management 1 ---
   I2cstart                                                 'start condition
   I2cwbyte &HD0                                            'write adress of MPU-6050
   I2cwbyte 107                                             'Register 107 Power Management 1
   I2cwbyte &B00001011                                      'No Reset / No Sleep / No Cycle / Temp_Sens: Dis / Clock Source: Z-Gyro
   I2cstop

 Xcerr = 0
 Ycerr = 0
 Zcerr = 0
Do
Xc = Tmp_accx(2)
Yc = Tmp_accy(2)
Zc = Tmp_accz(2)
 '*******************
 Do
 Call Read_gyro
 '*******************

Gyrox = Tmp_gyrox(2) * 256
Gyrox = Gyrox + Tmp_gyrox(1)
Gyrox = Gyrox + 256


Print Gyrox
Wait 1
Loop

 '*******************
  Call Read_acc
 '*******************

            Locate 1 , 1
            Lcd "x:" ; Tmp_accx(2) ; "     "

            Locate 1 , 8
            Lcd "y:" ; Tmp_accy(2) ; "     "

            Locate 1 , 15
            Lcd "z:" ; Tmp_accz(2) ; "  "

 '*******************
            If Tmp_accx(2) = 65 Then
               If Tmp_accz(2) = 0 Then
                              Locate 3 , 4
                              Lcd "X " ; "0" ; Chr(1) ; "  "
               End If
            End If
            If Tmp_accx(2) = 65 Then
               If Tmp_accz(2) = 255 Then
                              Locate 3 , 4
                              Lcd "X " ; "0" ; Chr(1) ; "  "
               End If
            End If
            If Tmp_accx(2) = 0 Then
               If Tmp_accz(2) = 64 Then
                              Locate 3 , 4
                              Lcd "X " ; "90" ; Chr(1) ; "  "
               End If
            End If
            If Tmp_accx(2) = 255 Then
               If Tmp_accz(2) = 64 Then
                              Locate 3 , 4
                              Lcd "X " ; "90" ; Chr(1) ; "  "
               End If
            End If
            If Tmp_accx(2) = 194 Then
               If Tmp_accz(2) = 0 Then
                              Locate 3 , 4
                              Lcd "X " ; "180" ; Chr(1) ; "  "
               End If
            End If
            If Tmp_accx(2) = 194 Then
               If Tmp_accz(2) = 255 Then
                              Locate 3 , 4
                              Lcd "X " ; "180" ; Chr(1) ; "  "
               End If
            End If
            If Tmp_accx(2) = 0 Then
               If Tmp_accz(2) = 189 Then
                              Locate 3 , 4
                              Lcd "X " ; "-90" ; Chr(1) ; "  "
               End If
            End If
            If Tmp_accx(2) = 255 Then
               If Tmp_accz(2) = 189 Then
                              Locate 3 , 4
                              Lcd "X " ; "-90" ; Chr(1) ; "  "
               End If
            End If
            If Tmp_accx(2) = 0 Then
               If Tmp_accz(2) = 189 Then
                              Locate 3 , 4
                              Lcd "X " ; "-90" ; Chr(1) ; "  "
               End If
            End If
            If Tmp_accx(2) = 255 Then
               If Tmp_accz(2) = 189 Then
                              Locate 3 , 4
                              Lcd "X " ; "-90" ; Chr(1) ; "  "
               End If
            End If
 '*******************
               If Tmp_accx(2) > 0 Then
                  If Tmp_accx(2) < 65 Then
                     If Tmp_accz(2) < 65 Then
                        If Tmp_accz(2) > 0 Then
                              Zv = Xc - 65
                              Zv = Zv * 1.407
                              Xv = -zv
                        End If
                     End If
                  End If
               End If

               If Tmp_accx(2) > 194 Then
                  If Tmp_accx(2) < 255 Then
                     If Tmp_accz(2) < 64 Then
                        If Tmp_accz(2) > 0 Then
                              Zv = 255 - Xc
                              Zv = Zv * 1.407
                              Xv = Zv + 90
                        End If
                     End If
                  End If
               End If

               If Tmp_accx(2) > 194 Then
                  If Tmp_accx(2) < 255 Then
                     If Tmp_accz(2) < 255 Then
                        If Tmp_accz(2) > 189 Then
                              Zv = 194 - Xc
                              Zv = Zv * 1.407
                              Xv = Zv
                        End If
                     End If
                  End If
               End If

               If Tmp_accx(2) > 0 Then
                  If Tmp_accx(2) < 65 Then
                     If Tmp_accz(2) < 255 Then
                        If Tmp_accz(2) > 189 Then
                              Zv = -xc
                              Zv = Zv * 1.407
                              Xv = Zv - 90
                        End If
                     End If
                  End If
               End If

               Locate 3 , 4
                     Lcd "X " ; Xv ; Chr(1) ; "  " ;
               Locate 4 , 15
                     Lcd Servos_v ; "   "

 '*******************
               If Tmp_accy(2) > 0 Then
                  If Tmp_accy(2) < 64 Then
                     If Tmp_accz(2) < 65 Then
                        If Tmp_accz(2) > 0 Then
                              Zv = 64 - Yc
                              Yv = Zv * 1.407
                        End If
                     End If
                  End If
               End If

               If Tmp_accy(2) > 190 Then
                  If Tmp_accy(2) < 255 Then
                     If Tmp_accz(2) < 65 Then
                        If Tmp_accz(2) > 0 Then
                              Zv = 255 - Yc
                              Zv = Zv * 1.407
                              Yv = Zv + 90
                        End If
                     End If
                  End If
               End If

               If Tmp_accy(2) > 190 Then
                  If Tmp_accy(2) < 255 Then
                     If Tmp_accz(2) < 255 Then
                        If Tmp_accz(2) > 189 Then
                              Zv = 255 - Yc
                              Zv = Zv * 1.407
                              Yv = Zv - 90
                        End If
                     End If
                  End If
               End If

               If Tmp_accy(2) > 0 Then
                  If Tmp_accy(2) < 64 Then
                     If Tmp_accz(2) < 255 Then
                        If Tmp_accz(2) > 189 Then
                              Zv = -yc
                              Zv = Zv * 1.407
                              Yv = Zv - 90
                        End If
                     End If
                  End If
               End If

Locate 3 , 11
Lcd "Y " ; Yv ; Chr(1) ; "  "

If Xv > 255 Then
   Xcerr = Xcerr + 1
End If
If Yv > 255 Then
   Ycerr = Ycerr + 1
End If
If Zc < 0 Then
   Zcerr = Zcerr + 1
End If

 '*******err************
  If Xcerr > 1 Then
   Xv = 0
   Xcerr = 0
End If

 If Ycerr > 1 Then
   Yv = 0
   Ycerr = 0
End If

 '******err*************
 Locate 2 , 1
   Lcd Gyrox ; "  " ; Tmp_accx(1) ; "   "

 Locate 4 , 1
   Lcd "err " ; Xcerr ; " " ; Ycerr ; " " ; Zcerr;

Loop

End




Sub Read_gyro()

   I2cstart
   I2cwbyte &HD0
   I2cwbyte 67
   I2crepstart
   I2cwbyte &HD1

   I2crbyte Tmp_gyrox(2) , Ack
   I2crbyte Tmp_gyrox(1) , Ack

   I2crbyte Tmp_gyroy(2) , Ack
   I2crbyte Tmp_gyroy(1) , Ack

   I2crbyte Tmp_gyroz(2) , Ack
   I2crbyte Tmp_gyroz(1) , Nack

   I2cstop


   'Gyrox = Gx
   'Gyroy = 0 - Gy
   'Gyroz = 0 - Gz

End Sub


Sub Read_acc()


   I2cstart
   I2cwbyte &HD0
   I2cwbyte 59
   I2crepstart
   I2cwbyte &HD1

   I2crbyte Tmp_accx(2) , Ack
   I2crbyte Tmp_accx(1) , Ack

   I2crbyte Tmp_accy(2) , Ack
   I2crbyte Tmp_accy(1) , Ack

   I2crbyte Tmp_accz(2) , Ack
   I2crbyte Tmp_accz(1) , Nack

   I2cstop

   'Accx = Ax
   'Accy = Ay
   'Accz = Az


End Sub

Continue Reading

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

Continue Reading

SPI SOFTSLAVE

'-------------------------------------------------------------------------------
'                        SPI-SOFTSLAVE.BAS
'                    (c) 2002-2003 MCS Electronics
' sample that shows how to implement a SPI SLAVE with software
'-------------------------------------------------------------------------------
'Some atmel chips like the 2313 do not have a SPI port.
'The BASCOM SPI routines are all master mode routines
'This example show how to create a slave using the 2313
'ISP slave code

'we use the 2313
$regfile = "2313def.dat"
$hwstack = 16
$swstack = 8
$framesize = 16
'XTAL used
$crystal = 4000000

'baud rate
$baud = 19200

'define the constants used by the SPI slave
Const _softslavespi_port = Portd                            ' we used portD
Const _softslavespi_pin = Pind                              'we use the PIND register for reading
Const _softslavespi_ddr = Ddrd                              ' data direction of port D

Const _softslavespi_clock = 5                               'pD.5 is used for the CLOCK
Const _softslavespi_miso = 3                                'pD.3 is MISO
Const _softslavespi_mosi = 4                                'pd.4 is MOSI
Const _softslavespi_ss = 2                                  ' pd.2 is SS
'while you may choose all pins you must use the INT0 pin for the SS
'for the 2313 this is pin 2

'PD.3(7),  MISO  must be output
'PD.4(8),  MOSI
'Pd.5(9) , Clock
'PD.2(6),  SS /INT0

'define the spi slave lib
$lib "spislave.lbx"
'sepcify wich routine to use
$external _spisoftslave

'we use the int0 interrupt to detect that our slave is addressed
On Int0 Isr_sspi Nosave
'we enable the int0 interrupt
Enable Int0
'we configure the INT0 interrupt to trigger when a falling edge is detected
Config Int0 = Falling
'finally we enabled interrupts
Enable Interrupts

'
Dim _ssspdr As Byte                                         ' this is out SPI SLAVE SPDR register
Dim _ssspif As Bit                                          ' SPI interrupt revceive bit
Dim Bsend As Byte , I As Byte , B As Byte                   ' some other demo variables

_ssspdr = 0                                                 ' we send a 0 the first time the master sends data
Do
   If _ssspif = 1 Then
   Print "received: " ; _ssspdr
   Reset _ssspif
   _ssspdr = _ssspdr + 1                                    ' we send this the next time
   End If
Loop

Continue Reading