Dimitri Gruebel
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
__________________
Diff: main.cpp
- Revision:
- 0:1447d2f773db
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Wed Jul 09 07:35:50 2014 +0000
@@ -0,0 +1,661 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////////////////////////
+// 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");
+}
\ No newline at end of file
Repository toolbox
| Export to desktop IDE |
| Build repository |
Repository details
| Type: | Program |
|---|---|
| Mbed OS support: | Mbed 2 deprecated |
| Created: | 09 Jul 2014 |
| Imports: | 7 |
| Forks: | 0 |
| Commits: | 1 |
| Dependents: | 0 |
| Dependencies: | 1 |
| Followers: | 4 |
