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 Tce0 = Timer , Prescale = 1 ' 16bit Timer -> max 65535
Tcd0_per = 6550
Enable Tce0_ovf , Hi
On Tce0_ovf Zaehler 'timer overflow
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
' Motors (PWM)
Config Portd.0 = Output 'Mot RH
Config Portd.1 = Output 'Mot RV
Config Portd.2 = Output 'Mot LH
Config Portd.3 = Output 'Mot LV
Config Portd.4 = Output 'Mot RM
Config Portd.5 = Output 'Mot LM
' LEDs / Beeper
Config Portc.4 = Output 'led 1 (Willa-LED) ROT
Config Portc.5 = Output 'led 2 (Willa-LED) BLAU
Config Portd.6 = Output 'led 3 (Willa-LED) GELB
'--Alias's
Led_rot Alias Portc.4
Led_blau Alias Portc.5
Led_gelb Alias Portd.6
Dim Test As Byte
Dim A As Word , B As Word , C As Word , D As Word
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 , Hi = Enabled
Enable Interrupts
A = 0
Do
Call Read_mpu()
Print #1 , Gyrox
' Waitms 100
B = A Mod 20
If B = 0 Then Toggle Led_rot
C = A Mod 201
If C = 0 Then Toggle Led_blau
D = A Mod 302
If D = 0 Then Toggle Led_gelb
Loop
End
'===IRQ-Routine=================================
Zaehler:
Incr A
' Toggle Led_gelb
Return
'===IRQ-Routine=================================
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) , Ack , #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) , Ack , #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) , Ack , #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
Der COde ist im Wesentlichen der von Chris, ich habe nur überflüssige I²C-Schreib- und -Leseops aus der Read_MPU-Routine raus geschmissen (Laufzeit!) und eine Timer-IRQ-Routine eingebaut, die die LEDs mehr oder weniger zyklisch blinken lässt. Die Takte der LEDs sind sowas von daneben, dass ich denke, ich habe da was Grundsätzliches verpeilt. Vielleicht kann mir ja der eine oder andere Leser den entscheidenden Tipp geben 
Zitieren
Lesezeichen