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-09-25
- Revision:
- 29:3efe34986347
- Parent:
- 28:f9349474a553
- Child:
- 30:dc68b509f930
File content as of revision 29:3efe34986347:
#include <Timer.h>
#include <math.h>
#include "mbed.h"
#include "stdio.h"
#include "deklaration.h"
#include "messen.h"
#include "filter/Kalman.h"
double gyro_pitch;
double gyro_yaw;
double gyro_roll;
#define RAD 57.29577951
int main()
{
z_off = 0;
drift_z = 0;
gyro_pitch = 0;
gyro_yaw = 0;
gyro_roll = 0;
Motor1.period_ms(2);
Motor2.period_ms(2);
Motor3.period_ms(2);
Motor4.period_ms(2);
initialisierung_gyro();
initialisierung_acc();
Kalman_pitch();
Kalman_yaw();
Kalman_roll();
aktuell_roh(&z_g, &x_g, &y_g, &z_a, &x_a, &y_a);
wait(1);
if (taster2)
{
viberationen(&rauschen, &Motor1, &Motor2, &Motor3, &Motor4, &taster4);
}
if (taster3)
{
anlernen(&Motor1, &Motor2, &Motor3, &Motor4, &taster1, &taster2, &taster4);
}
pc.printf("Druecke Taster1 fuer den Start und Taster2 fuers rauschen\n\r");
n1=n2=n3=n4=700;
Motor1.pulsewidth_us(n1);
Motor2.pulsewidth_us(n2);
Motor3.pulsewidth_us(n3);
Motor4.pulsewidth_us(n4);
while(1)
{
if (taster1)
{
while(1)
{
pc.printf("\n\rOffset und Driftberechnung wird durchgefuehrt, halte die Drohne still");
offset_gyro(&z_off, &x_off, &y_off);
//drift_gyro(&drift_z, &drift_x, &drift_y, &timer, &timer2, &gain_g, &roll_g, &pitch_g, &z_off, &x_off, &y_off);
pc.printf("\n\rOffgesamt:\n\rZ = %3.5f\tY = %3.5f\tZ = %3.5f\t\n\r", z_off, x_off, y_off);
pc.printf("\n\rDrift:Z: %3.10f\tX: %3.10f\tY: %3.10f\n\r", drift_z, drift_x, drift_y);
timer.reset();
timer.start();
timer2.reset();
timer2.start();
int i = 0;
while(1)
{
i++;
dt = timer.read_us() * 0.000001; //Zeit zwischen zwei Messpunkten
timer.reset();
aktuell_roh(&z_g, &x_g, &y_g, &z_a, &x_a, &y_a); //Rohdaten einlesen
y = y_a / 16384.00; //Umwandlung in G-Kraft
x = x_a / 16384.00; //Umwandlung in G-Kraft
z = z_a / 16384.00; //Umwandlung in G-Kraft
newAngle_pitch = atan2(-x, z) * RAD; //Umwandlung der G-Kraft in °
newRate_pitch = ((y_g - y_off) * 1/16.4); //Offset subtrahiert +++ Umwandlung in °/s
newAngle_roll = atan2(y, sqrt(x * x + z * z)) * RAD; //Umwandlung der G-Kraft in °
newRate_roll = ((x_g - x_off) * 1/16.4); //Offset subtrahiert +++ Umwandlung in °/s
newAngle_yaw = ((z_g - z_off) * 1/16.4); //Umwandlung der G-Kraft in °
newRate_yaw = ((z_g - z_off) * 1/16.4); //Offset subtrahiert +++ Umwandlung in °/s
pitch = getPitch(&newAngle_pitch, &newRate_pitch, &dt);
yaw = getYaw(&newAngle_yaw, &newRate_yaw, &dt);
roll = getRoll(&newAngle_roll, &newRate_roll, &dt);
if (i == 1000)
{
printf("%f2.3 \t%f2.3 \t%f2.3 \t%f2.3 \t%f2.3 \t%f2.3 \t%fd \t", pitch, yaw, roll, newAngle_pitch, newRate_pitch, newAngle_roll, newRate_roll, newAngle_yaw, n1);
i = 0;
}
Motorsteurung(&Motor1, &Motor2, &Motor3, &Motor4, &taster2, &taster3, &taster4, &n1, &n2, &n3, &n4);
}
}
}
if (taster2)
{
pc.printf("\n\rOffset und Driftberechnung wird durchgefuehrt, halte die Drohne still");
printf("\n\rpitch, yaw, roll, newAngle_pitch, newAngle_´roll, newRate_pitch, newRate_yaw, newRate_roll, n2\n\r");
while(1)
{
offset_gyro(&z_off, &x_off, &y_off);
//drift_gyro(&drift_z, &drift_x, &drift_y, &timer, &timer2, &gain_g, &roll_g, &pitch_g, &z_off, &x_off, &y_off);
//pc.printf("\n\rOffgesamt:\n\rZ = %3.5f\tY = %3.5f\tZ = %3.5f\t\n\r", z_off, x_off, y_off);
//pc.printf("\n\rDrift:Z: %3.10f\tX: %3.10f\tY: %3.10f\n\r", drift_z, drift_x, drift_y);
timer.reset();
timer.start();
timer2.reset();
timer2.start();
int i = 0;
while(1)
{
i++;
dt = timer.read_us() * 0.000001; //Zeit zwischen zwei Messpunkten
timer.reset();
aktuell_roh(&z_g, &x_g, &y_g, &z_a, &x_a, &y_a); //Rohdaten einlesen
y = y_a / 16384.00; //Umwandlung in G-Kraft
x = x_a / 16384.00; //Umwandlung in G-Kraft
z = z_a / 16384.00; //Umwandlung in G-Kraft
newAngle_pitch = atan2(-x, z) * RAD; //Umwandlung der G-Kraft in °
newRate_pitch = ((y_g - y_off) * 1/16.4); //Offset subtrahiert +++ Umwandlung in °/s
newAngle_roll = atan2(y, sqrt(x * x + z * z)) * RAD; //Umwandlung der G-Kraft in °
newRate_roll = ((x_g - x_off) * 1/16.4); //Offset subtrahiert +++ Umwandlung in °/s
newAngle_yaw = ((z_g - z_off) * 1/16.4); //Umwandlung der G-Kraft in °
newRate_yaw = ((z_g - z_off) * 1/16.4); //Offset subtrahiert +++ Umwandlung in °/s
pitch = getPitch(&newAngle_pitch, &newRate_pitch, &dt);
yaw = getYaw(&newAngle_yaw, &newRate_yaw, &dt);
roll = getRoll(&newAngle_roll, &newRate_roll, &dt);
gyro_pitch += dt * newRate_pitch;
gyro_yaw += dt * newRate_yaw;
gyro_roll += dt * newRate_roll;
if (i == 500)
{
printf(" %f3.2 \t\t %f3.2 \t\t %f3.2 \t\t %f3.2 \t\t %f3.2 \t\t %f3.2 \t\t %f3.2 \t\t %f3.2 \t\t %d\n\r", pitch, yaw, roll, newAngle_pitch, newAngle_´roll, newRate_pitch, newRate_yaw, newRate_roll, n2);
i = 0;
}
if (timer2.read_ms() >= 5000)
{
n1+=200;
n2+=200;
n3+=200;
n4+=200;
Motor1.pulsewidth_us(n1);
Motor2.pulsewidth_us(n2);
Motor3.pulsewidth_us(n3);
Motor4.pulsewidth_us(n4);
timer2.reset();
}
Motorsteurung(&Motor1, &Motor2, &Motor3, &Motor4, &taster2, &taster3, &taster4, &n1, &n2, &n3, &n4);
if (n1>1400)
{
n1=n2=n3=n4=700;
Motor1.pulsewidth_us(n1);
Motor2.pulsewidth_us(n2);
Motor3.pulsewidth_us(n3);
Motor4.pulsewidth_us(n4);
while(1);
}
}
}
}
}
}