Code:
$regfile = "xm32a4def.dat"
$crystal = 32000000
$framesize = 100
$hwstack = 100
$swstack = 100
$lib "xmega.lib"
$external _xmegafix_clear
$external _xmegafix_rol_r1014
Config Osc = Disabled , 32mhzosc = Enabled
Config Sysclock = 32mhz , Prescalea = 1 , Prescalebc = 1_1
Declare Sub Init_mpu()
Declare Sub Read_mpu()
Config Com2 = 38400 , Mode = Asynchroneous , Parity = None , Stopbits = 1 , Databits = 8
'Config Serialout1 = Buffered , Size = 200
Open "COM2:" For Binary As #1
Dim Twi_start As Byte
Open "twie" For Binary As #2
Config Twie = 400000
Const Mpuaddw = &B11010000 'AD0 auf Masse
Const Mpuaddr = &B11010001
Dim Test As Byte
Dim Gyrox As Integer
Dim Gyroy As Integer
Dim Gyroz As Integer
Dim Tmp_gyrox(2) As Byte At Gyrox Overlay
Dim Tmp_gyroy(2) As Byte At Gyroy Overlay
Dim Tmp_gyroz(2) As Byte At Gyroz Overlay
Dim Accx As Integer
Dim Accy As Integer
Dim Accz As Integer
Dim Tmp_accx(2) As Byte At Accx Overlay
Dim Tmp_accy(2) As Byte At Accy Overlay
Dim Tmp_accz(2) As Byte At Accz Overlay
I2cinit #2
Call Init_mpu()
Config Priority = Static , Vector = Application , Lo = Enabled , Med = Enabled
Enable Interrupts
Do
Call Read_mpu()
Print #1 , Gyrox
Waitms 100
Loop
End
Sub Init_mpu()
I2cstart #2 'start condition
I2cwbyte Mpuaddw , #2 'write adress of MPU-6050
I2cwbyte 117 , #2 'Register 117 "Who am I"
I2crepstart #2 'repeated start
I2cwbyte Mpuaddr , #2 'read adress of MPU-6050
I2crbyte Test , Nack , #2 'read byte WHO_AM_I (Reg 117)
I2cstop #2 'stop condition
Print #1 , Test
I2cstart #2 'start condition
I2cwbyte Mpuaddw , #2 'write adress of MPU-6050
I2cwbyte 25 , #2 'Register 25 Sample Rate Divider (1..8 kHz)
I2cwbyte &B00000000 , #2 'Divider set to 1 (soll)
I2cstop #2 'stop condition
I2cstart #2 'start condition
I2cwbyte Mpuaddw , #2 'write adress of MPU-6050
I2cwbyte 26 , #2 'Register 26 DLPF_CFG (digital lowpass filter) Configuration
I2cwbyte &B00000000 , #2 'Bits 0..2 = 000 - ACC:260Hz, 0.0ms; Gyro:256Hz, 0.98ms ( &B00000000 )
I2cstop #2 'stop condition
I2cstart #2 'start condition
I2cwbyte Mpuaddw , #2 'write adress of MPU-6050
I2cwbyte 27 , #2 'Register 27 Gyro Configuration
I2cwbyte &B00000000 , #2 'Bits 3+4 = 00 - Full Scale Range: +/-250°/s
I2cstop #2 'stop condition
I2cstart #2 'start condition
I2cwbyte Mpuaddw , #2 'write adress of MPU-6050
I2cwbyte 28 , #2 'Register 28 ACC Configuration
I2cwbyte &B00000000 , #2 'Bits 3+4 = 00 - Full Scale Range: +/-2g / No High Pass Filter
I2cstop #2 'stop condition
I2cstart #2 'start condition
I2cwbyte Mpuaddw , #2 'write adress of MPU-6050
I2cwbyte 107 , #2 'Register 107 Power Management 1
I2cwbyte &B00001011 , #2 'No Reset / No Sleep / No Cycle / Temp_Sens: Dis / Clock Source: Z-Gyro
I2cstop #2
End Sub
Sub Read_mpu()
I2cstart #2 'X ACC
I2cwbyte Mpuaddw , #2
I2cwbyte 59 , #2
I2crepstart #2
I2cwbyte Mpuaddr , #2
I2crbyte Tmp_accx(2) , Ack , #2
I2crbyte Tmp_accx(1) , Nack , #2
I2cstop #2
I2cstart #2 'Y ACC
I2cwbyte Mpuaddw , #2
I2cwbyte 61 , #2
I2crepstart #2
I2cwbyte Mpuaddr , #2
I2crbyte Tmp_accy(2) , Ack , #2
I2crbyte Tmp_accy(1) , Nack , #2
I2cstop #2
I2cstart #2 'Z ACC
I2cwbyte Mpuaddw , #2
I2cwbyte 63 , #2
I2crepstart #2
I2cwbyte Mpuaddr , #2
I2crbyte Tmp_accz(2) , Ack , #2
I2crbyte Tmp_accz(1) , Nack , #2
I2cstop #2
I2cstart #2 'X GYRO
I2cwbyte Mpuaddw , #2
I2cwbyte 67 , #2
I2crepstart #2
I2cwbyte Mpuaddr , #2
I2crbyte Tmp_gyrox(2) , Ack , #2
I2crbyte Tmp_gyrox(1) , Nack , #2
I2cstop #2
I2cstart #2 'Y GYRO
I2cwbyte Mpuaddw , #2
I2cwbyte 69 , #2
I2crepstart #2
I2cwbyte Mpuaddr , #2
I2crbyte Tmp_gyroy(2) , Ack , #2
I2crbyte Tmp_gyroy(1) , Nack , #2
I2cstop #2
I2cstart #2 'Z GYRO
I2cwbyte Mpuaddw , #2
I2cwbyte 71 , #2
I2crepstart #2
I2cwbyte Mpuaddr , #2
I2crbyte Tmp_gyroz(2) , Ack , #2
I2crbyte Tmp_gyroz(1) , Nack , #2
I2cstop #2
End Sub
Die Test-Ausgabe sollte einen Wert von 104 (glaube ich) haben.
Zitieren
Lesezeichen