Dimitri Gruebel
Reaction Wheel Actuated Satellite Dynamics Test Platform
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