Code:
Gosub MPU6050_init
'Hauptschleife
Do
'Alle 5ms Neue Sensordaten lesen, Filter, PID, Motoren beschreiben
'Variable Berechnen wird in einem 5ms Timer Interrupt gesetzt
If Berechnen = 1 Then
Berechnen = 0
Gosub Neue_Daten
'PID Regler Gyro X-Achse
Call PID_Cal(Soll_Gyr_X, Ist_Gyr_X, PID_out_G_X, Kp_G_X, Ki_G_X, Kd_G_X, eSum_G_X, eAlt_G_X)
'PID Regler Acc X-Achse
Call PID_Cal(Winkel_X_SW, Winkel_X_T, PID_out_X, Kp_X, Ki_X, Kd_X, eSum_X, eAlt_X)
'PID Regler Gyro Y-Achse
Call PID_Cal(Soll_Gyr_Y, Ist_Gyr_Y, PID_out_G_Y, Kp_G_Y, Ki_G_Y, Kd_G_Y, eSum_G_Y, eAlt_G_Y)
'PID Regler Acc Y-Achse
Call PID_Cal(Winkel_Y_SW, Winkel_Y_T, PID_out_Y, Kp_Y, Ki_Y, Kd_Y, eSum_Y, eAlt_Y)
'PID Regler Gyro Z-Achse
Call PID_Cal(Soll_Gyr_Z, Ist_Gyr_Z, PID_out_G_Z, Kp_G_Z, Ki_G_Z, Kd_G_Z, eSum_G_Z, eAlt_G_Z)
'Serielle Daten senden (alle 5ms 1 Wert)
'If send_data = 1 Then
Gosub Daten_Senden
'End if
'....ESC's beschreiben
'....Analogwerte Spannung/Strom verarbeiten
'....Diverse Funktionen RC-Empfänger
Loop
Neue_Daten:
Gosub MPU6050_read
'Gyrowinkel berechnen
'Offset abziehen
Gyr_X = Gyr_X - Gyr_X_Offset
Gyr_Y = Gyr_Y - Gyr_Y_Offset
Gyr_Z = Gyr_Z - Gyr_Z_Offset
'Winkel X-Achse
Ist_Gyr_X = Gyr_X / 32.8
Temp_s = Ist_Gyr_X * 0.005
Winkl_Gyr_X = Temp_s
'Winkel Y-Achse
Ist_Gyr_Y = Gyr_Y / 32.8
Temp_s = Ist_Gyr_Y * 0.005
Winkl_Gyr_Y = Temp_s
'Winkel Z-Achse
Ist_Gyr_Z = Gyr_Z / 32.8
Temp_s = Ist_Gyr_Z * 0.005
Winkl_Gyr_Z = Temp_s
'ACC Winkel berechnen X-Achse
Temp2_s = ACC_Y
Temp_s = ACC_X * ACC_X
Temp3_s = ACC_Z * ACC_Z
Temp_s = Temp_s + Temp3_s
Temp3_s = sqr(Temp_s)
Temp_s = atn2(Temp2_s , Temp3_s)
Winkl_ACC_X = rad2deg(Temp_s)
'ACC Winkel berechnen Y-Achse
Temp2_s = ACC_X
Temp_s = ACC_Y * ACC_Y
Temp3_s = ACC_Z * ACC_Z
Temp_s = Temp_s + Temp3_s
Temp3_s = sqr(Temp_s)
Temp_s = atn2(Temp2_s , Temp3_s)
Temp_s = Temp_s * -1
Winkl_ACC_Y = rad2deg(Temp_s)
'Gesamtbeschleunigung berechnen
Temp_s = ACC_X * ACC_X
Temp3_s = ACC_Z * ACC_Z
Temp_s = Temp_s + Temp3_s
Temp3_s = ACC_Y * ACC_Y
Temp_s = Temp_s + Temp3_s
Temp2_s = sqr(Temp_s)
ACC_Total = Temp2_s / 16383
ACC_Dif1g = 1 - ACC_Total
ACC_Dif1g = abs(ACC_Dif1g)
'Komplementärfilter 0,98*Gyro + 0.02*ACC
'Nur ausführen wenn gesammtbeschleunigung nahe 1g
'X-Achse
Temp_s = Winkel_X + Winkl_Gyr_X
if ACC_Dif1g < 0.5 then
Temp2_s = Temp_s * Kompl_Faktor
Temp_s = 1 - Kompl_Faktor
Temp3_s = Winkl_ACC_X * Temp_s
Winkel_X = Temp2_s + Temp3_s
Else
Winkel_X = Temp_s
End if
'Y-Achse
Temp_s = Winkel_Y + Winkl_Gyr_Y
if ACC_Dif1g < 0.5 then
Temp2_s = Temp_s * Kompl_Faktor
Temp_s = 1 - Kompl_Faktor
Temp3_s = Winkl_ACC_Y * Temp_s
Winkel_Y = Temp2_s + Temp3_s
Else
Winkel_Y = Temp_s
End if
'Drehmatrix um Z-Achse
Temp_s = deg2rad(Winkl_Gyr_Z)
GyroZSin = Sin(Temp_s) 'Drehung um Yaw Achse von Deg zu Rad konvertieren
GyroZCos = Cos(Temp_s)
Temp_X = GyroZCos * Winkel_X
Temp_Y = GyroZSin * Winkel_Y
Temp2_s = Temp_Y + Temp_X
Temp_X = GyroZSin * Winkel_X
Temp_Y = GyroZCos * Winkel_Y
Temp3_s = Temp_Y - Temp_X
'Werte zurückschreiben
Winkel_X = Temp2_s
Winkel_Y = Temp3_s
'Trimm Wert verrechnen
Winkel_X_T = Winkel_X + Winkel_X_Trimm
Winkel_Y_T = Winkel_Y + Winkel_Y_Trimm
Return
'---------- Beschleunigungssensor auslesen ----------
MPU6050_read:
I2cstart
I2cwbyte &HD0 'Schreibadresse
I2cwbyte &H3B 'Register h43 auswählen
I2cstop
I2cstart
I2cwbyte &HD1 'Leseadresse
I2crbyte Msb , Ack 'ACC X (Register 3B + 3C)
I2crbyte Lsb , Ack
ACC_X = Makeint(Lsb,Msb)
I2crbyte Msb , Ack 'ACC Y (Register 3D + 3E)
I2crbyte Lsb , Ack
ACC_Y = Makeint(Lsb,Msb)
I2crbyte Msb , Ack 'ACC Z (Register 3F + 40)
I2crbyte Lsb , Ack
ACC_Z = Makeint(Lsb,Msb)
I2crbyte Msb , Ack 'Temperatur (Register 41 + 42)
I2crbyte Lsb , Ack
I2crbyte Msb , Ack 'Gyro X (Register 43 + 44)
I2crbyte Lsb , Ack
Gyr_X = Makeint(Lsb,Msb)
I2crbyte Msb , Ack 'Gyro Y (Register 45 + 46)
I2crbyte Lsb , Ack
Gyr_Y = Makeint(Lsb,Msb)
I2crbyte Msb , Ack 'Gyro Z (Register 47 + 48)
I2crbyte Lsb , Nack
Gyr_Z = Makeint(Lsb,Msb)
Return
'---------- Beschleunigungssensor Initialisieren ----------
MPU6050_init:
'Sensor aktivieren
I2cstart
I2cwbyte &HD0 'Schreibadresse
I2cwbyte &H6B 'Register h6B Powermanagement
I2cwbyte &H00 'Wert 0 schreiben
I2cstop
waitms 200
Toggle LED_R
toggle LED_H
'Filter konfigurieren
' ACC Gyro
'Wert Bandbreite Delay Bandbreite Delay
'0 260Hz 0.0ms 256Hz 1.0ms
'1 184Hz 2.0ms 188Hz 1.9ms
'2 94Hz 3.0ms 98Hz 2.8ms
'3 44Hz 4.9ms 42Hz 4.8ms
I2cstart
I2cwbyte &HD0 'Schreibadresse
I2cwbyte &H1A 'Register h1A Filter
I2cwbyte 6 'Wert schreiben
I2cstop
waitms 200
Toggle LED_R
toggle LED_H
'Gyro Bereich festlegen
'Wert Bereich
'0 +/- 250°/s
'8 +/- 500°/s
'16 +/- 1000°/s
'24 +/- 2000°/s
I2cstart
I2cwbyte &HD0 'Schreibadresse
I2cwbyte &H1B 'Register h1B Gyro konfig
I2cwbyte 16 'Wert schreiben
I2cstop
waitms 200
Toggle LED_R
toggle LED_H
'ACC Bereich festlegen
'Wert Bereich
'0 +/- 2g
'8 +/- 4g
'16 +/- 8g
'24 +/- 16g
I2cstart
I2cwbyte &HD0 'Schreibadresse
I2cwbyte &H1C 'Register h1C ACC konfig
I2cwbyte 0 'Wert schreiben
I2cstop
waitms 200
Toggle LED_R
toggle LED_H
Gyr_X_Offset_Sum = 0
Gyr_Y_Offset_Sum = 0
Gyr_Z_Offset_Sum = 0
For i = 1 to 200
gosub MPU6050_READ
Gyr_X_Offset_Sum = Gyr_X_Offset_Sum + Gyr_X
Gyr_Y_Offset_Sum = Gyr_Y_Offset_Sum + Gyr_Y
Gyr_Z_Offset_Sum = Gyr_Z_Offset_Sum + Gyr_Z
waitms 1
next
Gyr_X_Offset = Gyr_X_Offset_Sum / i
Gyr_Y_Offset = Gyr_Y_Offset_Sum / i
Gyr_Z_Offset = Gyr_Z_Offset_Sum / i
Toggle LED_R
toggle LED_H
waitms 200
LED_R = 0
toggle LED_H
Return
ist leider nicht so schön programmiert
Zitieren
Lesezeichen