Diploma Thesis in Aerospace Engineering, January 2014

File content as of revision 0:1447d2f773db:

///////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////
// Programm zu Lageregelung der Testplattform mit Hilfe von Reaktionskreiseln.
// Mit UM6-LT IMU als Trägheitsplattform, XBee-Funkmodul zum seriellen Datenübertraggung und mbed
// Mikrokontroller als Regelungs- und Stuerungscomputer.
// Im Programm sind ein PID- und ein PD-Regler implementiert.                                       
//
// Datum: 10.01.2014  Autor: Grübel Dimitri
// 
///////////////////////////////////////////////////////////////////////////////////////////////////


#include "mbed.h"                                // MBED HEADER
#include "MODSERIAL.h"                           // MBED BUFFERED SERIAL HEADER
#include "UM6_usart.h"                           // UM6 USART HEADER
#include "UM6_config.h"                          // UM6 CONFIG HEADER

                                                 // KOMMUNIKATIONSART MIT MBED: USB/XBEE
Serial ios(p28, p27);                            // Serielle Verbi. mit XBee über Pin: tx-28, rx-27
//Serial ios(USBTX, USBRX);                      // Serielle Verbi. über USB Port vom PC

DigitalOut rst(p11);                             // Digital Reset für the XBee, 200ns für reset
PwmOut x_kreisel(p21);                           // Pin21-PwmOut ist für Drehung um X-Achse
PwmOut y_kreisel(p23);                           // Pin23-PwmOut ist für Drehung um Y-Achse
PwmOut z_kreisel(p22);                           // Pin22-PwmOut ist für Drehung um Z-Achse

                                                 // HIER WIRD PWM EINGESTELLT:
const float pulsweite = 10.0;                    // Pulsweite des Steuersignals in ms
const float pwwork = 2.0;                        // Pulsweite des Arbeitssignals in ms
const float startpw = 0.8;                       // Startpulsweite in ms
const float pwmfakt = (pwwork - startpw)/200.0;  // Pulsweite pro 1% Leistung
const float max_leistung = 75.0;                 // Leistungsbegrenzung global in %
const float min_leistung = 10.0;                 // Notwendige Mindestleistung des Motors in %

const float x_kp_min = 0.0;                      // min/max - Variablen kp für X/Y/Z-Achse
const float x_kp_max = 3.0;
const float y_kp_min = 0.0;
const float y_kp_max = 3.0;
const float z_kp_min = 0.0;
const float z_kp_max = 3.0;

const float x_kd_min = 0.0;                      // min/max - Variablen kd für X/Y/Z-Achse
const float x_kd_max = 3.0;
const float y_kd_min = 0.0;
const float y_kd_max = 3.0;
const float z_kd_min = 0.0;
const float z_kd_max = 3.0;

const float x_ki_min = 0.0;                      // min/max - Variablen ki für X/Y/Z-Achse
const float x_ki_max = 0.5;
const float y_ki_min = 0.0;
const float y_ki_max = 0.5;
const float z_ki_min = 0.0;
const float z_ki_max = 0.5;

const float x_winkel_min = -45.0;                // Winkelbegrenzung für X/Y/Z-Achse
const float x_winkel_max =  45.0;
const float y_winkel_min = -45.0;
const float y_winkel_max =  45.0;
const float z_winkel_min = -90.0;
const float z_winkel_max =  90.0;

const float regelgenauigkeit = 1.5;              // Regelgenauigkeit in Grad
                                                 
const float x_kp_def = 1.0;                      // DEFAULT REGELPARAMETER FÜR X/Y/Z-ACHSE
const float x_kd_def = 0.4;
const float x_ki_def = 0.01;
const float y_kp_def = 1.0; 
const float y_kd_def = 0.4;
const float y_ki_def = 0.01;
const float z_kp_def = 1.0; 
const float z_kd_def = 0.4;
const float z_ki_def = 0.01;

DigitalOut uart_activity(LED1);                  // LED1 = UM6 SERIAL für Kommunikation
       
                                                                   
/// FUNKTIONEN ////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////


///////////////////////////////////////////////////////////////////////////////////////////////////
// rxCallback // FUNKTION FÜR INTERRUPT - ABFRAGE DER DATEN DIE UM6-LT SENDET

void rxCallback(MODSERIAL_IRQ_INFO *q) 
{
    if (um6_uart.rxBufferGetCount() >=  MAX_PACKET_DATA) 
    {
        uart_activity = !uart_activity;          // LED leuchtet wenn RxBuff hat > 40 Bytes
        Process_um6_packet();
    }
}


///////////////////////////////////////////////////////////////////////////////////////////////////
// calcStellwert // FUNKTION ZUR STELLWERTBERECHNUNG FÜR MBED-PWM

float calcStellwert(float leistung) 
{                                
    return (startpw + ((leistung + 100) * pwmfakt)) / pulsweite;
}


///////////////////////////////////////////////////////////////////////////////////////////////////
// print_gyro_data // FUNKTION ZUR AUSGABE DER AKTUELLEN WINKEL UND WINKELGESCHWINDIGKEITEN

void print_gyro_data()
{                                              
    ios.printf("Gyro_Proc_X     %+6.1f deg/s\n", data.Gyro_Proc_X);
    ios.printf("Gyro_Proc_Y     %+6.1f deg/s\n", data.Gyro_Proc_Y);
    ios.printf("Gyro_Proc_Z     %+6.1f deg/s\n", data.Gyro_Proc_Z);
    ios.printf("Roll            %+6.1f deg\n", data.Roll);
    ios.printf("Pitch           %+6.1f deg\n", data.Pitch);
    ios.printf("Yaw             %+6.1f deg\n\n", data.Yaw);
}


///////////////////////////////////////////////////////////////////////////////////////////////////
// print_gyro_angles // FUNKTION ZUR AUSGABE DER AKTUELLEN WINKEL

void print_gyro_angles()
{                                            
    ios.printf("Roll            %+6.1f deg\n", data.Roll);
    ios.printf("Pitch           %+6.1f deg\n", data.Pitch);
    ios.printf("Yaw             %+6.1f deg\n\n", data.Yaw);
}


///////////////////////////////////////////////////////////////////////////////////////////////////
// LageregelungAchse // FUNKTION ZUR LAGEREGELUNG FÜR EINE ACHSE - STATISCH - PD Regler

void LageregelungAchse(float kp, float kd, float winkel, const float& gyro_veloc, 
                       const float& gyro_angle, PwmOut& kreisel)
{                   
    float start_angle = gyro_angle;
    float winkel_soll = gyro_angle + winkel;                                                      
    float abweichung = gyro_angle - winkel_soll;
    float leistung_alt = 0;                           // Variablen für Bremsverfahren
    bool start = true;
    
    while (fabs(abweichung) > regelgenauigkeit) 
    {
        float leistung = abweichung * kp - gyro_veloc * kd;
        if (leistung > max_leistung) leistung = max_leistung;
        if (leistung < -max_leistung) leistung = -max_leistung;
        if (leistung > -min_leistung && leistung < -0.001) leistung = -min_leistung;
        if (leistung < min_leistung && leistung > 0.001) leistung = min_leistung;
         
        int Bremsrichtung = 0;                        // Vorzeichenbestimmung der Abbremsrichtung
         
        if (leistung >= 0)
            Bremsrichtung = -1;
        else 
            Bremsrichtung = 1;
        
        float Stellwert = calcStellwert(leistung);    // Berechnung Stellwert
        kreisel.write(Stellwert);                     // Befehl an Motorregler über PWM
        wait_ms(20);
        abweichung = gyro_angle - winkel_soll;        // Kontrolle der Regelgenauigkeit
        
        if (start == true) 
        {                                             
            leistung_alt = leistung;
            start = false;
        }      
        else 
        {
            if (fabs(winkel) > 5.0) 
            {                                     
                if ( fabs(leistung) > min_leistung) 
                { 
                    float delta_leistung = fabs(leistung) - fabs(leistung_alt);     // Bestimmung Abfall Delta
                    if (delta_leistung < -0.001) 
                    {                                 // Abbremsung des Motors bei Leistungsabnahme
                        kreisel.write(calcStellwert(Bremsrichtung * min_leistung)); 
                        wait_ms(10);
                    }
                }
            }
            
            leistung_alt = leistung;
        }                                                       // Ende des Bremsverfahrens
        
        if (fabs(abweichung) <= regelgenauigkeit) 
        {             // Vorzeichen für Bremsung wird bestimmt
            print_gyro_data();                                  // Funktionsaufruf Gyrodaten ausgeben 
            kreisel.write(calcStellwert(Bremsrichtung * 100));  // Vollbremsung
            wait_ms(650); 
            kreisel.write(calcStellwert(0));                    // Motor aus
        }
    }
       
    float end_angle = gyro_angle;        
    ios.printf("Plattform wurde gedreht um %+6.1f deg\n\n", end_angle - start_angle);
}


///////////////////////////////////////////////////////////////////////////////////////////////////
// LageregelungAchseDyn // FUNKTION ZUR LAGEREGELUNG FÜR EINE ACHSE - DYNAMISCH - PID Regler

void LageregelungAchseDyn(float kp, float kd, float ki, float winkel, const float& gyro_veloc, 
                          const float& gyro_angle, PwmOut& kreisel, float& dyn_leistung_leerlauf)
{                   
    
    ios.printf("Start LageregelungAchseDyn\n");
    float start_angle = gyro_angle;
    float winkel_soll = gyro_angle + winkel;
    float abweichung = gyro_angle - winkel_soll;
    float abweichung_sum = abweichung;
    float leistung_alt = dyn_leistung_leerlauf;
    bool  start = true;
    float leistung = 0.0;
    float i_glied = 0.0;
    
    int cnt = 0; 
    while (fabs(abweichung) > regelgenauigkeit) 
    {
        cnt += 1;
        i_glied = abweichung_sum * 0.001 * ki;
        ios.printf("%f\n", i_glied);
        if (i_glied > 0.5) i_glied = 0.5;                       // I-Glied Limiter 
        if (i_glied < -0.5) i_glied = -0.5;
        // Sumierstelle (PID Glieder)
        leistung = dyn_leistung_leerlauf + abweichung * kp - gyro_veloc * kd + i_glied;            
        if (leistung > max_leistung) leistung = max_leistung;
        if (leistung < min_leistung) leistung = min_leistung;
        int Bremsrichtung = -1;                                 // Vorzeichen der Abbremsrichtung 

        float Stellwert = calcStellwert(leistung);              // Berechnung Stellwert
        kreisel.write(Stellwert);                               // Befehl an Motorregler über Pwm
        wait_ms(10);       
    
        if (start == true) 
        {                                   
            leistung_alt = leistung;
            start = false;
        }     
        /*else 
        {                       
            if (leistung > min_leistung) 
            { 
                float delta_leistung = leistung - leistung_alt; // Bestimmung Abfall Delta
                
                // bei Abnahme der Leistung - Motor abbremsen
                if (delta_leistung < -0.0001) 
                {                 
                    kreisel.write(calcStellwert(Bremsrichtung * min_leistung)); 
                    wait_ms(5);
                }
            }
        }
        */
        leistung_alt = leistung;
        abweichung = gyro_angle - winkel_soll;
        abweichung_sum += abweichung;
    }
    
    dyn_leistung_leerlauf = leistung - i_glied;
    if (dyn_leistung_leerlauf < min_leistung) dyn_leistung_leerlauf = min_leistung;
    if (dyn_leistung_leerlauf > max_leistung) dyn_leistung_leerlauf = max_leistung;
    ios.printf("Leerlaufleistung: %f\n", dyn_leistung_leerlauf);  
    float end_angle = gyro_angle;      
    ios.printf("Plattform wurde gedreht um %+6.1f deg\n\n", end_angle - start_angle);
    // Counter für durchlaufene Regelzyklen             
    ios.printf("\nEnde LageregelungAchseDyn; Counter: %i\n\n", cnt);                               
}


///////////////////////////////////////////////////////////////////////////////////////////////////
// main // Hauptprogramm

int main() 
{                                                          
    ios.baud(115200);                                                // Baudrate XBee Funkmodul
    um6_uart.baud(115200);                                           // Baudrate UM6-lt 
    um6_uart.attach(&rxCallback, MODSERIAL::RxIrq);                  // Interrupt Funktion für UART

    rst = 1;                                                         // Reset-Pin von XBee auf ON
    
    ios.printf("\nBitte warten, Startvorgang der Plattform...\n\n"); // Start-UP Prozedur 
    um6_uart.putc(0xAC);                                             // Nulliert die Rate-Gyros
    wait_ms(3500);
    um6_uart.putc(0xAC);                                             // Nulliert die Rate-Gyros
    wait_ms(3500);
    um6_uart.putc(0xAC);                                             // Nulliert die Rate-Gyros
    wait_ms(3500);

    x_kreisel.period_ms(pulsweite);                                 
    y_kreisel.period_ms(pulsweite);
    z_kreisel.period_ms(pulsweite);
    
    ios.printf("\n\nTESTPLATTFORM ZUR SATELLITENLAGEREGELUNG MIT REAKTIONSKREISELN:\n\n");
         
    int choice1 = 0;
    
    do 
    {
        do 
        {
            ios.printf("\n\nBitte folgende Anweisungen ausfuehren,falls nicht bereits erfolgt!\n"
               "Stromversorgung der Testplattform trennen und ohne Druckluft mit X-Achse auf mag."
               " Norden ausrichten.\n"
               "Testplattform mit Wasserwaage horizontal tarieren.\n"
               "Danach Stromversorgung herstellen und mbed Reset-Taste betaetigen.\n\n");
            
            x_kreisel.write(calcStellwert(0));                       //Initialisierung X-Motorrelger 
            y_kreisel.write(calcStellwert(0));                       //Initialisierung Y-Motorrelger
            z_kreisel.write(calcStellwert(0));                       //Initialisierung Z-Motorrelger
            
            ios.printf("Menueauswahl:\n");                          
            ios.printf("\t1 - Testplattform um einzelne Achsen drehen\n");
            ios.printf("\t2 - Testplattform um drei Achsen drehen\n");
            ios.printf("\t3 - Lage der Testplattform aktiv regeln fuer dynamische Fehler\n");
            ios.printf("\t4 - Gyrodaten ausgeben [Winkel/Winkelgeschwindigkeiten]\n");
            ios.printf("\t5 - Programm beenden\n");
            ios.printf("\nEingabe >");
            ios.scanf("%i", &choice1);
        }while (choice1 < 1 || choice1 > 5);
        
        if (choice1 == 1)
        { 
            ios.printf("\nTestplattform ist initialisiert.\n\n");
            print_gyro_angles();                                     // Funktionsaufruf Gyrodwinkel ausgeben     
            float x_kp = x_kp_def;
            float x_kd = x_kd_def;
            float x_winkel = 0.0;
            float y_kp = y_kp_def; 
            float y_kd = y_kd_def;
            float y_winkel = 0.0;
            float z_kp = z_kp_def; 
            float z_kd = z_kd_def;
            float z_winkel = 0.0;

            int choice2 = 0;
            
            do 
            {
                do 
                {
                    ios.printf("\n\nLAGEREGELUNG EINZELNER ACHSEN:\n\n");                         
                    ios.printf("Menueauswahl:\n");
                    ios.printf("\t1 - Satellitenplattform rollen (um X-Achse drehen)\n");
                    ios.printf("\t2 - Satellitenplattform nicken (um Y-Achse drehen)\n");
                    ios.printf("\t3 - Satellitenplattform gieren (um Z-Achse drehen)\n");
                    ios.printf("\t4 - Auswahl abbrechen (esc).\n");
                    ios.printf("\nEingabe >");
                    ios.scanf("%i", &choice2); 
                }while (choice2 < 1 || choice2 > 4);

                if (choice2 == 1)
                {
                    ios.printf("\n Reglerparameter kp eingeben [0 - +3] >");
                    ios.scanf("%f", &x_kp);          
                    if (x_kp > x_kp_max) x_kp = x_kp_max;                 // Begrenzung X-Achse kp Pos.
                    if (x_kp < x_kp_min) x_kp = x_kp_min;                 // Begrenzung X-Achse kp Neg.
                    ios.printf("\n Reglerparameter kd eingeben [0 - +3] >");
                    ios.scanf("%f", &x_kd); 
                    if (x_kd > x_kd_max) x_kd = x_kd_max;                 // Begrenzung X-Achse kd Pos.
                    if (x_kd < x_kd_min) x_kd = x_kd_min;                 // Begrenzung X-Achse kd Neg.      
                    ios.printf("\n Rollwinkel Phi eingeben [-45 - +45] >");
                    ios.scanf("%f", &x_winkel);
                    if (x_winkel > x_winkel_max) x_winkel = x_winkel_max; // Begrenzung X-Achse Winkel Pos.
                    if (x_winkel < x_winkel_min) x_winkel = x_winkel_min; // Begrenzung X-Achse Winkel Neg.   
                    if (fabs(x_winkel) < regelgenauigkeit) ios.printf("\nRegelung ist nicht notwendig!");
                    ios.printf("\nParameter kp = %f, Parameter kd = %f, Rollwinkel = %f\n", 
                               x_kp, x_kd, x_winkel); 
                    print_gyro_angles();
                    LageregelungAchse(x_kp, x_kd, x_winkel, data.Gyro_Proc_X, data.Roll, x_kreisel);
                }
                else if (choice2 == 2) 
                {
                    ios.printf("\n Reglerparameter kp eingeben [0 - +3] >");
                    ios.scanf("%f", &y_kp);
                    if (y_kp > y_kp_max) y_kp = y_kp_max;                 // Begrenzung Y-Achse kp Pos.
                    if (y_kp < y_kp_min) y_kp = y_kp_min;                 // Begrenzung Y-Achse kp Neg.
                    ios.printf("\n Reglerparameter kd eingeben [0 - +3] >");
                    ios.scanf("%f", &y_kd);
                    if (y_kd > y_kd_max) y_kd = y_kd_max;                 // Begrenzung Y-Achse kd Pos.
                    if (y_kd < y_kd_min) y_kd = y_kd_min;                 // Begrenzung Y-Achse kd Neg.
                    ios.printf("\n Nickwinkel Theta eingeben [-45 - +45] >");
                    ios.scanf("%f", &y_winkel);                             
                    if (y_winkel > y_winkel_max) y_winkel = y_winkel_max; // Begrenzung Y-Achse Winkel Pos.
                    if (y_winkel < y_winkel_min) y_winkel = y_winkel_min; // Begrenzung Y-Achse Winkel Neg.   
                    if (fabs(y_winkel) < regelgenauigkeit) ios.printf("\nRegelung ist nicht notwendig!");
                    ios.printf("\nParameter kp = %f, Parameter kd = %f, Nickwinkel = %f\n", 
                                y_kp, y_kd, y_winkel); 
                    print_gyro_angles();                                                  
                    LageregelungAchse(y_kp, y_kd, y_winkel, data.Gyro_Proc_Y, data.Pitch, y_kreisel);
                                     
                 } 
                 else if (choice2 == 3) 
                 {                    
                    ios.printf("\n Reglerparameter kp eingeben [0 - +3] >");
                    ios.scanf("%f", &z_kp);
                    if (z_kp > z_kp_max) z_kp = z_kp_max;                 // Begrenzung Z-Achse kp Pos.
                    if (z_kp < z_kp_min) z_kp = z_kp_min;                 // Begrenzung Z-Achse kp Neg.
                    ios.printf("\n Reglerparameter kd eingeben [0 - +3] >");
                    ios.scanf("%f", &z_kd);
                    if (z_kd > z_kd_max) z_kd = z_kd_max;                 // Begrenzung Z-Achse kd Pos.
                    if (z_kd < z_kd_min) z_kd = z_kd_min;                 // Begrenzung Z-Achse kd Neg.   
                    ios.printf("\n Gierwinkel Psi eingeben [-90 - +90] >");
                    ios.scanf("%f", &z_winkel);      
                    if (z_winkel > z_winkel_max) z_winkel = z_winkel_max; // Begrenzung Z-Achse Winkel Pos.
                    if (z_winkel < z_winkel_min) z_winkel = z_winkel_min; // Begrenzung Z-Achse Winkel Neg.   
                    if (fabs(z_winkel) < regelgenauigkeit) ios.printf("\nRegelung ist nicht notwendig!");
                    ios.printf("\nParameter kp = %f, Parameter kd = %f, Gierwinkel = %f\n", 
                                z_kp, z_kd, z_winkel); 
                    print_gyro_angles();
                    LageregelungAchse(z_kp, z_kd, z_winkel, data.Gyro_Proc_Z, data.Yaw, z_kreisel);
                }
            }while(choice2 != 4);
            
        }
        else if (choice1 == 2)
        {
            float x_kp = x_kp_def;
            float x_kd = x_kd_def;
            float x_winkel = 0.0;
            float y_kp = y_kp_def; 
            float y_kd = y_kd_def;
            float y_winkel = 0.0;
            float z_kp = z_kp_def; 
            float z_kd = z_kd_def;
            float z_winkel = 0.0;
       
            int choice3 = 0;
            
            do 
            {
                do 
                {
                    ios.printf("\n\nLAGEREGELUNG ALLER ACHSEN:\n\n");
                    ios.printf("Bitte Plattform in gewuenschte Ausgangsposition bringen.\n");
                    ios.printf("Plattform wird in Z-Y-X Reihenfolge gedreht!\n\n");
                    ios.printf("Aktuelle Werte fuer Regler X-Achse: kp = %f kd = %f\n\n", x_kp, x_kd);
                    ios.printf("Aktuelle Werte fuer Regler Y-Achse: kp = %f kd = %f\n\n", y_kp, y_kd);
                    ios.printf("Aktuelle Werte fuer Regler Z-Achse: kp = %f kd = %f\n\n", z_kp, z_kd);
                    ios.printf("Menueauswahl:\n");
                    ios.printf("\t1 - Regelparameter aendern?\n");
                    ios.printf("\t2 - Plattform drehen-Winkel eingeben\n");
                    ios.printf("\t3 - Auswahl abbrechen (esc).\n");
                    ios.printf("\nEingabe >");
                    ios.scanf("%i", &choice3); 
                }while (choice3 < 1 || choice3 > 3);
                      
                if (choice3 == 1)
                {
                    ios.printf("\n Reglerparameter x_kp eingeben [0 - +3] >");
                    ios.scanf("%f", &x_kp);          
                    if (x_kp > x_kp_max) x_kp = x_kp_max;                 // Begrenzung X-Achse kp Pos.
                    if (x_kp < x_kp_min) x_kp = x_kp_min;                 // Begrenzung X-Achse kp Neg.
                    ios.printf("\n Reglerparameter x_kd eingeben [0 - +3] >");
                    ios.scanf("%f", &x_kd); 
                    if (x_kd > x_kd_max) x_kd = x_kd_max;                 // Begrenzung X-Achse kd Pos.
                    if (x_kd < x_kd_min) x_kd = x_kd_min;                 // Begrenzung X-Achse kd Neg.
                    ios.printf("\n Reglerparameter y_kp eingeben [0 - +3] >");
                    ios.scanf("%f", &y_kp);
                    if (y_kp > y_kp_max) y_kp = y_kp_max;                 // Begrenzung Y-Achse kp Pos.
                    if (y_kp < y_kp_min) y_kp = y_kp_min;                 // Begrenzung Y-Achse kp Neg.
                    ios.printf("\n Reglerparameter y_kd eingeben [0 - +3] >");
                    ios.scanf("%f", &y_kd);
                    if (y_kd > y_kd_max) y_kd = y_kd_max;                 // Begrenzung Y-Achse kd Pos.
                    if (y_kd < y_kd_min) y_kd = y_kd_min;                 // Begrenzung Y-Achse kd Neg.
                    ios.printf("\n Reglerparameter z_kp eingeben [0 - +3] >");
                    ios.scanf("%f", &z_kp);
                    if (z_kp > z_kp_max) z_kp = z_kp_max;                 // Begrenzung Z-Achse kp Pos.
                    if (z_kp < z_kp_min) z_kp = z_kp_min;                 // Begrenzung Z-Achse kp Neg.
                    ios.printf("\n Reglerparameter z_kd eingeben [0 - +3] >");
                    ios.scanf("%f", &z_kd);
                    if (z_kd > z_kd_max) z_kd = z_kd_max;                 // Begrenzung Z-Achse kd Pos.
                    if (z_kd < z_kd_min) z_kd = z_kd_min;                 // Begrenzung Z-Achse kd Neg. 
                    ios.printf("\n\n"); 
                    ios.printf("Aktuelle Werte fuer Regler X-Achse: kp = %f kd = %f\n\n", x_kp, x_kd);
                    ios.printf("Aktuelle Werte fuer Regler Y-Achse: kp = %f kd = %f\n\n", y_kp, y_kd);
                    ios.printf("Aktuelle Werte fuer Regler Z-Achse: kp = %f kd = %f\n\n", z_kp, z_kd);
                }
                else if (choice3 == 2)
                {                                                         // Eingabe der Drehwinkel
                    ios.printf("\n Gierwinkel Psi eingeben [-90 - +90] >");
                    ios.scanf("%f", &z_winkel);      
                    if (z_winkel > z_winkel_max) z_winkel = z_winkel_max; // Begrenzung Z-Achse Winkel Pos.
                    if (z_winkel < z_winkel_min) z_winkel = z_winkel_min; // Begrenzung Z-Achse Winkel Neg.
                    ios.printf("\n Nickwinkel Theta eingeben [-45 - +45] >");
                    ios.scanf("%f", &y_winkel);                             
                    if (y_winkel > y_winkel_max) y_winkel = y_winkel_max; // Begrenzung Y-Achse Winkel Pos.
                    if (y_winkel < y_winkel_min) y_winkel = y_winkel_min; // Begrenzung Y-Achse Winkel Neg.
                    ios.printf("\n Rollwinkel Phi eingeben [-45 - +45] >");
                    ios.scanf("%f", &x_winkel);
                    if (x_winkel > x_winkel_max) x_winkel = x_winkel_max; // Begrenzung X-Achse Winkel Pos.
                    if (x_winkel < x_winkel_min) x_winkel = x_winkel_min; // Begrenzung X-Achse Winkel Neg.
                        
                    if (fabs(z_winkel) < regelgenauigkeit && 
                        fabs(y_winkel) < regelgenauigkeit && 
                        fabs(x_winkel) < regelgenauigkeit)
                    {
                        ios.printf("\nRegelung ist nicht notwendig!");
                    }
                    else 
                    {
                        print_gyro_angles();
                        LageregelungAchse(z_kp, z_kd, z_winkel, data.Gyro_Proc_Z, data.Yaw, z_kreisel);
                        wait_ms(250);   
                        LageregelungAchse(y_kp, y_kd, y_winkel, data.Gyro_Proc_Y, data.Pitch, y_kreisel);
                        wait_ms(250);
                        LageregelungAchse(x_kp, x_kd, x_winkel, data.Gyro_Proc_X, data.Roll, x_kreisel);
                        print_gyro_angles();
                        ios.printf("\n\nSoll die Plattform wieder in ihre Ausgangslage gefahren werden? "
                                   "ja/nein\n");
                        char yes_no[50]; 
                        ios.scanf("%s", yes_no);
                        
                        if (strcmp(yes_no, "ja") == 0)    // Plattform in auf Ausgangsposition drehen
                        {                       
                            LageregelungAchse(x_kp, x_kd, -x_winkel, data.Gyro_Proc_X, data.Roll, x_kreisel);
                            wait_ms(250);  
                            LageregelungAchse(y_kp, y_kd, -y_winkel, data.Gyro_Proc_Y, data.Pitch, y_kreisel);
                            wait_ms(250);
                            LageregelungAchse(z_kp, z_kd, -z_winkel, data.Gyro_Proc_Z, data.Yaw, z_kreisel);
                            print_gyro_angles();
                        }
                    }
                }  
                            
            }while(choice3 != 3);
        }
        else if (choice1 == 3)
        {
            float x_kp = x_kp_def;
            float x_kd = x_kd_def;
            float x_ki = x_ki_def;
            float y_kp = y_kp_def; 
            float y_kd = y_kd_def;
            float y_ki = y_ki_def;
            float z_kp = z_kp_def; 
            float z_kd = z_kd_def;
            float z_ki = z_ki_def;
       
            int choice4 = 0;
            
            do 
            {
                do 
                {
                    ios.printf("\n\nLage der Testplattform aktiv regeln fuer dynamische Fehler"
                               " (Position halten):\n\n");
                    ios.printf("\n\nBitte Plattform in gewuenschte Ausgangsposition bringen und halten "
                               "bis die Kreisel "
                               "ihre Leerlaufdrehzahl erreicht haben.\n\n");
                    ios.printf("Aktuelle Werte fuer Regler X-Achse: kp = %f kd = %f ki = %f\n", 
                                x_kp, x_kd, x_ki); 
                    ios.printf("Aktuelle Werte fuer Regler Y-Achse: kp = %f kd = %f ki = %f\n", 
                                y_kp, y_kd, y_ki);
                    ios.printf("Aktuelle Werte fuer Regler Z-Achse: kp = %f kd = %f ki = %f\n\n", 
                                z_kp, z_kd, z_ki);
                    ios.printf("Menueauswahl:\n");
                    ios.printf("\t1 - Regelparameter aendern?\n");
                    ios.printf("\t2 - Plattform aktivieren\n");
                    ios.printf("\t3 - Auswahl abbrechen (esc).\n");
                    ios.printf("\nEingabe >");
                    ios.scanf("%i", &choice4); 
                }while (choice4 < 1 || choice4 > 3);  
                       
                if (choice4 == 1)
                {
                    /*
                    ios.printf("\n Reglerparameter x_kp eingeben [0 - +3] >");
                    ios.scanf("%f", &x_kp);          
                    if (x_kp > x_kp_max) x_kp = x_kp_max;                 // Begrenzung X-Achse kp Pos.
                    if (x_kp < x_kp_min) x_kp = x_kp_min;                 // Begrenzung X-Achse kp Neg.
                    ios.printf("\n Reglerparameter x_kd eingeben [0 - +3] >");
                    ios.scanf("%f", &x_kd); 
                    if (x_kd > x_kd_max) x_kd = x_kd_max;                 // Begrenzung X-Achse kd Pos.
                    if (x_kd < x_kd_min) x_kd = x_kd_min;                 // Begrenzung X-Achse kd Neg.
                    ios.printf("\n Reglerparameter x_ki eingeben [0 - +0.5] >");
                    ios.scanf("%f", &x_ki); 
                    if (x_ki > x_ki_max) x_ki = x_ki_max;                 // Begrenzung X-Achse ki Pos.
                    if (x_ki < x_ki_min) x_ki = x_ki_min;                 // Begrenzung X-Achse ki Neg.
                    ios.printf("\n Reglerparameter y_kp eingeben [0 - +3] >");
                    ios.scanf("%f", &y_kp);
                    if (y_kp > y_kp_max) y_kp = y_kp_max;                 // Begrenzung Y-Achse kp Pos.
                    if (y_kp < y_kp_min) y_kp = y_kp_min;                 // Begrenzung Y-Achse kp Neg.
                    ios.printf("\n Reglerparameter y_kd eingeben [0 - +3] >");
                    ios.scanf("%f", &y_kd);
                    if (y_kd > y_kd_max) y_kd = y_kd_max;                 // Begrenzung Y-Achse kd Pos.
                    if (y_kd < y_kd_min) y_kd = y_kd_min;                 // Begrenzung Y-Achse kd Neg.
                    ios.printf("\n Reglerparameter y_ki eingeben [0 - +0.5] >");
                    ios.scanf("%f", &y_ki); 
                    if (y_ki > y_ki_max) y_ki = y_ki_max;                 // Begrenzung Y-Achse ki Pos.
                    if (y_ki < y_ki_min) y_ki = y_ki_min;                 // Begrenzung Y-Achse ki Neg.
                    */  
                    ios.printf("\n Reglerparameter z_kp eingeben [0 - +3] >");
                    ios.scanf("%f", &z_kp);
                    if (z_kp > z_kp_max) z_kp = z_kp_max;                 // Begrenzung Z-Achse kp Pos.
                    if (z_kp < z_kp_min) z_kp = z_kp_min;                 // Begrenzung Z-Achse kp Neg.
                    ios.printf("\n Reglerparameter z_kd eingeben [0 - +3] >");
                    ios.scanf("%f", &z_kd);
                    if (z_kd > z_kd_max) z_kd = z_kd_max;                 // Begrenzung Z-Achse kd Pos.
                    if (z_kd < z_kd_min) z_kd = z_kd_min;                 // Begrenzung Z-Achse kd Neg. 
                    ios.printf("\n Reglerparameter z_ki eingeben [0 - +0.5] >");
                    ios.scanf("%f", &z_ki); 
                    if (z_ki > z_ki_max) z_ki = z_ki_max;                 // Begrenzung Z-Achse ki Pos.
                    if (z_ki < z_ki_min) z_ki = z_ki_min;                 // Begrenzung Z-Achse ki Neg.
                    ios.printf("\n\n"); 
                    //ios.printf("Aktuelle Werte fuer Regler X-Achse: kp = %f kd = %f ki = %f\n\n", 
                    //            x_kp, x_kd, x_ki); 
                    //ios.printf("Aktuelle Werte fuer Regler Y-Achse: kp = %f kd = %f ki = %f\n\n", 
                    //            y_kp, y_kd, y_ki);
                    ios.printf("Aktuelle Werte fuer Regler Z-Achse: kp = %f kd = %f ki = %f\n\n", 
                                z_kp, z_kd, z_ki);
                }
                else if (choice4 == 2)
                {                                       
                    ios.printf("\n\nUm abzubrechen bitte Reset-Taste an mbed betaetigen!\n\n");
                    float ziel_x_winkel = data.Roll;
                    float ziel_y_winkel = data.Pitch;
                    float ziel_z_winkel = data.Yaw;
                    
                    print_gyro_angles();                                 
                    float x_dyn_leistung_leerlauf = (min_leistung + max_leistung)/2; // Leistung im Leerlauf 
                    float y_dyn_leistung_leerlauf = (min_leistung + max_leistung)/2; // Leistung im Leerlauf 
                    float z_dyn_leistung_leerlauf = (min_leistung + max_leistung)/2; // Leistung im Leerlauf
                    
                    wait_ms(2000);
                    z_kreisel.write(calcStellwert(z_dyn_leistung_leerlauf));
                    //y_kreisel.write(calcStellwert(y_dyn_leistung_leerlauf));
                    //x_kreisel.write(calcStellwert(x_dyn_leistung_leerlauf));
            
                    while(1)
                    {
                        while (fabs(ziel_z_winkel - data.Yaw) > regelgenauigkeit)
                        {     
                            LageregelungAchseDyn(z_kp, z_kd, z_ki, ziel_z_winkel - data.Yaw, data.Gyro_Proc_Z, 
                                                 data.Yaw, z_kreisel, z_dyn_leistung_leerlauf);
                        }
                        
                        /*
                        while (fabs(ziel_y_winkel - data.Pitch) > regelgenauigkeit)
                        {  
                            LageregelungAchseDyn(y_kp, y_kd, y_ki, ziel_y_winkel - data.Roll, data.Gyro_Proc_Y, 
                                                 data.Pitch, y_kreisel, y_dyn_leistung_leerlauf);  
                        }
                        
                        while (fabs(ziel_x_winkel - data.Roll) > regelgenauigkeit)
                        {
                            LageregelungAchseDyn(x_kp, x_kd, x_ki, ziel_x_winkel - data.Pitch, data.Gyro_Proc_X, 
                                                 data.Roll, x_kreisel, x_dyn_leistung_leerlauf);
                        }
                        */
                    }
                }
            }while(choice4 != 3);
        }
        else if (choice1 == 4)
        { 
            print_gyro_data();
        }
          
    }while(choice1 != 5); 
 
    ios.printf("\n\nProgramm beendet.\n");
}