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
main.cpp
- Committer:
- MarcoF89
- Date:
- 2017-08-16
- Revision:
- 12:4a4dad7a3432
- Parent:
- 11:8457b851e3e1
- Child:
- 13:5f0a2103c707
File content as of revision 12:4a4dad7a3432:
#include "mbed.h"
#include "stdio.h"
#include <Timer.h>
#include "messen.h"
#include <math.h>
static Serial pc(SERIAL_TX, SERIAL_RX);
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 DigitalOut db0(PC_8);
static DigitalOut db1(PC_9);
static DigitalOut db2(PC_10);
static DigitalOut db3(PC_11);
static DigitalOut db4(PC_12);
static DigitalOut db5(PC_13);
static DigitalOut db6(PC_14);
static DigitalOut db7(PC_15);
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
static int n1, n2, n3, n4;
float k;
float y_off, y_high_low_summe, y_winkel;
uint16_t i, j;
uint16_t zeit;
uint32_t zeit2;
Timer timer;
Timer timer2;
AnalogOut rauschen(PA_5);
int main()
{ Motor1.period_ms(2);
Motor2.period_ms(2);
Motor3.period_ms(2);
Motor4.period_ms(2);
wait_ms (10);
pc.printf("\n\r");
initialisierung_gyro();
initialisierung_acc();
wait_ms(20);
if (taster2)
{
rauschen = 1;
n1 = n2 = n3 = n4 = 700;
Motor1.pulsewidth_us(n1);
Motor2.pulsewidth_us(n2);
Motor3.pulsewidth_us(n3);
Motor4.pulsewidth_us(n4);
wait_ms(10000);
while(!taster4)
{
for(i = 1; i <= 1000; i++)
{
Motor1.pulsewidth_us(n1);
Motor2.pulsewidth_us(n2);
Motor3.pulsewidth_us(n3);
Motor4.pulsewidth_us(n4);
k = aktuell_acc_x()*aktuell_acc_x();
k = sqrt(k) * 0.0000438596491;
pc.printf("Winkel:%2.5f\n\r",k);
rauschen = k;
wait_ms(10);
}
for(i = 1, n1 = n2 = n3= 1400; i <= 3000; i++)
{
Motor1.pulsewidth_us(n1);
Motor2.pulsewidth_us(n2);
Motor3.pulsewidth_us(n3);
Motor4.pulsewidth_us(n4);
k = aktuell_acc_x()*aktuell_acc_x();
k = sqrt(k) * 0.0000438596491;
pc.printf("Winkel:%2.5f\n\r",k);
rauschen = k;
wait_ms(10);
}
rauschen = 0;
wait_ms(100000);
}
}
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%%\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%%\r",(n1-700)*100/(1900-700));
}
}
wait_ms (10);
pc.printf("Drehzahl aller Motoren: %d%%\n\r",(n1-700)*100/(1900-700));
pc.printf("Druecke Taster1 für den Start\n\r");
while(1)
{
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;
}
}
}
}
}
}
/*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);*/