Chris Elsholz / Mbed 2 deprecated Quadrocopter

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 Marco Friedmann

main.cpp@30:dc68b509f930, 2017-10-05 (annotated)

Committer:
chriselsholz
Date:
Thu Oct 05 18:02:40 2017 +0000
Revision:
30:dc68b509f930
Parent:
29:3efe34986347 
test;

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 15:742683a8efda 13 #define RAD 57.29577951
MarcoF89 25:a8a3cbc57c61 14
MarcoF89 6:27a09e8bebfb 15 int main()
MarcoF89 15:742683a8efda 16 {
MarcoF89 15:742683a8efda 17 z_off = 0;
MarcoF89 15:742683a8efda 18 drift_z = 0;
MarcoF89 29:3efe34986347 19 gyro_pitch = 0;
MarcoF89 29:3efe34986347 20 gyro_yaw = 0;
MarcoF89 29:3efe34986347 21 gyro_roll = 0;
MarcoF89 15:742683a8efda 22
MarcoF89 13:5f0a2103c707 23 Motor1.period_ms(2);
MarcoF89 11:8457b851e3e1 24 Motor2.period_ms(2);
MarcoF89 11:8457b851e3e1 25 Motor3.period_ms(2);
MarcoF89 11:8457b851e3e1 26 Motor4.period_ms(2);
MarcoF89 12:4a4dad7a3432 27 initialisierung_gyro();
chriselsholz 16:59d80bf88bf8 28 initialisierung_acc();
MarcoF89 25:a8a3cbc57c61 29 Kalman_pitch();
MarcoF89 25:a8a3cbc57c61 30 Kalman_yaw();
MarcoF89 25:a8a3cbc57c61 31 Kalman_roll();
MarcoF89 25:a8a3cbc57c61 32 aktuell_roh(&z_g, &x_g, &y_g, &z_a, &x_a, &y_a);
MarcoF89 25:a8a3cbc57c61 33 wait(1);
chriselsholz 23:c99a4bd60609 34 if (taster2)
chriselsholz 23:c99a4bd60609 35 {
chriselsholz 23:c99a4bd60609 36 viberationen(&rauschen, &Motor1, &Motor2, &Motor3, &Motor4, &taster4);
chriselsholz 23:c99a4bd60609 37 }
chriselsholz 23:c99a4bd60609 38 if (taster3)
chriselsholz 23:c99a4bd60609 39 {
chriselsholz 23:c99a4bd60609 40 anlernen(&Motor1, &Motor2, &Motor3, &Motor4, &taster1, &taster2, &taster4);
chriselsholz 23:c99a4bd60609 41 }
chriselsholz 23:c99a4bd60609 42
MarcoF89 27:67b388f6ac2e 43 pc.printf("Druecke Taster1 fuer den Start und Taster2 fuers rauschen\n\r");
chriselsholz 23:c99a4bd60609 44 n1=n2=n3=n4=700;
chriselsholz 23:c99a4bd60609 45 Motor1.pulsewidth_us(n1);
chriselsholz 23:c99a4bd60609 46 Motor2.pulsewidth_us(n2);
chriselsholz 23:c99a4bd60609 47 Motor3.pulsewidth_us(n3);
chriselsholz 23:c99a4bd60609 48 Motor4.pulsewidth_us(n4);
chriselsholz 23:c99a4bd60609 49
MarcoF89 13:5f0a2103c707 50 while(1)
MarcoF89 13:5f0a2103c707 51 {
chriselsholz 18:78e7de067ddb 52 if (taster1)
MarcoF89 11:8457b851e3e1 53 {
MarcoF89 25:a8a3cbc57c61 54 while(1)
MarcoF89 25:a8a3cbc57c61 55 {
MarcoF89 15:742683a8efda 56 pc.printf("\n\rOffset und Driftberechnung wird durchgefuehrt, halte die Drohne still");
MarcoF89 15:742683a8efda 57 offset_gyro(&z_off, &x_off, &y_off);
MarcoF89 24:aaa5b4703555 58 //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 59 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 60 pc.printf("\n\rDrift:Z: %3.10f\tX: %3.10f\tY: %3.10f\n\r", drift_z, drift_x, drift_y);
MarcoF89 25:a8a3cbc57c61 61
MarcoF89 15:742683a8efda 62 timer.reset();
MarcoF89 15:742683a8efda 63 timer.start();
MarcoF89 27:67b388f6ac2e 64 timer2.reset();
MarcoF89 27:67b388f6ac2e 65 timer2.start();
MarcoF89 15:742683a8efda 66 int i = 0;
MarcoF89 15:742683a8efda 67 while(1)
MarcoF89 15:742683a8efda 68 {
MarcoF89 15:742683a8efda 69 i++;
MarcoF89 25:a8a3cbc57c61 70 dt = timer.read_us() * 0.000001; //Zeit zwischen zwei Messpunkten
MarcoF89 15:742683a8efda 71 timer.reset();
MarcoF89 25:a8a3cbc57c61 72 aktuell_roh(&z_g, &x_g, &y_g, &z_a, &x_a, &y_a); //Rohdaten einlesen
MarcoF89 25:a8a3cbc57c61 73
MarcoF89 25:a8a3cbc57c61 74 y = y_a / 16384.00; //Umwandlung in G-Kraft
MarcoF89 25:a8a3cbc57c61 75 x = x_a / 16384.00; //Umwandlung in G-Kraft
MarcoF89 25:a8a3cbc57c61 76 z = z_a / 16384.00; //Umwandlung in G-Kraft
MarcoF89 25:a8a3cbc57c61 77
MarcoF89 25:a8a3cbc57c61 78 newAngle_pitch = atan2(-x, z) * RAD; //Umwandlung der G-Kraft in °
MarcoF89 25:a8a3cbc57c61 79 newRate_pitch = ((y_g - y_off) * 1/16.4); //Offset subtrahiert +++ Umwandlung in °/s
MarcoF89 24:aaa5b4703555 80
MarcoF89 25:a8a3cbc57c61 81 newAngle_roll = atan2(y, sqrt(x * x + z * z)) * RAD; //Umwandlung der G-Kraft in °
MarcoF89 25:a8a3cbc57c61 82 newRate_roll = ((x_g - x_off) * 1/16.4); //Offset subtrahiert +++ Umwandlung in °/s
MarcoF89 25:a8a3cbc57c61 83
MarcoF89 25:a8a3cbc57c61 84 newAngle_yaw = ((z_g - z_off) * 1/16.4); //Umwandlung der G-Kraft in °
MarcoF89 25:a8a3cbc57c61 85 newRate_yaw = ((z_g - z_off) * 1/16.4); //Offset subtrahiert +++ Umwandlung in °/s
MarcoF89 25:a8a3cbc57c61 86
MarcoF89 25:a8a3cbc57c61 87 pitch = getPitch(&newAngle_pitch, &newRate_pitch, &dt);
MarcoF89 25:a8a3cbc57c61 88 yaw = getYaw(&newAngle_yaw, &newRate_yaw, &dt);
MarcoF89 25:a8a3cbc57c61 89 roll = getRoll(&newAngle_roll, &newRate_roll, &dt);
MarcoF89 25:a8a3cbc57c61 90
MarcoF89 27:67b388f6ac2e 91 if (i == 1000)
MarcoF89 15:742683a8efda 92 {
MarcoF89 27:67b388f6ac2e 93 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 94 i = 0;
MarcoF89 15:742683a8efda 95 }
MarcoF89 25:a8a3cbc57c61 96 Motorsteurung(&Motor1, &Motor2, &Motor3, &Motor4, &taster2, &taster3, &taster4, &n1, &n2, &n3, &n4);
MarcoF89 15:742683a8efda 97 }
MarcoF89 11:8457b851e3e1 98 }
MarcoF89 11:8457b851e3e1 99 }
MarcoF89 27:67b388f6ac2e 100 if (taster2)
MarcoF89 27:67b388f6ac2e 101 {
MarcoF89 29:3efe34986347 102 pc.printf("\n\rOffset und Driftberechnung wird durchgefuehrt, halte die Drohne still");
chriselsholz 30:dc68b509f930 103 printf("\n\rpitch, yaw, roll, newAngle_pitch, newAngle_roll, gyro_pitch, gyro_yaw, gyro_roll, n2\n\r");
MarcoF89 27:67b388f6ac2e 104 while(1)
MarcoF89 27:67b388f6ac2e 105 {
MarcoF89 29:3efe34986347 106
MarcoF89 27:67b388f6ac2e 107 offset_gyro(&z_off, &x_off, &y_off);
MarcoF89 27:67b388f6ac2e 108 //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 109 //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 110 //pc.printf("\n\rDrift:Z: %3.10f\tX: %3.10f\tY: %3.10f\n\r", drift_z, drift_x, drift_y);
MarcoF89 27:67b388f6ac2e 111
MarcoF89 27:67b388f6ac2e 112 timer.reset();
MarcoF89 27:67b388f6ac2e 113 timer.start();
MarcoF89 27:67b388f6ac2e 114 timer2.reset();
MarcoF89 27:67b388f6ac2e 115 timer2.start();
MarcoF89 27:67b388f6ac2e 116 int i = 0;
MarcoF89 27:67b388f6ac2e 117 while(1)
MarcoF89 27:67b388f6ac2e 118 {
MarcoF89 27:67b388f6ac2e 119 i++;
MarcoF89 27:67b388f6ac2e 120 dt = timer.read_us() * 0.000001; //Zeit zwischen zwei Messpunkten
MarcoF89 27:67b388f6ac2e 121 timer.reset();
MarcoF89 27:67b388f6ac2e 122 aktuell_roh(&z_g, &x_g, &y_g, &z_a, &x_a, &y_a); //Rohdaten einlesen
MarcoF89 27:67b388f6ac2e 123
MarcoF89 27:67b388f6ac2e 124 y = y_a / 16384.00; //Umwandlung in G-Kraft
MarcoF89 27:67b388f6ac2e 125 x = x_a / 16384.00; //Umwandlung in G-Kraft
MarcoF89 27:67b388f6ac2e 126 z = z_a / 16384.00; //Umwandlung in G-Kraft
MarcoF89 27:67b388f6ac2e 127
MarcoF89 27:67b388f6ac2e 128 newAngle_pitch = atan2(-x, z) * RAD; //Umwandlung der G-Kraft in °
MarcoF89 27:67b388f6ac2e 129 newRate_pitch = ((y_g - y_off) * 1/16.4); //Offset subtrahiert +++ Umwandlung in °/s
MarcoF89 27:67b388f6ac2e 130
MarcoF89 27:67b388f6ac2e 131 newAngle_roll = atan2(y, sqrt(x * x + z * z)) * RAD; //Umwandlung der G-Kraft in °
MarcoF89 27:67b388f6ac2e 132 newRate_roll = ((x_g - x_off) * 1/16.4); //Offset subtrahiert +++ Umwandlung in °/s
MarcoF89 27:67b388f6ac2e 133
MarcoF89 27:67b388f6ac2e 134 newAngle_yaw = ((z_g - z_off) * 1/16.4); //Umwandlung der G-Kraft in °
MarcoF89 27:67b388f6ac2e 135 newRate_yaw = ((z_g - z_off) * 1/16.4); //Offset subtrahiert +++ Umwandlung in °/s
MarcoF89 27:67b388f6ac2e 136
MarcoF89 27:67b388f6ac2e 137 pitch = getPitch(&newAngle_pitch, &newRate_pitch, &dt);
MarcoF89 27:67b388f6ac2e 138 yaw = getYaw(&newAngle_yaw, &newRate_yaw, &dt);
MarcoF89 27:67b388f6ac2e 139 roll = getRoll(&newAngle_roll, &newRate_roll, &dt);
MarcoF89 27:67b388f6ac2e 140
MarcoF89 29:3efe34986347 141 gyro_pitch += dt * newRate_pitch;
MarcoF89 29:3efe34986347 142 gyro_yaw += dt * newRate_yaw;
MarcoF89 29:3efe34986347 143 gyro_roll += dt * newRate_roll;
MarcoF89 29:3efe34986347 144
MarcoF89 29:3efe34986347 145 if (i == 500)
MarcoF89 27:67b388f6ac2e 146 {
chriselsholz 30:dc68b509f930 147 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 148 i = 0;
MarcoF89 27:67b388f6ac2e 149 }
MarcoF89 29:3efe34986347 150 if (timer2.read_ms() >= 5000)
MarcoF89 27:67b388f6ac2e 151 {
MarcoF89 27:67b388f6ac2e 152 n1+=200;
MarcoF89 27:67b388f6ac2e 153 n2+=200;
MarcoF89 27:67b388f6ac2e 154 n3+=200;
MarcoF89 27:67b388f6ac2e 155 n4+=200;
MarcoF89 27:67b388f6ac2e 156 Motor1.pulsewidth_us(n1);
MarcoF89 27:67b388f6ac2e 157 Motor2.pulsewidth_us(n2);
MarcoF89 27:67b388f6ac2e 158 Motor3.pulsewidth_us(n3);
MarcoF89 27:67b388f6ac2e 159 Motor4.pulsewidth_us(n4);
MarcoF89 27:67b388f6ac2e 160 timer2.reset();
MarcoF89 27:67b388f6ac2e 161 }
chriselsholz 28:f9349474a553 162 Motorsteurung(&Motor1, &Motor2, &Motor3, &Motor4, &taster2, &taster3, &taster4, &n1, &n2, &n3, &n4);
chriselsholz 30:dc68b509f930 163 if (n1>1501)
chriselsholz 28:f9349474a553 164 {
chriselsholz 28:f9349474a553 165 n1=n2=n3=n4=700;
chriselsholz 28:f9349474a553 166 Motor1.pulsewidth_us(n1);
chriselsholz 28:f9349474a553 167 Motor2.pulsewidth_us(n2);
chriselsholz 28:f9349474a553 168 Motor3.pulsewidth_us(n3);
chriselsholz 28:f9349474a553 169 Motor4.pulsewidth_us(n4);
chriselsholz 28:f9349474a553 170 while(1);
chriselsholz 28:f9349474a553 171 }
MarcoF89 27:67b388f6ac2e 172 }
MarcoF89 27:67b388f6ac2e 173 }
MarcoF89 27:67b388f6ac2e 174 }
MarcoF89 10:16ca5e9ee0dc 175 }
chriselsholz 30:dc68b509f930 176 }