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@12:4a4dad7a3432, 2017-08-16 (annotated)

Committer:
MarcoF89
Date:
Wed Aug 16 09:45:50 2017 +0000
Revision:
12:4a4dad7a3432
Parent:
11:8457b851e3e1
Child:
13:5f0a2103c707
Taste zwei beim einschalten gedr?ckt halten --->

Who changed what in which revision?

UserRevisionLine numberNew contents of line
MarcoF89 6:27a09e8bebfb 1 #include "mbed.h"
MarcoF89 6:27a09e8bebfb 2 #include "stdio.h"
MarcoF89 6:27a09e8bebfb 3 #include <Timer.h>
MarcoF89 6:27a09e8bebfb 4 #include "messen.h"
MarcoF89 12:4a4dad7a3432 5 #include <math.h>
MarcoF89 7:a54c97795013 6
MarcoF89 6:27a09e8bebfb 7 static Serial pc(SERIAL_TX, SERIAL_RX);
MarcoF89 6:27a09e8bebfb 8 static SPI spi(PE_6,PE_5,PE_2); //mosi,miso,sclk
MarcoF89 6:27a09e8bebfb 9 static DigitalOut ncs(PE_4); //ssel
MarcoF89 6:27a09e8bebfb 10
MarcoF89 11:8457b851e3e1 11 static AnalogIn poti_1(PF_3);
MarcoF89 11:8457b851e3e1 12 static AnalogIn poti_2(PF_10);
MarcoF89 11:8457b851e3e1 13 static AnalogIn poti_3(PF_4);
MarcoF89 11:8457b851e3e1 14 static AnalogIn poti_4(PF_5);
MarcoF89 7:a54c97795013 15
MarcoF89 12:4a4dad7a3432 16
MarcoF89 6:27a09e8bebfb 17
MarcoF89 8:769ff3814355 18 static DigitalOut db0(PC_8);
MarcoF89 8:769ff3814355 19 static DigitalOut db1(PC_9);
MarcoF89 8:769ff3814355 20 static DigitalOut db2(PC_10);
MarcoF89 8:769ff3814355 21 static DigitalOut db3(PC_11);
MarcoF89 8:769ff3814355 22 static DigitalOut db4(PC_12);
MarcoF89 8:769ff3814355 23 static DigitalOut db5(PC_13);
MarcoF89 8:769ff3814355 24 static DigitalOut db6(PC_14);
MarcoF89 8:769ff3814355 25 static DigitalOut db7(PC_15);
MarcoF89 8:769ff3814355 26
MarcoF89 12:4a4dad7a3432 27 static PwmOut Motor1 (PC_8); // Schwarz QBRAIN: rot
MarcoF89 12:4a4dad7a3432 28 static PwmOut Motor2 (PC_9); // Weiß orange
MarcoF89 12:4a4dad7a3432 29 static PwmOut Motor3 (PC_6); // Grau weiß
MarcoF89 12:4a4dad7a3432 30 static PwmOut Motor4 (PB_9); // Blau braun
MarcoF89 12:4a4dad7a3432 31 // Gelb und Orange Vcc +5V
MarcoF89 12:4a4dad7a3432 32 // Gnd Rot
MarcoF89 12:4a4dad7a3432 33
MarcoF89 11:8457b851e3e1 34
MarcoF89 12:4a4dad7a3432 35 static int n1, n2, n3, n4;
MarcoF89 11:8457b851e3e1 36
MarcoF89 12:4a4dad7a3432 37 float k;
MarcoF89 11:8457b851e3e1 38 float y_off, y_high_low_summe, y_winkel;
MarcoF89 12:4a4dad7a3432 39
MarcoF89 11:8457b851e3e1 40 uint16_t i, j;
MarcoF89 11:8457b851e3e1 41
MarcoF89 11:8457b851e3e1 42 uint16_t zeit;
MarcoF89 11:8457b851e3e1 43 uint32_t zeit2;
MarcoF89 11:8457b851e3e1 44
MarcoF89 11:8457b851e3e1 45
MarcoF89 11:8457b851e3e1 46 Timer timer;
MarcoF89 11:8457b851e3e1 47 Timer timer2;
MarcoF89 9:9185d37e061f 48
MarcoF89 12:4a4dad7a3432 49 AnalogOut rauschen(PA_5);
MarcoF89 12:4a4dad7a3432 50
MarcoF89 8:769ff3814355 51
MarcoF89 6:27a09e8bebfb 52 int main()
MarcoF89 12:4a4dad7a3432 53 { Motor1.period_ms(2);
MarcoF89 11:8457b851e3e1 54 Motor2.period_ms(2);
MarcoF89 11:8457b851e3e1 55 Motor3.period_ms(2);
MarcoF89 11:8457b851e3e1 56 Motor4.period_ms(2);
MarcoF89 11:8457b851e3e1 57 wait_ms (10);
MarcoF89 12:4a4dad7a3432 58 pc.printf("\n\r");
MarcoF89 12:4a4dad7a3432 59 initialisierung_gyro();
MarcoF89 12:4a4dad7a3432 60 initialisierung_acc();
MarcoF89 12:4a4dad7a3432 61 wait_ms(20);
MarcoF89 12:4a4dad7a3432 62 if (taster2)
MarcoF89 12:4a4dad7a3432 63 {
MarcoF89 12:4a4dad7a3432 64 rauschen = 1;
MarcoF89 12:4a4dad7a3432 65 n1 = n2 = n3 = n4 = 700;
MarcoF89 12:4a4dad7a3432 66 Motor1.pulsewidth_us(n1);
MarcoF89 12:4a4dad7a3432 67 Motor2.pulsewidth_us(n2);
MarcoF89 12:4a4dad7a3432 68 Motor3.pulsewidth_us(n3);
MarcoF89 12:4a4dad7a3432 69 Motor4.pulsewidth_us(n4);
MarcoF89 12:4a4dad7a3432 70 wait_ms(10000);
MarcoF89 12:4a4dad7a3432 71 while(!taster4)
MarcoF89 12:4a4dad7a3432 72 {
MarcoF89 12:4a4dad7a3432 73 for(i = 1; i <= 1000; i++)
MarcoF89 12:4a4dad7a3432 74 {
MarcoF89 12:4a4dad7a3432 75 Motor1.pulsewidth_us(n1);
MarcoF89 12:4a4dad7a3432 76 Motor2.pulsewidth_us(n2);
MarcoF89 12:4a4dad7a3432 77 Motor3.pulsewidth_us(n3);
MarcoF89 12:4a4dad7a3432 78 Motor4.pulsewidth_us(n4);
MarcoF89 12:4a4dad7a3432 79 k = aktuell_acc_x()*aktuell_acc_x();
MarcoF89 12:4a4dad7a3432 80 k = sqrt(k) * 0.0000438596491;
MarcoF89 12:4a4dad7a3432 81 pc.printf("Winkel:%2.5f\n\r",k);
MarcoF89 12:4a4dad7a3432 82 rauschen = k;
MarcoF89 12:4a4dad7a3432 83 wait_ms(10);
MarcoF89 12:4a4dad7a3432 84 }
MarcoF89 12:4a4dad7a3432 85 for(i = 1, n1 = n2 = n3= 1400; i <= 3000; i++)
MarcoF89 12:4a4dad7a3432 86 {
MarcoF89 12:4a4dad7a3432 87 Motor1.pulsewidth_us(n1);
MarcoF89 12:4a4dad7a3432 88 Motor2.pulsewidth_us(n2);
MarcoF89 12:4a4dad7a3432 89 Motor3.pulsewidth_us(n3);
MarcoF89 12:4a4dad7a3432 90 Motor4.pulsewidth_us(n4);
MarcoF89 12:4a4dad7a3432 91 k = aktuell_acc_x()*aktuell_acc_x();
MarcoF89 12:4a4dad7a3432 92 k = sqrt(k) * 0.0000438596491;
MarcoF89 12:4a4dad7a3432 93 pc.printf("Winkel:%2.5f\n\r",k);
MarcoF89 12:4a4dad7a3432 94 rauschen = k;
MarcoF89 12:4a4dad7a3432 95 wait_ms(10);
MarcoF89 12:4a4dad7a3432 96 }
MarcoF89 12:4a4dad7a3432 97 rauschen = 0;
MarcoF89 12:4a4dad7a3432 98 wait_ms(100000);
MarcoF89 12:4a4dad7a3432 99 }
MarcoF89 12:4a4dad7a3432 100 }
MarcoF89 12:4a4dad7a3432 101
MarcoF89 12:4a4dad7a3432 102 if (taster3)
MarcoF89 12:4a4dad7a3432 103 {
MarcoF89 12:4a4dad7a3432 104 n1 = n2 = n3 = n4 = 1900;
MarcoF89 11:8457b851e3e1 105 Motor1.pulsewidth_us(n1);
MarcoF89 11:8457b851e3e1 106 Motor2.pulsewidth_us(n2);
MarcoF89 11:8457b851e3e1 107 Motor3.pulsewidth_us(n3);
MarcoF89 11:8457b851e3e1 108 Motor4.pulsewidth_us(n4);
MarcoF89 12:4a4dad7a3432 109 pc.printf("Nach einem langem PiepTon Taste1 betaetigen\n\r");
MarcoF89 12:4a4dad7a3432 110 pc.printf("Drehzahl aller Motoren: %d%%\r",(n1-700)*100/(1900-700));
MarcoF89 11:8457b851e3e1 111 while (!taster4)
MarcoF89 11:8457b851e3e1 112 {
MarcoF89 11:8457b851e3e1 113 if (taster1)
MarcoF89 11:8457b851e3e1 114 {
MarcoF89 11:8457b851e3e1 115 n1 = n2 =n3 = n4 = 700;
MarcoF89 11:8457b851e3e1 116 }
MarcoF89 11:8457b851e3e1 117 if (taster2)
MarcoF89 11:8457b851e3e1 118 {
MarcoF89 11:8457b851e3e1 119 n1 = n2 = n3 = n4 =1900;
MarcoF89 11:8457b851e3e1 120 }
MarcoF89 11:8457b851e3e1 121 Motor1.pulsewidth_us(n1);
MarcoF89 11:8457b851e3e1 122 Motor2.pulsewidth_us(n2);
MarcoF89 11:8457b851e3e1 123 Motor3.pulsewidth_us(n3);
MarcoF89 11:8457b851e3e1 124 Motor4.pulsewidth_us(n4);
MarcoF89 12:4a4dad7a3432 125 pc.printf("Drehzahl aller Motoren: %d%%\r",(n1-700)*100/(1900-700));
MarcoF89 11:8457b851e3e1 126 }
MarcoF89 11:8457b851e3e1 127 }
MarcoF89 12:4a4dad7a3432 128 wait_ms (10);
MarcoF89 12:4a4dad7a3432 129 pc.printf("Drehzahl aller Motoren: %d%%\n\r",(n1-700)*100/(1900-700));
MarcoF89 11:8457b851e3e1 130 pc.printf("Druecke Taster1 für den Start\n\r");
MarcoF89 6:27a09e8bebfb 131 while(1)
MarcoF89 11:8457b851e3e1 132 {
MarcoF89 11:8457b851e3e1 133 n1 = n2 = n3 = n4 =700;
MarcoF89 11:8457b851e3e1 134 Motor1.pulsewidth_us(n1);
MarcoF89 11:8457b851e3e1 135 Motor2.pulsewidth_us(n2);
MarcoF89 11:8457b851e3e1 136 Motor3.pulsewidth_us(n3);
MarcoF89 11:8457b851e3e1 137 Motor4.pulsewidth_us(n4);
MarcoF89 11:8457b851e3e1 138 if (taster1)
MarcoF89 11:8457b851e3e1 139 {
MarcoF89 11:8457b851e3e1 140 pc.printf("Du hast den Hobel gestartert, lauf!!!\n\r");
MarcoF89 11:8457b851e3e1 141 while(!taster4)
MarcoF89 11:8457b851e3e1 142 {
MarcoF89 11:8457b851e3e1 143 pc.printf("Halte still, es wird kalibiriert!!!\n\r");
MarcoF89 11:8457b851e3e1 144 //Offset:
MarcoF89 11:8457b851e3e1 145 for(i = 1; i <= 40000; i++)
MarcoF89 11:8457b851e3e1 146 {
MarcoF89 11:8457b851e3e1 147 y_off += aktuell_gyro_y();
MarcoF89 11:8457b851e3e1 148 }
MarcoF89 11:8457b851e3e1 149 y_off /= 40000;
MarcoF89 11:8457b851e3e1 150 pc.printf("Y_Off = %2.2f\n\r",y_off);
MarcoF89 11:8457b851e3e1 151 pc.printf("Ich habe fertig kalibiriert!!!\n\r");
MarcoF89 11:8457b851e3e1 152 for(i = 1; i<= 100; i++)
MarcoF89 11:8457b851e3e1 153 {
MarcoF89 11:8457b851e3e1 154 n2 += 1;
MarcoF89 11:8457b851e3e1 155 n4 += 1;
MarcoF89 11:8457b851e3e1 156 Motor4.pulsewidth_us(n4);
MarcoF89 11:8457b851e3e1 157 Motor2.pulsewidth_us(n2);
MarcoF89 11:8457b851e3e1 158 wait_ms(20);
MarcoF89 11:8457b851e3e1 159 }
MarcoF89 11:8457b851e3e1 160 wait_ms(2000);
MarcoF89 11:8457b851e3e1 161 //Messen
MarcoF89 11:8457b851e3e1 162 y_high_low_summe = 0;
MarcoF89 11:8457b851e3e1 163 i = 0;
MarcoF89 11:8457b851e3e1 164 j = 0;
MarcoF89 11:8457b851e3e1 165 timer.reset();
MarcoF89 11:8457b851e3e1 166 timer2.reset();
MarcoF89 11:8457b851e3e1 167 y_winkel = 0;
MarcoF89 11:8457b851e3e1 168 while(!taster4) {
MarcoF89 11:8457b851e3e1 169 i++; //Zähler für den Printf
MarcoF89 11:8457b851e3e1 170 j++; //Zähler für die Motoren
MarcoF89 11:8457b851e3e1 171 timer.start();
MarcoF89 11:8457b851e3e1 172 timer2.start();
MarcoF89 11:8457b851e3e1 173 zeit = timer.read_us();
MarcoF89 11:8457b851e3e1 174 timer.reset();
MarcoF89 11:8457b851e3e1 175 timer.start();
MarcoF89 11:8457b851e3e1 176 zeit2 = timer2.read_ms();
MarcoF89 11:8457b851e3e1 177 y_high_low_summe = aktuell_gyro_y() - y_off; //Offset vom Messwert subtrahieren
MarcoF89 11:8457b851e3e1 178 y_winkel = y_winkel + (y_high_low_summe * zeit * 0.000001 * 1/16.4); //Messwert multipliziert mit der Zeitdifferenz
MarcoF89 11:8457b851e3e1 179 if (y_winkel < 0 && j == 1000 && n4 < 1200)
MarcoF89 11:8457b851e3e1 180 {
MarcoF89 11:8457b851e3e1 181 n4++;
MarcoF89 11:8457b851e3e1 182 Motor4.pulsewidth_us(n4);
MarcoF89 11:8457b851e3e1 183 if (n2 > 800)
MarcoF89 11:8457b851e3e1 184 {
MarcoF89 11:8457b851e3e1 185 n2 --;
MarcoF89 11:8457b851e3e1 186 Motor2.pulsewidth_us(n2);
MarcoF89 11:8457b851e3e1 187 }
MarcoF89 11:8457b851e3e1 188 j = 0;
MarcoF89 11:8457b851e3e1 189 }
MarcoF89 11:8457b851e3e1 190 if (y_winkel > 0 && j == 1000 && n2 < 1200)
MarcoF89 11:8457b851e3e1 191 {
MarcoF89 11:8457b851e3e1 192 n2++;
MarcoF89 11:8457b851e3e1 193 if (n4 > 800)
MarcoF89 11:8457b851e3e1 194 {
MarcoF89 11:8457b851e3e1 195 n4 --;
MarcoF89 11:8457b851e3e1 196 Motor2.pulsewidth_us(n2);
MarcoF89 11:8457b851e3e1 197 }
MarcoF89 11:8457b851e3e1 198 Motor4.pulsewidth_us(n4);
MarcoF89 11:8457b851e3e1 199 j = 0;
MarcoF89 11:8457b851e3e1 200 }
MarcoF89 11:8457b851e3e1 201 if (i == 20000)
MarcoF89 11:8457b851e3e1 202 {
MarcoF89 11:8457b851e3e1 203 pc.printf("y_Winkel: = %3.5f\tMotor2:%d%%\tMotor4:%d%%\tMotor2:%d\tMotor4:%d\n\r", y_winkel, (n2-700)*100/(1900-700), (n4-700)*100/(1900-700),n2, n4);
MarcoF89 11:8457b851e3e1 204 i = 0;
MarcoF89 11:8457b851e3e1 205 }
MarcoF89 11:8457b851e3e1 206 }
MarcoF89 11:8457b851e3e1 207 }
MarcoF89 11:8457b851e3e1 208 }
MarcoF89 10:16ca5e9ee0dc 209 }
MarcoF89 10:16ca5e9ee0dc 210 }
MarcoF89 9:9185d37e061f 211
MarcoF89 10:16ca5e9ee0dc 212
MarcoF89 10:16ca5e9ee0dc 213
MarcoF89 9:9185d37e061f 214
MarcoF89 10:16ca5e9ee0dc 215
MarcoF89 11:8457b851e3e1 216
MarcoF89 11:8457b851e3e1 217
MarcoF89 11:8457b851e3e1 218
MarcoF89 11:8457b851e3e1 219 /*if(taster1.read())
MarcoF89 11:8457b851e3e1 220 {
MarcoF89 11:8457b851e3e1 221 i = 0.35*2000;
MarcoF89 11:8457b851e3e1 222 }
MarcoF89 11:8457b851e3e1 223 if(taster2.read())
MarcoF89 11:8457b851e3e1 224 {
MarcoF89 11:8457b851e3e1 225 i = 0.95*2000;
MarcoF89 11:8457b851e3e1 226 }*/
MarcoF89 11:8457b851e3e1 227
MarcoF89 11:8457b851e3e1 228
MarcoF89 10:16ca5e9ee0dc 229 /*
MarcoF89 9:9185d37e061f 230 float x = aktuell_acc_x ();
MarcoF89 9:9185d37e061f 231 float z = aktuell_acc_z ();
MarcoF89 9:9185d37e061f 232 winkel1 = (((float)atan2(x, z)));
MarcoF89 9:9185d37e061f 233 float y = aktuell_acc_y ();
MarcoF89 9:9185d37e061f 234 winkel2 = (((float)atan2(y, z)));
MarcoF89 11:8457b851e3e1 235
MarcoF89 10:16ca5e9ee0dc 236 pc.printf("%4.2f\t\t\t%4.2f\t\r",winkel1*360/6.28, winkel2*360/6.28);*/