Gondel Software

Inhaltsverzeichnis[Anzeigen]

 

Software

 

Positionssender()

 

Der Positionssender wird gepulst angesteuert. Die Pulsung erfolgt alle 200ms. Zu beginn der 200ms werden innerhalb von 0.2ms 8 Pulse generiert. Ziel ist es ein Signal mit 40kHz zu erhalten.

void Positionssender(){
  int long t;

  if(micros()-time>200000){		// alle 200ms
    time = micros();

    for(t=0; t<=200;){		        // in 0.2ms 8 Pulse
      t=micros()-time;
      
      digitalWrite(IN3_2, 1);
      digitalWrite(IN4_2, 0);
      delayMicroseconds(1);	        // 40kHz Signal erzeugen
                                        // am besten mit Oszilloskop den delay einstellen
      digitalWrite(IN3_2, 0);
      digitalWrite(IN4_2, 1);
      delayMicroseconds(1);
    }
  }
}



XBee_read()

 

Die über das Xbee übermittelten Daten liegen in der Form "abccdeefgg" vor.

a Informationen über den Abwurf 1=Kein Abwurf 2=Abwurf
b Richtung linker Motor 0=links 1=rechts
cc Geschwindigkeit linker Motor 0=Stopp 99=Maximaler Schub
d Richtung rechter Motor 0=links 1=rechts
ee Geschwindigkeit rechter Motor 0=Stopp 99=Maximaler Schub
f Richtung unterer Motor 0=hoch 1=runter
gg Geschwindigkeit unterer Motor 0=Stopp 99=Maximaler Schub

 

Beispiel: 2199199150 = Paket abwerfen, Geradeausflug mit maximalem Schub, halber Schub nach unten

 

void Xbee_read(){
  int xbeer[10];                         // Zwischenspeicher für Xbee-Daten, Werte sind Rückwerts eingelesen
  int xbeev[10];                         // Zwischenspeicher für Xbee-Daten, aber in richtiger Reihenfolge

  if(Serial.available()){
time2 = millis(); // timer für Xbee_check() for(i=9; i>=0; i--){ xbeer[i] = Serial.read(); // Xbee werte einlesen delay(10); } for(i=0; i<=9; i++){ xbeer[i] = xbeer[i] - 48; // ascii umwandeln } for(i=0; i<=9; i++){ xbeev[i] = xbeer[9-i]; // xbeer spiegeln, damit Daten in xbeev in richtiger Reihenfolge } for(i=0; i<=6; i++){ // Daten richtig zusammenfassen if(i==0){ daten[i] = xbeev[i]; // daten[0] = Abwurfbefehl } if(i==1){ daten[i] = xbeev[i]; // daten[1] = Richtung Motor links } if(i==2){ // daten[2] = Geschwindigkeit Motor links, dafür daten[i] = (xbeev[i]*10)+xbeev[i+1]; // xbeev[2] und xbeev[3] zusammenfassen } if(i==3){ daten[i] = xbeev[i+1]; // daten[3] = Richtung Motor rechts } if(i==4){ // daten[4] = Geschwindigkeit Motor rechts, dafür daten[i] = (xbeev[i+1]*10)+xbeev[i+2]; // xbeev[5] und xbeev[6] zusammenfassen } if(i==5){ daten[i] = xbeev[i+2]; // daten[5] = Richtung Motor rechts } if(i==6){ // daten[6] = Geschwindigkeit Motor unten, dafür daten[i] = (xbeev[i+2]*10)+xbeev[i+3]; // xbeev[8] und xbeev[9] zusammenfassen } } } }



XBee_check()

 

Aktiviert einen Notfallmodus, der die Gondel zum Boden befördert, indem der Höhenmotor aktiviert wird, falls mind. 5s keine neuen XBee-Daten vorliegen.

 

void Xbee_check(){
  if(millis()-time2 > 5000 && abwurf_check == 1){            // länger als 5s keine neuen Xbee-Daten und Paket abgeworfen
    daten[1] = 0;                                            // --> alle Motoren stopp, voller Schub mit Richtung nach unten
    daten[2] = 0;
    daten[3] = 0;
    daten[4] = 0;
    daten[5] = 0;
    daten[6] = 99;
  }
  if(millis()-time2 > 5000 && abwurf_check == 0){            // länger als 5s keine neuen Xbee-Daten und Paket noch nicht abgeworfen
    daten[1] = 0;                                            // --> alle Motoren stopp, voller Schub mit Richtung nach unten
    daten[2] = 0;
    daten[3] = 0;
    daten[4] = 0;
    daten[5] = 0;
    daten[6] = 50;
  }
  if(millis()-time2 > 15000){                                // länger als 15s keine neuen Xbee-Daten
    daten[1] = 0;                                            // --> alle Motoren stopp, voller Schub mit Richtung nach unten
    daten[2] = 0;
    daten[3] = 0;
    daten[4] = 0;
    daten[5] = 0;
    daten[6] = 99;
  }
}




Abwurf()

 

Wirft das Rettungspaket über den Servo, sobal die XBee-Nachricht an Stelle "a" (siehe XBee_read() ) eine 2 hat.

 

void Abwurf(){
  if(daten[0]==2){
abwurf_check = 1; // überprüfung ob Abwurf erfolgt für XBee_check() analogWrite(servo, 226); // Abwurf des Rettungspakets, Servo dreht um ca.90° } else{ analogWrite(servo, 113); // Ausgangsstellung des Servos } }



Motordaten()

 

Weist den einzelnen Motorvariabeln ihre Werte (für Richtung und Geschwindigkeit) zu, damit diese an die Motoren übergeben werden können.

 

void Motordaten(){
  if(daten[1]==0){                              // Richtung Motor links einstellen
    richtung_links = 0;
  }
  else{
    richtung_links = 1;
  }
  if(daten[2]==0){                              // Geschwindigkeit Motor links einstellen
    speed_links = 0;
  }
  else{
    speed_links = 1.97 * daten[2] + 60;         // y=mx+t ; t=minimal nötiger enable-Wert damit Motoren sich drehen, m=(255-t)/99
  }                                             // 255=maximaler Schub, 60=minimaler Schub; /99 wegen Xbee Schubdaten von 0-99


  if(daten[3]==0){                              // Richtung Motor rechts einstellen
    richtung_rechts = 0;
  }
  else{
    richtung_rechts = 1;
  }
  if(daten[4]==0){                              // Geschwindigkeit Motor links einstellen
    speed_rechts = 0;
  }
  else{
    speed_rechts = 1.97 * daten[4] + 60;        // y=mx+t ; t=minimal nötiger enable-Wert damit Motoren sich drehen, m=(255-t)/99
  }                                             // 255=maximaler Schub, 60=minimaler Schub; /99 wegen Xbee Schubdaten von 0-99


  if(daten[5]==0){                              // Richtung Motor unten einstellen
    richtung_unten = 1;
  }
  else{
    richtung_unten = 0;
  }
  if(daten[6]==0){                              // Geschwindigkeit Motor unten einstellen
    speed_unten = 0;
  }
  else{
   speed_unten = 1.97 * daten[6] + 60;          // y=mx+t ; t=minimal nötiger enable-Wert damit Motoren sich drehen, m=(255-t)/99
  }                                             // 255=maximaler Schub, 60=minimaler Schub; /99 wegen Xbee Schubdaten von 0-99
}



Motoren()

 

Ruft für jeden Motor eine einzelne Funktion auf. In diesen werden die Werte aus Motordaten() übergeben.

 

void Motoren(){
// Motor links
  analogWrite(EN1_1, speed_links);
  digitalWrite(IN1_1, !richtung_links);        // Motorwerte aus Motordaten() an Motoren übergeben
  digitalWrite(IN2_1, richtung_links);

// Motor rechts
  analogWrite(EN2_1, speed_rechts);
  digitalWrite(IN3_1, !richtung_rechts);       // Motorwerte aus Motordaten() an Motoren übergeben
  digitalWrite(IN4_1, richtung_rechts);

// Motor unten
  analogWrite(EN1_2, speed_unten);
  digitalWrite(IN1_2, !richtung_unten);        // Motorwerte aus Motordaten() an Motoren übergeben
  digitalWrite(IN2_2, richtung_unten);
}



IMU()

 

Zunächst haben wir nach einem fertigen Arduino Code für die IMU gesucht. Diesen haben wir auf dem Arduino Playground (http://playground.arduino.cc/Main/MPU-6050) gefunden. Dieser Code initialisiert alle nötigen Register, so dass die Daten ausgegeben werden können.

Die Aufgabe bestand nun darin, die ausgelesenen Roh-Werte in Werte mit Einheit umzurechnen.

Laut Datenblatt müssen die Werte durch 131.0 geteilt werden. Die Einheit beträgt Grad pro Sekunde.

Um nur Grad (°) als Ausgabewert zu erhalten, integrieren wir nun diesen Wert über der Zeit und erhalten den Winkel um die jeweilige Achse (accel_t_gyro.value.(x/y/z)_gyro). Wir haben im loop-Teil des Arduino Codes einen Zählervariable imu_delay eingeführt um den IMU-Code nur nach einem vorgegebenen Zeitintervall ausführen zu lassen. Dieses Zeitintervall legten wir auf 100ms fest (Zählervariable bis 1000 laufen lassen), was noch akzeptabel für die Integration ist bei der Rechenleistung des Arduino. Es werden die Werte, die bei dieser Berechnung herauskommen mit 100ms = 0,1s multipliziert und in der Variable angle aufsummiert, was einer ungenauen Integration entspricht (Rechtecke unter Kurve):

// Winkel Berechnung im Loop

angle = angle + ((accel_t_gyro.value.x_gyro/131.0)*0.1);

Der nächste Schritt ist, den natürlichen Drift des Gyros zu berechnen und diesen auszugleichen. Der Drift beträgt ungefähr -0,1313° pro 100ms - Loop. Die Berechnung des Drifts geschah mit folgendem Code:

float y[20];
float mittelsum = 0;
float arithm_mittel;
int m = 1;
//Mittelwerts Berechnung im Loop
y[m] = angle;
mittelsum = mittelsum + (y[m] - y[m-1]);
if(m == 19)
{
  arithm_mittel = mittelsum/19.0;
  Serial.println(arithm_mittel,DEC); 
  m = 0;
}
m++;

Die -0.1313° werden pro Loop vom eigentlichen winkel abgezogen:

 angle = angle + 0.1313;

Die Werte sind nun stabil und werden auf eine ganze Zahl gerundet.

 if(angle > 0) rundung = (int) (angle + 0.5);

 if(angle < 0) rundung = (int) (angle - 0.5);

Zur Verwertung der Daten wird ein Wertebereich von -180;180 benötigt. Dies wird mit einer einfachen while Schleife erreicht.

while(rundung >=180)

    {

      rundung = rundung - 360;

    }

 while(rundung <=180)

    {

      rundung = rundung + 360;

    }

 

Die Werte werden alle 200ms übermittelt.

 

Anhänge:
Diese Datei herunterladen (Code-Pack.rar)Code-Pack[Enthält die oben aufgelisteten Codes]%2013-%07-%16 %1:%Jul%+02:00