Dauerkalender

Autopilot

(Beschleunigungs- und Lagesensor MPU-6050)

Mit der integrierten Schaltung MPU-6050 (3-Achsen Beschleunigungs- und Lagesensor) kann man die Lage und Bewegung von Gegenständen feststellen und messen. Zusätzlich ist es möglich, die aktuelle Temperatur zu ermitteln. Die dazu notwendigen Sensoren sind direkt in dem Chip untergebracht. Die Daten werden von einem Analog-Digital-Wandler mit einer Auflösung von 16-Bit pro Kanal ausgegeben. Bei Anbindung an Arduino wird der I2C-Bus als Schnittstelle verwendet.

MPU-6050

Modul GY-521

Die Beschleunigungs- und Lagesensoren (IMU-Sensoren) werden sehr oft eingesetzt. Besonders bekannte Anwendungsbereiche sind z.B. Smartphones oder diverse Spielkonsolen. Immer öfter kommen sie in Bewegung-Messgeräten aus dem Fitnessbereich.
Der MPU-6050 liefert insgesamt sieben Werte (Rohdaten), die in einem Programm ausgewertet werden können. In dem folgenden Versuch wird nur ein Wert verwendet. Mit seiner Hilfe konstruieren wir einen einfachen Autopiloten, dessen Aufgabe darin besteht, eine Flugzeugimitation in horizontale Lage zu halten. Zum Einsatz kommt hier das Modul GY-521, auf dem der Sensor MPU-6050 untergebracht ist.

Schrittmotor 28BYJ mit Treiber

Für die Lagekorrekturen des Flugzeugs ist ein Schrittmotor (28BYJ-48) mit dem Treiber ULN2003 zuständig. Stellt der Sensor, der auf dem Flugzeug angebracht ist, eine Abweichung von der waagerechten Lage fest, wird das Flugzeugheck mithilfe des Motors entsprechend rauf- oder runterbewegt. Die Bewegung erfolgt solange, bis das Flugzeug die horizontale Lage wieder erreicht hat.

Flugzeug Imitation. In der Mitte das Modul mit dem Sensor MPU-6050

Als Störfaktor, der die üblichen Luftturbulenzen erzeugt, wird ein zweiter Schrittmotor eingesetzt. Er wird mit zwei Tastern manuell gesteuert und ist für die Bewegung des vorderen Teils des Flugzeugs zuständig. Die Aufgabe der Schaltung ist es, die durch Turbulenzen (Motor Nr. 1) verursachte Lageänderungen des Flugzeugs festzustellen und entsprechend zu korrigieren (Motor Nr. 2)

Der Schaltplan

Schaltplan

Die Testschaltung

Autopilot Testschaltung

Die komplette Versuchsanordnung

Die komplette Versuchsanordnung

Das Programm

Für den einfachen Versuch werden nicht alle Daten benötigt, die der MPU-6050-Sensor liefern kann. Für uns sind nur die Werte, die sich auf die Lage des Sensors in der Y-Richtung beziehen, relevant. Anhand dieser Daten kann man den Neigungswinkel errechnen und bei Bedarf entsprechend reagieren. In diesem Beispiel soll das Flugzeug möglichst stets waagerecht gehalten werden.
Mit zwei Schaltern werden Luftturbulenzen imitiert und das Flugzeug aus dem Gleichgewicht gebracht. Der Sensor muss diese Unregelmäßigkeiten erkennen und das Flugzeug wieder in horizontale Lage bringen.
Die empfangenen Daten weisen große Sprünge auf, einige Ausreiser sind auch dabei. Bevor man die Daten auswertet, wäre es empfehlenswert, eine gute Methode für ihre Stabilisierung auszusuchen. Hier wird lediglich ein Mittelwert aus den letzten 10 Werten errechnet. Die Werte kann man als eine Kurve auf dem IDE-Plotter beobachten. So sieht ungefähr der Verlauf bei diesem Versuch aus:

Serieller Plotter

Die Reaktion auf Wertänderung wird hier mit einfachem Kleiner-Größer-Vergleich realisiert. Für die Ansteuerung der Schrittmotoren stehen mehrere Bibliotheken zur Verfügung. Hier werden die Treiber ohne „fremde Hilfe“ direkt mit jeweils vier Ausgängen angesprochen.

#include<Wire.h>

const int MPU_addr=0x68;                        // I2C-Adresse MPU-6050
int16_t AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ;
int Wert [10];
int i;
int IstPos;
int Taster_Rauf = 52;
int Taster_Runter = 50;
unsigned long MillisAktuell = 0;
unsigned long MillisHalten;

bool Seq [8][4] = { {0,0,0,1},                  // IN1, IN2, IN3, IN4
                    {0,0,1,1},
                    {0,0,1,0},
                    {0,1,1,0},
                    {0,1,0,0},
                    {1,1,0,0},
                    {1,0,0,0},
                    {1,0,0,1} };
                                                // Ausgänge für Motoren
int AutoPilot_IN1 = 4;
int AutoPilot_IN2 = 5;
int AutoPilot_IN3 = 6;
int AutoPilot_IN4 = 7;
int Turbulenz_IN1 = 8;
int Turbulenz_IN2 = 9;
int Turbulenz_IN3 = 10;
int Turbulenz_IN4 = 11;

                                                // SetUp -----------------------
void setup(){
    Wire.begin();
    Wire.beginTransmission(MPU_addr);
    Wire.write(0x6B);                           // PWR_MGMT_1 register
    Wire.write(0);                              // MPU-6050 aktivieren
    Wire.endTransmission(true);
    pinMode (Taster_Rauf, INPUT_PULLUP);
    pinMode (Taster_Runter, INPUT_PULLUP);
    pinMode (AutoPilot_IN1, OUTPUT); 
    pinMode (AutoPilot_IN2, OUTPUT);
    pinMode (AutoPilot_IN3, OUTPUT);
    pinMode (AutoPilot_IN4, OUTPUT);
    pinMode (Turbulenz_IN1, OUTPUT); 
    pinMode (Turbulenz_IN2, OUTPUT);
    pinMode (Turbulenz_IN3, OUTPUT);
    pinMode (Turbulenz_IN4, OUTPUT);
    MillisHalten = millis();
    Serial.begin(115200);
}
                                                // Hauptprogramm ---------------
void loop(){
                                                // Zeitmessung
    MillisAktuell = millis();
    if ( MillisAktuell > (MillisHalten + 500))  {
        MillisHalten = MillisAktuell;
  
        Wire.beginTransmission(MPU_addr);
        Wire.write(0x3B);                         
        Wire.endTransmission(false);
        Wire.requestFrom(MPU_addr,14,true);     // 14 Register anfordern
        AcX=Wire.read()<<8|Wire.read();         // 0x3B (ACCEL_X)   
        AcY=Wire.read()<<8|Wire.read();         // 0x3D (ACCEL_Y)
        Tmp=Wire.read()<<8|Wire.read();         // 0x41 (TEMP)
        GyX=Wire.read()<<8|Wire.read();         // 0x43 (GYRO_X)
        GyY=Wire.read()<<8|Wire.read();         // 0x45 (GYRO_Y)
        GyZ=Wire.read()<<8|Wire.read();         // 0x47 (GYRO_Z)

                                                // Mittelwert (10 letzte Werte)
        for (i=9; i>0; i--) { Wert [i] = Wert [i-1]; }
        Wert [0] = AcY;
        int Summe = 0;
        for (i=0; i<10; i++) {
            Summe = Summe + Wert [i]; }
        IstPos = Summe / 10;          

        Serial.print(" | IstPos = "); Serial.print(IstPos);
        Serial.print(" | AcY = "); Serial.println(AcY+400);
    }

    if (IstPos < -200) {                        // Autopilot RAUF
        for (i=0; i<8; i++) { 
            if (Seq [i][0]) { digitalWrite (AutoPilot_IN1,HIGH); } else { digitalWrite (AutoPilot_IN1,LOW); }
            if (Seq [i][1]) { digitalWrite (AutoPilot_IN2,HIGH); } else { digitalWrite (AutoPilot_IN2,LOW); }
            if (Seq [i][2]) { digitalWrite (AutoPilot_IN3,HIGH); } else { digitalWrite (AutoPilot_IN3,LOW); }
            if (Seq [i][3]) { digitalWrite (AutoPilot_IN4,HIGH); } else { digitalWrite (AutoPilot_IN4,LOW); }
            delayMicroseconds(10000); } }  
  
    if (IstPos > 200) {                        // Autopilot RUNTER
        for (i=0; i<8; i++) { 
            if (Seq [i][0]) { digitalWrite (AutoPilot_IN4,HIGH); } else { digitalWrite (AutoPilot_IN4,LOW); }
            if (Seq [i][1]) { digitalWrite (AutoPilot_IN3,HIGH); } else { digitalWrite (AutoPilot_IN3,LOW); }
            if (Seq [i][2]) { digitalWrite (AutoPilot_IN2,HIGH); } else { digitalWrite (AutoPilot_IN2,LOW); }
            if (Seq [i][3]) { digitalWrite (AutoPilot_IN1,HIGH); } else { digitalWrite (AutoPilot_IN1,LOW); }
            delayMicroseconds(4000); } }

    // Bei Bedarf kann man alle ausgelesenen Werte auf dem Monitor anzeigen lassen.
    // Serial.print(" | AcZ = "); Serial.print(AcZ);
    // Serial.print(" | Tmp = "); Serial.print(Tmp/340.00+36.53);  // Formel aus dem Datenblatt
    // Serial.print(" | GyX = "); Serial.println(GyX);
    // Serial.print(" | GyY = "); Serial.print(GyY);
    // Serial.print(" | GyZ = "); Serial.println(GyZ);
    
                                     // Taster RAUF (Turbulenzen) ...........................
    if (digitalRead(Taster_Rauf) == LOW) {
        for (i=0; i<8; i++) { 
            if (Seq [i][0]) { digitalWrite (Turbulenz_IN1,HIGH); } else { digitalWrite (Turbulenz_IN1,LOW); }
            if (Seq [i][1]) { digitalWrite (Turbulenz_IN2,HIGH); } else { digitalWrite (Turbulenz_IN2,LOW); }
            if (Seq [i][2]) { digitalWrite (Turbulenz_IN3,HIGH); } else { digitalWrite (Turbulenz_IN3,LOW); }
            if (Seq [i][3]) { digitalWrite (Turbulenz_IN4,HIGH); } else { digitalWrite (Turbulenz_IN4,LOW); }
            delayMicroseconds(1000); } }
            
                                     // Taster RUNTER (Turbulenzen) ...........................
    if (digitalRead(Taster_Runter) == LOW) {
        for (i=0; i<8; i++) { 
            if (Seq [i][0]) { digitalWrite (Turbulenz_IN4,HIGH); } else { digitalWrite (Turbulenz_IN4,LOW); }
            if (Seq [i][1]) { digitalWrite (Turbulenz_IN3,HIGH); } else { digitalWrite (Turbulenz_IN3,LOW); }
            if (Seq [i][2]) { digitalWrite (Turbulenz_IN2,HIGH); } else { digitalWrite (Turbulenz_IN2,LOW); }
            if (Seq [i][3]) { digitalWrite (Turbulenz_IN1,HIGH); } else { digitalWrite (Turbulenz_IN1,LOW); }
            delayMicroseconds(1000); } }
}
// -----------------------------------------------------------
    

Kurzvideo

Kurzvideo: Autopilot mit MPU-6050