Dimitri Gruebel
/
Reaction Wheel Actuated Satellite Dynamics Test Platform
Featured
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 /////////////////////////////////////////////////////////////////////////////////////////////////// 00002 /////////////////////////////////////////////////////////////////////////////////////////////////// 00003 // Programm zu Lageregelung der Testplattform mit Hilfe von Reaktionskreiseln. 00004 // Mit UM6-LT IMU als Trägheitsplattform, XBee-Funkmodul zum seriellen Datenübertraggung und mbed 00005 // Mikrokontroller als Regelungs- und Stuerungscomputer. 00006 // Im Programm sind ein PID- und ein PD-Regler implementiert. 00007 // 00008 // Datum: 10.01.2014 Autor: Grübel Dimitri 00009 // 00010 /////////////////////////////////////////////////////////////////////////////////////////////////// 00011 00012 00013 #include "mbed.h" // MBED HEADER 00014 #include "MODSERIAL.h" // MBED BUFFERED SERIAL HEADER 00015 #include "UM6_usart.h" // UM6 USART HEADER 00016 #include "UM6_config.h" // UM6 CONFIG HEADER 00017 00018 // KOMMUNIKATIONSART MIT MBED: USB/XBEE 00019 Serial ios(p28, p27); // Serielle Verbi. mit XBee über Pin: tx-28, rx-27 00020 //Serial ios(USBTX, USBRX); // Serielle Verbi. über USB Port vom PC 00021 00022 DigitalOut rst(p11); // Digital Reset für the XBee, 200ns für reset 00023 PwmOut x_kreisel(p21); // Pin21-PwmOut ist für Drehung um X-Achse 00024 PwmOut y_kreisel(p23); // Pin23-PwmOut ist für Drehung um Y-Achse 00025 PwmOut z_kreisel(p22); // Pin22-PwmOut ist für Drehung um Z-Achse 00026 00027 // HIER WIRD PWM EINGESTELLT: 00028 const float pulsweite = 10.0; // Pulsweite des Steuersignals in ms 00029 const float pwwork = 2.0; // Pulsweite des Arbeitssignals in ms 00030 const float startpw = 0.8; // Startpulsweite in ms 00031 const float pwmfakt = (pwwork - startpw)/200.0; // Pulsweite pro 1% Leistung 00032 const float max_leistung = 75.0; // Leistungsbegrenzung global in % 00033 const float min_leistung = 10.0; // Notwendige Mindestleistung des Motors in % 00034 00035 const float x_kp_min = 0.0; // min/max - Variablen kp für X/Y/Z-Achse 00036 const float x_kp_max = 3.0; 00037 const float y_kp_min = 0.0; 00038 const float y_kp_max = 3.0; 00039 const float z_kp_min = 0.0; 00040 const float z_kp_max = 3.0; 00041 00042 const float x_kd_min = 0.0; // min/max - Variablen kd für X/Y/Z-Achse 00043 const float x_kd_max = 3.0; 00044 const float y_kd_min = 0.0; 00045 const float y_kd_max = 3.0; 00046 const float z_kd_min = 0.0; 00047 const float z_kd_max = 3.0; 00048 00049 const float x_ki_min = 0.0; // min/max - Variablen ki für X/Y/Z-Achse 00050 const float x_ki_max = 0.5; 00051 const float y_ki_min = 0.0; 00052 const float y_ki_max = 0.5; 00053 const float z_ki_min = 0.0; 00054 const float z_ki_max = 0.5; 00055 00056 const float x_winkel_min = -45.0; // Winkelbegrenzung für X/Y/Z-Achse 00057 const float x_winkel_max = 45.0; 00058 const float y_winkel_min = -45.0; 00059 const float y_winkel_max = 45.0; 00060 const float z_winkel_min = -90.0; 00061 const float z_winkel_max = 90.0; 00062 00063 const float regelgenauigkeit = 1.5; // Regelgenauigkeit in Grad 00064 00065 const float x_kp_def = 1.0; // DEFAULT REGELPARAMETER FÜR X/Y/Z-ACHSE 00066 const float x_kd_def = 0.4; 00067 const float x_ki_def = 0.01; 00068 const float y_kp_def = 1.0; 00069 const float y_kd_def = 0.4; 00070 const float y_ki_def = 0.01; 00071 const float z_kp_def = 1.0; 00072 const float z_kd_def = 0.4; 00073 const float z_ki_def = 0.01; 00074 00075 DigitalOut uart_activity(LED1); // LED1 = UM6 SERIAL für Kommunikation 00076 00077 00078 /// FUNKTIONEN //////////////////////////////////////////////////////////////////////////////////// 00079 /////////////////////////////////////////////////////////////////////////////////////////////////// 00080 00081 00082 /////////////////////////////////////////////////////////////////////////////////////////////////// 00083 // rxCallback // FUNKTION FÜR INTERRUPT - ABFRAGE DER DATEN DIE UM6-LT SENDET 00084 00085 void rxCallback(MODSERIAL_IRQ_INFO *q) 00086 { 00087 if (um6_uart.rxBufferGetCount() >= MAX_PACKET_DATA) 00088 { 00089 uart_activity = !uart_activity; // LED leuchtet wenn RxBuff hat > 40 Bytes 00090 Process_um6_packet(); 00091 } 00092 } 00093 00094 00095 /////////////////////////////////////////////////////////////////////////////////////////////////// 00096 // calcStellwert // FUNKTION ZUR STELLWERTBERECHNUNG FÜR MBED-PWM 00097 00098 float calcStellwert(float leistung) 00099 { 00100 return (startpw + ((leistung + 100) * pwmfakt)) / pulsweite; 00101 } 00102 00103 00104 /////////////////////////////////////////////////////////////////////////////////////////////////// 00105 // print_gyro_data // FUNKTION ZUR AUSGABE DER AKTUELLEN WINKEL UND WINKELGESCHWINDIGKEITEN 00106 00107 void print_gyro_data() 00108 { 00109 ios.printf("Gyro_Proc_X %+6.1f deg/s\n", data.Gyro_Proc_X); 00110 ios.printf("Gyro_Proc_Y %+6.1f deg/s\n", data.Gyro_Proc_Y); 00111 ios.printf("Gyro_Proc_Z %+6.1f deg/s\n", data.Gyro_Proc_Z); 00112 ios.printf("Roll %+6.1f deg\n", data.Roll); 00113 ios.printf("Pitch %+6.1f deg\n", data.Pitch); 00114 ios.printf("Yaw %+6.1f deg\n\n", data.Yaw); 00115 } 00116 00117 00118 /////////////////////////////////////////////////////////////////////////////////////////////////// 00119 // print_gyro_angles // FUNKTION ZUR AUSGABE DER AKTUELLEN WINKEL 00120 00121 void print_gyro_angles() 00122 { 00123 ios.printf("Roll %+6.1f deg\n", data.Roll); 00124 ios.printf("Pitch %+6.1f deg\n", data.Pitch); 00125 ios.printf("Yaw %+6.1f deg\n\n", data.Yaw); 00126 } 00127 00128 00129 /////////////////////////////////////////////////////////////////////////////////////////////////// 00130 // LageregelungAchse // FUNKTION ZUR LAGEREGELUNG FÜR EINE ACHSE - STATISCH - PD Regler 00131 00132 void LageregelungAchse(float kp, float kd, float winkel, const float& gyro_veloc, 00133 const float& gyro_angle, PwmOut& kreisel) 00134 { 00135 float start_angle = gyro_angle; 00136 float winkel_soll = gyro_angle + winkel; 00137 float abweichung = gyro_angle - winkel_soll; 00138 float leistung_alt = 0; // Variablen für Bremsverfahren 00139 bool start = true; 00140 00141 while (fabs(abweichung) > regelgenauigkeit) 00142 { 00143 float leistung = abweichung * kp - gyro_veloc * kd; 00144 if (leistung > max_leistung) leistung = max_leistung; 00145 if (leistung < -max_leistung) leistung = -max_leistung; 00146 if (leistung > -min_leistung && leistung < -0.001) leistung = -min_leistung; 00147 if (leistung < min_leistung && leistung > 0.001) leistung = min_leistung; 00148 00149 int Bremsrichtung = 0; // Vorzeichenbestimmung der Abbremsrichtung 00150 00151 if (leistung >= 0) 00152 Bremsrichtung = -1; 00153 else 00154 Bremsrichtung = 1; 00155 00156 float Stellwert = calcStellwert(leistung); // Berechnung Stellwert 00157 kreisel.write(Stellwert); // Befehl an Motorregler über PWM 00158 wait_ms(20); 00159 abweichung = gyro_angle - winkel_soll; // Kontrolle der Regelgenauigkeit 00160 00161 if (start == true) 00162 { 00163 leistung_alt = leistung; 00164 start = false; 00165 } 00166 else 00167 { 00168 if (fabs(winkel) > 5.0) 00169 { 00170 if ( fabs(leistung) > min_leistung) 00171 { 00172 float delta_leistung = fabs(leistung) - fabs(leistung_alt); // Bestimmung Abfall Delta 00173 if (delta_leistung < -0.001) 00174 { // Abbremsung des Motors bei Leistungsabnahme 00175 kreisel.write(calcStellwert(Bremsrichtung * min_leistung)); 00176 wait_ms(10); 00177 } 00178 } 00179 } 00180 00181 leistung_alt = leistung; 00182 } // Ende des Bremsverfahrens 00183 00184 if (fabs(abweichung) <= regelgenauigkeit) 00185 { // Vorzeichen für Bremsung wird bestimmt 00186 print_gyro_data(); // Funktionsaufruf Gyrodaten ausgeben 00187 kreisel.write(calcStellwert(Bremsrichtung * 100)); // Vollbremsung 00188 wait_ms(650); 00189 kreisel.write(calcStellwert(0)); // Motor aus 00190 } 00191 } 00192 00193 float end_angle = gyro_angle; 00194 ios.printf("Plattform wurde gedreht um %+6.1f deg\n\n", end_angle - start_angle); 00195 } 00196 00197 00198 /////////////////////////////////////////////////////////////////////////////////////////////////// 00199 // LageregelungAchseDyn // FUNKTION ZUR LAGEREGELUNG FÜR EINE ACHSE - DYNAMISCH - PID Regler 00200 00201 void LageregelungAchseDyn(float kp, float kd, float ki, float winkel, const float& gyro_veloc, 00202 const float& gyro_angle, PwmOut& kreisel, float& dyn_leistung_leerlauf) 00203 { 00204 00205 ios.printf("Start LageregelungAchseDyn\n"); 00206 float start_angle = gyro_angle; 00207 float winkel_soll = gyro_angle + winkel; 00208 float abweichung = gyro_angle - winkel_soll; 00209 float abweichung_sum = abweichung; 00210 float leistung_alt = dyn_leistung_leerlauf; 00211 bool start = true; 00212 float leistung = 0.0; 00213 float i_glied = 0.0; 00214 00215 int cnt = 0; 00216 while (fabs(abweichung) > regelgenauigkeit) 00217 { 00218 cnt += 1; 00219 i_glied = abweichung_sum * 0.001 * ki; 00220 ios.printf("%f\n", i_glied); 00221 if (i_glied > 0.5) i_glied = 0.5; // I-Glied Limiter 00222 if (i_glied < -0.5) i_glied = -0.5; 00223 // Sumierstelle (PID Glieder) 00224 leistung = dyn_leistung_leerlauf + abweichung * kp - gyro_veloc * kd + i_glied; 00225 if (leistung > max_leistung) leistung = max_leistung; 00226 if (leistung < min_leistung) leistung = min_leistung; 00227 int Bremsrichtung = -1; // Vorzeichen der Abbremsrichtung 00228 00229 float Stellwert = calcStellwert(leistung); // Berechnung Stellwert 00230 kreisel.write(Stellwert); // Befehl an Motorregler über Pwm 00231 wait_ms(10); 00232 00233 if (start == true) 00234 { 00235 leistung_alt = leistung; 00236 start = false; 00237 } 00238 /*else 00239 { 00240 if (leistung > min_leistung) 00241 { 00242 float delta_leistung = leistung - leistung_alt; // Bestimmung Abfall Delta 00243 00244 // bei Abnahme der Leistung - Motor abbremsen 00245 if (delta_leistung < -0.0001) 00246 { 00247 kreisel.write(calcStellwert(Bremsrichtung * min_leistung)); 00248 wait_ms(5); 00249 } 00250 } 00251 } 00252 */ 00253 leistung_alt = leistung; 00254 abweichung = gyro_angle - winkel_soll; 00255 abweichung_sum += abweichung; 00256 } 00257 00258 dyn_leistung_leerlauf = leistung - i_glied; 00259 if (dyn_leistung_leerlauf < min_leistung) dyn_leistung_leerlauf = min_leistung; 00260 if (dyn_leistung_leerlauf > max_leistung) dyn_leistung_leerlauf = max_leistung; 00261 ios.printf("Leerlaufleistung: %f\n", dyn_leistung_leerlauf); 00262 float end_angle = gyro_angle; 00263 ios.printf("Plattform wurde gedreht um %+6.1f deg\n\n", end_angle - start_angle); 00264 // Counter für durchlaufene Regelzyklen 00265 ios.printf("\nEnde LageregelungAchseDyn; Counter: %i\n\n", cnt); 00266 } 00267 00268 00269 /////////////////////////////////////////////////////////////////////////////////////////////////// 00270 // main // Hauptprogramm 00271 00272 int main() 00273 { 00274 ios.baud(115200); // Baudrate XBee Funkmodul 00275 um6_uart.baud(115200); // Baudrate UM6-lt 00276 um6_uart.attach(&rxCallback, MODSERIAL::RxIrq); // Interrupt Funktion für UART 00277 00278 rst = 1; // Reset-Pin von XBee auf ON 00279 00280 ios.printf("\nBitte warten, Startvorgang der Plattform...\n\n"); // Start-UP Prozedur 00281 um6_uart.putc(0xAC); // Nulliert die Rate-Gyros 00282 wait_ms(3500); 00283 um6_uart.putc(0xAC); // Nulliert die Rate-Gyros 00284 wait_ms(3500); 00285 um6_uart.putc(0xAC); // Nulliert die Rate-Gyros 00286 wait_ms(3500); 00287 00288 x_kreisel.period_ms(pulsweite); 00289 y_kreisel.period_ms(pulsweite); 00290 z_kreisel.period_ms(pulsweite); 00291 00292 ios.printf("\n\nTESTPLATTFORM ZUR SATELLITENLAGEREGELUNG MIT REAKTIONSKREISELN:\n\n"); 00293 00294 int choice1 = 0; 00295 00296 do 00297 { 00298 do 00299 { 00300 ios.printf("\n\nBitte folgende Anweisungen ausfuehren,falls nicht bereits erfolgt!\n" 00301 "Stromversorgung der Testplattform trennen und ohne Druckluft mit X-Achse auf mag." 00302 " Norden ausrichten.\n" 00303 "Testplattform mit Wasserwaage horizontal tarieren.\n" 00304 "Danach Stromversorgung herstellen und mbed Reset-Taste betaetigen.\n\n"); 00305 00306 x_kreisel.write(calcStellwert(0)); //Initialisierung X-Motorrelger 00307 y_kreisel.write(calcStellwert(0)); //Initialisierung Y-Motorrelger 00308 z_kreisel.write(calcStellwert(0)); //Initialisierung Z-Motorrelger 00309 00310 ios.printf("Menueauswahl:\n"); 00311 ios.printf("\t1 - Testplattform um einzelne Achsen drehen\n"); 00312 ios.printf("\t2 - Testplattform um drei Achsen drehen\n"); 00313 ios.printf("\t3 - Lage der Testplattform aktiv regeln fuer dynamische Fehler\n"); 00314 ios.printf("\t4 - Gyrodaten ausgeben [Winkel/Winkelgeschwindigkeiten]\n"); 00315 ios.printf("\t5 - Programm beenden\n"); 00316 ios.printf("\nEingabe >"); 00317 ios.scanf("%i", &choice1); 00318 }while (choice1 < 1 || choice1 > 5); 00319 00320 if (choice1 == 1) 00321 { 00322 ios.printf("\nTestplattform ist initialisiert.\n\n"); 00323 print_gyro_angles(); // Funktionsaufruf Gyrodwinkel ausgeben 00324 float x_kp = x_kp_def; 00325 float x_kd = x_kd_def; 00326 float x_winkel = 0.0; 00327 float y_kp = y_kp_def; 00328 float y_kd = y_kd_def; 00329 float y_winkel = 0.0; 00330 float z_kp = z_kp_def; 00331 float z_kd = z_kd_def; 00332 float z_winkel = 0.0; 00333 00334 int choice2 = 0; 00335 00336 do 00337 { 00338 do 00339 { 00340 ios.printf("\n\nLAGEREGELUNG EINZELNER ACHSEN:\n\n"); 00341 ios.printf("Menueauswahl:\n"); 00342 ios.printf("\t1 - Satellitenplattform rollen (um X-Achse drehen)\n"); 00343 ios.printf("\t2 - Satellitenplattform nicken (um Y-Achse drehen)\n"); 00344 ios.printf("\t3 - Satellitenplattform gieren (um Z-Achse drehen)\n"); 00345 ios.printf("\t4 - Auswahl abbrechen (esc).\n"); 00346 ios.printf("\nEingabe >"); 00347 ios.scanf("%i", &choice2); 00348 }while (choice2 < 1 || choice2 > 4); 00349 00350 if (choice2 == 1) 00351 { 00352 ios.printf("\n Reglerparameter kp eingeben [0 - +3] >"); 00353 ios.scanf("%f", &x_kp); 00354 if (x_kp > x_kp_max) x_kp = x_kp_max; // Begrenzung X-Achse kp Pos. 00355 if (x_kp < x_kp_min) x_kp = x_kp_min; // Begrenzung X-Achse kp Neg. 00356 ios.printf("\n Reglerparameter kd eingeben [0 - +3] >"); 00357 ios.scanf("%f", &x_kd); 00358 if (x_kd > x_kd_max) x_kd = x_kd_max; // Begrenzung X-Achse kd Pos. 00359 if (x_kd < x_kd_min) x_kd = x_kd_min; // Begrenzung X-Achse kd Neg. 00360 ios.printf("\n Rollwinkel Phi eingeben [-45 - +45] >"); 00361 ios.scanf("%f", &x_winkel); 00362 if (x_winkel > x_winkel_max) x_winkel = x_winkel_max; // Begrenzung X-Achse Winkel Pos. 00363 if (x_winkel < x_winkel_min) x_winkel = x_winkel_min; // Begrenzung X-Achse Winkel Neg. 00364 if (fabs(x_winkel) < regelgenauigkeit) ios.printf("\nRegelung ist nicht notwendig!"); 00365 ios.printf("\nParameter kp = %f, Parameter kd = %f, Rollwinkel = %f\n", 00366 x_kp, x_kd, x_winkel); 00367 print_gyro_angles(); 00368 LageregelungAchse(x_kp, x_kd, x_winkel, data.Gyro_Proc_X, data.Roll, x_kreisel); 00369 } 00370 else if (choice2 == 2) 00371 { 00372 ios.printf("\n Reglerparameter kp eingeben [0 - +3] >"); 00373 ios.scanf("%f", &y_kp); 00374 if (y_kp > y_kp_max) y_kp = y_kp_max; // Begrenzung Y-Achse kp Pos. 00375 if (y_kp < y_kp_min) y_kp = y_kp_min; // Begrenzung Y-Achse kp Neg. 00376 ios.printf("\n Reglerparameter kd eingeben [0 - +3] >"); 00377 ios.scanf("%f", &y_kd); 00378 if (y_kd > y_kd_max) y_kd = y_kd_max; // Begrenzung Y-Achse kd Pos. 00379 if (y_kd < y_kd_min) y_kd = y_kd_min; // Begrenzung Y-Achse kd Neg. 00380 ios.printf("\n Nickwinkel Theta eingeben [-45 - +45] >"); 00381 ios.scanf("%f", &y_winkel); 00382 if (y_winkel > y_winkel_max) y_winkel = y_winkel_max; // Begrenzung Y-Achse Winkel Pos. 00383 if (y_winkel < y_winkel_min) y_winkel = y_winkel_min; // Begrenzung Y-Achse Winkel Neg. 00384 if (fabs(y_winkel) < regelgenauigkeit) ios.printf("\nRegelung ist nicht notwendig!"); 00385 ios.printf("\nParameter kp = %f, Parameter kd = %f, Nickwinkel = %f\n", 00386 y_kp, y_kd, y_winkel); 00387 print_gyro_angles(); 00388 LageregelungAchse(y_kp, y_kd, y_winkel, data.Gyro_Proc_Y, data.Pitch, y_kreisel); 00389 00390 } 00391 else if (choice2 == 3) 00392 { 00393 ios.printf("\n Reglerparameter kp eingeben [0 - +3] >"); 00394 ios.scanf("%f", &z_kp); 00395 if (z_kp > z_kp_max) z_kp = z_kp_max; // Begrenzung Z-Achse kp Pos. 00396 if (z_kp < z_kp_min) z_kp = z_kp_min; // Begrenzung Z-Achse kp Neg. 00397 ios.printf("\n Reglerparameter kd eingeben [0 - +3] >"); 00398 ios.scanf("%f", &z_kd); 00399 if (z_kd > z_kd_max) z_kd = z_kd_max; // Begrenzung Z-Achse kd Pos. 00400 if (z_kd < z_kd_min) z_kd = z_kd_min; // Begrenzung Z-Achse kd Neg. 00401 ios.printf("\n Gierwinkel Psi eingeben [-90 - +90] >"); 00402 ios.scanf("%f", &z_winkel); 00403 if (z_winkel > z_winkel_max) z_winkel = z_winkel_max; // Begrenzung Z-Achse Winkel Pos. 00404 if (z_winkel < z_winkel_min) z_winkel = z_winkel_min; // Begrenzung Z-Achse Winkel Neg. 00405 if (fabs(z_winkel) < regelgenauigkeit) ios.printf("\nRegelung ist nicht notwendig!"); 00406 ios.printf("\nParameter kp = %f, Parameter kd = %f, Gierwinkel = %f\n", 00407 z_kp, z_kd, z_winkel); 00408 print_gyro_angles(); 00409 LageregelungAchse(z_kp, z_kd, z_winkel, data.Gyro_Proc_Z, data.Yaw, z_kreisel); 00410 } 00411 }while(choice2 != 4); 00412 00413 } 00414 else if (choice1 == 2) 00415 { 00416 float x_kp = x_kp_def; 00417 float x_kd = x_kd_def; 00418 float x_winkel = 0.0; 00419 float y_kp = y_kp_def; 00420 float y_kd = y_kd_def; 00421 float y_winkel = 0.0; 00422 float z_kp = z_kp_def; 00423 float z_kd = z_kd_def; 00424 float z_winkel = 0.0; 00425 00426 int choice3 = 0; 00427 00428 do 00429 { 00430 do 00431 { 00432 ios.printf("\n\nLAGEREGELUNG ALLER ACHSEN:\n\n"); 00433 ios.printf("Bitte Plattform in gewuenschte Ausgangsposition bringen.\n"); 00434 ios.printf("Plattform wird in Z-Y-X Reihenfolge gedreht!\n\n"); 00435 ios.printf("Aktuelle Werte fuer Regler X-Achse: kp = %f kd = %f\n\n", x_kp, x_kd); 00436 ios.printf("Aktuelle Werte fuer Regler Y-Achse: kp = %f kd = %f\n\n", y_kp, y_kd); 00437 ios.printf("Aktuelle Werte fuer Regler Z-Achse: kp = %f kd = %f\n\n", z_kp, z_kd); 00438 ios.printf("Menueauswahl:\n"); 00439 ios.printf("\t1 - Regelparameter aendern?\n"); 00440 ios.printf("\t2 - Plattform drehen-Winkel eingeben\n"); 00441 ios.printf("\t3 - Auswahl abbrechen (esc).\n"); 00442 ios.printf("\nEingabe >"); 00443 ios.scanf("%i", &choice3); 00444 }while (choice3 < 1 || choice3 > 3); 00445 00446 if (choice3 == 1) 00447 { 00448 ios.printf("\n Reglerparameter x_kp eingeben [0 - +3] >"); 00449 ios.scanf("%f", &x_kp); 00450 if (x_kp > x_kp_max) x_kp = x_kp_max; // Begrenzung X-Achse kp Pos. 00451 if (x_kp < x_kp_min) x_kp = x_kp_min; // Begrenzung X-Achse kp Neg. 00452 ios.printf("\n Reglerparameter x_kd eingeben [0 - +3] >"); 00453 ios.scanf("%f", &x_kd); 00454 if (x_kd > x_kd_max) x_kd = x_kd_max; // Begrenzung X-Achse kd Pos. 00455 if (x_kd < x_kd_min) x_kd = x_kd_min; // Begrenzung X-Achse kd Neg. 00456 ios.printf("\n Reglerparameter y_kp eingeben [0 - +3] >"); 00457 ios.scanf("%f", &y_kp); 00458 if (y_kp > y_kp_max) y_kp = y_kp_max; // Begrenzung Y-Achse kp Pos. 00459 if (y_kp < y_kp_min) y_kp = y_kp_min; // Begrenzung Y-Achse kp Neg. 00460 ios.printf("\n Reglerparameter y_kd eingeben [0 - +3] >"); 00461 ios.scanf("%f", &y_kd); 00462 if (y_kd > y_kd_max) y_kd = y_kd_max; // Begrenzung Y-Achse kd Pos. 00463 if (y_kd < y_kd_min) y_kd = y_kd_min; // Begrenzung Y-Achse kd Neg. 00464 ios.printf("\n Reglerparameter z_kp eingeben [0 - +3] >"); 00465 ios.scanf("%f", &z_kp); 00466 if (z_kp > z_kp_max) z_kp = z_kp_max; // Begrenzung Z-Achse kp Pos. 00467 if (z_kp < z_kp_min) z_kp = z_kp_min; // Begrenzung Z-Achse kp Neg. 00468 ios.printf("\n Reglerparameter z_kd eingeben [0 - +3] >"); 00469 ios.scanf("%f", &z_kd); 00470 if (z_kd > z_kd_max) z_kd = z_kd_max; // Begrenzung Z-Achse kd Pos. 00471 if (z_kd < z_kd_min) z_kd = z_kd_min; // Begrenzung Z-Achse kd Neg. 00472 ios.printf("\n\n"); 00473 ios.printf("Aktuelle Werte fuer Regler X-Achse: kp = %f kd = %f\n\n", x_kp, x_kd); 00474 ios.printf("Aktuelle Werte fuer Regler Y-Achse: kp = %f kd = %f\n\n", y_kp, y_kd); 00475 ios.printf("Aktuelle Werte fuer Regler Z-Achse: kp = %f kd = %f\n\n", z_kp, z_kd); 00476 } 00477 else if (choice3 == 2) 00478 { // Eingabe der Drehwinkel 00479 ios.printf("\n Gierwinkel Psi eingeben [-90 - +90] >"); 00480 ios.scanf("%f", &z_winkel); 00481 if (z_winkel > z_winkel_max) z_winkel = z_winkel_max; // Begrenzung Z-Achse Winkel Pos. 00482 if (z_winkel < z_winkel_min) z_winkel = z_winkel_min; // Begrenzung Z-Achse Winkel Neg. 00483 ios.printf("\n Nickwinkel Theta eingeben [-45 - +45] >"); 00484 ios.scanf("%f", &y_winkel); 00485 if (y_winkel > y_winkel_max) y_winkel = y_winkel_max; // Begrenzung Y-Achse Winkel Pos. 00486 if (y_winkel < y_winkel_min) y_winkel = y_winkel_min; // Begrenzung Y-Achse Winkel Neg. 00487 ios.printf("\n Rollwinkel Phi eingeben [-45 - +45] >"); 00488 ios.scanf("%f", &x_winkel); 00489 if (x_winkel > x_winkel_max) x_winkel = x_winkel_max; // Begrenzung X-Achse Winkel Pos. 00490 if (x_winkel < x_winkel_min) x_winkel = x_winkel_min; // Begrenzung X-Achse Winkel Neg. 00491 00492 if (fabs(z_winkel) < regelgenauigkeit && 00493 fabs(y_winkel) < regelgenauigkeit && 00494 fabs(x_winkel) < regelgenauigkeit) 00495 { 00496 ios.printf("\nRegelung ist nicht notwendig!"); 00497 } 00498 else 00499 { 00500 print_gyro_angles(); 00501 LageregelungAchse(z_kp, z_kd, z_winkel, data.Gyro_Proc_Z, data.Yaw, z_kreisel); 00502 wait_ms(250); 00503 LageregelungAchse(y_kp, y_kd, y_winkel, data.Gyro_Proc_Y, data.Pitch, y_kreisel); 00504 wait_ms(250); 00505 LageregelungAchse(x_kp, x_kd, x_winkel, data.Gyro_Proc_X, data.Roll, x_kreisel); 00506 print_gyro_angles(); 00507 ios.printf("\n\nSoll die Plattform wieder in ihre Ausgangslage gefahren werden? " 00508 "ja/nein\n"); 00509 char yes_no[50]; 00510 ios.scanf("%s", yes_no); 00511 00512 if (strcmp(yes_no, "ja") == 0) // Plattform in auf Ausgangsposition drehen 00513 { 00514 LageregelungAchse(x_kp, x_kd, -x_winkel, data.Gyro_Proc_X, data.Roll, x_kreisel); 00515 wait_ms(250); 00516 LageregelungAchse(y_kp, y_kd, -y_winkel, data.Gyro_Proc_Y, data.Pitch, y_kreisel); 00517 wait_ms(250); 00518 LageregelungAchse(z_kp, z_kd, -z_winkel, data.Gyro_Proc_Z, data.Yaw, z_kreisel); 00519 print_gyro_angles(); 00520 } 00521 } 00522 } 00523 00524 }while(choice3 != 3); 00525 } 00526 else if (choice1 == 3) 00527 { 00528 float x_kp = x_kp_def; 00529 float x_kd = x_kd_def; 00530 float x_ki = x_ki_def; 00531 float y_kp = y_kp_def; 00532 float y_kd = y_kd_def; 00533 float y_ki = y_ki_def; 00534 float z_kp = z_kp_def; 00535 float z_kd = z_kd_def; 00536 float z_ki = z_ki_def; 00537 00538 int choice4 = 0; 00539 00540 do 00541 { 00542 do 00543 { 00544 ios.printf("\n\nLage der Testplattform aktiv regeln fuer dynamische Fehler" 00545 " (Position halten):\n\n"); 00546 ios.printf("\n\nBitte Plattform in gewuenschte Ausgangsposition bringen und halten " 00547 "bis die Kreisel " 00548 "ihre Leerlaufdrehzahl erreicht haben.\n\n"); 00549 ios.printf("Aktuelle Werte fuer Regler X-Achse: kp = %f kd = %f ki = %f\n", 00550 x_kp, x_kd, x_ki); 00551 ios.printf("Aktuelle Werte fuer Regler Y-Achse: kp = %f kd = %f ki = %f\n", 00552 y_kp, y_kd, y_ki); 00553 ios.printf("Aktuelle Werte fuer Regler Z-Achse: kp = %f kd = %f ki = %f\n\n", 00554 z_kp, z_kd, z_ki); 00555 ios.printf("Menueauswahl:\n"); 00556 ios.printf("\t1 - Regelparameter aendern?\n"); 00557 ios.printf("\t2 - Plattform aktivieren\n"); 00558 ios.printf("\t3 - Auswahl abbrechen (esc).\n"); 00559 ios.printf("\nEingabe >"); 00560 ios.scanf("%i", &choice4); 00561 }while (choice4 < 1 || choice4 > 3); 00562 00563 if (choice4 == 1) 00564 { 00565 /* 00566 ios.printf("\n Reglerparameter x_kp eingeben [0 - +3] >"); 00567 ios.scanf("%f", &x_kp); 00568 if (x_kp > x_kp_max) x_kp = x_kp_max; // Begrenzung X-Achse kp Pos. 00569 if (x_kp < x_kp_min) x_kp = x_kp_min; // Begrenzung X-Achse kp Neg. 00570 ios.printf("\n Reglerparameter x_kd eingeben [0 - +3] >"); 00571 ios.scanf("%f", &x_kd); 00572 if (x_kd > x_kd_max) x_kd = x_kd_max; // Begrenzung X-Achse kd Pos. 00573 if (x_kd < x_kd_min) x_kd = x_kd_min; // Begrenzung X-Achse kd Neg. 00574 ios.printf("\n Reglerparameter x_ki eingeben [0 - +0.5] >"); 00575 ios.scanf("%f", &x_ki); 00576 if (x_ki > x_ki_max) x_ki = x_ki_max; // Begrenzung X-Achse ki Pos. 00577 if (x_ki < x_ki_min) x_ki = x_ki_min; // Begrenzung X-Achse ki Neg. 00578 ios.printf("\n Reglerparameter y_kp eingeben [0 - +3] >"); 00579 ios.scanf("%f", &y_kp); 00580 if (y_kp > y_kp_max) y_kp = y_kp_max; // Begrenzung Y-Achse kp Pos. 00581 if (y_kp < y_kp_min) y_kp = y_kp_min; // Begrenzung Y-Achse kp Neg. 00582 ios.printf("\n Reglerparameter y_kd eingeben [0 - +3] >"); 00583 ios.scanf("%f", &y_kd); 00584 if (y_kd > y_kd_max) y_kd = y_kd_max; // Begrenzung Y-Achse kd Pos. 00585 if (y_kd < y_kd_min) y_kd = y_kd_min; // Begrenzung Y-Achse kd Neg. 00586 ios.printf("\n Reglerparameter y_ki eingeben [0 - +0.5] >"); 00587 ios.scanf("%f", &y_ki); 00588 if (y_ki > y_ki_max) y_ki = y_ki_max; // Begrenzung Y-Achse ki Pos. 00589 if (y_ki < y_ki_min) y_ki = y_ki_min; // Begrenzung Y-Achse ki Neg. 00590 */ 00591 ios.printf("\n Reglerparameter z_kp eingeben [0 - +3] >"); 00592 ios.scanf("%f", &z_kp); 00593 if (z_kp > z_kp_max) z_kp = z_kp_max; // Begrenzung Z-Achse kp Pos. 00594 if (z_kp < z_kp_min) z_kp = z_kp_min; // Begrenzung Z-Achse kp Neg. 00595 ios.printf("\n Reglerparameter z_kd eingeben [0 - +3] >"); 00596 ios.scanf("%f", &z_kd); 00597 if (z_kd > z_kd_max) z_kd = z_kd_max; // Begrenzung Z-Achse kd Pos. 00598 if (z_kd < z_kd_min) z_kd = z_kd_min; // Begrenzung Z-Achse kd Neg. 00599 ios.printf("\n Reglerparameter z_ki eingeben [0 - +0.5] >"); 00600 ios.scanf("%f", &z_ki); 00601 if (z_ki > z_ki_max) z_ki = z_ki_max; // Begrenzung Z-Achse ki Pos. 00602 if (z_ki < z_ki_min) z_ki = z_ki_min; // Begrenzung Z-Achse ki Neg. 00603 ios.printf("\n\n"); 00604 //ios.printf("Aktuelle Werte fuer Regler X-Achse: kp = %f kd = %f ki = %f\n\n", 00605 // x_kp, x_kd, x_ki); 00606 //ios.printf("Aktuelle Werte fuer Regler Y-Achse: kp = %f kd = %f ki = %f\n\n", 00607 // y_kp, y_kd, y_ki); 00608 ios.printf("Aktuelle Werte fuer Regler Z-Achse: kp = %f kd = %f ki = %f\n\n", 00609 z_kp, z_kd, z_ki); 00610 } 00611 else if (choice4 == 2) 00612 { 00613 ios.printf("\n\nUm abzubrechen bitte Reset-Taste an mbed betaetigen!\n\n"); 00614 float ziel_x_winkel = data.Roll; 00615 float ziel_y_winkel = data.Pitch; 00616 float ziel_z_winkel = data.Yaw; 00617 00618 print_gyro_angles(); 00619 float x_dyn_leistung_leerlauf = (min_leistung + max_leistung)/2; // Leistung im Leerlauf 00620 float y_dyn_leistung_leerlauf = (min_leistung + max_leistung)/2; // Leistung im Leerlauf 00621 float z_dyn_leistung_leerlauf = (min_leistung + max_leistung)/2; // Leistung im Leerlauf 00622 00623 wait_ms(2000); 00624 z_kreisel.write(calcStellwert(z_dyn_leistung_leerlauf)); 00625 //y_kreisel.write(calcStellwert(y_dyn_leistung_leerlauf)); 00626 //x_kreisel.write(calcStellwert(x_dyn_leistung_leerlauf)); 00627 00628 while(1) 00629 { 00630 while (fabs(ziel_z_winkel - data.Yaw) > regelgenauigkeit) 00631 { 00632 LageregelungAchseDyn(z_kp, z_kd, z_ki, ziel_z_winkel - data.Yaw, data.Gyro_Proc_Z, 00633 data.Yaw, z_kreisel, z_dyn_leistung_leerlauf); 00634 } 00635 00636 /* 00637 while (fabs(ziel_y_winkel - data.Pitch) > regelgenauigkeit) 00638 { 00639 LageregelungAchseDyn(y_kp, y_kd, y_ki, ziel_y_winkel - data.Roll, data.Gyro_Proc_Y, 00640 data.Pitch, y_kreisel, y_dyn_leistung_leerlauf); 00641 } 00642 00643 while (fabs(ziel_x_winkel - data.Roll) > regelgenauigkeit) 00644 { 00645 LageregelungAchseDyn(x_kp, x_kd, x_ki, ziel_x_winkel - data.Pitch, data.Gyro_Proc_X, 00646 data.Roll, x_kreisel, x_dyn_leistung_leerlauf); 00647 } 00648 */ 00649 } 00650 } 00651 }while(choice4 != 3); 00652 } 00653 else if (choice1 == 4) 00654 { 00655 print_gyro_data(); 00656 } 00657 00658 }while(choice1 != 5); 00659 00660 ios.printf("\n\nProgramm beendet.\n"); 00661 }
Generated on Fri Jul 15 2022 05:24:10 by
1.7.2