$regfile = "M8def.dat"
$crystal = 8000000
$baud = 19200
$hwstack = 200
$swstack = 200
$framesize = 400

Config Scl = Portb.7
Config Sda = Portb.5

Declare Sub Read_gyro
Declare Sub Read_acc

Dim Tmp_gyrox(2) As Byte
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

Dim Gyrox As Integer
Dim Gyroy As Integer
Dim Gyroz As Integer

Dim Accx As Integer
Dim Accy As Integer
Dim Accz As Integer

Dim Gyroxx As Single
Dim Gyroyy As Single
Dim Gyrozz As Single

Dim Accxx As Single
Dim Accyy As Single
Dim Acczz As Single

I2cinit

   '--- (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

Do
  Call Read_gyro

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

  Gyroy = Tmp_gyroy(2) * 256
  Gyroy = Gyroy + Tmp_gyroy(1)
  Gyroyy = Gyroy * 0.00059875

  Gyroz = Tmp_gyroz(2) * 256
  Gyroz = Gyroz + Tmp_gyroz(1)
  Gyrozz = Gyroz * 0.00059875


  Print "Gyrox :" ; Gyroxx
  Print "Gyroy :" ; Gyroyy
  Print "Gyroz :" ; Gyrozz
  Print


  Call Read_acc

  Accx = Tmp_accx(2) * 256
  Accx = Accx + Tmp_accx(1)
  Accxx = Accx * 0.00059875

  Accy = Tmp_accy(2) * 256
  Accy = Accy + Tmp_accy(1)
  Accyy = Accy * 0.00059875

  Accz = Tmp_accz(2) * 256
  Accz = Accz + Tmp_accz(1)
  Acczz = Accz * 0.00059875

  Print "Accx :" ; Accxx
  Print "Accy :" ; Accyy
  Print "Accz :" ; Acczz

  Print Chr(27) ; "[1;1f";
  Waitms 200
  Print Chr(27) ; "[J";
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

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

End Sub