Reaction Wheel Actuated Satellite Dynamics Test Platform
Diploma Thesis in Aerospace Engineering, January 2014
University of Applied Sciences Munich, Faculty 03
Electronics:
- 1x mbed NXP LPC 1768 Microcontroller
- 2x XBee S1 Radios + Sparkfun USB Adapter
- 1x CHR UM6-lt IMU
- 4x Graupner BEC 8 Motor Controllers
- 4x ROXXY 2826/09 Brushless Motors
- 1x Hacker TopFuel LiPo 1300mAh Battery
- 1x big Selfmade BreakOutBoard to connect all components
- 1x small BreakOutBoard to connect IMU
Hardware developed with Catia V5R20
Manufactoring Technology: Rapid Prototyping - EOS Formiga P110
Controlled via text based menu with DockLight
__________________
main.cpp
- Committer:
- DimitriGruebel
- Date:
- 2014-07-09
- Revision:
- 0:1447d2f773db
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"); }