Merge pull request #3 from THOMAS-Projekt/Relais

Einbinden der Relay-Karte; Verbesserung der Hinderniserkennung; Protokoll-anpassungen für den Arduino; Einige Extra-Features

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,87 +178,34 @@ 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();
 	{
 		// 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();
 

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);

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();
 	}
 }
 
@@ -65,8 +61,8 @@ std::vector<short> CollisionDetection::CorrectWantedSpeed(short wantedSpeed[])
 	if(USensorMessurements.at(US_FRONT_LEFT) > _warnDistanceRL || USensorMessurements.at(US_FRONT_LEFT) == 0)
 	{
 		// Nach links fahren
-		newWantedSpeed.at(0) = 0;
-		newWantedSpeed.at(1) = 32767;
+		newWantedSpeed.at(0) = -100;
+		newWantedSpeed.at(1) = 100;
 
 		return newWantedSpeed;
 	}
@@ -75,8 +71,8 @@ std::vector<short> CollisionDetection::CorrectWantedSpeed(short wantedSpeed[])
 	if(USensorMessurements.at(US_FRONT_RIGHT) > _warnDistanceRL || USensorMessurements.at(US_FRONT_RIGHT) == 0)
 	{
 		// Nach rechts fahren
-		newWantedSpeed.at(0) = 32767;
-		newWantedSpeed.at(1) = 0;
+		newWantedSpeed.at(0) = 100;
+		newWantedSpeed.at(1) = -100;
 
 		return newWantedSpeed;
 

CollisionDetection.h

@@ -40,10 +40,10 @@ namespace THOMAS
 		ArduinoProtocol *_arduinoProtocol;
 
 		// Ab wann soll die Hindernisserkennung eingreifen (in cm)
-		int _warnDistance = 50;
+		int _warnDistance = 30;
 
 		// Ab wann soll die Hindernisserkennung eingreifen - Rechte und Linke Sensoren (in cm)
-		int _warnDistanceRL = 100;
+		int _warnDistanceRL = 40;
 
 		// Die Tolleranz der Messungen
 		int _tolerance = 10;

Makefile

@@ -2,8 +2,8 @@ CPPARGS= -std=c++11 -pthread
 OPENCVARGS= `pkg-config --cflags opencv`
 LINKERARGS= -pthread -g -O0 -v -da -Q `pkg-config --libs opencv`
 
-all: main.o RS232.o ArduinoCom.o ArduinoProtocol.o TCPServer.o MotorControl.o THOMASException.o TelemetryReceiver.o UDPClient.o StatusInformation.o CollisionDetection.o
-	g++ $(OPENCVARGS) THOMASException.o RS232.o ArduinoCom.o ArduinoProtocol.o TCPServer.o MotorControl.o TelemetryReceiver.o UDPClient.o StatusInformation.o main.o CollisionDetection.o -o thomas $(LINKERARGS)
+all: main.o RS232.o ArduinoCom.o ArduinoProtocol.o TCPServer.o MotorControl.o THOMASException.o TelemetryReceiver.o UDPClient.o StatusInformation.o CollisionDetection.o RelayProtocol.o
+	g++ $(OPENCVARGS) THOMASException.o RS232.o ArduinoCom.o ArduinoProtocol.o TCPServer.o MotorControl.o TelemetryReceiver.o UDPClient.o StatusInformation.o main.o CollisionDetection.o RelayProtocol.o -o thomas $(LINKERARGS)
 
 main.o: main.cpp
 	g++ -c main.cpp $(CPPARGS)
@@ -47,6 +47,9 @@ StatusInformation.o: StatusInformation.cpp StatusInformation.h
 CollisionDetection.o: CollisionDetection.cpp CollisionDetection.h
 	g++ -c CollisionDetection.cpp $(CPPARGS)
 
+RelayProtocol.o: RelayProtocol.cpp RelayProtocol.h 
+	g++ -c RelayProtocol.cpp $(CPPARGS)
+
 MotorControlTest.o: MotorControlTest.cpp
 	g++ -c MotorControlTest.cpp $(CPPARGS)
 

MotorControl.cpp

@@ -14,6 +14,9 @@ using namespace THOMAS;
 // CollisionDetection-Klasse
 #include "CollisionDetection.h"
 
+// RelayProtocol-Klasse
+#include "RelayProtocol.h"
+
 // C++-Stringstream-Klasse
 // Diese Klasse erlaubt die Verkettung von Zeichenfolgen, sie wird hier für die Erzeugung von aussagekräftigen Exeptions benötigt.
 #include <sstream>
@@ -71,8 +74,12 @@ void MotorControl::Run(ArduinoProtocol *arduinoProtocol)
 	// RS232-Verbindung herstellen
 	_rs232 = new RS232();
 
+	// CollisionDetection initialisieren
 	_collisionDetection = new CollisionDetection(_arduino);
 
+	// Relay-Steuerung initialisieren
+	_relayProtocol = new RelayProtocol();
+
 	// Motorgeschwindigkeitsanpassung starten
 	_controlMotorSpeedThread = new std::thread(&MotorControl::ControlMotorSpeedWrapper, this);
 
@@ -122,6 +129,9 @@ void MotorControl::ControlMotorSpeed()
 	// Motorgeschwindigkeit kontinuierlich regeln, bis die Motorsteuerung beendet wird
 	float corrSum; // Joystick-Korrektursumme
 	short wantedSpeed[2] = {0, 0}; // Die angestrebte Geschwindigkeit {links, rechts}
+	short newWantedSpeed[2] = {0,0}; // Anhand der Sensordaten korrigierte Geschwindigkeit
+	bool overwriteSpeed = false; // Gibt an ob die Beschleuningung umgangen werden soll
+
 	std::vector<short> speedVector (2); // Vector mit der Geschwindigkeit
 
 	while(_running)
@@ -161,12 +171,19 @@ void MotorControl::ControlMotorSpeed()
 
 		// Auf Hindernisse prüfen und ggf. Werte ändern
 		speedVector = _collisionDetection->CorrectWantedSpeed(wantedSpeed);
-		std::copy(speedVector.begin(), speedVector.end(), wantedSpeed);
+		std::copy(speedVector.begin(), speedVector.end(), newWantedSpeed);
+
+		// Prüfen ob die Beschleunigung umgangen werden soll
+		overwriteSpeed = !(wantedSpeed[0] == newWantedSpeed[0] && wantedSpeed[1] == newWantedSpeed[1]);
+
+		// Korrigierte Geschwindigkeit übernehmen
+		wantedSpeed[0] = newWantedSpeed[0];
+		wantedSpeed[1] = newWantedSpeed[1];
 
 		// Geschwindigkeitswerte angleichen: Links
 		{
 			// Würde die direkte Annahme der Wunschgeschwindigkeit in diesem Takt die Maximalbeschleunigung überschreiten?
-			if(abs(wantedSpeed[MLEFT_ARR] - _lastSpeed[MLEFT_ARR]) > _speedMaxAcc)
+			if(!overwriteSpeed && abs(wantedSpeed[MLEFT_ARR] - _lastSpeed[MLEFT_ARR]) > _speedMaxAcc)
 			{
 				// Motor angemessen beschleunigen, auch wenn Wunschgeschwindigkeit nicht erreicht wird
 				// Der ternäre Operator ist hier die schönste und kürzeste Variante; er passt nur je nach Vorzeichen der Geschwindigkeitsdifferenz das Vorzeichen der Beschleunigung an.
@@ -182,7 +199,7 @@ void MotorControl::ControlMotorSpeed()
 		// Geschwindigkeitswerte angleichen: Rechts
 		{
 			// Überschreitung der Maximalbeschleunigung?
-			if(abs(wantedSpeed[MRIGHT_ARR] - _lastSpeed[MRIGHT_ARR]) > _speedMaxAcc)
+			if(!overwriteSpeed && abs(wantedSpeed[MRIGHT_ARR] - _lastSpeed[MRIGHT_ARR]) > _speedMaxAcc)
 			{
 				// Motor angemessen beschleunigen
 				SendMotorSpeed(MRIGHT, _lastSpeed[MRIGHT_ARR] + (wantedSpeed[MRIGHT_ARR] > _lastSpeed[MRIGHT_ARR] ? _speedMaxAcc : -_speedMaxAcc));
@@ -210,6 +227,11 @@ void MotorControl::ComputeInputButtons()
 
 	// Tastendrücke bis zum Beenden der Motorsteuerung verarbeiten
 	bool kill_server = false; // Beenden-Anfrage
+	bool horn = false; // Hupe
+	bool radio = false; // Radio
+	bool radioButtonPressed = false; // Radio-Knopf gedrückt
+	bool radioIsPlaying = false; // Radio aktiv?
+
 	int cam_servo_direction = 0; // Richtung in den der Servo gedreht werden soll
 	while(_running)
 	{
@@ -221,6 +243,12 @@ void MotorControl::ComputeInputButtons()
 				// Not-Aus-Schalter abfragen
 				kill_server = (_joystickButtons[6] == 1);
 
+				// Feuerknopf abfragen
+				horn = (_joystickButtons[0] == 1);
+
+				// Radio Button
+				radio = (_joystickButtons[5] == 1);
+
 				// Servosteuerung nach Wert des kleinen Steuerknüppels
 				cam_servo_direction = _joystickAxis[4] < 0 ? -1 : _joystickAxis[4] > 0 ? 1 : 0;
 			}
@@ -237,6 +265,38 @@ void MotorControl::ComputeInputButtons()
 			throw THOMASException("Info: Der Server wurde durch einen der Clienten beendet.");
 		}
 
+		// Wurde der Status verändert?
+		if(horn != _hornActive)
+		{
+			// Relay schalten
+			_relayProtocol->SetRelay(1, horn);
+
+			// Status merken
+			_hornActive = horn;
+		}
+
+		// Wurde der Button losgelassen?
+		if (radioButtonPressed && !radio)
+		{
+			// Spielt das Radio ab?
+			if (radioIsPlaying)
+			{
+				// Radio beenden
+				system("./radio stop");
+			}
+			else
+			{
+				// Radio starten
+				system("./radio start");
+			}
+
+			// Radiozustand merken
+			radioIsPlaying = !radioIsPlaying;
+		}
+
+		// Tastenzustand merken
+		radioButtonPressed = radio;
+
 		// Kamera-Servo drehen?
 		if(cam_servo_direction != 0)
 		{

MotorControl.h

@@ -21,6 +21,9 @@ Verzögerte Beschleunigungen werden eingesetzt, um abrupte Geschwindigkeitsände
 // CollisionDetection-Klasse
 #include "CollisionDetection.h"
 
+// RelayProtocol-Klasse
+#include "RelayProtocol.h"
+
 // C++-mutex-Klasse
 #include <mutex>
 
@@ -75,6 +78,9 @@ namespace THOMAS
 		// Gibt an, ob die Steuerung aktiv ist.
 		bool _running = false;
 
+		// Hupt das Horn?
+		bool _hornActive = false;
+
 		// Arduino-Kommunikation
 		ArduinoProtocol *_arduino;
 
@@ -87,6 +93,9 @@ namespace THOMAS
 		// Kollision-Detection Klasse
 		CollisionDetection *_collisionDetection;
 
+		// RelayProtocl Klasse
+		RelayProtocol *_relayProtocol;
+
 		// Der Motorgeschwindigkeits-Anpassungs-Thread.
 		std::thread *_controlMotorSpeedThread;