Chris Elsholz
/
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.
Fork of
Quadrocopter
by 
Marco Friedmann
Diff: main.cpp
- Revision:
- 11:8457b851e3e1
- Parent:
- 10:16ca5e9ee0dc
- Child:
- 12:4a4dad7a3432
--- a/main.cpp Tue Aug 08 16:53:22 2017 +0000
+++ b/main.cpp Thu Aug 10 18:04:27 2017 +0000
@@ -7,15 +7,15 @@
static SPI spi(PE_6,PE_5,PE_2); //mosi,miso,sclk
static DigitalOut ncs(PE_4); //ssel
-static AnalogIn poti_1 (PF_3);
-static AnalogIn poti_2 (PF_10);
-static AnalogIn poti_3 (PF_4);
-static AnalogIn poti_4 (PF_5);
+static AnalogIn poti_1(PF_3);
+static AnalogIn poti_2(PF_10);
+static AnalogIn poti_3(PF_4);
+static AnalogIn poti_4(PF_5);
-static DigitalIn taster1 (PG_7);
-static DigitalIn taster2 (PD_10);
-static DigitalIn taster3 (PG_14);
-static DigitalIn taster4 (PF_12);
+static DigitalIn taster4(PG_4,PullDown);
+static DigitalIn taster3(PD_10,PullDown);
+static DigitalIn taster2(PG_14,PullDown);
+static DigitalIn taster1(PF_12,PullDown);
static DigitalOut db0(PC_8);
static DigitalOut db1(PC_9);
@@ -26,28 +26,154 @@
static DigitalOut db6(PC_14);
static DigitalOut db7(PC_15);
-static PwmOut Motor2 (PC_9); // Weiß
-static PwmOut Motor1 (PC_8); // Schwarz
-static PwmOut Motor3 (PC_6); // Grau
-static PwmOut Motor4 (PB_9); // Blau
- // Gelb und Orange Vcc +5V
- // Gnd Rot
+static PwmOut Motor1 (PC_8); // Schwarz QBRAIN: rot
+static PwmOut Motor2 (PC_9); // Weiß orange
+static PwmOut Motor3 (PC_6); // Grau weiß
+static PwmOut Motor4 (PB_9); // Blau braun
+// Gelb und Orange Vcc +5V
+// Gnd Rot
+
+
+
+float y_off, y_high_low_summe, y_winkel;
+int16_t n1, n2, n3, n4;
+uint16_t i, j;
+
+uint16_t zeit;
+uint32_t zeit2;
+
+
+Timer timer;
+Timer timer2;
int main()
-{
-
-Motor1.period_ms(2);
-Motor2.period_ms(2);
-Motor3.period_ms(2);
-Motor4.period_ms(2);
+{ i = j = 0;
+ //Motor auf Drehzahl null einstellen start
+ n1 = n2 = n3 = n4 =700;
+ Motor1.period_ms(2);
+ Motor2.period_ms(2);
+ Motor3.period_ms(2);
+ Motor4.period_ms(2);
+ wait_ms (10);
-
-pc.printf("\n\r");
+ Motor1.pulsewidth_us(n1);
+ Motor2.pulsewidth_us(n2);
+ Motor3.pulsewidth_us(n3);
+ Motor4.pulsewidth_us(n4);
+ wait_ms (10);
+ pc.printf("\n\r");
+ pc.printf("Drehzahl aller Motoren: %d%%\n\r",(n1-700)*100/(1900-700));
+ //Motor auf Drehzahl null einstellen ende
+ initialisierung_gyro();
+ wait_ms(20);
+ //Motor anlernen start
+ if (taster3)
+ {
+ n1 = n2 = n3 = n4 = 1900;
+ Motor1.pulsewidth_us(n1);
+ Motor2.pulsewidth_us(n2);
+ Motor3.pulsewidth_us(n3);
+ Motor4.pulsewidth_us(n4);
+ pc.printf("Nach einem langem PiepTon Taste1 betaetigen\n\r");
+ pc.printf("Drehzahl aller Motoren: %d%%\n\r",(n1-700)*100/(1900-700));
+ while (!taster4)
+ {
+ if (taster1)
+ {
+ n1 = n2 =n3 = n4 = 700;
+ }
+ if (taster2)
+ {
+ n1 = n2 = n3 = n4 =1900;
+ }
+ Motor1.pulsewidth_us(n1);
+ Motor2.pulsewidth_us(n2);
+ Motor3.pulsewidth_us(n3);
+ Motor4.pulsewidth_us(n4);
+ pc.printf("Drehzahl aller Motoren: %d%%\n\r",(n1-700)*100/(1900-700));
+ }
+ }
+ //Motor anlernen ende
+ pc.printf("Druecke Taster1 für den Start\n\r");
while(1)
- {
- Motor3.pulsewidth_us(poti_1.read_u16()*0.015305732*2);
- pc.printf("Poti = %2.2f\r",poti_1.read_u16()*0.015305732);
+ {
+ n1 = n2 = n3 = n4 =700;
+ Motor1.pulsewidth_us(n1);
+ Motor2.pulsewidth_us(n2);
+ Motor3.pulsewidth_us(n3);
+ Motor4.pulsewidth_us(n4);
+ if (taster1)
+ {
+ pc.printf("Du hast den Hobel gestartert, lauf!!!\n\r");
+ while(!taster4)
+ {
+ pc.printf("Halte still, es wird kalibiriert!!!\n\r");
+ //Offset:
+ for(i = 1; i <= 40000; i++)
+ {
+ y_off += aktuell_gyro_y();
+ }
+ y_off /= 40000;
+ pc.printf("Y_Off = %2.2f\n\r",y_off);
+ pc.printf("Ich habe fertig kalibiriert!!!\n\r");
+ for(i = 1; i<= 100; i++)
+ {
+ n2 += 1;
+ n4 += 1;
+ Motor4.pulsewidth_us(n4);
+ Motor2.pulsewidth_us(n2);
+ wait_ms(20);
+ }
+ wait_ms(2000);
+ //Messen
+ y_high_low_summe = 0;
+ i = 0;
+ j = 0;
+ timer.reset();
+ timer2.reset();
+ y_winkel = 0;
+ while(!taster4) {
+ i++; //Zähler für den Printf
+ j++; //Zähler für die Motoren
+ timer.start();
+ timer2.start();
+ zeit = timer.read_us();
+ timer.reset();
+ timer.start();
+ zeit2 = timer2.read_ms();
+ y_high_low_summe = aktuell_gyro_y() - y_off; //Offset vom Messwert subtrahieren
+ y_winkel = y_winkel + (y_high_low_summe * zeit * 0.000001 * 1/16.4); //Messwert multipliziert mit der Zeitdifferenz
+ if (y_winkel < 0 && j == 1000 && n4 < 1200)
+ {
+ n4++;
+ Motor4.pulsewidth_us(n4);
+ if (n2 > 800)
+ {
+ n2 --;
+ Motor2.pulsewidth_us(n2);
+ }
+ j = 0;
+ }
+ if (y_winkel > 0 && j == 1000 && n2 < 1200)
+ {
+ n2++;
+ if (n4 > 800)
+ {
+ n4 --;
+ Motor2.pulsewidth_us(n2);
+ }
+ Motor4.pulsewidth_us(n4);
+ j = 0;
+ }
+ if (i == 20000)
+ {
+ 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);
+ i = 0;
+ }
+ }
+ }
+ }
}
}
@@ -55,11 +181,24 @@
+
+
+
+/*if(taster1.read())
+{
+i = 0.35*2000;
+}
+if(taster2.read())
+{
+i = 0.95*2000;
+}*/
+
+
/*
float x = aktuell_acc_x ();
float z = aktuell_acc_z ();
winkel1 = (((float)atan2(x, z)));
float y = aktuell_acc_y ();
winkel2 = (((float)atan2(y, z)));
-
+
pc.printf("%4.2f\t\t\t%4.2f\t\r",winkel1*360/6.28, winkel2*360/6.28);*/
\ No newline at end of file