Marco Friedmann
/
Quadrocopter2
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 
Chris Elsholz
main.cpp
- Committer:
- MarcoF89
- Date:
- 2017-10-09
- Revision:
- 31:adfa5a816488
- Parent:
- 30:dc68b509f930
File content as of revision 31:adfa5a816488:
#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;
double e_pitch = 0;
double w_pitch = 0;
double yalt_pitch = 0;
double ealt_pitch = 0;
double ealt2_pitch = 0;
double q0_pitch = 0.1; //Kp + Ki * Ta + Kd/Ta
double q1_pitch = 0; //-Ki - 2 * Kd/Ta
double q2_pitch = 0; //Kd/Ta
#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, gyro_pitch, gyro_yaw, gyro_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(" %3.2f \t %3.2f \t %3.2f \t %3.2f \t\t %3.2f \t\t %3.2f \t\t %3.2f \t\t %3.2f \t\t %d\n\r", pitch, yaw, roll, newAngle_pitch, newAngle_roll, gyro_pitch, gyro_yaw, gyro_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>1501)
{
n1=n2=n3=n4=700;
Motor1.pulsewidth_us(n1);
Motor2.pulsewidth_us(n2);
Motor3.pulsewidth_us(n3);
Motor4.pulsewidth_us(n4);
while(1);
}
}
}
}
if (taster3) //Start Regelung
{
pc.printf("\n\rOffset und Driftberechnung wird durchgefuehrt, halte die Drohne still");
printf("\n\rpitch, yaw, roll, newAngle_pitch, newAngle_roll, gyro_pitch, gyro_yaw, gyro_roll, n2\n\r");
while(1)
{
offset_gyro(&z_off, &x_off, &y_off);
int i = 0;
timer.reset();
timer.start();
timer2.reset();
timer2.start();
n1 = n2 = n3 = n4 = 1200;
Motor1.pulsewidth_us(n1);
Motor2.pulsewidth_us(n2);
Motor3.pulsewidth_us(n3);
Motor4.pulsewidth_us(n4);
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);
e_pitch = w_pitch - pitch;
y_pitch = yalt_pitch + q0_pitch * e_pitch + q1_pitch * ealt_pitch + q2_pitch * ealt2_pitch;
ealt2_pitch = ealt_pitch;
ealt_pitch = e_pitch;
yalt_pitch = y_pitch;
n1 = n4 = y_pitch * 15.5 + 1200;
n2 = n3 = y_pitch * -15.5 + 1200;
if (i == 500)
{
printf("%3.2f\t%d\t%d\t%3.2f\t\n\r", pitch, n1, n2, q0_pitch);
i = 0;
}
if (taster1)
{
q0_pitch += 0.1;
}
if (taster2)
{
q0_pitch -= 0.1;
}
if (taster3)
{
n1=n2=n3=n4=700;
Motor1.pulsewidth_us(n1);
Motor2.pulsewidth_us(n2);
Motor3.pulsewidth_us(n3);
Motor4.pulsewidth_us(n4);
while(1);
}
}
}
}
}
}