$regfile = "m328pdef.dat"$crystal = 16000000
$baud = 9600
Config Sda = Portc.1
Config Scl = Portc.0
Config Twi = 100000
Declare Sub Read_gyro
Declare Sub Read_acc
Dim Gyrox As Integer
Dim Tmp_gyrox(2) As Byte At Gyrox Overlay
Dim Gyroy As Integer
Dim Gyroz As Integer
Dim Accx As Integer
Dim Accy As Integer
Dim Accz As Integer
Dim Xc As Integer
Dim Xv As Integer
Dim Yc As Integer
Dim Yv As Integer
Dim Zc As Integer
Dim Zv As Single
Dim Servos_c As Single
Dim Servos_v As Integer
Dim Xcerr As Integer
Dim Ycerr As Integer
Dim Zcerr As Integer
Dim Tmp_gyroy(2) As Byte
Dim Tmp_gyroz(2) As Byte
Dim Tmp_accx(2) As Byte
Dim Tmp_accy(2) As Byte
Dim Tmp_accz(2) As Byte
I2cinit
'--- (25) Sample Rate Divider = 1 ---
I2cstart 'start condition
I2cwbyte &HD0 'write adress of MPU-6050
I2cwbyte 25 'Register 25 Sample Rate Divider (1..8 kHz)
I2cwbyte &B00000000 'Divider set to 1 (soll)
I2cstop 'stop condition
'--- (26) DLPF = 42/44 Hz ---
I2cstart 'start condition
I2cwbyte &HD0 'write adress of MPU-6050
I2cwbyte 26 'Register 26 DLPF_CFG (digital lowpass filter) Configuration
I2cwbyte &B00000011 'Bits 0..2 = 011 (3) - ACC:44Hz, 4.9ms; Gyro:42Hz, 4.8ms
I2cstop 'stop condition
'--- (27) Gyro Full Range = +-2000°/s ---
I2cstart 'start condition
I2cwbyte &HD0 'write adress of MPU-6050
I2cwbyte 27 'Register 27 Gyro Configuration
I2cwbyte &B00011000 'Bits 3+4 = 11 - Full Scale Range: +/-2000°/ss
I2cstop 'stop condition
'--- (28) ACC Full Range = +-2g ---
I2cstart 'start condition
I2cwbyte &HD0 'write adress of MPU-6050
I2cwbyte 28 'Register 28 ACC Configuration
I2cwbyte &B00000000 'Bits 3+4 = 00 - Full Scale Range: +/-2g / No High Pass Filter
I2cstop 'stop condition
'--- (107) Power Management 1 ---
I2cstart 'start condition
I2cwbyte &HD0 'write adress of MPU-6050
I2cwbyte 107 'Register 107 Power Management 1
I2cwbyte &B00001011 'No Reset / No Sleep / No Cycle / Temp_Sens: Dis / Clock Source: Z-Gyro
I2cstop
Xcerr = 0
Ycerr = 0
Zcerr = 0
Do
Xc = Tmp_accx(2)
Yc = Tmp_accy(2)
Zc = Tmp_accz(2)
'*******************
Do
Call Read_gyro
'*******************
Gyrox = Tmp_gyrox(2) * 256
Gyrox = Gyrox + Tmp_gyrox(1)
Gyrox = Gyrox + 256
Print Gyrox
Wait 1
Loop
'*******************
Call Read_acc
'*******************
Locate 1 , 1
Lcd "x:" ; Tmp_accx(2) ; " "
Locate 1 , 8
Lcd "y:" ; Tmp_accy(2) ; " "
Locate 1 , 15
Lcd "z:" ; Tmp_accz(2) ; " "
'*******************
If Tmp_accx(2) = 65 Then
If Tmp_accz(2) = 0 Then
Locate 3 , 4
Lcd "X " ; "0" ; Chr(1) ; " "
End If
End If
If Tmp_accx(2) = 65 Then
If Tmp_accz(2) = 255 Then
Locate 3 , 4
Lcd "X " ; "0" ; Chr(1) ; " "
End If
End If
If Tmp_accx(2) = 0 Then
If Tmp_accz(2) = 64 Then
Locate 3 , 4
Lcd "X " ; "90" ; Chr(1) ; " "
End If
End If
If Tmp_accx(2) = 255 Then
If Tmp_accz(2) = 64 Then
Locate 3 , 4
Lcd "X " ; "90" ; Chr(1) ; " "
End If
End If
If Tmp_accx(2) = 194 Then
If Tmp_accz(2) = 0 Then
Locate 3 , 4
Lcd "X " ; "180" ; Chr(1) ; " "
End If
End If
If Tmp_accx(2) = 194 Then
If Tmp_accz(2) = 255 Then
Locate 3 , 4
Lcd "X " ; "180" ; Chr(1) ; " "
End If
End If
If Tmp_accx(2) = 0 Then
If Tmp_accz(2) = 189 Then
Locate 3 , 4
Lcd "X " ; "-90" ; Chr(1) ; " "
End If
End If
If Tmp_accx(2) = 255 Then
If Tmp_accz(2) = 189 Then
Locate 3 , 4
Lcd "X " ; "-90" ; Chr(1) ; " "
End If
End If
If Tmp_accx(2) = 0 Then
If Tmp_accz(2) = 189 Then
Locate 3 , 4
Lcd "X " ; "-90" ; Chr(1) ; " "
End If
End If
If Tmp_accx(2) = 255 Then
If Tmp_accz(2) = 189 Then
Locate 3 , 4
Lcd "X " ; "-90" ; Chr(1) ; " "
End If
End If
'*******************
If Tmp_accx(2) > 0 Then
If Tmp_accx(2) < 65 Then
If Tmp_accz(2) < 65 Then
If Tmp_accz(2) > 0 Then
Zv = Xc - 65
Zv = Zv * 1.407
Xv = -zv
End If
End If
End If
End If
If Tmp_accx(2) > 194 Then
If Tmp_accx(2) < 255 Then
If Tmp_accz(2) < 64 Then
If Tmp_accz(2) > 0 Then
Zv = 255 - Xc
Zv = Zv * 1.407
Xv = Zv + 90
End If
End If
End If
End If
If Tmp_accx(2) > 194 Then
If Tmp_accx(2) < 255 Then
If Tmp_accz(2) < 255 Then
If Tmp_accz(2) > 189 Then
Zv = 194 - Xc
Zv = Zv * 1.407
Xv = Zv
End If
End If
End If
End If
If Tmp_accx(2) > 0 Then
If Tmp_accx(2) < 65 Then
If Tmp_accz(2) < 255 Then
If Tmp_accz(2) > 189 Then
Zv = -xc
Zv = Zv * 1.407
Xv = Zv - 90
End If
End If
End If
End If
Locate 3 , 4
Lcd "X " ; Xv ; Chr(1) ; " " ;
Locate 4 , 15
Lcd Servos_v ; " "
'*******************
If Tmp_accy(2) > 0 Then
If Tmp_accy(2) < 64 Then
If Tmp_accz(2) < 65 Then
If Tmp_accz(2) > 0 Then
Zv = 64 - Yc
Yv = Zv * 1.407
End If
End If
End If
End If
If Tmp_accy(2) > 190 Then
If Tmp_accy(2) < 255 Then
If Tmp_accz(2) < 65 Then
If Tmp_accz(2) > 0 Then
Zv = 255 - Yc
Zv = Zv * 1.407
Yv = Zv + 90
End If
End If
End If
End If
If Tmp_accy(2) > 190 Then
If Tmp_accy(2) < 255 Then
If Tmp_accz(2) < 255 Then
If Tmp_accz(2) > 189 Then
Zv = 255 - Yc
Zv = Zv * 1.407
Yv = Zv - 90
End If
End If
End If
End If
If Tmp_accy(2) > 0 Then
If Tmp_accy(2) < 64 Then
If Tmp_accz(2) < 255 Then
If Tmp_accz(2) > 189 Then
Zv = -yc
Zv = Zv * 1.407
Yv = Zv - 90
End If
End If
End If
End If
Locate 3 , 11
Lcd "Y " ; Yv ; Chr(1) ; " "
If Xv > 255 Then
Xcerr = Xcerr + 1
End If
If Yv > 255 Then
Ycerr = Ycerr + 1
End If
If Zc < 0 Then
Zcerr = Zcerr + 1
End If
'*******err************
If Xcerr > 1 Then
Xv = 0
Xcerr = 0
End If
If Ycerr > 1 Then
Yv = 0
Ycerr = 0
End If
'******err*************
Locate 2 , 1
Lcd Gyrox ; " " ; Tmp_accx(1) ; " "
Locate 4 , 1
Lcd "err " ; Xcerr ; " " ; Ycerr ; " " ; Zcerr;
Loop
End
Sub Read_gyro()
I2cstart
I2cwbyte &HD0
I2cwbyte 67
I2crepstart
I2cwbyte &HD1
I2crbyte Tmp_gyrox(2) , Ack
I2crbyte Tmp_gyrox(1) , Ack
I2crbyte Tmp_gyroy(2) , Ack
I2crbyte Tmp_gyroy(1) , Ack
I2crbyte Tmp_gyroz(2) , Ack
I2crbyte Tmp_gyroz(1) , Nack
I2cstop
'Gyrox = Gx
'Gyroy = 0 - Gy
'Gyroz = 0 - Gz
End Sub
Sub Read_acc()
I2cstart
I2cwbyte &HD0
I2cwbyte 59
I2crepstart
I2cwbyte &HD1
I2crbyte Tmp_accx(2) , Ack
I2crbyte Tmp_accx(1) , Ack
I2crbyte Tmp_accy(2) , Ack
I2crbyte Tmp_accy(1) , Ack
I2crbyte Tmp_accz(2) , Ack
I2crbyte Tmp_accz(1) , Nack
I2cstop
'Accx = Ax
'Accy = Ay
'Accz = Az
End Sub
0 komentar:
Posting Komentar