Marco Friedmann / Mbed 2 deprecated 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.

Dependencies:   mbed TextLCD

Fork of Quadrocopter by Chris Elsholz

main.cpp@31:adfa5a816488, 2017-10-09 (annotated)

Committer:
MarcoF89
Date:
Mon Oct 09 08:57:22 2017 +0000
Revision:
31:adfa5a816488
Parent:
30:dc68b509f930 
PID

Who changed what in which revision?

UserRevisionLine numberNew contents of line
MarcoF89 13:5f0a2103c707 1 #include <Timer.h>
MarcoF89 13:5f0a2103c707 2 #include <math.h>
MarcoF89 15:742683a8efda 3 #include "mbed.h"
MarcoF89 15:742683a8efda 4 #include "stdio.h"
MarcoF89 13:5f0a2103c707 5 #include "deklaration.h"
MarcoF89 13:5f0a2103c707 6 #include "messen.h"
chriselsholz 18:78e7de067ddb 7 #include "filter/Kalman.h"
MarcoF89 25:a8a3cbc57c61 8
MarcoF89 29:3efe34986347 9 double gyro_pitch;
MarcoF89 29:3efe34986347 10 double gyro_yaw;
MarcoF89 29:3efe34986347 11 double gyro_roll;
MarcoF89 29:3efe34986347 12
MarcoF89 31:adfa5a816488 13 double e_pitch = 0;
MarcoF89 31:adfa5a816488 14 double w_pitch = 0;
MarcoF89 31:adfa5a816488 15
MarcoF89 31:adfa5a816488 16 double yalt_pitch = 0;
MarcoF89 31:adfa5a816488 17 double ealt_pitch = 0;
MarcoF89 31:adfa5a816488 18 double ealt2_pitch = 0;
MarcoF89 31:adfa5a816488 19 double q0_pitch = 0.1; //Kp + Ki * Ta + Kd/Ta
MarcoF89 31:adfa5a816488 20 double q1_pitch = 0; //-Ki - 2 * Kd/Ta
MarcoF89 31:adfa5a816488 21 double q2_pitch = 0; //Kd/Ta
MarcoF89 31:adfa5a816488 22
MarcoF89 31:adfa5a816488 23
MarcoF89 31:adfa5a816488 24
MarcoF89 31:adfa5a816488 25
MarcoF89 31:adfa5a816488 26
MarcoF89 15:742683a8efda 27 #define RAD 57.29577951
MarcoF89 25:a8a3cbc57c61 28
MarcoF89 6:27a09e8bebfb 29 int main()
MarcoF89 15:742683a8efda 30 {
MarcoF89 15:742683a8efda 31 z_off = 0;
MarcoF89 15:742683a8efda 32 drift_z = 0;
MarcoF89 29:3efe34986347 33 gyro_pitch = 0;
MarcoF89 29:3efe34986347 34 gyro_yaw = 0;
MarcoF89 29:3efe34986347 35 gyro_roll = 0;
MarcoF89 15:742683a8efda 36
MarcoF89 13:5f0a2103c707 37 Motor1.period_ms(2);
MarcoF89 11:8457b851e3e1 38 Motor2.period_ms(2);
MarcoF89 11:8457b851e3e1 39 Motor3.period_ms(2);
MarcoF89 11:8457b851e3e1 40 Motor4.period_ms(2);
MarcoF89 12:4a4dad7a3432 41 initialisierung_gyro();
chriselsholz 16:59d80bf88bf8 42 initialisierung_acc();
MarcoF89 25:a8a3cbc57c61 43 Kalman_pitch();
MarcoF89 25:a8a3cbc57c61 44 Kalman_yaw();
MarcoF89 25:a8a3cbc57c61 45 Kalman_roll();
MarcoF89 25:a8a3cbc57c61 46 aktuell_roh(&z_g, &x_g, &y_g, &z_a, &x_a, &y_a);
MarcoF89 25:a8a3cbc57c61 47 wait(1);
chriselsholz 23:c99a4bd60609 48 if (taster2)
chriselsholz 23:c99a4bd60609 49 {
chriselsholz 23:c99a4bd60609 50 viberationen(&rauschen, &Motor1, &Motor2, &Motor3, &Motor4, &taster4);
chriselsholz 23:c99a4bd60609 51 }
chriselsholz 23:c99a4bd60609 52 if (taster3)
chriselsholz 23:c99a4bd60609 53 {
chriselsholz 23:c99a4bd60609 54 anlernen(&Motor1, &Motor2, &Motor3, &Motor4, &taster1, &taster2, &taster4);
chriselsholz 23:c99a4bd60609 55 }
chriselsholz 23:c99a4bd60609 56
MarcoF89 27:67b388f6ac2e 57 pc.printf("Druecke Taster1 fuer den Start und Taster2 fuers rauschen\n\r");
chriselsholz 23:c99a4bd60609 58 n1=n2=n3=n4=700;
chriselsholz 23:c99a4bd60609 59 Motor1.pulsewidth_us(n1);
chriselsholz 23:c99a4bd60609 60 Motor2.pulsewidth_us(n2);
chriselsholz 23:c99a4bd60609 61 Motor3.pulsewidth_us(n3);
chriselsholz 23:c99a4bd60609 62 Motor4.pulsewidth_us(n4);
chriselsholz 23:c99a4bd60609 63
MarcoF89 13:5f0a2103c707 64 while(1)
MarcoF89 13:5f0a2103c707 65 {
chriselsholz 18:78e7de067ddb 66 if (taster1)
MarcoF89 11:8457b851e3e1 67 {
MarcoF89 25:a8a3cbc57c61 68 while(1)
MarcoF89 25:a8a3cbc57c61 69 {
MarcoF89 15:742683a8efda 70 pc.printf("\n\rOffset und Driftberechnung wird durchgefuehrt, halte die Drohne still");
MarcoF89 15:742683a8efda 71 offset_gyro(&z_off, &x_off, &y_off);
MarcoF89 24:aaa5b4703555 72 //drift_gyro(&drift_z, &drift_x, &drift_y, &timer, &timer2, &gain_g, &roll_g, &pitch_g, &z_off, &x_off, &y_off);
MarcoF89 15:742683a8efda 73 pc.printf("\n\rOffgesamt:\n\rZ = %3.5f\tY = %3.5f\tZ = %3.5f\t\n\r", z_off, x_off, y_off);
MarcoF89 15:742683a8efda 74 pc.printf("\n\rDrift:Z: %3.10f\tX: %3.10f\tY: %3.10f\n\r", drift_z, drift_x, drift_y);
MarcoF89 25:a8a3cbc57c61 75
MarcoF89 15:742683a8efda 76 timer.reset();
MarcoF89 15:742683a8efda 77 timer.start();
MarcoF89 27:67b388f6ac2e 78 timer2.reset();
MarcoF89 27:67b388f6ac2e 79 timer2.start();
MarcoF89 15:742683a8efda 80 int i = 0;
MarcoF89 15:742683a8efda 81 while(1)
MarcoF89 15:742683a8efda 82 {
MarcoF89 15:742683a8efda 83 i++;
MarcoF89 25:a8a3cbc57c61 84 dt = timer.read_us() * 0.000001; //Zeit zwischen zwei Messpunkten
MarcoF89 15:742683a8efda 85 timer.reset();
MarcoF89 25:a8a3cbc57c61 86 aktuell_roh(&z_g, &x_g, &y_g, &z_a, &x_a, &y_a); //Rohdaten einlesen
MarcoF89 25:a8a3cbc57c61 87
MarcoF89 25:a8a3cbc57c61 88 y = y_a / 16384.00; //Umwandlung in G-Kraft
MarcoF89 25:a8a3cbc57c61 89 x = x_a / 16384.00; //Umwandlung in G-Kraft
MarcoF89 25:a8a3cbc57c61 90 z = z_a / 16384.00; //Umwandlung in G-Kraft
MarcoF89 25:a8a3cbc57c61 91
MarcoF89 25:a8a3cbc57c61 92 newAngle_pitch = atan2(-x, z) * RAD; //Umwandlung der G-Kraft in °
MarcoF89 25:a8a3cbc57c61 93 newRate_pitch = ((y_g - y_off) * 1/16.4); //Offset subtrahiert +++ Umwandlung in °/s
MarcoF89 24:aaa5b4703555 94
MarcoF89 25:a8a3cbc57c61 95 newAngle_roll = atan2(y, sqrt(x * x + z * z)) * RAD; //Umwandlung der G-Kraft in °
MarcoF89 25:a8a3cbc57c61 96 newRate_roll = ((x_g - x_off) * 1/16.4); //Offset subtrahiert +++ Umwandlung in °/s
MarcoF89 25:a8a3cbc57c61 97
MarcoF89 25:a8a3cbc57c61 98 newAngle_yaw = ((z_g - z_off) * 1/16.4); //Umwandlung der G-Kraft in °
MarcoF89 25:a8a3cbc57c61 99 newRate_yaw = ((z_g - z_off) * 1/16.4); //Offset subtrahiert +++ Umwandlung in °/s
MarcoF89 25:a8a3cbc57c61 100
MarcoF89 25:a8a3cbc57c61 101 pitch = getPitch(&newAngle_pitch, &newRate_pitch, &dt);
MarcoF89 25:a8a3cbc57c61 102 yaw = getYaw(&newAngle_yaw, &newRate_yaw, &dt);
MarcoF89 25:a8a3cbc57c61 103 roll = getRoll(&newAngle_roll, &newRate_roll, &dt);
MarcoF89 25:a8a3cbc57c61 104
MarcoF89 27:67b388f6ac2e 105 if (i == 1000)
MarcoF89 15:742683a8efda 106 {
MarcoF89 27:67b388f6ac2e 107 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);
MarcoF89 15:742683a8efda 108 i = 0;
MarcoF89 15:742683a8efda 109 }
MarcoF89 25:a8a3cbc57c61 110 Motorsteurung(&Motor1, &Motor2, &Motor3, &Motor4, &taster2, &taster3, &taster4, &n1, &n2, &n3, &n4);
MarcoF89 15:742683a8efda 111 }
MarcoF89 11:8457b851e3e1 112 }
MarcoF89 11:8457b851e3e1 113 }
MarcoF89 27:67b388f6ac2e 114 if (taster2)
MarcoF89 27:67b388f6ac2e 115 {
MarcoF89 29:3efe34986347 116 pc.printf("\n\rOffset und Driftberechnung wird durchgefuehrt, halte die Drohne still");
chriselsholz 30:dc68b509f930 117 printf("\n\rpitch, yaw, roll, newAngle_pitch, newAngle_roll, gyro_pitch, gyro_yaw, gyro_roll, n2\n\r");
MarcoF89 27:67b388f6ac2e 118 while(1)
MarcoF89 27:67b388f6ac2e 119 {
MarcoF89 29:3efe34986347 120
MarcoF89 27:67b388f6ac2e 121 offset_gyro(&z_off, &x_off, &y_off);
MarcoF89 27:67b388f6ac2e 122 //drift_gyro(&drift_z, &drift_x, &drift_y, &timer, &timer2, &gain_g, &roll_g, &pitch_g, &z_off, &x_off, &y_off);
MarcoF89 29:3efe34986347 123 //pc.printf("\n\rOffgesamt:\n\rZ = %3.5f\tY = %3.5f\tZ = %3.5f\t\n\r", z_off, x_off, y_off);
MarcoF89 29:3efe34986347 124 //pc.printf("\n\rDrift:Z: %3.10f\tX: %3.10f\tY: %3.10f\n\r", drift_z, drift_x, drift_y);
MarcoF89 27:67b388f6ac2e 125
MarcoF89 27:67b388f6ac2e 126 timer.reset();
MarcoF89 27:67b388f6ac2e 127 timer.start();
MarcoF89 27:67b388f6ac2e 128 timer2.reset();
MarcoF89 27:67b388f6ac2e 129 timer2.start();
MarcoF89 27:67b388f6ac2e 130 int i = 0;
MarcoF89 27:67b388f6ac2e 131 while(1)
MarcoF89 27:67b388f6ac2e 132 {
MarcoF89 27:67b388f6ac2e 133 i++;
MarcoF89 27:67b388f6ac2e 134 dt = timer.read_us() * 0.000001; //Zeit zwischen zwei Messpunkten
MarcoF89 27:67b388f6ac2e 135 timer.reset();
MarcoF89 27:67b388f6ac2e 136 aktuell_roh(&z_g, &x_g, &y_g, &z_a, &x_a, &y_a); //Rohdaten einlesen
MarcoF89 27:67b388f6ac2e 137
MarcoF89 27:67b388f6ac2e 138 y = y_a / 16384.00; //Umwandlung in G-Kraft
MarcoF89 27:67b388f6ac2e 139 x = x_a / 16384.00; //Umwandlung in G-Kraft
MarcoF89 27:67b388f6ac2e 140 z = z_a / 16384.00; //Umwandlung in G-Kraft
MarcoF89 27:67b388f6ac2e 141
MarcoF89 27:67b388f6ac2e 142 newAngle_pitch = atan2(-x, z) * RAD; //Umwandlung der G-Kraft in °
MarcoF89 27:67b388f6ac2e 143 newRate_pitch = ((y_g - y_off) * 1/16.4); //Offset subtrahiert +++ Umwandlung in °/s
MarcoF89 27:67b388f6ac2e 144
MarcoF89 27:67b388f6ac2e 145 newAngle_roll = atan2(y, sqrt(x * x + z * z)) * RAD; //Umwandlung der G-Kraft in °
MarcoF89 27:67b388f6ac2e 146 newRate_roll = ((x_g - x_off) * 1/16.4); //Offset subtrahiert +++ Umwandlung in °/s
MarcoF89 27:67b388f6ac2e 147
MarcoF89 27:67b388f6ac2e 148 newAngle_yaw = ((z_g - z_off) * 1/16.4); //Umwandlung der G-Kraft in °
MarcoF89 27:67b388f6ac2e 149 newRate_yaw = ((z_g - z_off) * 1/16.4); //Offset subtrahiert +++ Umwandlung in °/s
MarcoF89 27:67b388f6ac2e 150
MarcoF89 27:67b388f6ac2e 151 pitch = getPitch(&newAngle_pitch, &newRate_pitch, &dt);
MarcoF89 27:67b388f6ac2e 152 yaw = getYaw(&newAngle_yaw, &newRate_yaw, &dt);
MarcoF89 27:67b388f6ac2e 153 roll = getRoll(&newAngle_roll, &newRate_roll, &dt);
MarcoF89 27:67b388f6ac2e 154
MarcoF89 29:3efe34986347 155 gyro_pitch += dt * newRate_pitch;
MarcoF89 29:3efe34986347 156 gyro_yaw += dt * newRate_yaw;
MarcoF89 29:3efe34986347 157 gyro_roll += dt * newRate_roll;
MarcoF89 29:3efe34986347 158
MarcoF89 29:3efe34986347 159 if (i == 500)
MarcoF89 27:67b388f6ac2e 160 {
chriselsholz 30:dc68b509f930 161 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);
MarcoF89 27:67b388f6ac2e 162 i = 0;
MarcoF89 27:67b388f6ac2e 163 }
MarcoF89 29:3efe34986347 164 if (timer2.read_ms() >= 5000)
MarcoF89 27:67b388f6ac2e 165 {
MarcoF89 27:67b388f6ac2e 166 n1+=200;
MarcoF89 27:67b388f6ac2e 167 n2+=200;
MarcoF89 27:67b388f6ac2e 168 n3+=200;
MarcoF89 27:67b388f6ac2e 169 n4+=200;
MarcoF89 27:67b388f6ac2e 170 Motor1.pulsewidth_us(n1);
MarcoF89 27:67b388f6ac2e 171 Motor2.pulsewidth_us(n2);
MarcoF89 27:67b388f6ac2e 172 Motor3.pulsewidth_us(n3);
MarcoF89 27:67b388f6ac2e 173 Motor4.pulsewidth_us(n4);
MarcoF89 27:67b388f6ac2e 174 timer2.reset();
MarcoF89 27:67b388f6ac2e 175 }
chriselsholz 28:f9349474a553 176 Motorsteurung(&Motor1, &Motor2, &Motor3, &Motor4, &taster2, &taster3, &taster4, &n1, &n2, &n3, &n4);
chriselsholz 30:dc68b509f930 177 if (n1>1501)
chriselsholz 28:f9349474a553 178 {
chriselsholz 28:f9349474a553 179 n1=n2=n3=n4=700;
chriselsholz 28:f9349474a553 180 Motor1.pulsewidth_us(n1);
chriselsholz 28:f9349474a553 181 Motor2.pulsewidth_us(n2);
chriselsholz 28:f9349474a553 182 Motor3.pulsewidth_us(n3);
chriselsholz 28:f9349474a553 183 Motor4.pulsewidth_us(n4);
chriselsholz 28:f9349474a553 184 while(1);
chriselsholz 28:f9349474a553 185 }
MarcoF89 27:67b388f6ac2e 186 }
MarcoF89 27:67b388f6ac2e 187 }
MarcoF89 27:67b388f6ac2e 188 }
MarcoF89 31:adfa5a816488 189 if (taster3) //Start Regelung
MarcoF89 31:adfa5a816488 190 {
MarcoF89 31:adfa5a816488 191 pc.printf("\n\rOffset und Driftberechnung wird durchgefuehrt, halte die Drohne still");
MarcoF89 31:adfa5a816488 192 printf("\n\rpitch, yaw, roll, newAngle_pitch, newAngle_roll, gyro_pitch, gyro_yaw, gyro_roll, n2\n\r");
MarcoF89 31:adfa5a816488 193 while(1)
MarcoF89 31:adfa5a816488 194 {
MarcoF89 31:adfa5a816488 195
MarcoF89 31:adfa5a816488 196 offset_gyro(&z_off, &x_off, &y_off);
MarcoF89 31:adfa5a816488 197
MarcoF89 31:adfa5a816488 198 int i = 0;
MarcoF89 31:adfa5a816488 199
MarcoF89 31:adfa5a816488 200 timer.reset();
MarcoF89 31:adfa5a816488 201 timer.start();
MarcoF89 31:adfa5a816488 202 timer2.reset();
MarcoF89 31:adfa5a816488 203 timer2.start();
MarcoF89 31:adfa5a816488 204
MarcoF89 31:adfa5a816488 205 n1 = n2 = n3 = n4 = 1200;
MarcoF89 31:adfa5a816488 206 Motor1.pulsewidth_us(n1);
MarcoF89 31:adfa5a816488 207 Motor2.pulsewidth_us(n2);
MarcoF89 31:adfa5a816488 208 Motor3.pulsewidth_us(n3);
MarcoF89 31:adfa5a816488 209 Motor4.pulsewidth_us(n4);
MarcoF89 31:adfa5a816488 210
MarcoF89 31:adfa5a816488 211 while(1)
MarcoF89 31:adfa5a816488 212 {
MarcoF89 31:adfa5a816488 213
MarcoF89 31:adfa5a816488 214 i++;
MarcoF89 31:adfa5a816488 215 dt = timer.read_us() * 0.000001; //Zeit zwischen zwei Messpunkten
MarcoF89 31:adfa5a816488 216 timer.reset();
MarcoF89 31:adfa5a816488 217 aktuell_roh(&z_g, &x_g, &y_g, &z_a, &x_a, &y_a); //Rohdaten einlesen
MarcoF89 31:adfa5a816488 218
MarcoF89 31:adfa5a816488 219 y = y_a / 16384.00; //Umwandlung in G-Kraft
MarcoF89 31:adfa5a816488 220 x = x_a / 16384.00; //Umwandlung in G-Kraft
MarcoF89 31:adfa5a816488 221 z = z_a / 16384.00; //Umwandlung in G-Kraft
MarcoF89 31:adfa5a816488 222
MarcoF89 31:adfa5a816488 223 newAngle_pitch = atan2(-x, z) * RAD; //Umwandlung der G-Kraft in °
MarcoF89 31:adfa5a816488 224 newRate_pitch = ((y_g - y_off) * 1/16.4); //Offset subtrahiert +++ Umwandlung in °/s
MarcoF89 31:adfa5a816488 225
MarcoF89 31:adfa5a816488 226 newAngle_roll = atan2(y, sqrt(x * x + z * z)) * RAD; //Umwandlung der G-Kraft in °
MarcoF89 31:adfa5a816488 227 newRate_roll = ((x_g - x_off) * 1/16.4); //Offset subtrahiert +++ Umwandlung in °/s
MarcoF89 31:adfa5a816488 228
MarcoF89 31:adfa5a816488 229 newAngle_yaw = ((z_g - z_off) * 1/16.4); //Umwandlung der G-Kraft in °
MarcoF89 31:adfa5a816488 230 newRate_yaw = ((z_g - z_off) * 1/16.4); //Offset subtrahiert +++ Umwandlung in °/s
MarcoF89 31:adfa5a816488 231
MarcoF89 31:adfa5a816488 232 pitch = getPitch(&newAngle_pitch, &newRate_pitch, &dt);
MarcoF89 31:adfa5a816488 233 yaw = getYaw(&newAngle_yaw, &newRate_yaw, &dt);
MarcoF89 31:adfa5a816488 234 roll = getRoll(&newAngle_roll, &newRate_roll, &dt);
MarcoF89 31:adfa5a816488 235
MarcoF89 31:adfa5a816488 236 e_pitch = w_pitch - pitch;
MarcoF89 31:adfa5a816488 237 y_pitch = yalt_pitch + q0_pitch * e_pitch + q1_pitch * ealt_pitch + q2_pitch * ealt2_pitch;
MarcoF89 31:adfa5a816488 238 ealt2_pitch = ealt_pitch;
MarcoF89 31:adfa5a816488 239 ealt_pitch = e_pitch;
MarcoF89 31:adfa5a816488 240 yalt_pitch = y_pitch;
MarcoF89 31:adfa5a816488 241
MarcoF89 31:adfa5a816488 242 n1 = n4 = y_pitch * 15.5 + 1200;
MarcoF89 31:adfa5a816488 243 n2 = n3 = y_pitch * -15.5 + 1200;
MarcoF89 31:adfa5a816488 244
MarcoF89 31:adfa5a816488 245 if (i == 500)
MarcoF89 31:adfa5a816488 246 {
MarcoF89 31:adfa5a816488 247 printf("%3.2f\t%d\t%d\t%3.2f\t\n\r", pitch, n1, n2, q0_pitch);
MarcoF89 31:adfa5a816488 248 i = 0;
MarcoF89 31:adfa5a816488 249 }
MarcoF89 31:adfa5a816488 250
MarcoF89 31:adfa5a816488 251 if (taster1)
MarcoF89 31:adfa5a816488 252 {
MarcoF89 31:adfa5a816488 253 q0_pitch += 0.1;
MarcoF89 31:adfa5a816488 254 }
MarcoF89 31:adfa5a816488 255 if (taster2)
MarcoF89 31:adfa5a816488 256 {
MarcoF89 31:adfa5a816488 257 q0_pitch -= 0.1;
MarcoF89 31:adfa5a816488 258 }
MarcoF89 31:adfa5a816488 259 if (taster3)
MarcoF89 31:adfa5a816488 260 {
MarcoF89 31:adfa5a816488 261 n1=n2=n3=n4=700;
MarcoF89 31:adfa5a816488 262 Motor1.pulsewidth_us(n1);
MarcoF89 31:adfa5a816488 263 Motor2.pulsewidth_us(n2);
MarcoF89 31:adfa5a816488 264 Motor3.pulsewidth_us(n3);
MarcoF89 31:adfa5a816488 265 Motor4.pulsewidth_us(n4);
MarcoF89 31:adfa5a816488 266 while(1);
MarcoF89 31:adfa5a816488 267 }
MarcoF89 31:adfa5a816488 268 }
MarcoF89 31:adfa5a816488 269 }
MarcoF89 31:adfa5a816488 270 }
MarcoF89 10:16ca5e9ee0dc 271 }
chriselsholz 30:dc68b509f930 272 }