Code:
$regfile = "xm32a4def.dat"
$crystal = 32000000
$framesize = 150 '100
$swstack = 150 '100
$hwstack = 150 '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_gyro()
Declare Sub Read_acc()
Declare Sub Get_gyro_offset()
'--UART Settings--
Config Com2 = 115200 , Mode = Asynchroneous , Parity = None , Stopbits = 1 , Databits = 8
'Config Serialin1 = Buffered , Size = 254
'Config Serialout1 = Buffered , Size = 250
Open "COM2:" For Binary As #1
'Clear Serialin1
'Clear Serialout1
'--TWI Settings--
Dim Twi_start As Byte
Open "twie" For Binary As #2
Config Twie = 400000
I2cinit #2
'--MPU 60X0--
Const Mpuaddw = &B11010000
Const Mpuaddr = &B11010001
Dim Gyrox As Long
Dim Gyroy As Long
Dim Gyroz As Long
Dim Accx As Long
Dim Accy As Long
Dim Accz As Long
Dim Gx As Integer
Dim Gy As Integer
Dim Gz As Integer
Dim Tmp_gyrox(2) As Byte At Gx Overlay
Dim Tmp_gyroy(2) As Byte At Gy Overlay
Dim Tmp_gyroz(2) As Byte At Gz Overlay
Dim Ax As Integer
Dim Ay As Integer
Dim Az As Integer
Dim Tmp_accx(2) As Byte At Ax Overlay
Dim Tmp_accy(2) As Byte At Ay Overlay
Dim Tmp_accz(2) As Byte At Az Overlay
Dim Gyrox_offset As Integer
Dim Gyroy_offset As Integer
Dim Gyroz_offset As Integer
Dim Accx_offset As Integer
Dim Accy_offset As Integer
Dim Accz_offset As Integer
Dim Roll_check(50) As Integer
Dim Nick_check(50) As Integer
Dim Yaw_check(50) As Integer
Dim Check_tmp As Integer
Dim Offset_tmp As Long
Dim Movement As Byte
Dim Compare_tmp As Integer
Dim I As Byte
Dim Test As Byte
Dim Acc_conf As Byte
Acc_conf = 0
Const Offset_schwelle = 3
'--Init Gyro & ACC--
Waitms 500
Call Init_mpu()
'Waitms 100
'Call Init_mpu()
'Waitms 100
'Call Init_mpu()
Waitms 250
Wait 4
I2cstart #2
I2cwbyte Mpuaddw , #2
I2cwbyte 28 , #2
I2crepstart #2
I2crbyte Test , Nack , #2
I2cstop #2
While Test <> Acc_conf
I2cstart #2
I2cwbyte Mpuaddw , #2
I2cwbyte 28 , #2
I2cwbyte Acc_conf , #2
I2cstop #2
I2cstart #2
I2cwbyte Mpuaddw , #2
I2cwbyte 28 , #2
I2crepstart #2
I2crbyte Test , Nack , #2
I2cstop #2
Print #1 , Test
Wend
Print #1 , "ACC-Config = " ; Bin(test)
Call Get_gyro_offset()
Do
Call Read_acc()
Print #1 , Accx ; " : " ; Accy ; " : " ; Accz
Waitms 100
Loop
End
Sub Get_gyro_offset()
Gyrox_offset = 0
Gyroy_offset = 0
Gyroz_offset = 0
Movement = 1
While Movement = 1
Movement = 0
For I = 1 To 50
Call Read_gyro()
Roll_check(i) = Gyrox
Nick_check(i) = Gyroy
Yaw_check(i) = Gyroz
Waitms 10
Next I
Offset_tmp = 0
For I = 1 To 50
Offset_tmp = Offset_tmp + Roll_check(i)
Next I
Offset_tmp = Offset_tmp / 50
For I = 1 To 50
Compare_tmp = Offset_tmp - Roll_check(i)
Compare_tmp = Abs(compare_tmp)
If Compare_tmp >= Offset_schwelle Then Movement = 1
Next I
Offset_tmp = 0
For I = 1 To 50
Offset_tmp = Offset_tmp + Nick_check(i)
Next I
Offset_tmp = Offset_tmp / 50
For I = 1 To 50
Compare_tmp = Offset_tmp - Nick_check(i)
Compare_tmp = Abs(compare_tmp)
If Compare_tmp >= Offset_schwelle Then Movement = 1
Next I
Offset_tmp = 0
For I = 1 To 50
Offset_tmp = Offset_tmp + Yaw_check(i)
Next I
Offset_tmp = Offset_tmp / 50
For I = 1 To 50
Compare_tmp = Offset_tmp - Yaw_check(i)
Compare_tmp = Abs(compare_tmp)
If Compare_tmp >= Offset_schwelle Then Movement = 1
Next I
Compare_tmp = Roll_check(1) - Roll_check(50)
Compare_tmp = Abs(compare_tmp)
If Compare_tmp >= Offset_schwelle Then Movement = 1
Compare_tmp = Roll_check(1) - Roll_check(25)
Compare_tmp = Abs(compare_tmp)
If Compare_tmp >= Offset_schwelle Then Movement = 1
Compare_tmp = Nick_check(1) - Nick_check(50)
Compare_tmp = Abs(compare_tmp)
If Compare_tmp >= Offset_schwelle Then Movement = 1
Compare_tmp = Nick_check(1) - Nick_check(25)
Compare_tmp = Abs(compare_tmp)
If Compare_tmp >= Offset_schwelle Then Movement = 1
Compare_tmp = Yaw_check(1) - Yaw_check(50)
Compare_tmp = Abs(compare_tmp)
If Compare_tmp >= Offset_schwelle Then Movement = 1
Compare_tmp = Yaw_check(1) - Yaw_check(25)
Compare_tmp = Abs(compare_tmp)
If Compare_tmp >= Offset_schwelle Then Movement = 1
Wend
Offset_tmp = 0
For I = 1 To 50
Offset_tmp = Offset_tmp + Roll_check(i)
Next I
Offset_tmp = Offset_tmp / 50
Gyrox_offset = Offset_tmp
Offset_tmp = 0
For I = 1 To 50
Offset_tmp = Offset_tmp + Nick_check(i)
Next I
Offset_tmp = Offset_tmp / 50
Gyroy_offset = Offset_tmp
Offset_tmp = 0
For I = 1 To 50
Offset_tmp = Offset_tmp + Yaw_check(i)
Next I
Offset_tmp = Offset_tmp / 50
Gyroz_offset = Offset_tmp
'Accx_offset = 110
'Accy_offset = 55
'Accz_offset = 8192
Accx_offset = 0
Accy_offset = 0
Accz_offset = 0
End Sub
Sub Init_mpu()
I2cstart #2 'Reset MPU60X0
I2cwbyte Mpuaddw , #2
I2cwbyte 107 , #2
I2cwbyte &B10000000 , #2
I2cstop #2
Waitms 10
I2cstart #2 'Set Samplerate Register
I2cwbyte Mpuaddw , #2
I2cwbyte 25 , #2
I2cwbyte &B00000000 , #2
I2cstop #2
I2cstart #2 'Set Powermanagement Register
I2cwbyte Mpuaddw , #2
I2cwbyte 107 , #2
I2cwbyte &B00001011 , #2
I2cstop #2
I2cstart #2 'Set Gyro-Config to 2000°/s
I2cwbyte Mpuaddw , #2
I2cwbyte 27 , #2
I2cwbyte &B00011000 , #2
I2cstop #2
I2cstart #2 'Set ACC-Config to +/-2g
I2cwbyte Mpuaddw , #2
I2cwbyte 28 , #2
I2cwbyte Acc_conf , #2 '0
I2cstop #2
I2cstart #2 'Set DLPF-Config to 42/44 Hz
I2cwbyte Mpuaddw , #2
I2cwbyte 26 , #2
I2cwbyte &B00000011 , #2
I2cstop #2
End Sub
Sub Read_gyro()
I2cstart #2
I2cwbyte Mpuaddw , #2
I2cwbyte 67 , #2
I2crepstart #2
I2cwbyte Mpuaddr , #2
I2crbyte Tmp_gyrox(2) , Ack , #2
I2crbyte Tmp_gyrox(1) , Ack , #2
I2crbyte Tmp_gyroy(2) , Ack , #2
I2crbyte Tmp_gyroy(1) , Ack , #2
I2crbyte Tmp_gyroz(2) , Ack , #2
I2crbyte Tmp_gyroz(1) , Nack , #2
I2cstop #2
Gyrox = Gx
Gyroy = 0 - Gy
Gyroz = 0 - Gz
Gyrox = Gyrox - Gyrox_offset
Gyroy = Gyroy - Gyroy_offset
Gyroz = Gyroz - Gyroz_offset
End Sub
Sub Read_acc()
I2cstart #2
I2cwbyte Mpuaddw , #2
I2cwbyte 59 , #2
I2crepstart #2
I2cwbyte Mpuaddr , #2
I2crbyte Tmp_accx(2) , Ack , #2
I2crbyte Tmp_accx(1) , Ack , #2
I2crbyte Tmp_accy(2) , Ack , #2
I2crbyte Tmp_accy(1) , Ack , #2
I2crbyte Tmp_accz(2) , Ack , #2
I2crbyte Tmp_accz(1) , Nack , #2
I2cstop #2
Accx_offset = 0
Accy_offset = 0
Accz_offset = 0
Accx = Ax
Accy = Ay
Accz = Az
Accx = Accx - Accx_offset
Accy = Accy - Accy_offset
Accz = Accz - Accz_offset
End Sub
Hat jemand vielleicht sowas ähnliches schonmal beobachtet und hat einen Tipp für mich?
Zitieren
Lesezeichen