Projekt 86: Low-Cost-Drohne mit Ardunio
Autoren: Sergej Vogel, Janis Ostermann
Betreuer: Prof. Schneider
→ zurück zur Übersicht: WS 18/19: Angewandte Elektrotechnik (BSE)
Einleitung
Im Rahmen des Moduls "angewandte Elektrotechnik" aus dem Studiengang "Business and Systemsengineering" der Hochschule Hamm-Lippstadt in Lippstadt konnten die Studenten ihr bereits erlerntes Wissen unter Beweis stellen und praktische Erfahrungen sammeln, indem sie eine elektrotechnisches Projekt realisieren. Thema dieses Projekts soll die Auslegung und Konstruktion einer kostengünstigen Drohne bei Zuhilfenahme eines Arduino Mikrocontrollers sein. Über die Entwicklung der Hardware hinaus soll zusätzlich ein rudimentäres Programm zur Präsentation der Funktionsweise der Drohne entwickelt werden. Genauere Anforderungen sind unter dem Punkt "Erwartungen an die Projektlösung" einsehbar.
Erwartungen an die Projektlösung
- Recherche bisheriger Lösungen
- Entwurf der Drone, deren elektrischer Schaltung (Sensoren, Aktoren), Konstruktion und Beschaffung der Bauteile
- Realisierung des Aufbaus
- Darstellung der regelungstechnischen Theorie
- Systemidentifikation (Übertragungsfunktion der Regelstrecke bestimmen)
- Vergleichen und bewerten Sie verschiedene Regleransätze (P, PI, PID und andere).
- Test und wiss. Dokumentation
- Machen Sie ein tolles Videos, welches die Funktion visualisiert.
- Live Vorführung während der Abschlusspräsentation
Kür: Modellbasierte Programmierung der Hardware via Simulink
Projekt
Zielsetzung
Das Ziel des Projektes war es die Drohne von Null auf zu konzipieren und zusammenzubauen. Dabei sollte der Kostenaspekt mit beachtet werden. Ein weiteres Ziel stellt das geregelte Schweben der Drohne auf einer bestimmten Höhe dar.
Projektplan
Das Projekt „Low-Cost-Drohne“ sollte in einer Zeitspanne von 16 Wochen realisiert werden. Um einen reibungslosen Ablauf zu gewährleisten, wird das Projekt in folgende Projektschritte gegliedert, die in bestimmten Zeitspannen zu bewältigen sind. Daraus resultiert der Projektplan (siehe Abbildung 1).
Projektdurchführung
Die Recherche rund um den Bereich "Drohne" stellt den Start in das Projekt dar. Da die Studenten seitens der Hochschule finanziell unterstützt wurden, gab es eine zeitliche Vorgabe zur Erstellung einer Einkaufsliste, mit den Teilen die für das jeweilige Projekt benötigt werden. Deshalb war es wichtig, die Hauptrecherche (zur Realisierung der Drohne) schnell abzuschließen. In dieser Phase haben sich mehrere Lösungen zur Realisierung der Drohne angeboten. Diese waren zwar einzeln für sich nicht überzeugend, aber durch die Entnahme einiger Teilbereiche und Indizierung eigener Ideen ist ein individueller Realisierungsplan entstanden. Dabei dient das YouTube-Video[1] als Inspiration und zentrale Grundlage für den technischen Bereich. Im Softwarebereich stehen ebenfalls viele Beispiele und Bibliotheken zur Verfügung. Doch auch da gibt es keine ideale Lösung für die Steuerung und Regelung der Drohne. Somit besteht auch hier die Aufgabe, Bibliotheken/Teillösungen mit einander zu kombinieren bzw. aufeinander abzustimmen.
Entwurf der Drohne
In der Recherchephase war es zunächst geplant, den Rahmen der Drohne selbstständig mittels CAD zu entwerfen und mit dem 3-D-Druckverfahren zu fertigen. Da sich die Realisierung des Rahmens als enormer zeitlicher Aufwand herausgestellt hat, wurde die Idee verworfen. Stattdessen wird eine vorgefertigte CAD Konstruktion eines Drohnenrahmen [2] verwendet, bei der die Größenverhältnisse anzupassen sind, da:
- der 3-D-Drucker nur einen begrenzten Druckraum besitzt, weswegen die Gesamtkonstruktion verkleinert werden muss und
- die Bauteile wie Rotoren und Akku bereits bestellt sind, weswegen die Auflageflächen nochmals angepasst werden müssen.
Um den Abstand der Drohne zum Boden zu messen, soll ein Ultraschallsensor zum Einsatz kommen, der mittig der Drohne, mit der Ausrichtung nach unten zum Boden, angebracht werden soll. Das Problem liegt darin, dass der Sensor erst einen Abstand von mindestens 2cm messen kann. Aus diesem Grund werden längere Standbeine benötigt, die aus einer weiteren CAD Konstruktion [3] entnommen und auf unserer Konstruktion angepasst werden. Das Ergebnis ist die CAD-Konstruktion in der Abbildung 2.
Verwendete Komponenten
In der Tabelle 1 werden alle Komponenten aufgeführt, die für den Zusammenbau der Drohne, sowie für einen Bau einer Sicherungsplattform (Erläuterung folgt im Abschnitt "3.5.2 Umsetzung der Drohne"), benötigt werden.
Kosten der Drohne
In der Tabelle 2 werden die Kosten für die Bauteile aufgeführt, die für den Bau der Drohne benötigt werden.
| Anzahl | Bauteilbezeichnung | Kosten Pro Teil in € | Gesamtkosten in € |
|---|---|---|---|
| 1 | Arduino Uno R3 | 22,99 | 22,99 |
| 1 | Ultraschall-Sensor | 2,95 | 6,29 |
| 1 | MPU-6050 Gyroskop Beschleunigungssensor | 3,40 | 3,40 |
| 2 | Brushless Motor CW | 6,29 | 12,58 |
| 2 | Brushless Motor CCW | 6,29 | 12,58 |
| 1 | Lipo-Akku | 23,75 | 23,75 |
| 1 | Rotoren-Pack | 2,72 | 2,72 |
| 4 | Afro Mini ESC | 6,33 | 25,32 |
| 1 | XT60 Stecker (male) Anschlusskabel für LiPo, Quadrocopter | 3,89 | 3,89 |
| 1 | Ladekabel XT60 für Lipo Akku | 4,95 | 4,95 |
| 1 | 3D-Druck-Rahmen | 0,00 | 0,00 |
| 1 | Taster | 0,00 | 0,00 |
| 1 | Schrauben | 3,09 | 3,09 |
| - | Summe | - | 118,22 |
Schaltung (Sensoren/Aktoren)
Alle Sensoren und Motorcontroller (ESCs) sind direkt mit dem Arduino Uno verbunden. Dazu werden Kabel durch die Kabeldurchführungen zwischen den verschiedenen Ebenen der Drohne verlegt. Die Aktoren, bzw. die Motor Controller sind zusammen mit dem Arduino UNO in einer Parallelschaltung mit dem Lithium Polymer Akku verbunden. Die Sensoren erhalten ihre Spannung über den 5 Volt Pin des Arduino. So können auch noch Sensordaten ausgelesen werden wenn der Arduino per Serieller Schnittstelle mit dem PC verbunden ist, während der Akku von der Drohne getrennt ist. Um Problemen durch Potentialunterschiede vorzubeugen sind alle Komponenten über einen gemeinsamen Ground verbunden. Die ESCs erhalten ein PWM (Pulsweiten Moulation) Signal über die PWM fähigen Digital-Pins des Arduino. Der Ultraschallsensor ist über die Digital Pins 11 Und 12 angebunden. Die Kommunikation des MPU-6050 Sensors wurde über die I2C Schnittstelle des Arduino realisiert. Dazu wird er an die SLC und SDA Pins mit dem Mikrocontroller verbunden. Die ESCs regeln einen Drehstrom in Abhängigkeit vom PWM Signal um so die Geschwindigkeit der Motoren zu regulieren. Der Modus-Wechsel-Taster ist über einer der Interrupt Pins und den Grund Pin an den Arduino angebunden. Beim Druck des Tasters wird der Interrupt Pin auf das Potential des Ground gezogen. Der Interrupt Pin ist mit einem Pull Up Widerstand auf 5 Volt gelegt. Dadurch Erkennt der Arduino das drücken des Tasters und löst eine Interrupt Routine aus.
Umsetzung der Drohne
Für die Realisierungsphase wird das Drohnengerüst benötigt. Hierzu wird die Drohne innerhalb der CAD-Konstruktion in ihre Einzelkomponenten zerlegt. Nun können die einzelnen Komponenten so positioniert werden, dass sich das Drucken von Hilfsstrukturen reduziert. Das hat den Vorteil, dass die Komponenten eine bessere Struktur erhalten und sich zusätzlich die Druckzeit, sowie die Nachbearbeitung reduziert. Sind die Einzelteile fertig, so werden sie von der Hilfstruktur befreit und geschliffen. Im letzten Schritt werden die Komponenten zu einem Drohnenrahmen zusammengeschraubt (siehe Abbildung 3). Gut zu erkennen sind die nutzbaren Flächen, die im weiteren Verlauf mit den Bauteilen wie Motoren, Akku, Arduino und Sensoren bestückt werden. Doch zuvor werden einige Richtlinien bzw. Punkte für den Zusammenbau aufgestellt, die dabei helfen sollen, die Drohne direkt beim ersten Versuch so zu fertigen, dass weitere Veränderungen vorgebeugt werden. Hier einige Richtlinien:
- Die Komponenten wie den Arduino, den Lipo-Akku und die Sensoren so anordnen, dass sich der Gewichtsschwerpunkt in der Mitte der Drohne befindet
- Rotoren haben dadurch in etwa dieselbe Belastung
- Im Hinblick auf die Bestimmung der Drohnenlage und die Messung des Abstandes zum Boden ergibt es einen Vorteil, wenn sich die Sensoren zentral an der Drohne befinden
- Verkabelung kurz halten (sieht ordentlich aus und keine Gefahr das die Kabel in die Rotoren geraten)
- Der Startknopf soll so positioniert sein, dass beim Drücken des Knopfes keine Gefahr für die Hände entsteht
- Komponenten so anbringen, dass diese schnell und problemlos ausgetauscht werden können
Die Struktur des Gerüstes ist für die Einhaltung der Richtlinien bestens geeignet. So gibt es in der Mitte des Gerüstes eine obere und eine untere Auflagefläche, wo die weiteren Bauteile angebracht werden können, um den Schwerpunkt der Drohne zentral zu halten. Dies ist auch der Plan.
Der Arduino soll einfach zugänglich sein, da die meisten Kabel dort zusammenlaufen und somit ein einfacher bzw. schneller Zugriff von Vorteil ist. Aus diesem Grund wird der Arduino auf die obere Ablagefläche positioniert und mit einem Kabelbinder fixiert (siehe Abbildung 4)
Die beiden Auflageflächen sind durch kleine Balken mit einander verbunden und grenzen dadurch einen optimalen Raum für den Akku ein. So befindet sich der Akku in der Mitte der Drohne (siehe Abbildung 5) und kann durch kurze Leitungen sowohl den Arduino, die ESCs und die Rotoren oben, als auch die Sensoren unten mit Strom versorgen.
Wie bereits erwähnt sollen der Gyro-Sensor (Beschleunigungs- und Lagesensor) und der Ultraschallsensor (für die Messung des Abstandes zum Boden) unterhalb der zweiten Platte angebracht werden. Da sich dort aber bereits Versorgungsleitungen befinden, wird eine weitere Fläche benötigt, damit die Sensoren parallel zum Boden angebracht werden können. Dafür wird die obere Auflagenfläche aus der CAD-Konstruktion entnommen und nochmals mit dem 3-D-Druckverfahren angefertigt. Daraufhin werden die Sensoren mit Schrauben an der Platte befestigt und die Platte wird mit Kabelbindern mit der nun mittleren Auflagefläche verbunden (siehe Abbildung 6).
Die Rotoren haben bereits einen vordefinierten Platz und die dazugehörigen ESCs werden jeweils unterhalb des Drohnenarms mit Kabelbindern befestigt (siehe Abbildung 7 und Abbildung 8).
Zum Schluss wird der Startknopf der Drohne neben dem Akku angebracht (siehe Abbildung 9), sodass kein Verletzungsrisiko besteht, sobald man die Drohne startet.
Durch die Zusammenführung der Komponenten entsteht eine vollwertige Drohne (siehe Abbildung 10) deren Schwerpunkt sich im Zentrum befindet, die Komponenten leicht zugänglich sind und somit jederzeit ohne viel Aufwand ausgewechselt werden können.
Für den Softwareteil wird zusätzlich eine Sicherheitsplattform gebaut (siehe Abbildung 10), die die Bewegungen der Drohne einschränken soll, falls diese nicht wie gewünscht reagiert.
Softwareentwicklung
#include "HCSR04.h"
#include <ESC.h>
#include <PID_v1.h>
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
// is used in I2Cdev.h
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//Defines
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
#define MOTOR_PIN1 (6) // ESC Pin von Motor 1 -> BEI UNS Nr. 3 VORNE RECHTS
#define MOTOR_PIN2 (9) // ESC Pin von Motor 2 -> BEI UNS Nr. 4 HINTEN RECHTS
#define MOTOR_PIN3 (10) // ESC Pin von Motor 3 -> BEI UNS Nr. 1 HINTEN LINKS
#define MOTOR_PIN4 (5) // ESC Pin von Motor 4 -> BEI UNS Nr. 2 VORNE LINKS
#define SPEED_MIN (1440) // Set the Minimum ESC Speed in microseconds -> mindestens 1060 -> Drehen ab 1440
#define SPEED_MAX (1550) // Set the Maximum ESC Speed in microseconds -> Bis zu 1860
#define SPEED_ARM (1000) // Set the Arm ESC Speed in microseconds -> noch austesten
#define ULTRASONIC_TRIGGER_PIN (12) // Trigger Pin von Ultraschallsensor
#define ULTRASONIC_ECHO_PIN (11) // Echo Pin von Ultraschallsensor
#define INTERRUPT_PIN 2 // use pin 2 on Arduino Uno & most boards
#define LED_PIN 13 // (Arduino is 13, Teensy is 11, Teensy++ is 6)
MPU6050 mpu;
UltraSonicDistanceSensor distanceSensor(ULTRASONIC_TRIGGER_PIN, ULTRASONIC_ECHO_PIN);
int State = 0, TextShown = 0;
volatile unsigned long alteZeit=0, entprellZeit=100;
bool EnalbedFlying = 0;
int throttle = 1400;//1450; // Gas zwischen 1000 und 2000 (besser 1800)
double throttleWithPID = 0;
double hoehe = 30;
double dist_offset, winkel_offset_roll = 4.97, winkel_offset_pitch = 2.2, winkel_offset_yaw = 0;
int cal_int;
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//ESC Library Variablen
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
double esc_1, esc_2, esc_3, esc_4;
ESC myESC1 (MOTOR_PIN1, SPEED_MIN, SPEED_MAX, SPEED_ARM); // ESC_Name (ESC PIN, Minimum Value, Maximum Value, Arm Value)
ESC myESC2 (MOTOR_PIN2, SPEED_MIN, SPEED_MAX, SPEED_ARM); // ESC_Name (ESC PIN, Minimum Value, Maximum Value, Arm Value)
ESC myESC3 (MOTOR_PIN3, SPEED_MIN, SPEED_MAX, SPEED_ARM); // ESC_Name (ESC PIN, Minimum Value, Maximum Value, Arm Value)
ESC myESC4 (MOTOR_PIN4, SPEED_MIN, SPEED_MAX, SPEED_ARM); // ESC_Name (ESC PIN, Minimum Value, Maximum Value, Arm Value)
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//PID gain and limit settings
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
double pid_p_gain_roll = 0.3; //Gain setting for the roll P-controller
double pid_i_gain_roll = 0;//.006;//0.04; //Gain setting for the roll I-controller
double pid_d_gain_roll = 0.2;//.06;//18.0; //Gain setting for the roll D-controller
int pid_max_roll = 20; //Maximum output of the PID-controller (+/-)
double pid_pitch_setpoint = 0;
double pid_p_gain_pitch = pid_p_gain_roll; //Gain setting for the pitch P-controller.
double pid_i_gain_pitch = pid_i_gain_roll; //Gain setting for the pitch I-controller.
double pid_d_gain_pitch = pid_d_gain_roll; //Gain setting for the pitch D-controller.
int pid_max_pitch = pid_max_roll; //Maximum output of the PID-controller (+/-)
double pid_roll_setpoint = 0;
double pid_p_gain_yaw = 0.025; //1.0; //4.0 //Gain setting for the pitch P-controller. //4.0
double pid_i_gain_yaw = 0.0; //0.02; //Gain setting for the pitch I-controller. //0.02
double pid_d_gain_yaw = 0.0; //Gain setting for the pitch D-controller.
int pid_max_yaw = 400; //Maximum output of the PID-controller (+/-)
double pid_yaw_setpoint = 0;
double pid_p_gain_throttle = 3; //1.0; //4.0 //Gain setting for the pitch P-controller. //4.0
double pid_i_gain_throttle = 15; //0.02; //Gain setting for the pitch I-controller. //0.02
double pid_d_gain_throttle = 0.5; //Gain setting for the pitch D-controller.
double pid_max_throttle = 200; //Maximum output of the PID-controller (+/-)
double pid_min_throttle = 0; //Maximum output of the PID-controller (+/-)
double pid_dist_setpoint = 30;
boolean auto_level = true; //Auto level on (true) or off (false)
double angle_roll = 0, angle_pitch = 0, angle_yaw = 0;
double pid_angle_roll = 0, pid_angle_pitch = 0, pid_angle_yaw = 0;
double angle_roll_raw = 0, angle_pitch_raw = 0, angle_yaw_raw = 0;
double last_angle_roll_raw = 0, last_angle_pitch_raw = 0, last_angle_yaw_raw = 0;
double pid_output_roll = 0, pid_output_pitch = 0, pid_output_yaw = 0;
double maDist, rawDist, DistDiff;
PID rollPID(&pid_angle_roll, &pid_output_roll, &pid_roll_setpoint, pid_p_gain_roll, pid_i_gain_roll, pid_d_gain_roll, DIRECT);
PID pitchPID(&pid_angle_pitch, &pid_output_pitch, &pid_pitch_setpoint, pid_p_gain_pitch, pid_i_gain_pitch, pid_d_gain_pitch, DIRECT);
// MPU control/status vars
bool dmpReady = false; // set true if DMP init was successful
uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU
uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount; // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer
// orientation/motion vars
Quaternion q; // [w, x, y, z] quaternion container
VectorInt16 aa; // [x, y, z] accel sensor measurements
VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements
VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements
VectorFloat gravity; // [x, y, z] gravity vector
float euler[3]; // [psi, theta, phi] Euler angle container
float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
// ================================================================
// === INTERRUPT DETECTION ROUTINE ===
// ================================================================
volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
mpuInterrupt = true;
}
// ================================================================
// === INITIAL SETUP ===
// ================================================================
void setup() {
// join I2C bus (I2Cdev library doesn't do this automatically)
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
Wire.begin();
Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(400, true);
#endif
Serial.begin(115200);
//Interrupt Pin
pinMode(3, INPUT); // Pin 2 ist INT0
digitalWrite(3, HIGH); // interner Pull up Widerstand auf 5V
attachInterrupt(1, interruptKnopfRoutine, LOW); // Pin 2 (INT 0) geht auf 0V (LOW) dann interruptRoutine aufrufen
//Use the led on the Arduino for startup indication.
pinMode(13, OUTPUT);
digitalWrite(13,HIGH); //Turn on the warning led.
// initialize device
Serial.println(F("Initializing I2C devices..."));
mpu.initialize();
pinMode(INTERRUPT_PIN, INPUT);
// verify connection
Serial.println(F("Testing device connections..."));
Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
// load and configure the DMP
Serial.println(F("Initializing DMP..."));
devStatus = mpu.dmpInitialize();
// supply your own gyro offsets here, scaled for min sensitivity
mpu.setXGyroOffset(71);
mpu.setYGyroOffset(47);
mpu.setZGyroOffset(-74);
mpu.setXAccelOffset(-1287); // 1688 factory default for my test chip
mpu.setYAccelOffset(-1412);
mpu.setZAccelOffset(1788);
// make sure it worked (returns 0 if so)
if (devStatus == 0) {
// turn on the DMP, now that it's ready
Serial.println(F("Enabling DMP..."));
mpu.setDMPEnabled(true);
// enable Arduino interrupt detection
Serial.print(F("Enabling interrupt detection (Arduino external interrupt "));
Serial.print(digitalPinToInterrupt(INTERRUPT_PIN));
Serial.println(F(")..."));
attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus();
// set our DMP Ready flag so the main loop() function knows it's okay to use it
Serial.println(F("DMP ready! Waiting for first interrupt..."));
dmpReady = true;
// get expected DMP packet size for later comparison
packetSize = mpu.dmpGetFIFOPacketSize();
} else {
// ERROR!
// 1 = initial memory load failed
// 2 = DMP configuration updates failed
// (if it's going to break, usually the code will be 1)
Serial.print(F("DMP Initialization failed (code "));
Serial.print(devStatus);
Serial.println(F(")"));
}
Serial.println("Kalibrierung gestartet! Drohne nicht bewegen!");
for (cal_int = 0; cal_int < 100 ; cal_int ++){ //Take 2000 readings for calibration.
rawDist = distanceSensor.measureDistanceCm(); //Messung der Distanz zum Boden in cm
maDist = movingAverage(rawDist);
dist_offset += maDist; //Ad distance value to dist_offset
delay(3); //Wait 3 milliseconds before the next loop.
}
//Now that we have 100 measures, we need to devide by 2000 to get the average offset.
dist_offset /= 100; //Devide the distance by 2000.
Serial.println("Kalibrierung beendet!");
pitchPID.SetOutputLimits(pid_max_pitch*(-1), pid_max_pitch);
rollPID.SetOutputLimits(pid_max_roll*(-1), pid_max_roll);
pitchPID.SetMode(AUTOMATIC);
rollPID.SetMode(AUTOMATIC);
pitchPID.SetControllerDirection(REVERSE);
rollPID.SetControllerDirection(REVERSE);
State = 0; //Set State back to 0.
//When everything is done, turn off the led.
digitalWrite(13,LOW); //Turn on the warning led.
}
// ================================================================
// === MAIN PROGRAM LOOP ===
// ================================================================
void loop() {
// if programming failed, don't try to do anything
if (!dmpReady) return;
// wait for MPU interrupt or extra packet(s) available
while (!mpuInterrupt && fifoCount < packetSize) {
if (mpuInterrupt && fifoCount < packetSize) {
// try to get out of the infinite loop
fifoCount = mpu.getFIFOCount();
}
// other program behavior stuff here
}
// reset interrupt flag and get INT_STATUS byte
mpuInterrupt = false;
mpuIntStatus = mpu.getIntStatus();
// get current FIFO count
fifoCount = mpu.getFIFOCount();
// check for overflow (this should never happen unless our code is too inefficient)
if ((mpuIntStatus & _BV(MPU6050_INTERRUPT_FIFO_OFLOW_BIT)) || fifoCount >= 1024) {
// reset so we can continue cleanly
mpu.resetFIFO();
fifoCount = mpu.getFIFOCount();
Serial.println(F("FIFO overflow!"));
// otherwise, check for DMP data ready interrupt (this should happen frequently)
} else if (mpuIntStatus & _BV(MPU6050_INTERRUPT_DMP_INT_BIT)) {
// wait for correct available data length, should be a VERY short wait
while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
// read a packet from FIFO
mpu.getFIFOBytes(fifoBuffer, packetSize);
// track FIFO count here in case there is > 1 packet available
// (this lets us immediately read more without waiting for an interrupt)
fifoCount -= packetSize;
// display Euler angles in degrees
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
angle_yaw_raw = (ypr[0] * 180/M_PI);
angle_pitch_raw = (ypr[1] * 180/M_PI);
angle_roll_raw = (ypr[2] * 180/M_PI);
pid_angle_yaw = angle_yaw_raw-winkel_offset_yaw;
pid_angle_pitch = angle_pitch_raw-winkel_offset_pitch;
pid_angle_roll = angle_roll_raw-winkel_offset_roll;
}
rawDist = distanceSensor.measureDistanceCm(); //Messung der Distanz zum Boden in cm
maDist = movingAverage(rawDist)-dist_offset; //Filterung der Distanz Messwerte mit gleitendem Mittelwertfilter
switch (State) {
////////////////////////////////////////////////////////////////////////////////////////////////////// Ruhezustand
case 0:
if(TextShown == 0){
Serial.println("State: Stop");
Serial.println("\nPress Button to Start!");
TextShown = 1;
}
myESC1.stop(); // tell ESC to stop
myESC2.stop(); // tell ESC to stop
myESC3.stop(); // tell ESC to stop
myESC4.stop(); // tell ESC to stop
break;
////////////////////////////////////////////////////////////////////////////////////////////////////// Arm-Zustand
case 1:
if(TextShown == 0){
Serial.println("State: Arm");
TextShown = 1;
}
myESC1.arm();
myESC2.arm();
myESC3.arm();
myESC4.arm();
delay(3000); // Wait for a while
break;
////////////////////////////////////////////////////////////////////////////////////////////////////// PID-Initialisierung
case 2:
if(TextShown == 0){
Serial.println("State: Initialisiere Flug");
TextShown = 1;
}
Serial.print("Höhe: ");Serial.print((int)maDist);
Serial.print(" \t\tWinkelX (Roll): ");Serial.print(angle_roll_raw);
Serial.print("\tWinkelY (Pitch): ");Serial.println(angle_pitch_raw);
// Serial.print("\tAbstand: ");Serial.println(maDist-dist_offset);
if(EnalbedFlying == 1){
State = 3;
TextShown = 0;
}
break;
////////////////////////////////////////////////////////////////////////////////////////////////////// Flug
case 3:
if(TextShown == 0){
Serial.println("State: Flug gestartet");
TextShown = 1;
}
pid_roll_setpoint = 0;
pid_pitch_setpoint = 0;
pid_yaw_setpoint = 0;
calculate_pid(); //PID inputs are known. So we can calculate the pid output.
esc_1 = (pid_output_roll/2) - (pid_output_pitch/2);// - pid_output_yaw; //Calculate the pulse for esc 1 (front-right - CCW) !!!! Drehrichtung beachten / kontrollieren!!!!
esc_2 = (pid_output_roll/2) + (pid_output_pitch/2);// + pid_output_yaw; //Calculate the pulse for esc 2 (rear-right - CW) !!!! Drehrichtung beachten / kontrollieren!!!!
esc_3 = ((pid_output_roll/2)*(-1)) + (pid_output_pitch/2);// - pid_output_yaw; //Calculate the pulse for esc 3 (rear-left - CCW) !!!! Drehrichtung beachten / kontrollieren!!!!
esc_4 = ((pid_output_roll/2)*(-1)) - (pid_output_pitch/2);// + pid_output_yaw; //Calculate the pulse for esc 4 (front-left - CW) !!!! Drehrichtung beachten / kontrollieren!!!!
if (esc_1 < -30) esc_1 = -30; //Keep the motors running.
if (esc_2 < -30) esc_2 = -30; //Keep the motors running.
if (esc_3 < -30) esc_3 = -30; //Keep the motors running.
if (esc_4 < -30) esc_4 = -30; //Keep the motors running.
if(esc_1 > 30)esc_1 = 30; //Limit the esc-1 pulse to 2000us.
if(esc_2 > 30)esc_2 = 30; //Limit the esc-2 pulse to 2000us.
if(esc_3 > 30)esc_3 = 30; //Limit the esc-3 pulse to 2000us.
if(esc_4 > 30)esc_4 = 30; //Limit the esc-4 pulse to 2000us.
esc_1 = (esc_1);//+30;
esc_2 = (esc_2);//+30;
esc_3 = (esc_3);//+30;
esc_4 = (esc_4);//+30;
myESC1.speed(esc_1+throttle);//+(int)throttleWithPID); // tell ESC to go to the oESC speed value
myESC2.speed(esc_2+throttle);//+(int)throttleWithPID); // tell ESC to go to the oESC speed value
myESC3.speed(esc_3+throttle);//+(int)throttleWithPID); // tell ESC to go to the oESC speed value
myESC4.speed(esc_4+throttle);//+(int)throttleWithPID); // tell ESC to go to the oESC speed value
Serial.print("Höhe: ");Serial.print((int)maDist);
Serial.print("\tESC 1: ");Serial.print(esc_1+throttle);
Serial.print("\tESC 2: ");Serial.print(esc_2+throttle);
Serial.print("\tESC 3: ");Serial.println(esc_3+throttle);
Serial.print("\tESC 4: ");Serial.println(esc_4+throttle);
delay(10);
break;
}
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//Interrupt Funktion
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void interruptKnopfRoutine() {
if((millis() - alteZeit) > entprellZeit) { // innerhalb der entprellZeit nichts machen
Serial.println("Knopf erkannt!");
TextShown = 0;
if(EnalbedFlying == 1){
if (State < 3) {
State += 1;
}
else {
State = 0;
}
}
else{
if (State < 2) {
State += 1;
}
else {
State = 0;
}
}
}
alteZeit = millis(); // letzte Schaltzeit merken
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//Subroutine for calculating pid outputs
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void calculate_pid(){
pitchPID.Compute();
rollPID.Compute();
// yawPID.Compute();
// throttlePID.Compute();
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//Moving Average Funktion
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
float movingAverage(float M) {
#define LM_SIZE 3
static float LM[LM_SIZE]; // LastMeasurements
static byte index = 0;
static float sum = 0;
static byte count = 0;
// keep sum updated to improve speed.
sum -= LM[index];
LM[index] = M;
sum += LM[index];
index++;
index = index % LM_SIZE;
if (count < LM_SIZE) count++;
return sum / count;
}
Tests & Vergleich der Regleransätze
?????????
Ergebnis
Hardware
Die Zielsetzung zur Hardware wurde vollständig erfüllt. Es mag zwar klingen, dass eine 118,22€ teure Drohne keine „Low-Cost-Drohne“ ist, jedoch sollte bedacht werden, dass es sich um einen Prototypen handelt und sich die Kosten der Drohne durch eine Massenfertigung deutlich reduzierten lassen. Des Weiteren wurde die Drohne so umgesetzt, dass sich einzelne Komponenten einfach austauschen lassen, falls diese beschädigt oder defekt sein sollten. Damit das Verletzungsrisiko durch die Drohne minimal ist, wurde eine Plattform aufgebaut, die die Bewegungen der Drohne einschränkt.
Software
????
Ausblick
Zusammenfassung
Um eine gewisse Orientierung zu erhalten, wurde zunächst Recherche betrieben und der Projektablaufplan aufgestellt. Im Anschluss darauf gelang es die notwendigen Bauteile zu erfassen und zu bestellen. In der ersten Hauptphase wurde die Hardware anhand von recherchierten Unterlagen und eigener Ideen realisiert. Dafür gab es eigene Richtlinien die bei der Umsetzung zu beachten waren. In der zweiten Hauptphase ging es um die Entwicklung der Software.?????????????????????????????????????????????????????????????????????????????????
Insgesamt betragen die Kosten für den Bau der Drohne 118,22€, dies sind jedoch die Kosten für den Prototypen und lassen sich in einer Massenfertigung stark reduzieren.
Lessons Learned
Die Realisierung einer kostengünstigen Drohne stellte eine enorme Herausforderung dar. Dabei konnte und musste man bereits erlerntes Wissen mit neuen Erfahrungen kombinieren, während man durchgehend unter Zeitdruck stand. Das simuliert mehr oder weniger das Leben eines Ingenieurs wieder. Das erlernte Grundwissen aus dem Mechatronik Bachelor sowie dem Business and Systems Engineering Master in den Bereichen der Elektrotechnik und Softwareentwicklung stellten eine gute Grundlage für das Projekt dar. Des Weiteren konnten durch kleine Fehler und Unachtsamkeiten (leider auf Kosten der Zeit) Erfahrungen gesammelt werden.
Im Detail:
- Teillösungen, mit einander kombinieren, aufeinander aufbauen und eigene Ideen einfließen lassen, um daraus eine neue Lösung zu erarbeiten
- Planung, wie die Hardware optimal umgesetzt werden kann. Dabei unterschiedliche Betrachtungswinkel einnehmen. Nicht nur funktionstechnischen Aufbau realisieren, sondern so konzepieren, dass defekte Teile schnell und problemlos ausgetauscht werden können
- Praktisches Arbeiten und Umgehen mit Werkzeugen z.B.: beim Sägen und Löten
- Den Sicherheitsaspekt in solchen Projekten ernst nehmen und nicht vernachlässigen
- Tiefenentladungsschutz??????????
- Alufolie?????????
Projektunterlagen
YouTube Video
Das YouTube Video als visueller Beleg steht unter folgendem Link zur Verfügung:
Weblinks
Literatur
---
→ zurück zur Übersicht: WS 18/19: Angewandte Elektrotechnik (BSE)