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@0:1447d2f773db, 2014-07-09 (annotated)
- Committer:
- DimitriGruebel
- Date:
- Wed Jul 09 07:35:50 2014 +0000
- Revision:
- 0:1447d2f773db
Dynamics Test Platform
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
DimitriGruebel | 0:1447d2f773db | 1 | /////////////////////////////////////////////////////////////////////////////////////////////////// |
DimitriGruebel | 0:1447d2f773db | 2 | /////////////////////////////////////////////////////////////////////////////////////////////////// |
DimitriGruebel | 0:1447d2f773db | 3 | // Programm zu Lageregelung der Testplattform mit Hilfe von Reaktionskreiseln. |
DimitriGruebel | 0:1447d2f773db | 4 | // Mit UM6-LT IMU als Trägheitsplattform, XBee-Funkmodul zum seriellen Datenübertraggung und mbed |
DimitriGruebel | 0:1447d2f773db | 5 | // Mikrokontroller als Regelungs- und Stuerungscomputer. |
DimitriGruebel | 0:1447d2f773db | 6 | // Im Programm sind ein PID- und ein PD-Regler implementiert. |
DimitriGruebel | 0:1447d2f773db | 7 | // |
DimitriGruebel | 0:1447d2f773db | 8 | // Datum: 10.01.2014 Autor: Grübel Dimitri |
DimitriGruebel | 0:1447d2f773db | 9 | // |
DimitriGruebel | 0:1447d2f773db | 10 | /////////////////////////////////////////////////////////////////////////////////////////////////// |
DimitriGruebel | 0:1447d2f773db | 11 | |
DimitriGruebel | 0:1447d2f773db | 12 | |
DimitriGruebel | 0:1447d2f773db | 13 | #include "mbed.h" // MBED HEADER |
DimitriGruebel | 0:1447d2f773db | 14 | #include "MODSERIAL.h" // MBED BUFFERED SERIAL HEADER |
DimitriGruebel | 0:1447d2f773db | 15 | #include "UM6_usart.h" // UM6 USART HEADER |
DimitriGruebel | 0:1447d2f773db | 16 | #include "UM6_config.h" // UM6 CONFIG HEADER |
DimitriGruebel | 0:1447d2f773db | 17 | |
DimitriGruebel | 0:1447d2f773db | 18 | // KOMMUNIKATIONSART MIT MBED: USB/XBEE |
DimitriGruebel | 0:1447d2f773db | 19 | Serial ios(p28, p27); // Serielle Verbi. mit XBee über Pin: tx-28, rx-27 |
DimitriGruebel | 0:1447d2f773db | 20 | //Serial ios(USBTX, USBRX); // Serielle Verbi. über USB Port vom PC |
DimitriGruebel | 0:1447d2f773db | 21 | |
DimitriGruebel | 0:1447d2f773db | 22 | DigitalOut rst(p11); // Digital Reset für the XBee, 200ns für reset |
DimitriGruebel | 0:1447d2f773db | 23 | PwmOut x_kreisel(p21); // Pin21-PwmOut ist für Drehung um X-Achse |
DimitriGruebel | 0:1447d2f773db | 24 | PwmOut y_kreisel(p23); // Pin23-PwmOut ist für Drehung um Y-Achse |
DimitriGruebel | 0:1447d2f773db | 25 | PwmOut z_kreisel(p22); // Pin22-PwmOut ist für Drehung um Z-Achse |
DimitriGruebel | 0:1447d2f773db | 26 | |
DimitriGruebel | 0:1447d2f773db | 27 | // HIER WIRD PWM EINGESTELLT: |
DimitriGruebel | 0:1447d2f773db | 28 | const float pulsweite = 10.0; // Pulsweite des Steuersignals in ms |
DimitriGruebel | 0:1447d2f773db | 29 | const float pwwork = 2.0; // Pulsweite des Arbeitssignals in ms |
DimitriGruebel | 0:1447d2f773db | 30 | const float startpw = 0.8; // Startpulsweite in ms |
DimitriGruebel | 0:1447d2f773db | 31 | const float pwmfakt = (pwwork - startpw)/200.0; // Pulsweite pro 1% Leistung |
DimitriGruebel | 0:1447d2f773db | 32 | const float max_leistung = 75.0; // Leistungsbegrenzung global in % |
DimitriGruebel | 0:1447d2f773db | 33 | const float min_leistung = 10.0; // Notwendige Mindestleistung des Motors in % |
DimitriGruebel | 0:1447d2f773db | 34 | |
DimitriGruebel | 0:1447d2f773db | 35 | const float x_kp_min = 0.0; // min/max - Variablen kp für X/Y/Z-Achse |
DimitriGruebel | 0:1447d2f773db | 36 | const float x_kp_max = 3.0; |
DimitriGruebel | 0:1447d2f773db | 37 | const float y_kp_min = 0.0; |
DimitriGruebel | 0:1447d2f773db | 38 | const float y_kp_max = 3.0; |
DimitriGruebel | 0:1447d2f773db | 39 | const float z_kp_min = 0.0; |
DimitriGruebel | 0:1447d2f773db | 40 | const float z_kp_max = 3.0; |
DimitriGruebel | 0:1447d2f773db | 41 | |
DimitriGruebel | 0:1447d2f773db | 42 | const float x_kd_min = 0.0; // min/max - Variablen kd für X/Y/Z-Achse |
DimitriGruebel | 0:1447d2f773db | 43 | const float x_kd_max = 3.0; |
DimitriGruebel | 0:1447d2f773db | 44 | const float y_kd_min = 0.0; |
DimitriGruebel | 0:1447d2f773db | 45 | const float y_kd_max = 3.0; |
DimitriGruebel | 0:1447d2f773db | 46 | const float z_kd_min = 0.0; |
DimitriGruebel | 0:1447d2f773db | 47 | const float z_kd_max = 3.0; |
DimitriGruebel | 0:1447d2f773db | 48 | |
DimitriGruebel | 0:1447d2f773db | 49 | const float x_ki_min = 0.0; // min/max - Variablen ki für X/Y/Z-Achse |
DimitriGruebel | 0:1447d2f773db | 50 | const float x_ki_max = 0.5; |
DimitriGruebel | 0:1447d2f773db | 51 | const float y_ki_min = 0.0; |
DimitriGruebel | 0:1447d2f773db | 52 | const float y_ki_max = 0.5; |
DimitriGruebel | 0:1447d2f773db | 53 | const float z_ki_min = 0.0; |
DimitriGruebel | 0:1447d2f773db | 54 | const float z_ki_max = 0.5; |
DimitriGruebel | 0:1447d2f773db | 55 | |
DimitriGruebel | 0:1447d2f773db | 56 | const float x_winkel_min = -45.0; // Winkelbegrenzung für X/Y/Z-Achse |
DimitriGruebel | 0:1447d2f773db | 57 | const float x_winkel_max = 45.0; |
DimitriGruebel | 0:1447d2f773db | 58 | const float y_winkel_min = -45.0; |
DimitriGruebel | 0:1447d2f773db | 59 | const float y_winkel_max = 45.0; |
DimitriGruebel | 0:1447d2f773db | 60 | const float z_winkel_min = -90.0; |
DimitriGruebel | 0:1447d2f773db | 61 | const float z_winkel_max = 90.0; |
DimitriGruebel | 0:1447d2f773db | 62 | |
DimitriGruebel | 0:1447d2f773db | 63 | const float regelgenauigkeit = 1.5; // Regelgenauigkeit in Grad |
DimitriGruebel | 0:1447d2f773db | 64 | |
DimitriGruebel | 0:1447d2f773db | 65 | const float x_kp_def = 1.0; // DEFAULT REGELPARAMETER FÜR X/Y/Z-ACHSE |
DimitriGruebel | 0:1447d2f773db | 66 | const float x_kd_def = 0.4; |
DimitriGruebel | 0:1447d2f773db | 67 | const float x_ki_def = 0.01; |
DimitriGruebel | 0:1447d2f773db | 68 | const float y_kp_def = 1.0; |
DimitriGruebel | 0:1447d2f773db | 69 | const float y_kd_def = 0.4; |
DimitriGruebel | 0:1447d2f773db | 70 | const float y_ki_def = 0.01; |
DimitriGruebel | 0:1447d2f773db | 71 | const float z_kp_def = 1.0; |
DimitriGruebel | 0:1447d2f773db | 72 | const float z_kd_def = 0.4; |
DimitriGruebel | 0:1447d2f773db | 73 | const float z_ki_def = 0.01; |
DimitriGruebel | 0:1447d2f773db | 74 | |
DimitriGruebel | 0:1447d2f773db | 75 | DigitalOut uart_activity(LED1); // LED1 = UM6 SERIAL für Kommunikation |
DimitriGruebel | 0:1447d2f773db | 76 | |
DimitriGruebel | 0:1447d2f773db | 77 | |
DimitriGruebel | 0:1447d2f773db | 78 | /// FUNKTIONEN //////////////////////////////////////////////////////////////////////////////////// |
DimitriGruebel | 0:1447d2f773db | 79 | /////////////////////////////////////////////////////////////////////////////////////////////////// |
DimitriGruebel | 0:1447d2f773db | 80 | |
DimitriGruebel | 0:1447d2f773db | 81 | |
DimitriGruebel | 0:1447d2f773db | 82 | /////////////////////////////////////////////////////////////////////////////////////////////////// |
DimitriGruebel | 0:1447d2f773db | 83 | // rxCallback // FUNKTION FÜR INTERRUPT - ABFRAGE DER DATEN DIE UM6-LT SENDET |
DimitriGruebel | 0:1447d2f773db | 84 | |
DimitriGruebel | 0:1447d2f773db | 85 | void rxCallback(MODSERIAL_IRQ_INFO *q) |
DimitriGruebel | 0:1447d2f773db | 86 | { |
DimitriGruebel | 0:1447d2f773db | 87 | if (um6_uart.rxBufferGetCount() >= MAX_PACKET_DATA) |
DimitriGruebel | 0:1447d2f773db | 88 | { |
DimitriGruebel | 0:1447d2f773db | 89 | uart_activity = !uart_activity; // LED leuchtet wenn RxBuff hat > 40 Bytes |
DimitriGruebel | 0:1447d2f773db | 90 | Process_um6_packet(); |
DimitriGruebel | 0:1447d2f773db | 91 | } |
DimitriGruebel | 0:1447d2f773db | 92 | } |
DimitriGruebel | 0:1447d2f773db | 93 | |
DimitriGruebel | 0:1447d2f773db | 94 | |
DimitriGruebel | 0:1447d2f773db | 95 | /////////////////////////////////////////////////////////////////////////////////////////////////// |
DimitriGruebel | 0:1447d2f773db | 96 | // calcStellwert // FUNKTION ZUR STELLWERTBERECHNUNG FÜR MBED-PWM |
DimitriGruebel | 0:1447d2f773db | 97 | |
DimitriGruebel | 0:1447d2f773db | 98 | float calcStellwert(float leistung) |
DimitriGruebel | 0:1447d2f773db | 99 | { |
DimitriGruebel | 0:1447d2f773db | 100 | return (startpw + ((leistung + 100) * pwmfakt)) / pulsweite; |
DimitriGruebel | 0:1447d2f773db | 101 | } |
DimitriGruebel | 0:1447d2f773db | 102 | |
DimitriGruebel | 0:1447d2f773db | 103 | |
DimitriGruebel | 0:1447d2f773db | 104 | /////////////////////////////////////////////////////////////////////////////////////////////////// |
DimitriGruebel | 0:1447d2f773db | 105 | // print_gyro_data // FUNKTION ZUR AUSGABE DER AKTUELLEN WINKEL UND WINKELGESCHWINDIGKEITEN |
DimitriGruebel | 0:1447d2f773db | 106 | |
DimitriGruebel | 0:1447d2f773db | 107 | void print_gyro_data() |
DimitriGruebel | 0:1447d2f773db | 108 | { |
DimitriGruebel | 0:1447d2f773db | 109 | ios.printf("Gyro_Proc_X %+6.1f deg/s\n", data.Gyro_Proc_X); |
DimitriGruebel | 0:1447d2f773db | 110 | ios.printf("Gyro_Proc_Y %+6.1f deg/s\n", data.Gyro_Proc_Y); |
DimitriGruebel | 0:1447d2f773db | 111 | ios.printf("Gyro_Proc_Z %+6.1f deg/s\n", data.Gyro_Proc_Z); |
DimitriGruebel | 0:1447d2f773db | 112 | ios.printf("Roll %+6.1f deg\n", data.Roll); |
DimitriGruebel | 0:1447d2f773db | 113 | ios.printf("Pitch %+6.1f deg\n", data.Pitch); |
DimitriGruebel | 0:1447d2f773db | 114 | ios.printf("Yaw %+6.1f deg\n\n", data.Yaw); |
DimitriGruebel | 0:1447d2f773db | 115 | } |
DimitriGruebel | 0:1447d2f773db | 116 | |
DimitriGruebel | 0:1447d2f773db | 117 | |
DimitriGruebel | 0:1447d2f773db | 118 | /////////////////////////////////////////////////////////////////////////////////////////////////// |
DimitriGruebel | 0:1447d2f773db | 119 | // print_gyro_angles // FUNKTION ZUR AUSGABE DER AKTUELLEN WINKEL |
DimitriGruebel | 0:1447d2f773db | 120 | |
DimitriGruebel | 0:1447d2f773db | 121 | void print_gyro_angles() |
DimitriGruebel | 0:1447d2f773db | 122 | { |
DimitriGruebel | 0:1447d2f773db | 123 | ios.printf("Roll %+6.1f deg\n", data.Roll); |
DimitriGruebel | 0:1447d2f773db | 124 | ios.printf("Pitch %+6.1f deg\n", data.Pitch); |
DimitriGruebel | 0:1447d2f773db | 125 | ios.printf("Yaw %+6.1f deg\n\n", data.Yaw); |
DimitriGruebel | 0:1447d2f773db | 126 | } |
DimitriGruebel | 0:1447d2f773db | 127 | |
DimitriGruebel | 0:1447d2f773db | 128 | |
DimitriGruebel | 0:1447d2f773db | 129 | /////////////////////////////////////////////////////////////////////////////////////////////////// |
DimitriGruebel | 0:1447d2f773db | 130 | // LageregelungAchse // FUNKTION ZUR LAGEREGELUNG FÜR EINE ACHSE - STATISCH - PD Regler |
DimitriGruebel | 0:1447d2f773db | 131 | |
DimitriGruebel | 0:1447d2f773db | 132 | void LageregelungAchse(float kp, float kd, float winkel, const float& gyro_veloc, |
DimitriGruebel | 0:1447d2f773db | 133 | const float& gyro_angle, PwmOut& kreisel) |
DimitriGruebel | 0:1447d2f773db | 134 | { |
DimitriGruebel | 0:1447d2f773db | 135 | float start_angle = gyro_angle; |
DimitriGruebel | 0:1447d2f773db | 136 | float winkel_soll = gyro_angle + winkel; |
DimitriGruebel | 0:1447d2f773db | 137 | float abweichung = gyro_angle - winkel_soll; |
DimitriGruebel | 0:1447d2f773db | 138 | float leistung_alt = 0; // Variablen für Bremsverfahren |
DimitriGruebel | 0:1447d2f773db | 139 | bool start = true; |
DimitriGruebel | 0:1447d2f773db | 140 | |
DimitriGruebel | 0:1447d2f773db | 141 | while (fabs(abweichung) > regelgenauigkeit) |
DimitriGruebel | 0:1447d2f773db | 142 | { |
DimitriGruebel | 0:1447d2f773db | 143 | float leistung = abweichung * kp - gyro_veloc * kd; |
DimitriGruebel | 0:1447d2f773db | 144 | if (leistung > max_leistung) leistung = max_leistung; |
DimitriGruebel | 0:1447d2f773db | 145 | if (leistung < -max_leistung) leistung = -max_leistung; |
DimitriGruebel | 0:1447d2f773db | 146 | if (leistung > -min_leistung && leistung < -0.001) leistung = -min_leistung; |
DimitriGruebel | 0:1447d2f773db | 147 | if (leistung < min_leistung && leistung > 0.001) leistung = min_leistung; |
DimitriGruebel | 0:1447d2f773db | 148 | |
DimitriGruebel | 0:1447d2f773db | 149 | int Bremsrichtung = 0; // Vorzeichenbestimmung der Abbremsrichtung |
DimitriGruebel | 0:1447d2f773db | 150 | |
DimitriGruebel | 0:1447d2f773db | 151 | if (leistung >= 0) |
DimitriGruebel | 0:1447d2f773db | 152 | Bremsrichtung = -1; |
DimitriGruebel | 0:1447d2f773db | 153 | else |
DimitriGruebel | 0:1447d2f773db | 154 | Bremsrichtung = 1; |
DimitriGruebel | 0:1447d2f773db | 155 | |
DimitriGruebel | 0:1447d2f773db | 156 | float Stellwert = calcStellwert(leistung); // Berechnung Stellwert |
DimitriGruebel | 0:1447d2f773db | 157 | kreisel.write(Stellwert); // Befehl an Motorregler über PWM |
DimitriGruebel | 0:1447d2f773db | 158 | wait_ms(20); |
DimitriGruebel | 0:1447d2f773db | 159 | abweichung = gyro_angle - winkel_soll; // Kontrolle der Regelgenauigkeit |
DimitriGruebel | 0:1447d2f773db | 160 | |
DimitriGruebel | 0:1447d2f773db | 161 | if (start == true) |
DimitriGruebel | 0:1447d2f773db | 162 | { |
DimitriGruebel | 0:1447d2f773db | 163 | leistung_alt = leistung; |
DimitriGruebel | 0:1447d2f773db | 164 | start = false; |
DimitriGruebel | 0:1447d2f773db | 165 | } |
DimitriGruebel | 0:1447d2f773db | 166 | else |
DimitriGruebel | 0:1447d2f773db | 167 | { |
DimitriGruebel | 0:1447d2f773db | 168 | if (fabs(winkel) > 5.0) |
DimitriGruebel | 0:1447d2f773db | 169 | { |
DimitriGruebel | 0:1447d2f773db | 170 | if ( fabs(leistung) > min_leistung) |
DimitriGruebel | 0:1447d2f773db | 171 | { |
DimitriGruebel | 0:1447d2f773db | 172 | float delta_leistung = fabs(leistung) - fabs(leistung_alt); // Bestimmung Abfall Delta |
DimitriGruebel | 0:1447d2f773db | 173 | if (delta_leistung < -0.001) |
DimitriGruebel | 0:1447d2f773db | 174 | { // Abbremsung des Motors bei Leistungsabnahme |
DimitriGruebel | 0:1447d2f773db | 175 | kreisel.write(calcStellwert(Bremsrichtung * min_leistung)); |
DimitriGruebel | 0:1447d2f773db | 176 | wait_ms(10); |
DimitriGruebel | 0:1447d2f773db | 177 | } |
DimitriGruebel | 0:1447d2f773db | 178 | } |
DimitriGruebel | 0:1447d2f773db | 179 | } |
DimitriGruebel | 0:1447d2f773db | 180 | |
DimitriGruebel | 0:1447d2f773db | 181 | leistung_alt = leistung; |
DimitriGruebel | 0:1447d2f773db | 182 | } // Ende des Bremsverfahrens |
DimitriGruebel | 0:1447d2f773db | 183 | |
DimitriGruebel | 0:1447d2f773db | 184 | if (fabs(abweichung) <= regelgenauigkeit) |
DimitriGruebel | 0:1447d2f773db | 185 | { // Vorzeichen für Bremsung wird bestimmt |
DimitriGruebel | 0:1447d2f773db | 186 | print_gyro_data(); // Funktionsaufruf Gyrodaten ausgeben |
DimitriGruebel | 0:1447d2f773db | 187 | kreisel.write(calcStellwert(Bremsrichtung * 100)); // Vollbremsung |
DimitriGruebel | 0:1447d2f773db | 188 | wait_ms(650); |
DimitriGruebel | 0:1447d2f773db | 189 | kreisel.write(calcStellwert(0)); // Motor aus |
DimitriGruebel | 0:1447d2f773db | 190 | } |
DimitriGruebel | 0:1447d2f773db | 191 | } |
DimitriGruebel | 0:1447d2f773db | 192 | |
DimitriGruebel | 0:1447d2f773db | 193 | float end_angle = gyro_angle; |
DimitriGruebel | 0:1447d2f773db | 194 | ios.printf("Plattform wurde gedreht um %+6.1f deg\n\n", end_angle - start_angle); |
DimitriGruebel | 0:1447d2f773db | 195 | } |
DimitriGruebel | 0:1447d2f773db | 196 | |
DimitriGruebel | 0:1447d2f773db | 197 | |
DimitriGruebel | 0:1447d2f773db | 198 | /////////////////////////////////////////////////////////////////////////////////////////////////// |
DimitriGruebel | 0:1447d2f773db | 199 | // LageregelungAchseDyn // FUNKTION ZUR LAGEREGELUNG FÜR EINE ACHSE - DYNAMISCH - PID Regler |
DimitriGruebel | 0:1447d2f773db | 200 | |
DimitriGruebel | 0:1447d2f773db | 201 | void LageregelungAchseDyn(float kp, float kd, float ki, float winkel, const float& gyro_veloc, |
DimitriGruebel | 0:1447d2f773db | 202 | const float& gyro_angle, PwmOut& kreisel, float& dyn_leistung_leerlauf) |
DimitriGruebel | 0:1447d2f773db | 203 | { |
DimitriGruebel | 0:1447d2f773db | 204 | |
DimitriGruebel | 0:1447d2f773db | 205 | ios.printf("Start LageregelungAchseDyn\n"); |
DimitriGruebel | 0:1447d2f773db | 206 | float start_angle = gyro_angle; |
DimitriGruebel | 0:1447d2f773db | 207 | float winkel_soll = gyro_angle + winkel; |
DimitriGruebel | 0:1447d2f773db | 208 | float abweichung = gyro_angle - winkel_soll; |
DimitriGruebel | 0:1447d2f773db | 209 | float abweichung_sum = abweichung; |
DimitriGruebel | 0:1447d2f773db | 210 | float leistung_alt = dyn_leistung_leerlauf; |
DimitriGruebel | 0:1447d2f773db | 211 | bool start = true; |
DimitriGruebel | 0:1447d2f773db | 212 | float leistung = 0.0; |
DimitriGruebel | 0:1447d2f773db | 213 | float i_glied = 0.0; |
DimitriGruebel | 0:1447d2f773db | 214 | |
DimitriGruebel | 0:1447d2f773db | 215 | int cnt = 0; |
DimitriGruebel | 0:1447d2f773db | 216 | while (fabs(abweichung) > regelgenauigkeit) |
DimitriGruebel | 0:1447d2f773db | 217 | { |
DimitriGruebel | 0:1447d2f773db | 218 | cnt += 1; |
DimitriGruebel | 0:1447d2f773db | 219 | i_glied = abweichung_sum * 0.001 * ki; |
DimitriGruebel | 0:1447d2f773db | 220 | ios.printf("%f\n", i_glied); |
DimitriGruebel | 0:1447d2f773db | 221 | if (i_glied > 0.5) i_glied = 0.5; // I-Glied Limiter |
DimitriGruebel | 0:1447d2f773db | 222 | if (i_glied < -0.5) i_glied = -0.5; |
DimitriGruebel | 0:1447d2f773db | 223 | // Sumierstelle (PID Glieder) |
DimitriGruebel | 0:1447d2f773db | 224 | leistung = dyn_leistung_leerlauf + abweichung * kp - gyro_veloc * kd + i_glied; |
DimitriGruebel | 0:1447d2f773db | 225 | if (leistung > max_leistung) leistung = max_leistung; |
DimitriGruebel | 0:1447d2f773db | 226 | if (leistung < min_leistung) leistung = min_leistung; |
DimitriGruebel | 0:1447d2f773db | 227 | int Bremsrichtung = -1; // Vorzeichen der Abbremsrichtung |
DimitriGruebel | 0:1447d2f773db | 228 | |
DimitriGruebel | 0:1447d2f773db | 229 | float Stellwert = calcStellwert(leistung); // Berechnung Stellwert |
DimitriGruebel | 0:1447d2f773db | 230 | kreisel.write(Stellwert); // Befehl an Motorregler über Pwm |
DimitriGruebel | 0:1447d2f773db | 231 | wait_ms(10); |
DimitriGruebel | 0:1447d2f773db | 232 | |
DimitriGruebel | 0:1447d2f773db | 233 | if (start == true) |
DimitriGruebel | 0:1447d2f773db | 234 | { |
DimitriGruebel | 0:1447d2f773db | 235 | leistung_alt = leistung; |
DimitriGruebel | 0:1447d2f773db | 236 | start = false; |
DimitriGruebel | 0:1447d2f773db | 237 | } |
DimitriGruebel | 0:1447d2f773db | 238 | /*else |
DimitriGruebel | 0:1447d2f773db | 239 | { |
DimitriGruebel | 0:1447d2f773db | 240 | if (leistung > min_leistung) |
DimitriGruebel | 0:1447d2f773db | 241 | { |
DimitriGruebel | 0:1447d2f773db | 242 | float delta_leistung = leistung - leistung_alt; // Bestimmung Abfall Delta |
DimitriGruebel | 0:1447d2f773db | 243 | |
DimitriGruebel | 0:1447d2f773db | 244 | // bei Abnahme der Leistung - Motor abbremsen |
DimitriGruebel | 0:1447d2f773db | 245 | if (delta_leistung < -0.0001) |
DimitriGruebel | 0:1447d2f773db | 246 | { |
DimitriGruebel | 0:1447d2f773db | 247 | kreisel.write(calcStellwert(Bremsrichtung * min_leistung)); |
DimitriGruebel | 0:1447d2f773db | 248 | wait_ms(5); |
DimitriGruebel | 0:1447d2f773db | 249 | } |
DimitriGruebel | 0:1447d2f773db | 250 | } |
DimitriGruebel | 0:1447d2f773db | 251 | } |
DimitriGruebel | 0:1447d2f773db | 252 | */ |
DimitriGruebel | 0:1447d2f773db | 253 | leistung_alt = leistung; |
DimitriGruebel | 0:1447d2f773db | 254 | abweichung = gyro_angle - winkel_soll; |
DimitriGruebel | 0:1447d2f773db | 255 | abweichung_sum += abweichung; |
DimitriGruebel | 0:1447d2f773db | 256 | } |
DimitriGruebel | 0:1447d2f773db | 257 | |
DimitriGruebel | 0:1447d2f773db | 258 | dyn_leistung_leerlauf = leistung - i_glied; |
DimitriGruebel | 0:1447d2f773db | 259 | if (dyn_leistung_leerlauf < min_leistung) dyn_leistung_leerlauf = min_leistung; |
DimitriGruebel | 0:1447d2f773db | 260 | if (dyn_leistung_leerlauf > max_leistung) dyn_leistung_leerlauf = max_leistung; |
DimitriGruebel | 0:1447d2f773db | 261 | ios.printf("Leerlaufleistung: %f\n", dyn_leistung_leerlauf); |
DimitriGruebel | 0:1447d2f773db | 262 | float end_angle = gyro_angle; |
DimitriGruebel | 0:1447d2f773db | 263 | ios.printf("Plattform wurde gedreht um %+6.1f deg\n\n", end_angle - start_angle); |
DimitriGruebel | 0:1447d2f773db | 264 | // Counter für durchlaufene Regelzyklen |
DimitriGruebel | 0:1447d2f773db | 265 | ios.printf("\nEnde LageregelungAchseDyn; Counter: %i\n\n", cnt); |
DimitriGruebel | 0:1447d2f773db | 266 | } |
DimitriGruebel | 0:1447d2f773db | 267 | |
DimitriGruebel | 0:1447d2f773db | 268 | |
DimitriGruebel | 0:1447d2f773db | 269 | /////////////////////////////////////////////////////////////////////////////////////////////////// |
DimitriGruebel | 0:1447d2f773db | 270 | // main // Hauptprogramm |
DimitriGruebel | 0:1447d2f773db | 271 | |
DimitriGruebel | 0:1447d2f773db | 272 | int main() |
DimitriGruebel | 0:1447d2f773db | 273 | { |
DimitriGruebel | 0:1447d2f773db | 274 | ios.baud(115200); // Baudrate XBee Funkmodul |
DimitriGruebel | 0:1447d2f773db | 275 | um6_uart.baud(115200); // Baudrate UM6-lt |
DimitriGruebel | 0:1447d2f773db | 276 | um6_uart.attach(&rxCallback, MODSERIAL::RxIrq); // Interrupt Funktion für UART |
DimitriGruebel | 0:1447d2f773db | 277 | |
DimitriGruebel | 0:1447d2f773db | 278 | rst = 1; // Reset-Pin von XBee auf ON |
DimitriGruebel | 0:1447d2f773db | 279 | |
DimitriGruebel | 0:1447d2f773db | 280 | ios.printf("\nBitte warten, Startvorgang der Plattform...\n\n"); // Start-UP Prozedur |
DimitriGruebel | 0:1447d2f773db | 281 | um6_uart.putc(0xAC); // Nulliert die Rate-Gyros |
DimitriGruebel | 0:1447d2f773db | 282 | wait_ms(3500); |
DimitriGruebel | 0:1447d2f773db | 283 | um6_uart.putc(0xAC); // Nulliert die Rate-Gyros |
DimitriGruebel | 0:1447d2f773db | 284 | wait_ms(3500); |
DimitriGruebel | 0:1447d2f773db | 285 | um6_uart.putc(0xAC); // Nulliert die Rate-Gyros |
DimitriGruebel | 0:1447d2f773db | 286 | wait_ms(3500); |
DimitriGruebel | 0:1447d2f773db | 287 | |
DimitriGruebel | 0:1447d2f773db | 288 | x_kreisel.period_ms(pulsweite); |
DimitriGruebel | 0:1447d2f773db | 289 | y_kreisel.period_ms(pulsweite); |
DimitriGruebel | 0:1447d2f773db | 290 | z_kreisel.period_ms(pulsweite); |
DimitriGruebel | 0:1447d2f773db | 291 | |
DimitriGruebel | 0:1447d2f773db | 292 | ios.printf("\n\nTESTPLATTFORM ZUR SATELLITENLAGEREGELUNG MIT REAKTIONSKREISELN:\n\n"); |
DimitriGruebel | 0:1447d2f773db | 293 | |
DimitriGruebel | 0:1447d2f773db | 294 | int choice1 = 0; |
DimitriGruebel | 0:1447d2f773db | 295 | |
DimitriGruebel | 0:1447d2f773db | 296 | do |
DimitriGruebel | 0:1447d2f773db | 297 | { |
DimitriGruebel | 0:1447d2f773db | 298 | do |
DimitriGruebel | 0:1447d2f773db | 299 | { |
DimitriGruebel | 0:1447d2f773db | 300 | ios.printf("\n\nBitte folgende Anweisungen ausfuehren,falls nicht bereits erfolgt!\n" |
DimitriGruebel | 0:1447d2f773db | 301 | "Stromversorgung der Testplattform trennen und ohne Druckluft mit X-Achse auf mag." |
DimitriGruebel | 0:1447d2f773db | 302 | " Norden ausrichten.\n" |
DimitriGruebel | 0:1447d2f773db | 303 | "Testplattform mit Wasserwaage horizontal tarieren.\n" |
DimitriGruebel | 0:1447d2f773db | 304 | "Danach Stromversorgung herstellen und mbed Reset-Taste betaetigen.\n\n"); |
DimitriGruebel | 0:1447d2f773db | 305 | |
DimitriGruebel | 0:1447d2f773db | 306 | x_kreisel.write(calcStellwert(0)); //Initialisierung X-Motorrelger |
DimitriGruebel | 0:1447d2f773db | 307 | y_kreisel.write(calcStellwert(0)); //Initialisierung Y-Motorrelger |
DimitriGruebel | 0:1447d2f773db | 308 | z_kreisel.write(calcStellwert(0)); //Initialisierung Z-Motorrelger |
DimitriGruebel | 0:1447d2f773db | 309 | |
DimitriGruebel | 0:1447d2f773db | 310 | ios.printf("Menueauswahl:\n"); |
DimitriGruebel | 0:1447d2f773db | 311 | ios.printf("\t1 - Testplattform um einzelne Achsen drehen\n"); |
DimitriGruebel | 0:1447d2f773db | 312 | ios.printf("\t2 - Testplattform um drei Achsen drehen\n"); |
DimitriGruebel | 0:1447d2f773db | 313 | ios.printf("\t3 - Lage der Testplattform aktiv regeln fuer dynamische Fehler\n"); |
DimitriGruebel | 0:1447d2f773db | 314 | ios.printf("\t4 - Gyrodaten ausgeben [Winkel/Winkelgeschwindigkeiten]\n"); |
DimitriGruebel | 0:1447d2f773db | 315 | ios.printf("\t5 - Programm beenden\n"); |
DimitriGruebel | 0:1447d2f773db | 316 | ios.printf("\nEingabe >"); |
DimitriGruebel | 0:1447d2f773db | 317 | ios.scanf("%i", &choice1); |
DimitriGruebel | 0:1447d2f773db | 318 | }while (choice1 < 1 || choice1 > 5); |
DimitriGruebel | 0:1447d2f773db | 319 | |
DimitriGruebel | 0:1447d2f773db | 320 | if (choice1 == 1) |
DimitriGruebel | 0:1447d2f773db | 321 | { |
DimitriGruebel | 0:1447d2f773db | 322 | ios.printf("\nTestplattform ist initialisiert.\n\n"); |
DimitriGruebel | 0:1447d2f773db | 323 | print_gyro_angles(); // Funktionsaufruf Gyrodwinkel ausgeben |
DimitriGruebel | 0:1447d2f773db | 324 | float x_kp = x_kp_def; |
DimitriGruebel | 0:1447d2f773db | 325 | float x_kd = x_kd_def; |
DimitriGruebel | 0:1447d2f773db | 326 | float x_winkel = 0.0; |
DimitriGruebel | 0:1447d2f773db | 327 | float y_kp = y_kp_def; |
DimitriGruebel | 0:1447d2f773db | 328 | float y_kd = y_kd_def; |
DimitriGruebel | 0:1447d2f773db | 329 | float y_winkel = 0.0; |
DimitriGruebel | 0:1447d2f773db | 330 | float z_kp = z_kp_def; |
DimitriGruebel | 0:1447d2f773db | 331 | float z_kd = z_kd_def; |
DimitriGruebel | 0:1447d2f773db | 332 | float z_winkel = 0.0; |
DimitriGruebel | 0:1447d2f773db | 333 | |
DimitriGruebel | 0:1447d2f773db | 334 | int choice2 = 0; |
DimitriGruebel | 0:1447d2f773db | 335 | |
DimitriGruebel | 0:1447d2f773db | 336 | do |
DimitriGruebel | 0:1447d2f773db | 337 | { |
DimitriGruebel | 0:1447d2f773db | 338 | do |
DimitriGruebel | 0:1447d2f773db | 339 | { |
DimitriGruebel | 0:1447d2f773db | 340 | ios.printf("\n\nLAGEREGELUNG EINZELNER ACHSEN:\n\n"); |
DimitriGruebel | 0:1447d2f773db | 341 | ios.printf("Menueauswahl:\n"); |
DimitriGruebel | 0:1447d2f773db | 342 | ios.printf("\t1 - Satellitenplattform rollen (um X-Achse drehen)\n"); |
DimitriGruebel | 0:1447d2f773db | 343 | ios.printf("\t2 - Satellitenplattform nicken (um Y-Achse drehen)\n"); |
DimitriGruebel | 0:1447d2f773db | 344 | ios.printf("\t3 - Satellitenplattform gieren (um Z-Achse drehen)\n"); |
DimitriGruebel | 0:1447d2f773db | 345 | ios.printf("\t4 - Auswahl abbrechen (esc).\n"); |
DimitriGruebel | 0:1447d2f773db | 346 | ios.printf("\nEingabe >"); |
DimitriGruebel | 0:1447d2f773db | 347 | ios.scanf("%i", &choice2); |
DimitriGruebel | 0:1447d2f773db | 348 | }while (choice2 < 1 || choice2 > 4); |
DimitriGruebel | 0:1447d2f773db | 349 | |
DimitriGruebel | 0:1447d2f773db | 350 | if (choice2 == 1) |
DimitriGruebel | 0:1447d2f773db | 351 | { |
DimitriGruebel | 0:1447d2f773db | 352 | ios.printf("\n Reglerparameter kp eingeben [0 - +3] >"); |
DimitriGruebel | 0:1447d2f773db | 353 | ios.scanf("%f", &x_kp); |
DimitriGruebel | 0:1447d2f773db | 354 | if (x_kp > x_kp_max) x_kp = x_kp_max; // Begrenzung X-Achse kp Pos. |
DimitriGruebel | 0:1447d2f773db | 355 | if (x_kp < x_kp_min) x_kp = x_kp_min; // Begrenzung X-Achse kp Neg. |
DimitriGruebel | 0:1447d2f773db | 356 | ios.printf("\n Reglerparameter kd eingeben [0 - +3] >"); |
DimitriGruebel | 0:1447d2f773db | 357 | ios.scanf("%f", &x_kd); |
DimitriGruebel | 0:1447d2f773db | 358 | if (x_kd > x_kd_max) x_kd = x_kd_max; // Begrenzung X-Achse kd Pos. |
DimitriGruebel | 0:1447d2f773db | 359 | if (x_kd < x_kd_min) x_kd = x_kd_min; // Begrenzung X-Achse kd Neg. |
DimitriGruebel | 0:1447d2f773db | 360 | ios.printf("\n Rollwinkel Phi eingeben [-45 - +45] >"); |
DimitriGruebel | 0:1447d2f773db | 361 | ios.scanf("%f", &x_winkel); |
DimitriGruebel | 0:1447d2f773db | 362 | if (x_winkel > x_winkel_max) x_winkel = x_winkel_max; // Begrenzung X-Achse Winkel Pos. |
DimitriGruebel | 0:1447d2f773db | 363 | if (x_winkel < x_winkel_min) x_winkel = x_winkel_min; // Begrenzung X-Achse Winkel Neg. |
DimitriGruebel | 0:1447d2f773db | 364 | if (fabs(x_winkel) < regelgenauigkeit) ios.printf("\nRegelung ist nicht notwendig!"); |
DimitriGruebel | 0:1447d2f773db | 365 | ios.printf("\nParameter kp = %f, Parameter kd = %f, Rollwinkel = %f\n", |
DimitriGruebel | 0:1447d2f773db | 366 | x_kp, x_kd, x_winkel); |
DimitriGruebel | 0:1447d2f773db | 367 | print_gyro_angles(); |
DimitriGruebel | 0:1447d2f773db | 368 | LageregelungAchse(x_kp, x_kd, x_winkel, data.Gyro_Proc_X, data.Roll, x_kreisel); |
DimitriGruebel | 0:1447d2f773db | 369 | } |
DimitriGruebel | 0:1447d2f773db | 370 | else if (choice2 == 2) |
DimitriGruebel | 0:1447d2f773db | 371 | { |
DimitriGruebel | 0:1447d2f773db | 372 | ios.printf("\n Reglerparameter kp eingeben [0 - +3] >"); |
DimitriGruebel | 0:1447d2f773db | 373 | ios.scanf("%f", &y_kp); |
DimitriGruebel | 0:1447d2f773db | 374 | if (y_kp > y_kp_max) y_kp = y_kp_max; // Begrenzung Y-Achse kp Pos. |
DimitriGruebel | 0:1447d2f773db | 375 | if (y_kp < y_kp_min) y_kp = y_kp_min; // Begrenzung Y-Achse kp Neg. |
DimitriGruebel | 0:1447d2f773db | 376 | ios.printf("\n Reglerparameter kd eingeben [0 - +3] >"); |
DimitriGruebel | 0:1447d2f773db | 377 | ios.scanf("%f", &y_kd); |
DimitriGruebel | 0:1447d2f773db | 378 | if (y_kd > y_kd_max) y_kd = y_kd_max; // Begrenzung Y-Achse kd Pos. |
DimitriGruebel | 0:1447d2f773db | 379 | if (y_kd < y_kd_min) y_kd = y_kd_min; // Begrenzung Y-Achse kd Neg. |
DimitriGruebel | 0:1447d2f773db | 380 | ios.printf("\n Nickwinkel Theta eingeben [-45 - +45] >"); |
DimitriGruebel | 0:1447d2f773db | 381 | ios.scanf("%f", &y_winkel); |
DimitriGruebel | 0:1447d2f773db | 382 | if (y_winkel > y_winkel_max) y_winkel = y_winkel_max; // Begrenzung Y-Achse Winkel Pos. |
DimitriGruebel | 0:1447d2f773db | 383 | if (y_winkel < y_winkel_min) y_winkel = y_winkel_min; // Begrenzung Y-Achse Winkel Neg. |
DimitriGruebel | 0:1447d2f773db | 384 | if (fabs(y_winkel) < regelgenauigkeit) ios.printf("\nRegelung ist nicht notwendig!"); |
DimitriGruebel | 0:1447d2f773db | 385 | ios.printf("\nParameter kp = %f, Parameter kd = %f, Nickwinkel = %f\n", |
DimitriGruebel | 0:1447d2f773db | 386 | y_kp, y_kd, y_winkel); |
DimitriGruebel | 0:1447d2f773db | 387 | print_gyro_angles(); |
DimitriGruebel | 0:1447d2f773db | 388 | LageregelungAchse(y_kp, y_kd, y_winkel, data.Gyro_Proc_Y, data.Pitch, y_kreisel); |
DimitriGruebel | 0:1447d2f773db | 389 | |
DimitriGruebel | 0:1447d2f773db | 390 | } |
DimitriGruebel | 0:1447d2f773db | 391 | else if (choice2 == 3) |
DimitriGruebel | 0:1447d2f773db | 392 | { |
DimitriGruebel | 0:1447d2f773db | 393 | ios.printf("\n Reglerparameter kp eingeben [0 - +3] >"); |
DimitriGruebel | 0:1447d2f773db | 394 | ios.scanf("%f", &z_kp); |
DimitriGruebel | 0:1447d2f773db | 395 | if (z_kp > z_kp_max) z_kp = z_kp_max; // Begrenzung Z-Achse kp Pos. |
DimitriGruebel | 0:1447d2f773db | 396 | if (z_kp < z_kp_min) z_kp = z_kp_min; // Begrenzung Z-Achse kp Neg. |
DimitriGruebel | 0:1447d2f773db | 397 | ios.printf("\n Reglerparameter kd eingeben [0 - +3] >"); |
DimitriGruebel | 0:1447d2f773db | 398 | ios.scanf("%f", &z_kd); |
DimitriGruebel | 0:1447d2f773db | 399 | if (z_kd > z_kd_max) z_kd = z_kd_max; // Begrenzung Z-Achse kd Pos. |
DimitriGruebel | 0:1447d2f773db | 400 | if (z_kd < z_kd_min) z_kd = z_kd_min; // Begrenzung Z-Achse kd Neg. |
DimitriGruebel | 0:1447d2f773db | 401 | ios.printf("\n Gierwinkel Psi eingeben [-90 - +90] >"); |
DimitriGruebel | 0:1447d2f773db | 402 | ios.scanf("%f", &z_winkel); |
DimitriGruebel | 0:1447d2f773db | 403 | if (z_winkel > z_winkel_max) z_winkel = z_winkel_max; // Begrenzung Z-Achse Winkel Pos. |
DimitriGruebel | 0:1447d2f773db | 404 | if (z_winkel < z_winkel_min) z_winkel = z_winkel_min; // Begrenzung Z-Achse Winkel Neg. |
DimitriGruebel | 0:1447d2f773db | 405 | if (fabs(z_winkel) < regelgenauigkeit) ios.printf("\nRegelung ist nicht notwendig!"); |
DimitriGruebel | 0:1447d2f773db | 406 | ios.printf("\nParameter kp = %f, Parameter kd = %f, Gierwinkel = %f\n", |
DimitriGruebel | 0:1447d2f773db | 407 | z_kp, z_kd, z_winkel); |
DimitriGruebel | 0:1447d2f773db | 408 | print_gyro_angles(); |
DimitriGruebel | 0:1447d2f773db | 409 | LageregelungAchse(z_kp, z_kd, z_winkel, data.Gyro_Proc_Z, data.Yaw, z_kreisel); |
DimitriGruebel | 0:1447d2f773db | 410 | } |
DimitriGruebel | 0:1447d2f773db | 411 | }while(choice2 != 4); |
DimitriGruebel | 0:1447d2f773db | 412 | |
DimitriGruebel | 0:1447d2f773db | 413 | } |
DimitriGruebel | 0:1447d2f773db | 414 | else if (choice1 == 2) |
DimitriGruebel | 0:1447d2f773db | 415 | { |
DimitriGruebel | 0:1447d2f773db | 416 | float x_kp = x_kp_def; |
DimitriGruebel | 0:1447d2f773db | 417 | float x_kd = x_kd_def; |
DimitriGruebel | 0:1447d2f773db | 418 | float x_winkel = 0.0; |
DimitriGruebel | 0:1447d2f773db | 419 | float y_kp = y_kp_def; |
DimitriGruebel | 0:1447d2f773db | 420 | float y_kd = y_kd_def; |
DimitriGruebel | 0:1447d2f773db | 421 | float y_winkel = 0.0; |
DimitriGruebel | 0:1447d2f773db | 422 | float z_kp = z_kp_def; |
DimitriGruebel | 0:1447d2f773db | 423 | float z_kd = z_kd_def; |
DimitriGruebel | 0:1447d2f773db | 424 | float z_winkel = 0.0; |
DimitriGruebel | 0:1447d2f773db | 425 | |
DimitriGruebel | 0:1447d2f773db | 426 | int choice3 = 0; |
DimitriGruebel | 0:1447d2f773db | 427 | |
DimitriGruebel | 0:1447d2f773db | 428 | do |
DimitriGruebel | 0:1447d2f773db | 429 | { |
DimitriGruebel | 0:1447d2f773db | 430 | do |
DimitriGruebel | 0:1447d2f773db | 431 | { |
DimitriGruebel | 0:1447d2f773db | 432 | ios.printf("\n\nLAGEREGELUNG ALLER ACHSEN:\n\n"); |
DimitriGruebel | 0:1447d2f773db | 433 | ios.printf("Bitte Plattform in gewuenschte Ausgangsposition bringen.\n"); |
DimitriGruebel | 0:1447d2f773db | 434 | ios.printf("Plattform wird in Z-Y-X Reihenfolge gedreht!\n\n"); |
DimitriGruebel | 0:1447d2f773db | 435 | ios.printf("Aktuelle Werte fuer Regler X-Achse: kp = %f kd = %f\n\n", x_kp, x_kd); |
DimitriGruebel | 0:1447d2f773db | 436 | ios.printf("Aktuelle Werte fuer Regler Y-Achse: kp = %f kd = %f\n\n", y_kp, y_kd); |
DimitriGruebel | 0:1447d2f773db | 437 | ios.printf("Aktuelle Werte fuer Regler Z-Achse: kp = %f kd = %f\n\n", z_kp, z_kd); |
DimitriGruebel | 0:1447d2f773db | 438 | ios.printf("Menueauswahl:\n"); |
DimitriGruebel | 0:1447d2f773db | 439 | ios.printf("\t1 - Regelparameter aendern?\n"); |
DimitriGruebel | 0:1447d2f773db | 440 | ios.printf("\t2 - Plattform drehen-Winkel eingeben\n"); |
DimitriGruebel | 0:1447d2f773db | 441 | ios.printf("\t3 - Auswahl abbrechen (esc).\n"); |
DimitriGruebel | 0:1447d2f773db | 442 | ios.printf("\nEingabe >"); |
DimitriGruebel | 0:1447d2f773db | 443 | ios.scanf("%i", &choice3); |
DimitriGruebel | 0:1447d2f773db | 444 | }while (choice3 < 1 || choice3 > 3); |
DimitriGruebel | 0:1447d2f773db | 445 | |
DimitriGruebel | 0:1447d2f773db | 446 | if (choice3 == 1) |
DimitriGruebel | 0:1447d2f773db | 447 | { |
DimitriGruebel | 0:1447d2f773db | 448 | ios.printf("\n Reglerparameter x_kp eingeben [0 - +3] >"); |
DimitriGruebel | 0:1447d2f773db | 449 | ios.scanf("%f", &x_kp); |
DimitriGruebel | 0:1447d2f773db | 450 | if (x_kp > x_kp_max) x_kp = x_kp_max; // Begrenzung X-Achse kp Pos. |
DimitriGruebel | 0:1447d2f773db | 451 | if (x_kp < x_kp_min) x_kp = x_kp_min; // Begrenzung X-Achse kp Neg. |
DimitriGruebel | 0:1447d2f773db | 452 | ios.printf("\n Reglerparameter x_kd eingeben [0 - +3] >"); |
DimitriGruebel | 0:1447d2f773db | 453 | ios.scanf("%f", &x_kd); |
DimitriGruebel | 0:1447d2f773db | 454 | if (x_kd > x_kd_max) x_kd = x_kd_max; // Begrenzung X-Achse kd Pos. |
DimitriGruebel | 0:1447d2f773db | 455 | if (x_kd < x_kd_min) x_kd = x_kd_min; // Begrenzung X-Achse kd Neg. |
DimitriGruebel | 0:1447d2f773db | 456 | ios.printf("\n Reglerparameter y_kp eingeben [0 - +3] >"); |
DimitriGruebel | 0:1447d2f773db | 457 | ios.scanf("%f", &y_kp); |
DimitriGruebel | 0:1447d2f773db | 458 | if (y_kp > y_kp_max) y_kp = y_kp_max; // Begrenzung Y-Achse kp Pos. |
DimitriGruebel | 0:1447d2f773db | 459 | if (y_kp < y_kp_min) y_kp = y_kp_min; // Begrenzung Y-Achse kp Neg. |
DimitriGruebel | 0:1447d2f773db | 460 | ios.printf("\n Reglerparameter y_kd eingeben [0 - +3] >"); |
DimitriGruebel | 0:1447d2f773db | 461 | ios.scanf("%f", &y_kd); |
DimitriGruebel | 0:1447d2f773db | 462 | if (y_kd > y_kd_max) y_kd = y_kd_max; // Begrenzung Y-Achse kd Pos. |
DimitriGruebel | 0:1447d2f773db | 463 | if (y_kd < y_kd_min) y_kd = y_kd_min; // Begrenzung Y-Achse kd Neg. |
DimitriGruebel | 0:1447d2f773db | 464 | ios.printf("\n Reglerparameter z_kp eingeben [0 - +3] >"); |
DimitriGruebel | 0:1447d2f773db | 465 | ios.scanf("%f", &z_kp); |
DimitriGruebel | 0:1447d2f773db | 466 | if (z_kp > z_kp_max) z_kp = z_kp_max; // Begrenzung Z-Achse kp Pos. |
DimitriGruebel | 0:1447d2f773db | 467 | if (z_kp < z_kp_min) z_kp = z_kp_min; // Begrenzung Z-Achse kp Neg. |
DimitriGruebel | 0:1447d2f773db | 468 | ios.printf("\n Reglerparameter z_kd eingeben [0 - +3] >"); |
DimitriGruebel | 0:1447d2f773db | 469 | ios.scanf("%f", &z_kd); |
DimitriGruebel | 0:1447d2f773db | 470 | if (z_kd > z_kd_max) z_kd = z_kd_max; // Begrenzung Z-Achse kd Pos. |
DimitriGruebel | 0:1447d2f773db | 471 | if (z_kd < z_kd_min) z_kd = z_kd_min; // Begrenzung Z-Achse kd Neg. |
DimitriGruebel | 0:1447d2f773db | 472 | ios.printf("\n\n"); |
DimitriGruebel | 0:1447d2f773db | 473 | ios.printf("Aktuelle Werte fuer Regler X-Achse: kp = %f kd = %f\n\n", x_kp, x_kd); |
DimitriGruebel | 0:1447d2f773db | 474 | ios.printf("Aktuelle Werte fuer Regler Y-Achse: kp = %f kd = %f\n\n", y_kp, y_kd); |
DimitriGruebel | 0:1447d2f773db | 475 | ios.printf("Aktuelle Werte fuer Regler Z-Achse: kp = %f kd = %f\n\n", z_kp, z_kd); |
DimitriGruebel | 0:1447d2f773db | 476 | } |
DimitriGruebel | 0:1447d2f773db | 477 | else if (choice3 == 2) |
DimitriGruebel | 0:1447d2f773db | 478 | { // Eingabe der Drehwinkel |
DimitriGruebel | 0:1447d2f773db | 479 | ios.printf("\n Gierwinkel Psi eingeben [-90 - +90] >"); |
DimitriGruebel | 0:1447d2f773db | 480 | ios.scanf("%f", &z_winkel); |
DimitriGruebel | 0:1447d2f773db | 481 | if (z_winkel > z_winkel_max) z_winkel = z_winkel_max; // Begrenzung Z-Achse Winkel Pos. |
DimitriGruebel | 0:1447d2f773db | 482 | if (z_winkel < z_winkel_min) z_winkel = z_winkel_min; // Begrenzung Z-Achse Winkel Neg. |
DimitriGruebel | 0:1447d2f773db | 483 | ios.printf("\n Nickwinkel Theta eingeben [-45 - +45] >"); |
DimitriGruebel | 0:1447d2f773db | 484 | ios.scanf("%f", &y_winkel); |
DimitriGruebel | 0:1447d2f773db | 485 | if (y_winkel > y_winkel_max) y_winkel = y_winkel_max; // Begrenzung Y-Achse Winkel Pos. |
DimitriGruebel | 0:1447d2f773db | 486 | if (y_winkel < y_winkel_min) y_winkel = y_winkel_min; // Begrenzung Y-Achse Winkel Neg. |
DimitriGruebel | 0:1447d2f773db | 487 | ios.printf("\n Rollwinkel Phi eingeben [-45 - +45] >"); |
DimitriGruebel | 0:1447d2f773db | 488 | ios.scanf("%f", &x_winkel); |
DimitriGruebel | 0:1447d2f773db | 489 | if (x_winkel > x_winkel_max) x_winkel = x_winkel_max; // Begrenzung X-Achse Winkel Pos. |
DimitriGruebel | 0:1447d2f773db | 490 | if (x_winkel < x_winkel_min) x_winkel = x_winkel_min; // Begrenzung X-Achse Winkel Neg. |
DimitriGruebel | 0:1447d2f773db | 491 | |
DimitriGruebel | 0:1447d2f773db | 492 | if (fabs(z_winkel) < regelgenauigkeit && |
DimitriGruebel | 0:1447d2f773db | 493 | fabs(y_winkel) < regelgenauigkeit && |
DimitriGruebel | 0:1447d2f773db | 494 | fabs(x_winkel) < regelgenauigkeit) |
DimitriGruebel | 0:1447d2f773db | 495 | { |
DimitriGruebel | 0:1447d2f773db | 496 | ios.printf("\nRegelung ist nicht notwendig!"); |
DimitriGruebel | 0:1447d2f773db | 497 | } |
DimitriGruebel | 0:1447d2f773db | 498 | else |
DimitriGruebel | 0:1447d2f773db | 499 | { |
DimitriGruebel | 0:1447d2f773db | 500 | print_gyro_angles(); |
DimitriGruebel | 0:1447d2f773db | 501 | LageregelungAchse(z_kp, z_kd, z_winkel, data.Gyro_Proc_Z, data.Yaw, z_kreisel); |
DimitriGruebel | 0:1447d2f773db | 502 | wait_ms(250); |
DimitriGruebel | 0:1447d2f773db | 503 | LageregelungAchse(y_kp, y_kd, y_winkel, data.Gyro_Proc_Y, data.Pitch, y_kreisel); |
DimitriGruebel | 0:1447d2f773db | 504 | wait_ms(250); |
DimitriGruebel | 0:1447d2f773db | 505 | LageregelungAchse(x_kp, x_kd, x_winkel, data.Gyro_Proc_X, data.Roll, x_kreisel); |
DimitriGruebel | 0:1447d2f773db | 506 | print_gyro_angles(); |
DimitriGruebel | 0:1447d2f773db | 507 | ios.printf("\n\nSoll die Plattform wieder in ihre Ausgangslage gefahren werden? " |
DimitriGruebel | 0:1447d2f773db | 508 | "ja/nein\n"); |
DimitriGruebel | 0:1447d2f773db | 509 | char yes_no[50]; |
DimitriGruebel | 0:1447d2f773db | 510 | ios.scanf("%s", yes_no); |
DimitriGruebel | 0:1447d2f773db | 511 | |
DimitriGruebel | 0:1447d2f773db | 512 | if (strcmp(yes_no, "ja") == 0) // Plattform in auf Ausgangsposition drehen |
DimitriGruebel | 0:1447d2f773db | 513 | { |
DimitriGruebel | 0:1447d2f773db | 514 | LageregelungAchse(x_kp, x_kd, -x_winkel, data.Gyro_Proc_X, data.Roll, x_kreisel); |
DimitriGruebel | 0:1447d2f773db | 515 | wait_ms(250); |
DimitriGruebel | 0:1447d2f773db | 516 | LageregelungAchse(y_kp, y_kd, -y_winkel, data.Gyro_Proc_Y, data.Pitch, y_kreisel); |
DimitriGruebel | 0:1447d2f773db | 517 | wait_ms(250); |
DimitriGruebel | 0:1447d2f773db | 518 | LageregelungAchse(z_kp, z_kd, -z_winkel, data.Gyro_Proc_Z, data.Yaw, z_kreisel); |
DimitriGruebel | 0:1447d2f773db | 519 | print_gyro_angles(); |
DimitriGruebel | 0:1447d2f773db | 520 | } |
DimitriGruebel | 0:1447d2f773db | 521 | } |
DimitriGruebel | 0:1447d2f773db | 522 | } |
DimitriGruebel | 0:1447d2f773db | 523 | |
DimitriGruebel | 0:1447d2f773db | 524 | }while(choice3 != 3); |
DimitriGruebel | 0:1447d2f773db | 525 | } |
DimitriGruebel | 0:1447d2f773db | 526 | else if (choice1 == 3) |
DimitriGruebel | 0:1447d2f773db | 527 | { |
DimitriGruebel | 0:1447d2f773db | 528 | float x_kp = x_kp_def; |
DimitriGruebel | 0:1447d2f773db | 529 | float x_kd = x_kd_def; |
DimitriGruebel | 0:1447d2f773db | 530 | float x_ki = x_ki_def; |
DimitriGruebel | 0:1447d2f773db | 531 | float y_kp = y_kp_def; |
DimitriGruebel | 0:1447d2f773db | 532 | float y_kd = y_kd_def; |
DimitriGruebel | 0:1447d2f773db | 533 | float y_ki = y_ki_def; |
DimitriGruebel | 0:1447d2f773db | 534 | float z_kp = z_kp_def; |
DimitriGruebel | 0:1447d2f773db | 535 | float z_kd = z_kd_def; |
DimitriGruebel | 0:1447d2f773db | 536 | float z_ki = z_ki_def; |
DimitriGruebel | 0:1447d2f773db | 537 | |
DimitriGruebel | 0:1447d2f773db | 538 | int choice4 = 0; |
DimitriGruebel | 0:1447d2f773db | 539 | |
DimitriGruebel | 0:1447d2f773db | 540 | do |
DimitriGruebel | 0:1447d2f773db | 541 | { |
DimitriGruebel | 0:1447d2f773db | 542 | do |
DimitriGruebel | 0:1447d2f773db | 543 | { |
DimitriGruebel | 0:1447d2f773db | 544 | ios.printf("\n\nLage der Testplattform aktiv regeln fuer dynamische Fehler" |
DimitriGruebel | 0:1447d2f773db | 545 | " (Position halten):\n\n"); |
DimitriGruebel | 0:1447d2f773db | 546 | ios.printf("\n\nBitte Plattform in gewuenschte Ausgangsposition bringen und halten " |
DimitriGruebel | 0:1447d2f773db | 547 | "bis die Kreisel " |
DimitriGruebel | 0:1447d2f773db | 548 | "ihre Leerlaufdrehzahl erreicht haben.\n\n"); |
DimitriGruebel | 0:1447d2f773db | 549 | ios.printf("Aktuelle Werte fuer Regler X-Achse: kp = %f kd = %f ki = %f\n", |
DimitriGruebel | 0:1447d2f773db | 550 | x_kp, x_kd, x_ki); |
DimitriGruebel | 0:1447d2f773db | 551 | ios.printf("Aktuelle Werte fuer Regler Y-Achse: kp = %f kd = %f ki = %f\n", |
DimitriGruebel | 0:1447d2f773db | 552 | y_kp, y_kd, y_ki); |
DimitriGruebel | 0:1447d2f773db | 553 | ios.printf("Aktuelle Werte fuer Regler Z-Achse: kp = %f kd = %f ki = %f\n\n", |
DimitriGruebel | 0:1447d2f773db | 554 | z_kp, z_kd, z_ki); |
DimitriGruebel | 0:1447d2f773db | 555 | ios.printf("Menueauswahl:\n"); |
DimitriGruebel | 0:1447d2f773db | 556 | ios.printf("\t1 - Regelparameter aendern?\n"); |
DimitriGruebel | 0:1447d2f773db | 557 | ios.printf("\t2 - Plattform aktivieren\n"); |
DimitriGruebel | 0:1447d2f773db | 558 | ios.printf("\t3 - Auswahl abbrechen (esc).\n"); |
DimitriGruebel | 0:1447d2f773db | 559 | ios.printf("\nEingabe >"); |
DimitriGruebel | 0:1447d2f773db | 560 | ios.scanf("%i", &choice4); |
DimitriGruebel | 0:1447d2f773db | 561 | }while (choice4 < 1 || choice4 > 3); |
DimitriGruebel | 0:1447d2f773db | 562 | |
DimitriGruebel | 0:1447d2f773db | 563 | if (choice4 == 1) |
DimitriGruebel | 0:1447d2f773db | 564 | { |
DimitriGruebel | 0:1447d2f773db | 565 | /* |
DimitriGruebel | 0:1447d2f773db | 566 | ios.printf("\n Reglerparameter x_kp eingeben [0 - +3] >"); |
DimitriGruebel | 0:1447d2f773db | 567 | ios.scanf("%f", &x_kp); |
DimitriGruebel | 0:1447d2f773db | 568 | if (x_kp > x_kp_max) x_kp = x_kp_max; // Begrenzung X-Achse kp Pos. |
DimitriGruebel | 0:1447d2f773db | 569 | if (x_kp < x_kp_min) x_kp = x_kp_min; // Begrenzung X-Achse kp Neg. |
DimitriGruebel | 0:1447d2f773db | 570 | ios.printf("\n Reglerparameter x_kd eingeben [0 - +3] >"); |
DimitriGruebel | 0:1447d2f773db | 571 | ios.scanf("%f", &x_kd); |
DimitriGruebel | 0:1447d2f773db | 572 | if (x_kd > x_kd_max) x_kd = x_kd_max; // Begrenzung X-Achse kd Pos. |
DimitriGruebel | 0:1447d2f773db | 573 | if (x_kd < x_kd_min) x_kd = x_kd_min; // Begrenzung X-Achse kd Neg. |
DimitriGruebel | 0:1447d2f773db | 574 | ios.printf("\n Reglerparameter x_ki eingeben [0 - +0.5] >"); |
DimitriGruebel | 0:1447d2f773db | 575 | ios.scanf("%f", &x_ki); |
DimitriGruebel | 0:1447d2f773db | 576 | if (x_ki > x_ki_max) x_ki = x_ki_max; // Begrenzung X-Achse ki Pos. |
DimitriGruebel | 0:1447d2f773db | 577 | if (x_ki < x_ki_min) x_ki = x_ki_min; // Begrenzung X-Achse ki Neg. |
DimitriGruebel | 0:1447d2f773db | 578 | ios.printf("\n Reglerparameter y_kp eingeben [0 - +3] >"); |
DimitriGruebel | 0:1447d2f773db | 579 | ios.scanf("%f", &y_kp); |
DimitriGruebel | 0:1447d2f773db | 580 | if (y_kp > y_kp_max) y_kp = y_kp_max; // Begrenzung Y-Achse kp Pos. |
DimitriGruebel | 0:1447d2f773db | 581 | if (y_kp < y_kp_min) y_kp = y_kp_min; // Begrenzung Y-Achse kp Neg. |
DimitriGruebel | 0:1447d2f773db | 582 | ios.printf("\n Reglerparameter y_kd eingeben [0 - +3] >"); |
DimitriGruebel | 0:1447d2f773db | 583 | ios.scanf("%f", &y_kd); |
DimitriGruebel | 0:1447d2f773db | 584 | if (y_kd > y_kd_max) y_kd = y_kd_max; // Begrenzung Y-Achse kd Pos. |
DimitriGruebel | 0:1447d2f773db | 585 | if (y_kd < y_kd_min) y_kd = y_kd_min; // Begrenzung Y-Achse kd Neg. |
DimitriGruebel | 0:1447d2f773db | 586 | ios.printf("\n Reglerparameter y_ki eingeben [0 - +0.5] >"); |
DimitriGruebel | 0:1447d2f773db | 587 | ios.scanf("%f", &y_ki); |
DimitriGruebel | 0:1447d2f773db | 588 | if (y_ki > y_ki_max) y_ki = y_ki_max; // Begrenzung Y-Achse ki Pos. |
DimitriGruebel | 0:1447d2f773db | 589 | if (y_ki < y_ki_min) y_ki = y_ki_min; // Begrenzung Y-Achse ki Neg. |
DimitriGruebel | 0:1447d2f773db | 590 | */ |
DimitriGruebel | 0:1447d2f773db | 591 | ios.printf("\n Reglerparameter z_kp eingeben [0 - +3] >"); |
DimitriGruebel | 0:1447d2f773db | 592 | ios.scanf("%f", &z_kp); |
DimitriGruebel | 0:1447d2f773db | 593 | if (z_kp > z_kp_max) z_kp = z_kp_max; // Begrenzung Z-Achse kp Pos. |
DimitriGruebel | 0:1447d2f773db | 594 | if (z_kp < z_kp_min) z_kp = z_kp_min; // Begrenzung Z-Achse kp Neg. |
DimitriGruebel | 0:1447d2f773db | 595 | ios.printf("\n Reglerparameter z_kd eingeben [0 - +3] >"); |
DimitriGruebel | 0:1447d2f773db | 596 | ios.scanf("%f", &z_kd); |
DimitriGruebel | 0:1447d2f773db | 597 | if (z_kd > z_kd_max) z_kd = z_kd_max; // Begrenzung Z-Achse kd Pos. |
DimitriGruebel | 0:1447d2f773db | 598 | if (z_kd < z_kd_min) z_kd = z_kd_min; // Begrenzung Z-Achse kd Neg. |
DimitriGruebel | 0:1447d2f773db | 599 | ios.printf("\n Reglerparameter z_ki eingeben [0 - +0.5] >"); |
DimitriGruebel | 0:1447d2f773db | 600 | ios.scanf("%f", &z_ki); |
DimitriGruebel | 0:1447d2f773db | 601 | if (z_ki > z_ki_max) z_ki = z_ki_max; // Begrenzung Z-Achse ki Pos. |
DimitriGruebel | 0:1447d2f773db | 602 | if (z_ki < z_ki_min) z_ki = z_ki_min; // Begrenzung Z-Achse ki Neg. |
DimitriGruebel | 0:1447d2f773db | 603 | ios.printf("\n\n"); |
DimitriGruebel | 0:1447d2f773db | 604 | //ios.printf("Aktuelle Werte fuer Regler X-Achse: kp = %f kd = %f ki = %f\n\n", |
DimitriGruebel | 0:1447d2f773db | 605 | // x_kp, x_kd, x_ki); |
DimitriGruebel | 0:1447d2f773db | 606 | //ios.printf("Aktuelle Werte fuer Regler Y-Achse: kp = %f kd = %f ki = %f\n\n", |
DimitriGruebel | 0:1447d2f773db | 607 | // y_kp, y_kd, y_ki); |
DimitriGruebel | 0:1447d2f773db | 608 | ios.printf("Aktuelle Werte fuer Regler Z-Achse: kp = %f kd = %f ki = %f\n\n", |
DimitriGruebel | 0:1447d2f773db | 609 | z_kp, z_kd, z_ki); |
DimitriGruebel | 0:1447d2f773db | 610 | } |
DimitriGruebel | 0:1447d2f773db | 611 | else if (choice4 == 2) |
DimitriGruebel | 0:1447d2f773db | 612 | { |
DimitriGruebel | 0:1447d2f773db | 613 | ios.printf("\n\nUm abzubrechen bitte Reset-Taste an mbed betaetigen!\n\n"); |
DimitriGruebel | 0:1447d2f773db | 614 | float ziel_x_winkel = data.Roll; |
DimitriGruebel | 0:1447d2f773db | 615 | float ziel_y_winkel = data.Pitch; |
DimitriGruebel | 0:1447d2f773db | 616 | float ziel_z_winkel = data.Yaw; |
DimitriGruebel | 0:1447d2f773db | 617 | |
DimitriGruebel | 0:1447d2f773db | 618 | print_gyro_angles(); |
DimitriGruebel | 0:1447d2f773db | 619 | float x_dyn_leistung_leerlauf = (min_leistung + max_leistung)/2; // Leistung im Leerlauf |
DimitriGruebel | 0:1447d2f773db | 620 | float y_dyn_leistung_leerlauf = (min_leistung + max_leistung)/2; // Leistung im Leerlauf |
DimitriGruebel | 0:1447d2f773db | 621 | float z_dyn_leistung_leerlauf = (min_leistung + max_leistung)/2; // Leistung im Leerlauf |
DimitriGruebel | 0:1447d2f773db | 622 | |
DimitriGruebel | 0:1447d2f773db | 623 | wait_ms(2000); |
DimitriGruebel | 0:1447d2f773db | 624 | z_kreisel.write(calcStellwert(z_dyn_leistung_leerlauf)); |
DimitriGruebel | 0:1447d2f773db | 625 | //y_kreisel.write(calcStellwert(y_dyn_leistung_leerlauf)); |
DimitriGruebel | 0:1447d2f773db | 626 | //x_kreisel.write(calcStellwert(x_dyn_leistung_leerlauf)); |
DimitriGruebel | 0:1447d2f773db | 627 | |
DimitriGruebel | 0:1447d2f773db | 628 | while(1) |
DimitriGruebel | 0:1447d2f773db | 629 | { |
DimitriGruebel | 0:1447d2f773db | 630 | while (fabs(ziel_z_winkel - data.Yaw) > regelgenauigkeit) |
DimitriGruebel | 0:1447d2f773db | 631 | { |
DimitriGruebel | 0:1447d2f773db | 632 | LageregelungAchseDyn(z_kp, z_kd, z_ki, ziel_z_winkel - data.Yaw, data.Gyro_Proc_Z, |
DimitriGruebel | 0:1447d2f773db | 633 | data.Yaw, z_kreisel, z_dyn_leistung_leerlauf); |
DimitriGruebel | 0:1447d2f773db | 634 | } |
DimitriGruebel | 0:1447d2f773db | 635 | |
DimitriGruebel | 0:1447d2f773db | 636 | /* |
DimitriGruebel | 0:1447d2f773db | 637 | while (fabs(ziel_y_winkel - data.Pitch) > regelgenauigkeit) |
DimitriGruebel | 0:1447d2f773db | 638 | { |
DimitriGruebel | 0:1447d2f773db | 639 | LageregelungAchseDyn(y_kp, y_kd, y_ki, ziel_y_winkel - data.Roll, data.Gyro_Proc_Y, |
DimitriGruebel | 0:1447d2f773db | 640 | data.Pitch, y_kreisel, y_dyn_leistung_leerlauf); |
DimitriGruebel | 0:1447d2f773db | 641 | } |
DimitriGruebel | 0:1447d2f773db | 642 | |
DimitriGruebel | 0:1447d2f773db | 643 | while (fabs(ziel_x_winkel - data.Roll) > regelgenauigkeit) |
DimitriGruebel | 0:1447d2f773db | 644 | { |
DimitriGruebel | 0:1447d2f773db | 645 | LageregelungAchseDyn(x_kp, x_kd, x_ki, ziel_x_winkel - data.Pitch, data.Gyro_Proc_X, |
DimitriGruebel | 0:1447d2f773db | 646 | data.Roll, x_kreisel, x_dyn_leistung_leerlauf); |
DimitriGruebel | 0:1447d2f773db | 647 | } |
DimitriGruebel | 0:1447d2f773db | 648 | */ |
DimitriGruebel | 0:1447d2f773db | 649 | } |
DimitriGruebel | 0:1447d2f773db | 650 | } |
DimitriGruebel | 0:1447d2f773db | 651 | }while(choice4 != 3); |
DimitriGruebel | 0:1447d2f773db | 652 | } |
DimitriGruebel | 0:1447d2f773db | 653 | else if (choice1 == 4) |
DimitriGruebel | 0:1447d2f773db | 654 | { |
DimitriGruebel | 0:1447d2f773db | 655 | print_gyro_data(); |
DimitriGruebel | 0:1447d2f773db | 656 | } |
DimitriGruebel | 0:1447d2f773db | 657 | |
DimitriGruebel | 0:1447d2f773db | 658 | }while(choice1 != 5); |
DimitriGruebel | 0:1447d2f773db | 659 | |
DimitriGruebel | 0:1447d2f773db | 660 | ios.printf("\n\nProgramm beendet.\n"); |
DimitriGruebel | 0:1447d2f773db | 661 | } |