$crystal = 11059200
$large
Config Scl = P2.2
Config Sda = P2.0
Dim I2ctemp As Integer
Dim Ba As Word
Config Lcdpin = Pin , Db4 = P1.4 , Db5 = P1.5 , Db6 = P1.6 , Db7 = P1.7 , E = P1.3 , Rs = P1.2
Const Mpu6050_wr = &HD0
Const Mpu6050_rd = &HD1
Dim T As Single , X As Word , S As Word , Count As Word , Clock As Word , T1 As Single

Do
   X = 0
   S = 0
   Clock = 0
   T = 0
   Waitms 200
   Call Write_i2c
   If I2ctemp < 100 Then X = X + 1
   If I2ctemp > 110 And I2ctemp < 245 Then S = S + 1
   Waitms 10
   Clock = X + S
   T = 500 / Clock
   Cls
   Lcd "SPEED = " ; T
   Lowerline
   Lcd "VALUE = " ; I2ctemp
   Waitms 50
   If T >= 100 Then P0.0 = 0
   If T <= 99 Then P0.0 = 1
   Waitms 200
Loop

Sub Write_i2c
   I2cstart
   I2cwbyte Mpu6050_wr
   I2cwbyte &H6B
   I2cwbyte 09
   I2cstart
   I2cwbyte Mpu6050_wr
   I2cwbyte &H1B
   I2cwbyte 3
   I2cstart
   I2cwbyte Mpu6050_wr
   I2cwbyte &H19
   I2cwbyte 0
   I2cstart
   I2cwbyte Mpu6050_wr
   I2cwbyte &H47
   I2cstop
   Call Read_i2c
   Waitms 10
End Sub

Sub Read_i2c
   I2cstart
   I2cwbyte Mpu6050_rd
   I2crbyte I2ctemp , Nack
   I2cstop
   Waitms 10
   Cls
   Lowerline
   Lcd "VALUE= " ; I2ctemp
End Sub
 

'----------------------------------------------------------------

$regfile = "Xm32A4def.dat"
$crystal = 32000000
$hwstack = 84
$swstack = 80
$framesize = 80

$lib "xmega.lib"
$lib"i2c_twi.lbx"
$external _xmegafix_clear
$external _xmegafix_rol_r1014

Config Osc = Enabled , 32mhzosc = Enabled
Config Sysclock = 32mhz
Config Sysclock = 32mhz , Prescalea = 1 , Prescalebc = 1_1  'Internal 32MHz, no prescaler
Config Submode = New
Config Priority = Static , Vector = Application , Lo = Enabled , Med = Enabled       ' the RTC uses LO priority interrupts

Config Lcdpin = Pin , Db4 = Porta.4 , Db5 = Porta.5 , Db6 = Porta.6 , Db7 = Porta.7 , E = Porta.2 , Rs = Porta.0
Config Lcd = 20x4 , Chipset = Dogm162v3

Cursor Off, NoBlink
Cls



Config Com1 = 19200 , Mode = Asynchroneous , Parity = None , Stopbits = 1 , Databits = 8
Config Input1 = Cr , Echo = Crlf                           ' CR is used for input, we echo back CR and LF

Open "COM1:" For Binary As #1


Cls
Dim Twi_start As Byte 'This Variable is used by the I2C functions
Open "twic" For Binary As #2
Config Twic = 100000 ' set the I2C bus to 'fast' speed
I2cinit #2


Declare Sub Write_i2c
Declare Sub Read_i2c
Dim I2ctemp As Integer
Dim Ba As Word
Const Mpu6050_wr = &HD0
Const Mpu6050_rd = &HD1
Dim T As Single , X1 As Word , S As Word , Count As Word , Clock As Word , T1 As Single
'-------------------------------------------------------------------------------
'*****[ Start of main loop ]****************************************************

Do
  Print #1 , "Main loop "
   X1 = 0
   S = 0
   Clock = 0
   T = 0
   Waitms 200
   Call Write_i2c
   If I2ctemp < 100 Then X1 = X1 + 1
   If I2ctemp > 110 And I2ctemp < 245 Then S = S + 1
   Waitms 10
   Clock = X1 + S
   T = 500 / Clock
   Cls
   Lcd "SPEED = " ; T
   Lowerline
   Lcd "VALUE = " ; I2ctemp
   Waitms 50
   'If T >= 100 Then P0.0 = 0
   'If T <= 99 Then P0.0 = 1
   Waitms 200
Loop

Sub Write_i2c
  Print #1 , "Write_i2c :"
   I2cstart
   I2cwbyte Mpu6050_wr
   I2cwbyte &H6B
   I2cwbyte 09
   I2cstart
   I2cwbyte Mpu6050_wr
   I2cwbyte &H1B
   I2cwbyte 3
   I2cstart
   I2cwbyte Mpu6050_wr
   I2cwbyte &H19
   I2cwbyte 0
   I2cstart
   I2cwbyte Mpu6050_wr
   I2cwbyte &H47
   I2cstop
   Print #1 , "Call Read_i2c :"
   Call Read_i2c
   Waitms 10
End Sub

Sub Read_i2c
Print #1 , "Read_i2c:"
   I2cstart
   I2cwbyte Mpu6050_rd
   I2crbyte I2ctemp , Nack
   I2cstop
   Waitms 10
   Cls
   Lowerline
   Lcd "VALUE= " ; I2ctemp
End Sub