Mini Moduł MPU-6500 to czujnik mierzacy przeciążenia działające na układ. Umożliwia pomiar przeciążeń w trzech kierunkach. To rozwiązanie bardzo precyzyjne ponieważ zawiera 16 bitowy przetwornik analogowo-cyfrowy dla każdego kanału x,y i z jednocześnie.
Właściwości
- napięcie zasilania DC 5V
- pobór prądu 5mA
- komunikacja I2C TWI
- w komplecie goldpiny
- wymiary 20x15mm
- Zakres żyroskopu : +250 500 1000 2000 o/s
- Zakres akcelerometru : ±2 ±4 ±8 ±16 g
Kod dla Bascom-AVR
$regfile = "m32def.dat"
$crystal = 12000000
'$baud = 9600
'*** Konfiguracja Uart dla Hc-05
Open "comd.7:9600,8,n,1" For Output As #1
Open "comd.6:9600,8,n,1" For Input As #2
'*** Konfiguracja i2c
Config Scl = Portc.0
Config Sda = Portc.1
Config Twi = 400000
Declare Sub Mpu6050_write(regadr As Byte , Byval Wert As Byte)
Declare Function Mpu6050_read(regadr As Byte) As Byte
Dim Mpu6050_adr As Byte
Dim Mpu6050_adr1 As Byte
Dim Mpu6050_who_am_i As Byte
Dim Mpu6050_accel_xout_h As Byte
Dim Mpu6050_accel_xout_l As Byte
Dim Mpu6050_accel_yout_h As Byte
Dim Mpu6050_accel_yout_l As Byte
Dim Mpu6050_accel_zout_h As Byte
Dim Mpu6050_accel_zout_l As Byte
Dim Mpu6050_gyro_xout_h As Byte
Dim Mpu6050_gyro_xout_l As Byte
Dim Mpu6050_gyro_yout_h As Byte
Dim Mpu6050_gyro_yout_l As Byte
Dim Mpu6050_gyro_zout_h As Byte
Dim Mpu6050_gyro_zout_l As Byte
Dim Mpu6050_pwr_mgmt_1 As Byte
Dim Mpu6050_temp_h As Byte
Dim Mpu6050_temp_l As Byte
Dim Accel_x_h As Byte
Dim Accel_x_l As Byte
Dim Accel_x_int As Single
Dim Accel_x_real As Single
Dim Accel_y_h As Byte
Dim Accel_y_l As Byte
Dim Accel_y_int As Single
Dim Accel_y_real As Single
Dim Accel_z_h As Byte
Dim Accel_z_l As Byte
Dim Accel_z_int As Single
Dim Accel_z_real As Single
Dim Accel_x_angle As Single
Dim Accel_y_angle As Single
Dim Gyro_x_h As Byte
Dim Gyro_x_l As Byte
Dim Gyro_x_int As Single
Dim Gyro_x_real As Single
Dim Gyro_y_h As Byte
Dim Gyro_y_l As Byte
Dim Gyro_y_int As Single
Dim Gyro_y_real As Single
Dim Gyro_z_h As Byte
Dim Gyro_z_l As Byte
Dim Gyro_z_int As Single
Dim Gyro_z_real As Single
Dim Temp_h As Byte
Dim Temp_l As Byte
Dim Temp_int As Integer
Dim Temp_real As Single
Dim Antwort As Byte
Dim Ergebnis As Byte
Waitms 50
Mpu6050_adr = &HD0 '
Mpu6050_adr1 = &HD1
Mpu6050_who_am_i = &H75
Mpu6050_accel_xout_h = &H3B
Mpu6050_accel_xout_l = &H3C
Mpu6050_accel_yout_h = &H3D
Mpu6050_accel_yout_l = &H4E
Mpu6050_accel_zout_h = &H3F
Mpu6050_accel_zout_l = &H40
Mpu6050_temp_h = &H41
Mpu6050_temp_l = &H42
Mpu6050_gyro_xout_h = &H43
Mpu6050_gyro_xout_l = &H44
Mpu6050_gyro_yout_h = &H45
Mpu6050_gyro_yout_l = &H46
Mpu6050_gyro_zout_h = &H47
Mpu6050_gyro_zout_l = &H48
Mpu6050_pwr_mgmt_1 = &H6B
Antwort = Mpu6050_read(mpu6050_who_am_i)
If Antwort = &H68 Then Print #1 , " AT+mpu6050 sprawdz polaczenie "
Call Mpu6050_write(mpu6050_pwr_mgmt_1 , 0)
Do
Wait 1
Accel_x_h = Mpu6050_read(mpu6050_accel_xout_h)
Accel_x_l = Mpu6050_read(mpu6050_accel_xout_l)
Accel_x_int = 256 * Accel_x_h
Accel_x_int = Accel_x_int + Accel_x_l
Accel_x_real = Accel_x_int * 0.00059875
Accel_y_h = Mpu6050_read(mpu6050_accel_yout_h)
Accel_y_l = Mpu6050_read(mpu6050_accel_yout_l)
Accel_y_int = 256 * Accel_y_h
Accel_y_int = Accel_y_int + Accel_y_l
Accel_y_real = Accel_y_int * 0.00059875
Accel_z_h = Mpu6050_read(mpu6050_accel_zout_h)
Accel_z_l = Mpu6050_read(mpu6050_accel_zout_l)
Accel_z_int = 256 * Accel_z_h
Accel_z_int = Accel_z_int + Accel_z_l
Accel_z_real = Accel_z_int * 0.00059875
Print #1 , Accel_x_real ; " X m/s2 "
Print #1 , Accel_y_real ; " Y m/s2 "
Print #1 , Accel_z_real ; " Z m/s2 "
Accel_x_angle = Atn2(accel_x_int , Accel_z_int)
Accel_x_angle = Accel_x_angle * 180
Accel_x_angle = Accel_x_angle / 3.14
Accel_y_angle = Atn2(accel_y_int , Accel_z_int)
Accel_y_angle = Accel_y_angle * 180
Accel_y_angle = Accel_y_angle / 3.14
Print #1 , "WYCHYLENIE X: " ; Accel_x_angle ; " Y: " ; Accel_y_angle
Temp_h = Mpu6050_read(mpu6050_temp_h)
Temp_l = Mpu6050_read(mpu6050_temp_l)
Temp_int = 256 * Temp_h
Temp_int = Temp_int + Temp_l
Temp_real = Temp_int * 0.002941
Temp_real = Temp_real + 36.53 'vgl. S. 31 von RM_MPU-6000A.pdf
Print #1 , Temp_real ; " mpu6050 Temperatura"
Gyro_x_h = Mpu6050_read(mpu6050_gyro_xout_h)
Gyro_x_l = Mpu6050_read(mpu6050_gyro_xout_l)
Gyro_x_int = Gyro_x_h * 256
Gyro_x_int = Gyro_x_int + Gyro_x_l
Gyro_x_int = Gyro_x_h + 256
Gyro_y_h = Mpu6050_read(mpu6050_gyro_yout_h)
Gyro_y_l = Mpu6050_read(mpu6050_gyro_yout_l)
Gyro_y_int = Gyro_y_h * 256
Gyro_y_int = Gyro_y_int + Gyro_y_l
Gyro_x_int = Gyro_x_h + 256
Gyro_z_h = Mpu6050_read(mpu6050_gyro_zout_h)
Gyro_z_l = Mpu6050_read(mpu6050_gyro_zout_l)
Gyro_z_int = Gyro_z_h * 256
Gyro_z_int = Gyro_z_int + Gyro_z_l
Gyro_z_int = Gyro_z_h + 256
Print #1 , "GYRO : X " ; Gyro_x_int ; " Y " ; Gyro_y_int ; " Z " ; Gyro_z_int
Loop
End 'end program
'**********************************************************
'******************* ***********************
Function Mpu6050_read(regadr As Byte) As Byte
Ergebnis = 0
I2cstart
I2cwbyte Mpu6050_adr
If Err = 0 Then
I2cwbyte Regadr
I2cstart
I2cwbyte Mpu6050_adr1
I2crbyte Ergebnis , Nack
End If
I2cstop
Mpu6050_read = Ergebnis
End Function
'**********************************************************
'******************* PODPROGRAMY ***********************
Sub Mpu6050_write(regadr As Byte , Byval Wert As Byte)
I2cstart
I2cwbyte Mpu6050_adr
If Err = 0 Then
I2cwbyte Regadr
I2cwbyte Wert
End If
I2cstop
End Sub
