Offboard Software


1. Einleitung

2. Manuelle Steuerung

3. Wegpunkteflug


4. Regelung


4.1. Vorwärts

4.2. Drehregelung

 

Einleitung:

Das Programm auf der Basisstation übernimmt den Großteil der Arbeiten und dient als Knotenpunkt aller anderen Systeme. Hinein kommen die Positionsdaten des IPS sowie die Sensordaten von der Gondel und auch die Werte des Joysticks, heraus gehen die errechneten Werte zur Ansteuerung der Gondelpropeller und einige Parameter für die übrige Regelung auf der Gondel.

https://github.com/Daedalus-TUM/PP_mwmbt/blob/master/sketchbooks/Basisstation/Basisstation.ino

(link zur letzten Version)

 

Manuelle Steuerung:


Für die ersten Testflüge wurde ein Programm entwickelt mitdem sich der Ballon über einen PlayStation-Joystick, der an das Arduino-board angeschlossen ist, steuern lässt.

Durch eine einfache Schaltung lässt sich der Stick mit dem Arduino verbinden:

 


Im Programmcode werden dann die eingelesenen Analog- und Digitalwerte auf die Motorwerte angepasst und zur Gondel gesendet.

vertical = analogRead(VERT) -510; // will be 0-1023
  horizontal = analogRead(HORIZ) -510; // will be 0-1023
  select = digitalRead(SEL); // will be HIGH (1) if not pressed, and LOW (0) if pressed

  if(select == HIGH){
// Serial.println("not pressed");
    Motor_Z = -120;
  }else{
// Serial.println("PRESSED!");
    Motor_Z = 0;
  }
 
 if((-40 > vertical) || (vertical > 40))Motor_N = int8_t(vertical/4.2);
  else Motor_N = 0;
  
  if((-40 > horizontal) || (horizontal > 40))Motor_Rot = int8_t(horizontal/4.6);
  else Motor_Rot = 0;

 

Wegpunkteflug:

Anhand der IPS-Daten und den IMU-Werten erkennt das Programm Richtungswinkel und Position der Gondel. In einem Wegpunktevektor werden die Koordinaten der abzufliegenden Wegpunkte gespeichert. Bei jedem Aufruf der loop - Funktion wird die letzte übermittelte IPS-Position mit den Koordinaten des nächsten Wegpunktes verglichen. Zur Sicherheit wird außerdem der aktuelle Richtungswinkel mit dem Winkel, von dem erreichten Wegpunkt zu dem nächsten verglichen. Für den Fall das die Richtung und die Position sich innerhalb eines bestimmten Toleranzbereiches befinden, gilt der Wegpunkt als erreicht.

 

  //WEGPUNKTE FLUG************************************
  
    //berechnen eines Winkels zwischen zwei Wegpunkten
  WP_WP_winkel = (float) atan((double)(WP[WP_nr + 1][0]- WP[WP_nr][0])/(double)(WP[WP_nr + 1][1]- WP[WP_nr][1]));
  WP_WP_winkel = WP_WP_winkel *360/(2*3.14159);
  
    //testen ob ein Wegpunkt erreicht wurde
  if((((float)(x - WP[WP_nr][0])*(float)(x - WP[WP_nr][0]) + (float)(y - WP[WP_nr][1])*(float)(y - WP[WP_nr][1])) < 3600)
      && (abs(ist_winkel - WP_WP_winkel) < 60) ){
        
        if(WP_nr == abwurf){      //Wegpunkt ist der abwurfpunkt
          Soll_h = 40; 
        }else if(WP_nr == box){   //an dem wegpunkt befindet sich die kiste -> höhe anpassen
          Soll_h = 90;
        }else Soll_h = 130;
        
        WP_nr++;  //der Wegpunkt wird abgehakt
        Serial.print(" WPnr: ");Serial.print(WP_nr);
        Serial.print(" x: ");Serial.print(WP[WP_nr][0]);Serial.print(" y: ");Serial.println(WP[WP_nr][1]);
      }else Soll_h = 130;

 

Regelung

Vorwärts

Der Schub des Frontmotors wird davon abhängig gemacht, in welchem Winkel die Gondel zum nächsten Wegpunkt steht. Dabei gehen wir in unserem Modell davon aus, dass der Frontmotor stets in Bewegungsrichtung zeigt und das Luftschiff nicht seitlich driftet. 

Je kleiner die Winkeldifferenz, desto größer ist der Schub des Frontmotors. Wendet sich unser Luftschiff vom Wegpunkt ab, so wird auch der Schub konstant geringer. Für diese Regelung verwenden wir einen einfachen P-Regler. Ein negativer Schub wird in unserer Implementierung auf 0 gesetzt. Unsere Gondel kann also nicht rückwärts fliegen oder bremsen.

 

 

 

int8_t vorwaertsregelung(float P, float ist_winkel, float soll_winkel){
  
  int8_t N_speed = 127 - abs((winkelDiff(ist_winkel,soll_winkel)) * P);
  if(N_speed > 0) return N_speed;
  else return 0;
  
}

 

 

Drehregelung

Der Code für die Drehregelung ist ebenfalls an die Regler-Implementierung von Team 2 angelehnt. 

Für Ableitungen und Integrale werden folgende zwei Funktionen verwendet:

float derivation(float tm,float tn){
   
  float dt = 1000.0*((tm-tn)/(t_tm-t_tn));
  return dt;
}
float integral(float tm,float tn){
   
  // trapezoidal method
  float dtau = ((tm+tn)/2) * (t_tm-t_tn) * 0.001;
     
  return dtau;
}

 

Die Regelung ist folgendermaßen implementiert:

int8_t drehregelung(float Rot_p,float Rot_i,float Rot_d, float ist_winkel, float soll_winkel){
  
  // Refresh time value
  t_tn = t_tm;
  t_tm = millis();
  
  winkel_tn = winkel_tm;
  winkel_tm = ist_winkel;
  
  // calculates difference
  float diff = winkelDiff(ist_winkel,soll_winkel);
  //Serial.print("winkel differenz:");Serial.println(diff);
  
  // calculate slope
  float winkel_slope = derivation(winkel_tm, winkel_tn);
  float winkel_int = integral(winkel_tm, winkel_tn);
  
  // calculates integral
  int sum_winkel = sum_winkel + winkel_int;
  //float f = f + diff_winkel;
 
  int8_t mspeed_Rot= (Rot_p * diff + Rot_i * sum_winkel + Rot_d * winkel_slope)*.1;
  return mspeed_Rot;
}