$regfile = "m1284pdef.dat"
$crystal = 20000000

$framesize = 32
$swstack = 32
$hwstack = 32

Config Portc = Output                                       'Fürs LCD
Config Scl = Portc.0                                        'Ports fuer IIC-Bus
Config Sda = Portc.1
Config Lcdpin = Pin , Db4 = Portc.2 , Db5 = Portc.3 , Db6 = Portc.4 , Db7 = Portc.5 , E = Portc.6 , Rs = Portc.7
Config Lcd = 20 * 4
Config Portd = Input                                        'Fernsteuerung D4 - D7 ,Taster u.s.w

Declare Sub 6050_write(regadr As Byte , Byval Wert As Byte)
Declare Sub 6050_read(regadr As Byte) As Byte
Declare Sub Werte_berechnen
Declare Sub Werte_anzeigen

Dim 6050_adr As Byte
Dim 6050_adr1 As Byte
Dim 6050_accel_x_h As Byte
Dim 6050_pwr_mgmt_1 As Byte

Dim Accel_x_h As Byte
Dim Accel_x_l As Byte
Dim Accel_y_h As Byte
Dim Accel_y_l As Byte
Dim Accel_z_h As Byte
Dim Accel_z_l As Byte
Dim Temp_h As Byte
Dim Temp_l As Byte
Dim Gyro_x_h As Byte
Dim Gyro_x_l As Byte
Dim Gyro_y_h As Byte
Dim Gyro_y_l As Byte
Dim Gyro_z_h As Byte
Dim Gyro_z_l As Byte

Dim Accel_x_int As Integer
Dim Accel_x_real As Single
Dim Accel_y_int As Integer
Dim Accel_y_real As Single
Dim Accel_z_int As Integer
Dim Accel_z_real As Single
Dim Temp_int As Integer
Dim Temp_real As Single
Dim Gyro_x_int As Integer
Dim Gyro_x_real As Single
Dim Gyro_y_int As Integer
Dim Gyro_y_real As Single
Dim Gyro_z_int As Integer
Dim Gyro_z_real As Single

6050_adr = &HD0
6050_adr1 = &HD1
6050_accel_x_h = &H3B
6050_pwr_mgmt_1 = &H6B

Call 6050_write(6050_pwr_mgmt_1 , 0)                        'aus dem sleepmodus wecken


Do

Wait 1                                                      'Jede Sekunde 1 mal
  Call 6050_read(6050_accel_x_h)                            'register auslesen
  Call Werte_berechnen                                      'Werte errechnen
  Call Werte_anzeigen

Loop
End


Sub 6050_write(regadr As Byte , Byval Wert As Byte)
  I2cstart
  I2cwbyte 6050_adr
  If Err = 0 Then
    I2cwbyte Regadr
    I2cwbyte Wert
  End If
  I2cstop
End Sub

Sub 6050_read(regadr As Byte) As Byte                       'Gyro Temp + Accelerometer
  I2cstart
  I2cwbyte 6050_adr
  If Err = 0 Then
    I2cwbyte Regadr
    I2cstart
    I2cwbyte 6050_adr1
    I2crbyte Accel_x_h , Ack
    I2crbyte Accel_x_l , Ack
    I2crbyte Accel_y_h , Ack
    I2crbyte Accel_y_l , Ack
    I2crbyte Accel_z_h , Ack
    I2crbyte Accel_z_l , Ack
    I2crbyte Temp_h , Ack
    I2crbyte Temp_l , Ack
    I2crbyte Gyro_x_h , Ack
    I2crbyte Gyro_x_l , Ack
    I2crbyte Gyro_y_h , Ack
    I2crbyte Gyro_y_l , Ack
    I2crbyte Gyro_z_h , Ack
    I2crbyte Gyro_z_l , Nack
  End If
  I2cstop
End Sub

Sub Werte_berechnen

Accel_x_int = 256 * Accel_x_h
  Accel_x_int = Accel_x_int + Accel_x_l                     '1 g entspricht 2^14 = 16384 bei Standard Einstellung 2g; also Beschl = Acceleration *9.81/16384
  Accel_x_real = Accel_x_int * 9.81
  Accel_x_real = Accel_x_real / 16384

Accel_y_int = 256 * Accel_y_h
  Accel_y_int = Accel_y_int + Accel_y_l
  Accel_y_real = Accel_y_int * 9.81
  Accel_y_real = Accel_y_real / 16384

Accel_z_int = 256 * Accel_z_h
  Accel_z_int = Accel_z_int + Accel_z_l
  Accel_z_real = Accel_z_int * 9.81
  Accel_z_real = Accel_z_real / 16384

Temp_int = 256 * Temp_h                                     'lt. Anleitung
  Temp_int = Temp_int + Temp_l
  Temp_real = Temp_int / 340
  Temp_real = Temp_real + 36.53

Gyro_x_int = 256 * Gyro_x_h
  Gyro_x_int = Gyro_x_int + Gyro_x_l

Gyro_y_int = 256 * Gyro_y_h
  Gyro_y_int = Gyro_y_int + Gyro_y_l

Gyro_z_int = 256 * Gyro_z_h
  Gyro_z_int = Gyro_z_int + Gyro_z_l
End Sub

Sub Werte_anzeigen

Cls
Locate 1 , 1 : Lcd "AX=" ; Accel_x_real
Locate 1 , 10 : Lcd "AY=" ; Accel_y_real
Locate 2 , 1 : Lcd "AZ=" ; Accel_z_real
Locate 2 , 10 : Lcd "T=" ; Temp_real ; "°C"
Locate 3 , 1 : Lcd "GX=" ; Gyro_x_int ; "GY=" ; Gyro_y_int
Locate 4 , 1 : Lcd "GZ=" ; Gyro_z_int

End Sub