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@24:aaa5b4703555, 2017-09-14 (annotated)

Committer:
MarcoF89
Date:
Thu Sep 14 16:41:02 2017 +0000
Revision:
24:aaa5b4703555
Parent:
23:c99a4bd60609
Child:
25:a8a3cbc57c61
Drehzahlerh?hung w?hrend Messschleife

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 15:742683a8efda 8
MarcoF89 15:742683a8efda 9 #define RAD 57.29577951
MarcoF89 15:742683a8efda 10
MarcoF89 21:f09823a13ac6 11 double dt;
MarcoF89 15:742683a8efda 12 uint32_t zeit2;
MarcoF89 15:742683a8efda 13
MarcoF89 15:742683a8efda 14 uint8_t k;
MarcoF89 15:742683a8efda 15
MarcoF89 19:ac98ad146067 16
MarcoF89 15:742683a8efda 17
MarcoF89 15:742683a8efda 18
MarcoF89 13:5f0a2103c707 19
MarcoF89 6:27a09e8bebfb 20 int main()
MarcoF89 15:742683a8efda 21 {
MarcoF89 15:742683a8efda 22 gain_g = 0;
MarcoF89 15:742683a8efda 23 z_off = 0;
MarcoF89 20:76e25a3c8684 24 dt = 0;
MarcoF89 15:742683a8efda 25 k = 0;
MarcoF89 15:742683a8efda 26 drift_z = 0;
MarcoF89 15:742683a8efda 27
MarcoF89 13:5f0a2103c707 28 Motor1.period_ms(2);
MarcoF89 11:8457b851e3e1 29 Motor2.period_ms(2);
MarcoF89 11:8457b851e3e1 30 Motor3.period_ms(2);
MarcoF89 11:8457b851e3e1 31 Motor4.period_ms(2);
MarcoF89 12:4a4dad7a3432 32 initialisierung_gyro();
chriselsholz 16:59d80bf88bf8 33 initialisierung_acc();
MarcoF89 19:ac98ad146067 34 Kalman(&Q_angle, &Q_bias, &R_measure, &bias, P);
chriselsholz 16:59d80bf88bf8 35
chriselsholz 16:59d80bf88bf8 36 if (taster1)
chriselsholz 16:59d80bf88bf8 37 {
chriselsholz 17:de729cb71f1e 38 uint16_t flanke1,hilfe1=0,flanke2,hilfe2=0,flanke3,hilfe3=0,flanke4,hilfe4=0;
chriselsholz 16:59d80bf88bf8 39 pc.printf("Taster-Modus aktiv\n\r");
chriselsholz 17:de729cb71f1e 40 n1=n2=n3=n4=625;
chriselsholz 16:59d80bf88bf8 41 while(1)
chriselsholz 16:59d80bf88bf8 42 {
chriselsholz 16:59d80bf88bf8 43 flanke2 = taster2;
chriselsholz 16:59d80bf88bf8 44 if ((flanke2 != 0) && (hilfe2 == 0))
chriselsholz 16:59d80bf88bf8 45 {
chriselsholz 17:de729cb71f1e 46 n1+=50;
chriselsholz 17:de729cb71f1e 47 n2+=50;
chriselsholz 17:de729cb71f1e 48 n3+=50;
chriselsholz 17:de729cb71f1e 49 n4+=50;
chriselsholz 16:59d80bf88bf8 50 }
chriselsholz 16:59d80bf88bf8 51 hilfe2=flanke2;
chriselsholz 16:59d80bf88bf8 52 flanke3 = taster3;
chriselsholz 16:59d80bf88bf8 53 if ((flanke3 != 0) && (hilfe3 == 0))
chriselsholz 16:59d80bf88bf8 54 {
chriselsholz 17:de729cb71f1e 55 n1-=50;
chriselsholz 17:de729cb71f1e 56 n2-=50;
chriselsholz 17:de729cb71f1e 57 n3-=50;
chriselsholz 17:de729cb71f1e 58 n4-=50;
chriselsholz 16:59d80bf88bf8 59 }
chriselsholz 16:59d80bf88bf8 60 hilfe3=flanke3;
chriselsholz 17:de729cb71f1e 61 flanke4 = taster4;
chriselsholz 17:de729cb71f1e 62 if ((flanke4 != 0) && (hilfe4 == 0))
chriselsholz 17:de729cb71f1e 63 {
MarcoF89 24:aaa5b4703555 64 n1=n2=n3=n4=650;
chriselsholz 16:59d80bf88bf8 65 }
chriselsholz 17:de729cb71f1e 66 hilfe4=flanke4;
chriselsholz 16:59d80bf88bf8 67 Motor1.pulsewidth_us(n1);
chriselsholz 16:59d80bf88bf8 68 Motor2.pulsewidth_us(n2);
chriselsholz 16:59d80bf88bf8 69 Motor3.pulsewidth_us(n3);
chriselsholz 18:78e7de067ddb 70 Motor4.pulsewidth_us(n4);
chriselsholz 18:78e7de067ddb 71
chriselsholz 17:de729cb71f1e 72 pc.printf("Drehzahl= %d\r= %d",n1);
chriselsholz 16:59d80bf88bf8 73 }
chriselsholz 23:c99a4bd60609 74 }
chriselsholz 16:59d80bf88bf8 75
chriselsholz 23:c99a4bd60609 76 if (taster2)
chriselsholz 23:c99a4bd60609 77 {
chriselsholz 23:c99a4bd60609 78 viberationen(&rauschen, &Motor1, &Motor2, &Motor3, &Motor4, &taster4);
chriselsholz 23:c99a4bd60609 79 }
chriselsholz 23:c99a4bd60609 80 if (taster3)
chriselsholz 23:c99a4bd60609 81 {
chriselsholz 23:c99a4bd60609 82 anlernen(&Motor1, &Motor2, &Motor3, &Motor4, &taster1, &taster2, &taster4);
chriselsholz 23:c99a4bd60609 83 }
chriselsholz 23:c99a4bd60609 84
chriselsholz 23:c99a4bd60609 85 pc.printf("Druecke Taster1 fuer den Start\n\r");
chriselsholz 23:c99a4bd60609 86 n1=n2=n3=n4=700;
chriselsholz 23:c99a4bd60609 87 Motor1.pulsewidth_us(n1);
chriselsholz 23:c99a4bd60609 88 Motor2.pulsewidth_us(n2);
chriselsholz 23:c99a4bd60609 89 Motor3.pulsewidth_us(n3);
chriselsholz 23:c99a4bd60609 90 Motor4.pulsewidth_us(n4);
chriselsholz 23:c99a4bd60609 91
MarcoF89 13:5f0a2103c707 92 while(1)
MarcoF89 13:5f0a2103c707 93 {
chriselsholz 18:78e7de067ddb 94 if (taster1)
MarcoF89 11:8457b851e3e1 95 {
MarcoF89 15:742683a8efda 96 while(1)//(!taster4)
MarcoF89 24:aaa5b4703555 97 {
MarcoF89 24:aaa5b4703555 98 uint16_t flanke1,hilfe1=0,flanke2,hilfe2=0,flanke3,hilfe3=0,flanke4,hilfe4=0;
MarcoF89 15:742683a8efda 99 pc.printf("\n\rOffset und Driftberechnung wird durchgefuehrt, halte die Drohne still");
MarcoF89 15:742683a8efda 100 offset_gyro(&z_off, &x_off, &y_off);
MarcoF89 24:aaa5b4703555 101 //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 102 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 103 pc.printf("\n\rDrift:Z: %3.10f\tX: %3.10f\tY: %3.10f\n\r", drift_z, drift_x, drift_y);
MarcoF89 15:742683a8efda 104
MarcoF89 15:742683a8efda 105 /********//******/
MarcoF89 15:742683a8efda 106 /*Messen*//*Gyro*/
MarcoF89 15:742683a8efda 107 /********//******/
MarcoF89 15:742683a8efda 108 timer.stop();
MarcoF89 15:742683a8efda 109 timer2.stop();
MarcoF89 15:742683a8efda 110 timer.reset();
MarcoF89 15:742683a8efda 111 timer2.reset();
MarcoF89 15:742683a8efda 112 gain_g = 0;
MarcoF89 15:742683a8efda 113 pitch_g = 0;
MarcoF89 15:742683a8efda 114 roll_g = 0;
MarcoF89 15:742683a8efda 115 timer.start();
MarcoF89 15:742683a8efda 116 timer2.start();
MarcoF89 15:742683a8efda 117 int i = 0;
MarcoF89 15:742683a8efda 118 while(1)
MarcoF89 15:742683a8efda 119 {
MarcoF89 15:742683a8efda 120 i++;
MarcoF89 24:aaa5b4703555 121 dt = timer.read_us() * 0.000001;
MarcoF89 15:742683a8efda 122 timer.reset();
MarcoF89 15:742683a8efda 123 aktuell_roh(&z_g, &x_g, &y_g, &z_a, &x_a, &y_a);
MarcoF89 21:f09823a13ac6 124 gain_g = ((z_g - z_off) * 1/16.4);
MarcoF89 21:f09823a13ac6 125 pitch_g = ((y_g - y_off) * 1/16.4);
MarcoF89 24:aaa5b4703555 126 roll_g = ((x_g - x_off) * 1/16.4);
MarcoF89 15:742683a8efda 127 y = y_a / 16384.00;
MarcoF89 15:742683a8efda 128 x = x_a / 16384.00;
MarcoF89 15:742683a8efda 129 z = z_a / 16384.00;
MarcoF89 15:742683a8efda 130 roll_a = atan2(y, sqrt(x * x + z * z)) * RAD;
MarcoF89 15:742683a8efda 131 pitch_a = atan2(-x, z) * RAD;
MarcoF89 24:aaa5b4703555 132 newAngle = pitch_a;
MarcoF89 24:aaa5b4703555 133 newRate = pitch_g;
MarcoF89 24:aaa5b4703555 134 /*if (timer2.read_ms() >= 2000)
MarcoF89 15:742683a8efda 135 {
MarcoF89 15:742683a8efda 136 gain_g -= drift_z;
MarcoF89 15:742683a8efda 137 pitch_g -= drift_y;
MarcoF89 15:742683a8efda 138 roll_g -= drift_x;
MarcoF89 15:742683a8efda 139 timer2.reset();
MarcoF89 24:aaa5b4703555 140 }*/
MarcoF89 20:76e25a3c8684 141 //gain = gain_g;
MarcoF89 20:76e25a3c8684 142 //pitch = pitch_g * 0.9 + pitch_a * 0.1;
MarcoF89 24:aaa5b4703555 143
MarcoF89 24:aaa5b4703555 144 pitch = getAngle(&newAngle, &newRate, &dt, &Q_angle, &Q_bias, &R_measure, &bias);
MarcoF89 15:742683a8efda 145 if (i == 2000)
MarcoF89 15:742683a8efda 146 {
MarcoF89 24:aaa5b4703555 147 pc.printf("roll: %2.5f°\tnewAngle: %2.5f°\tnewRate: %2.5f°/s\tdt: %2.5fus\tDrehzahl: %d\n\r",pitch, newAngle, newRate, dt*1000000,n1);
MarcoF89 15:742683a8efda 148 i = 0;
MarcoF89 15:742683a8efda 149 }
MarcoF89 24:aaa5b4703555 150 flanke2 = taster2;
MarcoF89 24:aaa5b4703555 151 if ((flanke2 != 0) && (hilfe2 == 0))
MarcoF89 24:aaa5b4703555 152 {
MarcoF89 24:aaa5b4703555 153 n1+=50;
MarcoF89 24:aaa5b4703555 154 n2+=50;
MarcoF89 24:aaa5b4703555 155 n3+=50;
MarcoF89 24:aaa5b4703555 156 n4+=50;
MarcoF89 24:aaa5b4703555 157 }
MarcoF89 24:aaa5b4703555 158 hilfe2=flanke2;
MarcoF89 24:aaa5b4703555 159 flanke3 = taster3;
MarcoF89 24:aaa5b4703555 160 if ((flanke3 != 0) && (hilfe3 == 0))
MarcoF89 24:aaa5b4703555 161 {
MarcoF89 24:aaa5b4703555 162 n1-=50;
MarcoF89 24:aaa5b4703555 163 n2-=50;
MarcoF89 24:aaa5b4703555 164 n3-=50;
MarcoF89 24:aaa5b4703555 165 n4-=50;
MarcoF89 24:aaa5b4703555 166 }
MarcoF89 24:aaa5b4703555 167 hilfe3=flanke3;
MarcoF89 24:aaa5b4703555 168 flanke4 = taster4;
MarcoF89 24:aaa5b4703555 169 if ((flanke4 != 0) && (hilfe4 == 0))
MarcoF89 24:aaa5b4703555 170 {
MarcoF89 24:aaa5b4703555 171 n1=n2=n3=n4=650;
MarcoF89 24:aaa5b4703555 172 }
MarcoF89 24:aaa5b4703555 173 hilfe4=flanke4;
chriselsholz 23:c99a4bd60609 174 Motor1.pulsewidth_us(n1);
chriselsholz 23:c99a4bd60609 175 Motor2.pulsewidth_us(n2);
chriselsholz 23:c99a4bd60609 176 Motor3.pulsewidth_us(n3);
MarcoF89 24:aaa5b4703555 177 Motor4.pulsewidth_us(n4);
chriselsholz 23:c99a4bd60609 178
MarcoF89 24:aaa5b4703555 179
MarcoF89 24:aaa5b4703555 180
MarcoF89 15:742683a8efda 181 }
MarcoF89 11:8457b851e3e1 182 }
MarcoF89 11:8457b851e3e1 183 }
MarcoF89 10:16ca5e9ee0dc 184 }
MarcoF89 13:5f0a2103c707 185 }