From 48f5f59c57d01f37a6b04c797b651c17ae8e5ec4 Mon Sep 17 00:00:00 2001 From: Waishon Date: Fri, 17 Jul 2015 21:01:33 +0200 Subject: [PATCH 1/8] =?UTF-8?q?RelayProtocol=20hinzugef=C3=BCgt?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Makefile | 7 +- RelayProtocol.cpp | 183 ++++++++++++++++++++++++++++++++++++++++++++++ RelayProtocol.h | 46 ++++++++++++ 3 files changed, 234 insertions(+), 2 deletions(-) create mode 100644 RelayProtocol.cpp create mode 100644 RelayProtocol.h diff --git a/Makefile b/Makefile index 7b11d79..1433e06 100644 --- a/Makefile +++ b/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) diff --git a/RelayProtocol.cpp b/RelayProtocol.cpp new file mode 100644 index 0000000..72f2328 --- /dev/null +++ b/RelayProtocol.cpp @@ -0,0 +1,183 @@ +/* +-- RelayProtocol-KLASSE :: IMPLEMENTIERUNG -- +*/ + + +/* INCLUDES */ + +// Klassenheader +#include "RelayProtocol.h" +using namespace THOMAS; + +// Enthält Methoden für I/O +#include + +// Utilities Klasse +#include + +// Enhält Methoden für Strings +#include + +// POSIX-Definition +#include + +// File-Control-Definitionen +#include + +// Terminal-IO-Definitionen +#include + +// IOStream Klasse +#include + +RelayProtocol::RelayProtocol() +{ + // Anschlussoptionen Struktur + struct termios options; + + // Anschlusshandle erstellen + _handle = open("/dev/ttyUSB1", O_RDWR | O_NOCTTY | O_NDELAY); + + // Anschluss-Flags leeren + fcntl(_handle, F_SETFL, 0); + + // Aktuelle Anschluss-Optionen abrufen + tcgetattr(_handle, &options); + + // Baundrate setzen (19200) + cfsetispeed(&options, B19200); // Input + cfsetospeed(&options, B19200); // Output + + options.c_cflag &= ~PARENB; // kein Partybit + options.c_cflag &= ~CSTOPB; // 1 Stopbit + options.c_cflag &= ~CSIZE; // 8 Datenbits + options.c_cflag |= CS8; + options.c_cflag |= (CLOCAL | CREAD); // CD Signal ignorieren + + options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG); + options.c_oflag &= ~OPOST; // "raw" Input + options.c_cc[VMIN] = 0; // Auf 0 Zeichen warten + options.c_cc[VTIME] = 10; // Timeout 10 Sekunden + + // Einstellungen flushen + tcflush(_handle,TCIOFLUSH); + + tcsetattr(_handle, TCSAFLUSH, &options); + + fcntl(_handle, F_SETFL, FNDELAY); + fcntl(_handle, F_SETFL, 0); + + // Relay initialisieren + SendCommand(1, 1, 0); + + // Status lesen + if(!ReceiveStatus()) + std::cout << "Fehler beim Initialisieren des Relays" << std::endl; +} + +void RelayProtocol::SendCommand(UBYTE command, UBYTE address, UBYTE data) +{ + // Neues Buffer Array erstellen + UBYTE buff[4]; + + // Buffer füllen + buff[0] = command; + buff[1] = address; + buff[2] = data; + + // Prüfsumme erstellen + buff[3] = buff[0]^buff[1]^buff[2]; + + // 50ms warten + usleep(50000); + + // Daten absenden und ggf. Fehler ausgeben + if(!write(_handle, buff, 4)) + std::cout << "Fehler beim Senden eines Statusbytes an das Relay!" << std::endl; +} + +bool RelayProtocol::ReceiveStatus() +{ + // Neues Buffer Array erstellen + UBYTE buff[4]; + + // Daten einlesen + read(_handle, buff, 4); + + // Prüfsumme berechnen + int pruefsumme = buff[0]^buff[1]^buff[2]; + + // Stimmen die Prüfsummen überein? + if(pruefsumme != buff[3]) + return false; + + return true; +} + +void RelayProtocol::SetRelay(int port, int status) +{ + // Port Value + UBYTE val = -1; + + // Status prüfen + switch (status) + { + // Relay ausschalten + case OFF: + { + // Port in Binary umwandeln + val = ~(1 << (port - 1)); + + break; + } + + // Relay anschalten + case ON: + { + // Port in Binary umwandeln + val = 1 << (port - 1); + + break; + } + } + + // Status senden + SendCommand(3, 1, val); + + // Read-Buffer leeren + ReceiveStatus(); +} + +void RelayProtocol::SetAll(int status) +{ + // Port Value + UBYTE val = -1; + + // Status prüfen + switch (status) + { + // Relay ausschalten + case OFF: + { + // Port in Binary umwandeln + val = 0; + + break; + } + + // Relay anschalten + case ON: + { + // Port in Binary umwandeln + val = 255; + + break; + } + } + + // Status senden + SendCommand(3, 1, val); + + // Read-Buffer leeren + ReceiveStatus(); +} diff --git a/RelayProtocol.h b/RelayProtocol.h new file mode 100644 index 0000000..bb6f2ed --- /dev/null +++ b/RelayProtocol.h @@ -0,0 +1,46 @@ +#pragma once +/* +-- RelayProtocol-Klasse :: HEADER -- +Definiert die RelayProtocol-Klasse +Diese Klasse übernimmt die direkte Steuerung des Relays +*/ + + +/* INCLUDES */ + + +/* KONSTANTEN */ + +// BYTE-Typ. +#define BYTE char +#define UBYTE unsigned char + +#define OFF 0 +#define ON 1 + +/* KLASSE */ +namespace THOMAS +{ + + class RelayProtocol + { + private: + // USB-Handle + int _handle; + + void SendCommand(UBYTE command, UBYTE address, UBYTE data); + + bool ReceiveStatus(); + + public: + // Konstruktor + // Stellt die Verbindung zum Relay her und initialisiert diese + RelayProtocol(); + + // Setzt den Status eines Ports + void SetRelay(int port, int status); + + // Setzt alle Ports auf den jeweiligen Status + void SetAll(int status); + }; +} From 75b8489bd948ed4004d0c61c07c695627aa9dba8 Mon Sep 17 00:00:00 2001 From: Waishon Date: Fri, 17 Jul 2015 21:24:22 +0200 Subject: [PATCH 2/8] =?UTF-8?q?Sch=C3=B6nheitskorrekturen?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- RelayProtocol.cpp | 19 +++++++++++-------- RelayProtocol.h | 9 ++++----- 2 files changed, 15 insertions(+), 13 deletions(-) diff --git a/RelayProtocol.cpp b/RelayProtocol.cpp index 72f2328..451ed68 100644 --- a/RelayProtocol.cpp +++ b/RelayProtocol.cpp @@ -48,6 +48,7 @@ RelayProtocol::RelayProtocol() cfsetispeed(&options, B19200); // Input cfsetospeed(&options, B19200); // Output + // Schnittstellen-Flags setzen options.c_cflag &= ~PARENB; // kein Partybit options.c_cflag &= ~CSTOPB; // 1 Stopbit options.c_cflag &= ~CSIZE; // 8 Datenbits @@ -62,6 +63,7 @@ RelayProtocol::RelayProtocol() // Einstellungen flushen tcflush(_handle,TCIOFLUSH); + // Attribute setzen tcsetattr(_handle, TCSAFLUSH, &options); fcntl(_handle, F_SETFL, FNDELAY); @@ -86,7 +88,7 @@ void RelayProtocol::SendCommand(UBYTE command, UBYTE address, UBYTE data) buff[2] = data; // Prüfsumme erstellen - buff[3] = buff[0]^buff[1]^buff[2]; + buff[3] = buff[0] ^ buff[1] ^ buff[2]; // 50ms warten usleep(50000); @@ -105,16 +107,17 @@ bool RelayProtocol::ReceiveStatus() read(_handle, buff, 4); // Prüfsumme berechnen - int pruefsumme = buff[0]^buff[1]^buff[2]; + int pruefsumme = buff[0] ^ buff[1] ^ buff[2]; // Stimmen die Prüfsummen überein? if(pruefsumme != buff[3]) return false; + // Erfolg zurückgeben return true; } -void RelayProtocol::SetRelay(int port, int status) +void RelayProtocol::SetRelay(int port, bool status) { // Port Value UBYTE val = -1; @@ -123,7 +126,7 @@ void RelayProtocol::SetRelay(int port, int status) switch (status) { // Relay ausschalten - case OFF: + case false: { // Port in Binary umwandeln val = ~(1 << (port - 1)); @@ -132,7 +135,7 @@ void RelayProtocol::SetRelay(int port, int status) } // Relay anschalten - case ON: + case true: { // Port in Binary umwandeln val = 1 << (port - 1); @@ -148,7 +151,7 @@ void RelayProtocol::SetRelay(int port, int status) ReceiveStatus(); } -void RelayProtocol::SetAll(int status) +void RelayProtocol::SetAll(bool status) { // Port Value UBYTE val = -1; @@ -157,7 +160,7 @@ void RelayProtocol::SetAll(int status) switch (status) { // Relay ausschalten - case OFF: + case false: { // Port in Binary umwandeln val = 0; @@ -166,7 +169,7 @@ void RelayProtocol::SetAll(int status) } // Relay anschalten - case ON: + case true: { // Port in Binary umwandeln val = 255; diff --git a/RelayProtocol.h b/RelayProtocol.h index bb6f2ed..912f0c8 100644 --- a/RelayProtocol.h +++ b/RelayProtocol.h @@ -15,9 +15,6 @@ Diese Klasse übernimmt die direkte Steuerung des Relays #define BYTE char #define UBYTE unsigned char -#define OFF 0 -#define ON 1 - /* KLASSE */ namespace THOMAS { @@ -28,8 +25,10 @@ namespace THOMAS // USB-Handle int _handle; + // Sendet die Bytes an das Relay void SendCommand(UBYTE command, UBYTE address, UBYTE data); + // Relay Status bool ReceiveStatus(); public: @@ -38,9 +37,9 @@ namespace THOMAS RelayProtocol(); // Setzt den Status eines Ports - void SetRelay(int port, int status); + void SetRelay(int port, bool status); // Setzt alle Ports auf den jeweiligen Status - void SetAll(int status); + void SetAll(bool status); }; } From 92e1c26aabba2ead62bf56cd591c148cbbee2f2a Mon Sep 17 00:00:00 2001 From: Waishon Date: Sat, 18 Jul 2015 14:22:41 +0200 Subject: [PATCH 3/8] =?UTF-8?q?Hupe=20hinzugef=C3=BCgt?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- MotorControl.cpp | 22 ++++++++++++++++++++++ MotorControl.h | 9 +++++++++ 2 files changed, 31 insertions(+) diff --git a/MotorControl.cpp b/MotorControl.cpp index 76b7e13..6727c23 100644 --- a/MotorControl.cpp +++ b/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 @@ -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); @@ -210,6 +217,8 @@ void MotorControl::ComputeInputButtons() // Tastendrücke bis zum Beenden der Motorsteuerung verarbeiten bool kill_server = false; // Beenden-Anfrage + bool horn = false; // Hupe + int cam_servo_direction = 0; // Richtung in den der Servo gedreht werden soll while(_running) { @@ -221,6 +230,9 @@ void MotorControl::ComputeInputButtons() // Not-Aus-Schalter abfragen kill_server = (_joystickButtons[6] == 1); + // Feuerknopf abfragen + horn = (_joystickButtons[0] == 1); + // Servosteuerung nach Wert des kleinen Steuerknüppels cam_servo_direction = _joystickAxis[4] < 0 ? -1 : _joystickAxis[4] > 0 ? 1 : 0; } @@ -237,6 +249,16 @@ 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; + } + // Kamera-Servo drehen? if(cam_servo_direction != 0) { diff --git a/MotorControl.h b/MotorControl.h index 08f9099..3c90fe2 100644 --- a/MotorControl.h +++ b/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 @@ -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; From 6013bf78f1bf70411354bebb39aaf1080e58e7c7 Mon Sep 17 00:00:00 2001 From: thomas Date: Sat, 18 Jul 2015 16:24:22 +0200 Subject: [PATCH 4/8] =?UTF-8?q?Relay=C3=A4nderungen?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- RelayProtocol.cpp | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/RelayProtocol.cpp b/RelayProtocol.cpp index 451ed68..e271178 100644 --- a/RelayProtocol.cpp +++ b/RelayProtocol.cpp @@ -36,7 +36,7 @@ RelayProtocol::RelayProtocol() struct termios options; // Anschlusshandle erstellen - _handle = open("/dev/ttyUSB1", O_RDWR | O_NOCTTY | O_NDELAY); + _handle = open("/dev/ttyUSB0", O_RDWR | O_NOCTTY | O_NDELAY); // Anschluss-Flags leeren fcntl(_handle, F_SETFL, 0); @@ -122,6 +122,9 @@ void RelayProtocol::SetRelay(int port, bool status) // Port Value UBYTE val = -1; + // Enhält die Relay Daten + UBYTE rval; + // Status prüfen switch (status) { @@ -131,6 +134,9 @@ void RelayProtocol::SetRelay(int port, bool status) // Port in Binary umwandeln val = ~(1 << (port - 1)); + // Bitweises UND + rval = rval & val; + break; } @@ -140,12 +146,15 @@ void RelayProtocol::SetRelay(int port, bool status) // Port in Binary umwandeln val = 1 << (port - 1); + // Bitweises ODER + rval = rval | val; + break; } } // Status senden - SendCommand(3, 1, val); + SendCommand(3, 1, rval); // Read-Buffer leeren ReceiveStatus(); From ffbef16a5036b1dd993250d61629f6cfa891b063 Mon Sep 17 00:00:00 2001 From: thomas Date: Sat, 18 Jul 2015 16:58:40 +0200 Subject: [PATCH 5/8] =?UTF-8?q?Funktion=20zum=20Umgehen=20der=20Beschleuni?= =?UTF-8?q?gung=20eingef=C3=BCgt?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- MotorControl.cpp | 16 +++++++++++++--- 1 file changed, 13 insertions(+), 3 deletions(-) diff --git a/MotorControl.cpp b/MotorControl.cpp index 6727c23..cc81444 100644 --- a/MotorControl.cpp +++ b/MotorControl.cpp @@ -129,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 speedVector (2); // Vector mit der Geschwindigkeit while(_running) @@ -168,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. @@ -189,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)); From 11b630c573312df74ffef2133f3f653e2c11354d Mon Sep 17 00:00:00 2001 From: thomas Date: Sat, 18 Jul 2015 17:59:43 +0200 Subject: [PATCH 6/8] Alle Ultraschallmesswerte auf einemal abrufen (Made by Soeren) --- ArduinoProtocol.cpp | 86 +++++++++--------------------------------- ArduinoProtocol.h | 13 +++---- CollisionDetection.cpp | 20 ++++------ 3 files changed, 31 insertions(+), 88 deletions(-) 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 +// Vector-Klasse +#include + // UNIX-Standard-Funktionen [Non-Standard] // Die sleep()-Funktion wird benötigt. #include @@ -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 ArduinoProtocol::GetDistance() { // Speicher für die Daten - int data; + std::vector 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 +// Vector Klasse +#include + /* 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 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 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(); } } From 7c84cf6b95e1459395cc797e56f0b001598b24c1 Mon Sep 17 00:00:00 2001 From: thomas Date: Sat, 18 Jul 2015 18:55:08 +0200 Subject: [PATCH 7/8] Hinderniserkennung optimiert --- CollisionDetection.cpp | 8 ++++---- CollisionDetection.h | 4 ++-- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/CollisionDetection.cpp b/CollisionDetection.cpp index 02923d7..5311de1 100644 --- a/CollisionDetection.cpp +++ b/CollisionDetection.cpp @@ -61,8 +61,8 @@ std::vector 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; } @@ -71,8 +71,8 @@ std::vector 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; diff --git a/CollisionDetection.h b/CollisionDetection.h index 940c7f2..e4a8f7b 100644 --- a/CollisionDetection.h +++ b/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; From 6fcd135c6f5c50ba77677e6afabf3abbc6bfbe8e Mon Sep 17 00:00:00 2001 From: thomas Date: Sat, 18 Jul 2015 19:53:29 +0200 Subject: [PATCH 8/8] Implementierung eines... *hust* ...Radios! --- MotorControl.cpp | 28 ++++++++++++++++++++++++++++ radio | 8 ++++++++ 2 files changed, 36 insertions(+) create mode 100755 radio diff --git a/MotorControl.cpp b/MotorControl.cpp index cc81444..6459d88 100644 --- a/MotorControl.cpp +++ b/MotorControl.cpp @@ -228,6 +228,9 @@ 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) @@ -243,6 +246,9 @@ void MotorControl::ComputeInputButtons() // 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; } @@ -269,6 +275,28 @@ void MotorControl::ComputeInputButtons() _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) { diff --git a/radio b/radio new file mode 100755 index 0000000..43d78a6 --- /dev/null +++ b/radio @@ -0,0 +1,8 @@ +#!/bin/bash +if [ "$1" == "start" ]; then + screen -d -m -S radio mplayer http://mp3ad.energybremen.c.nmdn.net/energybremen_mobile/livestream.mp3 +fi + +if [ "$1" == "stop" ]; then + screen -X -S radio quit +fi