Marco Friedmann
/
Quadrocopter2
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.
Fork of
Quadrocopter
by 
Chris Elsholz
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 #include <Timer.h> 00002 #include <math.h> 00003 #include "mbed.h" 00004 #include "stdio.h" 00005 #include "deklaration.h" 00006 #include "messen.h" 00007 #include "filter/Kalman.h" 00008 00009 double gyro_pitch; 00010 double gyro_yaw; 00011 double gyro_roll; 00012 00013 double e_pitch = 0; 00014 double w_pitch = 0; 00015 00016 double yalt_pitch = 0; 00017 double ealt_pitch = 0; 00018 double ealt2_pitch = 0; 00019 double q0_pitch = 0.1; //Kp + Ki * Ta + Kd/Ta 00020 double q1_pitch = 0; //-Ki - 2 * Kd/Ta 00021 double q2_pitch = 0; //Kd/Ta 00022 00023 00024 00025 00026 00027 #define RAD 57.29577951 00028 00029 int main() 00030 { 00031 z_off = 0; 00032 drift_z = 0; 00033 gyro_pitch = 0; 00034 gyro_yaw = 0; 00035 gyro_roll = 0; 00036 00037 Motor1.period_ms(2); 00038 Motor2.period_ms(2); 00039 Motor3.period_ms(2); 00040 Motor4.period_ms(2); 00041 initialisierung_gyro(); 00042 initialisierung_acc(); 00043 Kalman_pitch(); 00044 Kalman_yaw(); 00045 Kalman_roll(); 00046 aktuell_roh(&z_g, &x_g, &y_g, &z_a, &x_a, &y_a); 00047 wait(1); 00048 if (taster2) 00049 { 00050 viberationen(&rauschen, &Motor1, &Motor2, &Motor3, &Motor4, &taster4); 00051 } 00052 if (taster3) 00053 { 00054 anlernen(&Motor1, &Motor2, &Motor3, &Motor4, &taster1, &taster2, &taster4); 00055 } 00056 00057 pc.printf("Druecke Taster1 fuer den Start und Taster2 fuers rauschen\n\r"); 00058 n1=n2=n3=n4=700; 00059 Motor1.pulsewidth_us(n1); 00060 Motor2.pulsewidth_us(n2); 00061 Motor3.pulsewidth_us(n3); 00062 Motor4.pulsewidth_us(n4); 00063 00064 while(1) 00065 { 00066 if (taster1) 00067 { 00068 while(1) 00069 { 00070 pc.printf("\n\rOffset und Driftberechnung wird durchgefuehrt, halte die Drohne still"); 00071 offset_gyro(&z_off, &x_off, &y_off); 00072 //drift_gyro(&drift_z, &drift_x, &drift_y, &timer, &timer2, &gain_g, &roll_g, &pitch_g, &z_off, &x_off, &y_off); 00073 pc.printf("\n\rOffgesamt:\n\rZ = %3.5f\tY = %3.5f\tZ = %3.5f\t\n\r", z_off, x_off, y_off); 00074 pc.printf("\n\rDrift:Z: %3.10f\tX: %3.10f\tY: %3.10f\n\r", drift_z, drift_x, drift_y); 00075 00076 timer.reset(); 00077 timer.start(); 00078 timer2.reset(); 00079 timer2.start(); 00080 int i = 0; 00081 while(1) 00082 { 00083 i++; 00084 dt = timer.read_us() * 0.000001; //Zeit zwischen zwei Messpunkten 00085 timer.reset(); 00086 aktuell_roh(&z_g, &x_g, &y_g, &z_a, &x_a, &y_a); //Rohdaten einlesen 00087 00088 y = y_a / 16384.00; //Umwandlung in G-Kraft 00089 x = x_a / 16384.00; //Umwandlung in G-Kraft 00090 z = z_a / 16384.00; //Umwandlung in G-Kraft 00091 00092 newAngle_pitch = atan2(-x, z) * RAD; //Umwandlung der G-Kraft in ° 00093 newRate_pitch = ((y_g - y_off) * 1/16.4); //Offset subtrahiert +++ Umwandlung in °/s 00094 00095 newAngle_roll = atan2(y, sqrt(x * x + z * z)) * RAD; //Umwandlung der G-Kraft in ° 00096 newRate_roll = ((x_g - x_off) * 1/16.4); //Offset subtrahiert +++ Umwandlung in °/s 00097 00098 newAngle_yaw = ((z_g - z_off) * 1/16.4); //Umwandlung der G-Kraft in ° 00099 newRate_yaw = ((z_g - z_off) * 1/16.4); //Offset subtrahiert +++ Umwandlung in °/s 00100 00101 pitch = getPitch(&newAngle_pitch, &newRate_pitch, &dt); 00102 yaw = getYaw(&newAngle_yaw, &newRate_yaw, &dt); 00103 roll = getRoll(&newAngle_roll, &newRate_roll, &dt); 00104 00105 if (i == 1000) 00106 { 00107 printf("%f2.3 \t%f2.3 \t%f2.3 \t%f2.3 \t%f2.3 \t%f2.3 \t%fd \t", pitch, yaw, roll, newAngle_pitch, newRate_pitch, newAngle_roll, newRate_roll, newAngle_yaw, n1); 00108 i = 0; 00109 } 00110 Motorsteurung(&Motor1, &Motor2, &Motor3, &Motor4, &taster2, &taster3, &taster4, &n1, &n2, &n3, &n4); 00111 } 00112 } 00113 } 00114 if (taster2) 00115 { 00116 pc.printf("\n\rOffset und Driftberechnung wird durchgefuehrt, halte die Drohne still"); 00117 printf("\n\rpitch, yaw, roll, newAngle_pitch, newAngle_roll, gyro_pitch, gyro_yaw, gyro_roll, n2\n\r"); 00118 while(1) 00119 { 00120 00121 offset_gyro(&z_off, &x_off, &y_off); 00122 //drift_gyro(&drift_z, &drift_x, &drift_y, &timer, &timer2, &gain_g, &roll_g, &pitch_g, &z_off, &x_off, &y_off); 00123 //pc.printf("\n\rOffgesamt:\n\rZ = %3.5f\tY = %3.5f\tZ = %3.5f\t\n\r", z_off, x_off, y_off); 00124 //pc.printf("\n\rDrift:Z: %3.10f\tX: %3.10f\tY: %3.10f\n\r", drift_z, drift_x, drift_y); 00125 00126 timer.reset(); 00127 timer.start(); 00128 timer2.reset(); 00129 timer2.start(); 00130 int i = 0; 00131 while(1) 00132 { 00133 i++; 00134 dt = timer.read_us() * 0.000001; //Zeit zwischen zwei Messpunkten 00135 timer.reset(); 00136 aktuell_roh(&z_g, &x_g, &y_g, &z_a, &x_a, &y_a); //Rohdaten einlesen 00137 00138 y = y_a / 16384.00; //Umwandlung in G-Kraft 00139 x = x_a / 16384.00; //Umwandlung in G-Kraft 00140 z = z_a / 16384.00; //Umwandlung in G-Kraft 00141 00142 newAngle_pitch = atan2(-x, z) * RAD; //Umwandlung der G-Kraft in ° 00143 newRate_pitch = ((y_g - y_off) * 1/16.4); //Offset subtrahiert +++ Umwandlung in °/s 00144 00145 newAngle_roll = atan2(y, sqrt(x * x + z * z)) * RAD; //Umwandlung der G-Kraft in ° 00146 newRate_roll = ((x_g - x_off) * 1/16.4); //Offset subtrahiert +++ Umwandlung in °/s 00147 00148 newAngle_yaw = ((z_g - z_off) * 1/16.4); //Umwandlung der G-Kraft in ° 00149 newRate_yaw = ((z_g - z_off) * 1/16.4); //Offset subtrahiert +++ Umwandlung in °/s 00150 00151 pitch = getPitch(&newAngle_pitch, &newRate_pitch, &dt); 00152 yaw = getYaw(&newAngle_yaw, &newRate_yaw, &dt); 00153 roll = getRoll(&newAngle_roll, &newRate_roll, &dt); 00154 00155 gyro_pitch += dt * newRate_pitch; 00156 gyro_yaw += dt * newRate_yaw; 00157 gyro_roll += dt * newRate_roll; 00158 00159 if (i == 500) 00160 { 00161 printf(" %3.2f \t %3.2f \t %3.2f \t %3.2f \t\t %3.2f \t\t %3.2f \t\t %3.2f \t\t %3.2f \t\t %d\n\r", pitch, yaw, roll, newAngle_pitch, newAngle_roll, gyro_pitch, gyro_yaw, gyro_roll, n2); 00162 i = 0; 00163 } 00164 if (timer2.read_ms() >= 5000) 00165 { 00166 n1+=200; 00167 n2+=200; 00168 n3+=200; 00169 n4+=200; 00170 Motor1.pulsewidth_us(n1); 00171 Motor2.pulsewidth_us(n2); 00172 Motor3.pulsewidth_us(n3); 00173 Motor4.pulsewidth_us(n4); 00174 timer2.reset(); 00175 } 00176 Motorsteurung(&Motor1, &Motor2, &Motor3, &Motor4, &taster2, &taster3, &taster4, &n1, &n2, &n3, &n4); 00177 if (n1>1501) 00178 { 00179 n1=n2=n3=n4=700; 00180 Motor1.pulsewidth_us(n1); 00181 Motor2.pulsewidth_us(n2); 00182 Motor3.pulsewidth_us(n3); 00183 Motor4.pulsewidth_us(n4); 00184 while(1); 00185 } 00186 } 00187 } 00188 } 00189 if (taster3) //Start Regelung 00190 { 00191 pc.printf("\n\rOffset und Driftberechnung wird durchgefuehrt, halte die Drohne still"); 00192 printf("\n\rpitch, yaw, roll, newAngle_pitch, newAngle_roll, gyro_pitch, gyro_yaw, gyro_roll, n2\n\r"); 00193 while(1) 00194 { 00195 00196 offset_gyro(&z_off, &x_off, &y_off); 00197 00198 int i = 0; 00199 00200 timer.reset(); 00201 timer.start(); 00202 timer2.reset(); 00203 timer2.start(); 00204 00205 n1 = n2 = n3 = n4 = 1200; 00206 Motor1.pulsewidth_us(n1); 00207 Motor2.pulsewidth_us(n2); 00208 Motor3.pulsewidth_us(n3); 00209 Motor4.pulsewidth_us(n4); 00210 00211 while(1) 00212 { 00213 00214 i++; 00215 dt = timer.read_us() * 0.000001; //Zeit zwischen zwei Messpunkten 00216 timer.reset(); 00217 aktuell_roh(&z_g, &x_g, &y_g, &z_a, &x_a, &y_a); //Rohdaten einlesen 00218 00219 y = y_a / 16384.00; //Umwandlung in G-Kraft 00220 x = x_a / 16384.00; //Umwandlung in G-Kraft 00221 z = z_a / 16384.00; //Umwandlung in G-Kraft 00222 00223 newAngle_pitch = atan2(-x, z) * RAD; //Umwandlung der G-Kraft in ° 00224 newRate_pitch = ((y_g - y_off) * 1/16.4); //Offset subtrahiert +++ Umwandlung in °/s 00225 00226 newAngle_roll = atan2(y, sqrt(x * x + z * z)) * RAD; //Umwandlung der G-Kraft in ° 00227 newRate_roll = ((x_g - x_off) * 1/16.4); //Offset subtrahiert +++ Umwandlung in °/s 00228 00229 newAngle_yaw = ((z_g - z_off) * 1/16.4); //Umwandlung der G-Kraft in ° 00230 newRate_yaw = ((z_g - z_off) * 1/16.4); //Offset subtrahiert +++ Umwandlung in °/s 00231 00232 pitch = getPitch(&newAngle_pitch, &newRate_pitch, &dt); 00233 yaw = getYaw(&newAngle_yaw, &newRate_yaw, &dt); 00234 roll = getRoll(&newAngle_roll, &newRate_roll, &dt); 00235 00236 e_pitch = w_pitch - pitch; 00237 y_pitch = yalt_pitch + q0_pitch * e_pitch + q1_pitch * ealt_pitch + q2_pitch * ealt2_pitch; 00238 ealt2_pitch = ealt_pitch; 00239 ealt_pitch = e_pitch; 00240 yalt_pitch = y_pitch; 00241 00242 n1 = n4 = y_pitch * 15.5 + 1200; 00243 n2 = n3 = y_pitch * -15.5 + 1200; 00244 00245 if (i == 500) 00246 { 00247 printf("%3.2f\t%d\t%d\t%3.2f\t\n\r", pitch, n1, n2, q0_pitch); 00248 i = 0; 00249 } 00250 00251 if (taster1) 00252 { 00253 q0_pitch += 0.1; 00254 } 00255 if (taster2) 00256 { 00257 q0_pitch -= 0.1; 00258 } 00259 if (taster3) 00260 { 00261 n1=n2=n3=n4=700; 00262 Motor1.pulsewidth_us(n1); 00263 Motor2.pulsewidth_us(n2); 00264 Motor3.pulsewidth_us(n3); 00265 Motor4.pulsewidth_us(n4); 00266 while(1); 00267 } 00268 } 00269 } 00270 } 00271 } 00272 }
Generated on Sun Aug 14 2022 07:38:34 by
1.7.2