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@11:8457b851e3e1, 2017-08-10 (annotated)

Committer:
MarcoF89
Date:
Thu Aug 10 18:04:27 2017 +0000
Revision:
11:8457b851e3e1
Parent:
10:16ca5e9ee0dc
Child:
12:4a4dad7a3432
Kalibrierung des Q-Brains integriert. Taster 3 dr?cken und die Stromversorgung einschalten, dann kommt man in den Programmiermodus. Durch bet?tigen des Tasters4, kommt man wieder zur?ck.; Das Hauptprogramm wird mit Taster 1 gestartet und 4 beendet

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