Gondel Software

1. Einleitung

2. Kommunikation

3. IPS - Signal

4. Höhenregelung

4.1. Globale Variablen

4.2. Regelparameter

4.3. PID - Regler

 

Einleitung:

Das Programm auf der Gondel wurde nach Möglichkeit auf die notwendigsten Funktionen reduziert um komlexere Arbeiten auf die Basisstation auszulagern. Auf diesem Weg können Parameter und Funktionen abgeändert werden während die Gondel in der Luft ist.

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

(link zur letzten Version)

 

Kommunikation:

Die Kommunikation der Gondel mit der Basisstation funktioniert mit einem bereits vorhandenem Protokoll, mit dem bestimmte Datenpakete über das NRF-Modul gesendet und empfangen werden können. Das System ist bei Gondel- und Basisprogramm identisch.

Für unser Projekt wurden folgende Pakettypen hinzugefügt:

  • case 30:           Parameter für die Gondel
  • case 33:           Steuerwerte für die Gondelmotoren
  • case 100 - 103: Sensorwerte aus dem IMU-Board für die Regelung auf der Basis

 

IPS - Signal:

Alle 200ms sendet die Gondel ein 40kHz Infarot- und Ultraschallsignal um von den IPS-Stationen geortet werden zu können. Das Signal wird mit delays erzeugt, da das herkömmliche PWM-Signal die benötigte Frequenz nicht erreicht.

 

 
void ips_signal()
{
  if(micros()-time>200000)
  {
    time = micros();
    
    
    for(t=0; t<=200;)
    {
      t = micros()-time;
  
      digitalWrite(signal, HIGH);
   // delayMicroseconds(3);
   // delayMicroseconds(2);
      delayMicroseconds(1);
      delayMicroseconds(1);
      delayMicroseconds(1);
      
      digitalWrite(signal, LOW);
   // delayMicroseconds(3);
   // delayMicroseconds(2);
      delayMicroseconds(1);
      delayMicroseconds(1);
      delayMicroseconds(1);
      
    }
  }
}
 

 Höhenregelung

Globale Variablen

Initialwerte für Höhenregelung und Drehmomentausgleich: 

float P_h = 4;
float I_h = 0.04;
float D_h = 0;
height_soll = 130;
float Drehmoment = 0.05;

 

 

Regelparamater

Die Regelparameter können  über die Funkverbindung von der Basisstation aus gesetzt werden.

Funktion für Empfangen und Interpretieren der Daten von der Basisstation:

byte parseMsg() {
    byte data[12];
    Mirf.getData(data); // Daten empfangen
.
.
    switch(data[2]) {
.
.
.
    case 30: {
        P_h = data[5]/10.0; 
        I_h = data[6]/100.0;
        D_h = data[7]/100.0;    

        height_soll = data[8];
        Drehmoment = data[9]/100.0;
    }
    break;
.
.
.
    }

 


PID-Regler

Für die Implementierung des PID-Reglers benutzten wir den Code von Team2.


Ableitungsfunktion:

float derivation(float tm,float tn){

float dt = 1000.0*((tm-tn)/(t_tm-t_tn));
return dt;

 }

 

Integralfunktion:

float integral(float tm,float tn){

// trapezoidal method
float dtau = ((tm+tn)/2) * (t_tm-t_tn) * 0.001;

return dtau;
}

 

Höhenregelungsfunktion:

int hoehenregelung(float H_p,float H_i,float H_d,int height){

// Save values from previous cycle
h_tn = h_tm;
// Get current values
h_tm = height;

// Refresh time value
t_tn = t_tm;
t_tm = millis();


// calculate slope
float h_slope = derivation(h_tm, h_tn);
float h_int = integral(h_tm, h_tn);

// calculates difference
int diff = height_soll - height;

// calculates integral
int sum_h = sum_h + h_int;
float f = f + diff;

int mspeed_h= H_p * diff + H_i * sum_h + H_d * h_slope;


// PID-controller
if(mspeed_h > 0)
{
Z_direction = 0;
return mspeed_h;
}
else{
//Z_direction = 1;
//return mspeed_h*0.8;
return 0;
}

}

 

Die Regelung wird zeitdiskret berechnet. Dabei entspricht "h_tm" dem aktuellen und "h_tn" dem vorherigen Messwert. Analog
steht "t_tm" für den aktuellen und "t_tn" für den vorherigen Zeitpunkt, sodass "t_tm - t_tn" der Dauer eines Zeitschritts
entspricht.

 


Aufruf der Höhenregelung in der loop-Funktion 

void loop() { 
...
// Messen der aktuellen Höhe, alle 70 Millisekunden

if(millis() - h_time > 70){
height= (i2cGetMeasurement(byte(240))*0.3) + (height2*0.4) +(height3*0.3);
h_time = millis();
}
...

 

// Aufruf der Regelungsfunktion
Z_speed = hoehenregelung(P_h,I_h,D_h,height);
}