diff --git a/ArduinoProtocol.cpp b/ArduinoProtocol.cpp
index 4a0738c..067a422 100644
--- a/ArduinoProtocol.cpp
+++ b/ArduinoProtocol.cpp
@@ -20,6 +20,9 @@ using namespace THOMAS;
 // CString - Enthält underanderem die strcpy Funktion
 #include <cstring>
 
+// Vector-Klasse
+#include <vector>
+
 // UNIX-Standard-Funktionen [Non-Standard]
 // Die sleep()-Funktion wird benötigt.
 #include <unistd.h>
@@ -175,11 +178,11 @@ void ArduinoProtocol::WriteMessage(std::string text, unsigned char priority)
 	arduinoMutex->unlock();
 }
 
-// Gibt den Messwert des angegebenen Ultraschallsensors in cm zurück
-int ArduinoProtocol::GetDistance(unsigned char sensorID)
+// Gibt den Messwert aller Ultraschallsensoren in cm zurück
+std::vector<int> ArduinoProtocol::GetDistance()
 {
 	// Speicher für die Daten
-	int data;
+	std::vector<int> data(5);
 
 	// Kommunikation sperren
 	arduinoMutex->lock();
@@ -187,75 +190,22 @@ int ArduinoProtocol::GetDistance(unsigned char sensorID)
 		// Paket erstellen:
 		// 2 = Sensoren ansprechen
 		// 0 = Ultraschallsensoren
-		// x = SensorID
-		// 2 = Messwet abrufen
-		UBYTE package[4] = {2, 0, sensorID, 2};
+		// 0 = Messwete abrufen
+		UBYTE package[3] = {2, 0, 0};
 
 		// Sende das Paket an den Arduino
-		arduinoCom->Send(package, 4);
-
-		// Gelesenen Wert in cm umrechnen und zurückgeben FIXME: Den Wert stattdessen als genaueren Integer übertragen.
-		data = (int) arduinoCom->Receive()[0] * 2;
-	}
-	arduinoMutex->unlock();
-
-	// Daten zurückgeben
-	return data;
-}
-
-// Gibt den Messwert zurück. Dabei werden Fehlmessungen ausgeschlossen
-int ArduinoProtocol::GetRealDistance(unsigned char sensorID, int tolerance)
-{
-	int distance[2];
-
-	// Maximale Messungen
-	int max = 10;
-
-	int i = 0;
-	do
-	{
-		// Erste Messung durchführen
-		distance[0] = GetDistance(sensorID);
-
-		// TODO: Evtl. anpassen
-		usleep(10000);
-
-		// Zweite Messung durchführen
-		distance[1] = GetDistance(sensorID);
+		arduinoCom->Send(package, 3);
 
-		// Zähler erhöhen
-		i++;
-	}
-	while(!(distance[0] - distance[1] <= tolerance && distance[0] - distance[1] >= (-1) * tolerance) && i != max);
+		// Werte in Array konvertieren
+		unsigned char buff[5] = {};
+		memcpy(buff, arduinoCom->Receive(), 5);
 
-	// Messwerte zurückgeben
-	return (distance[0] + distance[1]) / 2;
-}
-
-// Ruft den Status des angegebenen Sensors ab, bzw. aktualisiert diesen vorher
-int ArduinoProtocol::GetStatus(unsigned char sensorID, bool newRequest)
-{
-	// Speicher für die Daten
-	int data;
-
-	// Kommunikation sperren
-	arduinoMutex->lock();
-	{
-		// Wähle zwischen aktuallisieren und abrufen
-		UBYTE requestType = newRequest ? 0 : 1;
-
-		// Paket erstellen:
-		// 2 = Sensoren ansprechen
-		// 0 = Ultraschallsensoren
-		// x = SensorID
-		// 1/2 Status aktuallisieren/abrufen
-		UBYTE package[4] = {2, 0, sensorID, requestType};
-
-		// Sende das Paket an den Arduino
-		arduinoCom->Send(package, 4);
-
-		// Auf eine Antwort warten und den Status zurückgeben
-		data = (int) arduinoCom->Receive()[0];
+		// Messwerte durchlaufen
+		for(int i = 0; i < 5; i++)
+		{
+			// Messwert in cm umrechnen und zum Vector hinzufügen
+			data.at(i) = (int) buff[i] * 2;
+		}
 	}
 	arduinoMutex->unlock();
 
diff --git a/ArduinoProtocol.h b/ArduinoProtocol.h
index 56d7336..dac080b 100644
--- a/ArduinoProtocol.h
+++ b/ArduinoProtocol.h
@@ -20,6 +20,9 @@ Diese Klasse enthält das Protokoll zur Kommunikation mit dem Arduino
 // C++-mutex-Klasse
 #include <mutex>
 
+// Vector Klasse
+#include <vector>
+
 /* KONSTANTEN */
 
 // BYTE-Typ.
@@ -96,14 +99,8 @@ namespace THOMAS
 		// Setze Text auf LCD
 		void WriteMessage(std::string text, unsigned char priority);
 
-		// Entfernung des Ultraschallsensors auslesen
-		int GetDistance(unsigned char sensorID);
-
-		// Echte Entfernung bestimmen
-		int GetRealDistance(unsigned char sensorID, int tolerance);
-
-		// Sensorstatus abrufen und evtl. vorher aktualisieren
-		int GetStatus(unsigned char sensorID, bool newRequest);
+		// Messwerte der Ultraschallsensoren abrufen
+		std::vector<int> GetDistance();
 
 		// Position der Kamera setzen
 		int SetCamPosition(unsigned char camera, unsigned char degree);
diff --git a/CollisionDetection.cpp b/CollisionDetection.cpp
index 8c6d6aa..02923d7 100644
--- a/CollisionDetection.cpp
+++ b/CollisionDetection.cpp
@@ -28,21 +28,17 @@ void CollisionDetection::UpdateUSensorData()
 	// Wiederhole immer
 	while(true)
 	{
-		// Durch die Sensoren iterieren
-		for(int sensorID = 0; sensorID < SENSOR_COUNT; sensorID++)
-		{
-			// Messwert abrufen und speichern
-			int readDistance = _arduinoProtocol->GetRealDistance(sensorID, _tolerance);
+		// Messwerte abrufen und speichern
+		std::vector<int> distance = _arduinoProtocol->GetDistance();
 
-			// Zugriff von anderen Threads sperren
-			USensorMutex.lock();
+		// Zugriff von anderen Threads sperren
+		USensorMutex.lock();
 
-			// Aktuellen Messwert in das Array an entsprechende Stelle kopieren
-			USensorMessurements.at(sensorID) = readDistance;
+		// Aktuelle Messwerte in das Array kopieren
+		USensorMessurements = distance;
 
-			// Zugriff von anderen Threads erlauben
-			USensorMutex.unlock();
-		}
+		// Zugriff von anderen Threads erlauben
+		USensorMutex.unlock();
 	}
 }