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@17:de729cb71f1e, 2017-09-11 (annotated)

Committer:
chriselsholz
Date:
Mon Sep 11 07:49:00 2017 +0000
Revision:
17:de729cb71f1e
Parent:
16:59d80bf88bf8
Child:
18:78e7de067ddb
Test-Programm auf Taster 1

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