Dimitri Gruebel / Mbed 2 deprecated Reaction Wheel Actuated Satellite Dynamics Test Platform Featured

Reaction Wheel Actuated Satellite Dynamics Test Platform

Dependencies:   mbed

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?

UserRevisionLine numberNew 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 }