Too big too handle

AutonomousMode/ControlModule/control_module.cpp

@@ -1,16 +1,16 @@
 #include "control_module.h"
 
-ControlModule::ControlModule(): ControlModule(1.0, 1.0, 1.0)
+ControlModule::ControlModule(): ControlModule(0.0, 1.0, 1.0)
 {    
 }
 
-ControlModule::ControlModule(float forwardSpeed, float rotateSpeed, float moveSideSpeed)
+ControlModule::ControlModule(double maxSpeed, double rotateSpeed, double moveSideSpeed)
 {
     motors[0] = 0.0;
     motors[1] = 0.0;
     motors[2] = 0.0;
     motors[3] = 0.0;
-    this->forwardSpeed = forwardSpeed;
+    this->maxSpeed = maxSpeed;
     this->rotateSpeed = rotateSpeed;
     this->moveSideSpeed = moveSideSpeed;
 }
@@ -19,30 +19,53 @@ ControlModule::~ControlModule()
 {
 }
 
-void ControlModule::moveForward(){
+void ControlModule::adjustSpeed(){
-    for(int i = 0; i < 4; i++){
+    double factor = 0.0;
-        motors[i] += forwardSpeed;
+    if(maxSpeed > 1.0)
+    {
+        factor = 1.0;
+    }
+    else if(maxSpeed < -1.0)
+    {
+        factor = -1.0;
+    }
+    else
+    {
+        factor = maxSpeed;
+    }
+
+    for(int i = 0; i < 4; i++)
+    {
+        motors[i] *= factor;
     }
 };
 
 void ControlModule::moveSide(int imageColumsMiddle, int contourColumsMiddle){
-    float speed = moveSideSpeed * static_cast<float>(contourColumsMiddle - imageColumsMiddle)/static_cast<float>(imageColumsMiddle);
+    double speed = moveSideSpeed * static_cast<double>(contourColumsMiddle - imageColumsMiddle)/static_cast<double>(imageColumsMiddle);
     motors[0] += speed;
     motors[1] -= speed;
     motors[2] -= speed;
     motors[3] += speed;
 }
 
-void ControlModule::rotate(int angle){
+void ControlModule::drive(double ratio)
-    float speed = rotateSpeed * (static_cast<float>(angle) + 90.0f)/90.0f;
+{
-    motors[0] += speed;
+    motors[0] += ratio;
-    motors[1] -= speed;
+    motors[1] += ratio;
-    motors[2] += speed;
+    motors[2] += ratio;
-    motors[3] -= speed;
+    motors[3] += ratio;
+}
+
+void ControlModule::rotate(double ratio)
+{
+    motors[0] += ratio;
+    motors[1] -= ratio;
+    motors[2] += ratio;
+    motors[3] -= ratio;
 }
 
 void ControlModule::unit(){
-    float max = 1.0E-6;
+    double max = 1.0E-6;
     for(int i = 0; i <= sizeof(motors)/sizeof(int); i++){
         if(motors[i] > max)
         max = motors[i];
@@ -58,16 +81,42 @@ void ControlModule::unit(){
 
 }
 
-void ControlModule::calcSpeeds(int imageColumsMiddle, int contourColumsMiddle, int angle){
+void ControlModule::calcSpeeds(int imageColumsMiddle, int contourColumsMiddle, double angle){
     std::unique_lock<std::mutex> lock(mtx);
-    moveForward();
+    double rotationRatio = ratio(angle);
-    moveSide(imageColumsMiddle, contourColumsMiddle);
+    rotate(rotationRatio);
-    rotate(angle);
+    drive(1.0-std::fabs(rotationRatio));
     unit();
+    adjustSpeed();
 }
 
-std::vector<float> ControlModule::readMotors()
+std::vector<double> ControlModule::readMotors()
 {
     std::unique_lock<std::mutex> lock(mtx);
-    return std::vector<float> {motors[0], motors[1], motors[2], motors[3]};
+    return std::vector<double> {motors[0], motors[1], motors[2], motors[3]};
+}
+
+double ControlModule::ratio(double angle)
+{
+    double minAngularSpeed = -1.0;
+    double maxAngularSpeed = 1.0;
+    double speedRange = maxAngularSpeed - minAngularSpeed;
+
+    double minAngle = -20.0;
+    double maxAngle = 20.0;
+    double angularRange = maxAngle - minAngle;
+
+    if(angle < minAngle)
+    {
+        angle = minAngle;
+    }
+    if(angle > maxAngle)
+    {
+        angle = maxAngle;
+    }
+    double progress = angle - minAngle;
+    double progressPercent = progress/angularRange;
+
+    double speed = minAngularSpeed + progressPercent*speedRange;
+    return speed;
 }

AutonomousMode/ControlModule/control_module.h

@@ -2,24 +2,27 @@
 #include <mutex>
 #include <vector>
 #include <iostream>
+#include <cmath>
 
 class ControlModule
 {
 private:
     mutable std::mutex mtx;
-    float motors[4]; //LeftFront; RightFront; LeftBack; RightBack
+    double motors[4]; //LeftFront; RightFront; LeftBack; RightBack
-    float forwardSpeed;
+    double maxSpeed;
-    float rotateSpeed;
+    double rotateSpeed;
-    float moveSideSpeed;
+    double moveSideSpeed;
 public:
     ControlModule(/* args */);
-    ControlModule(float forwardSpeed, float rotateSpeed, float moveSideSpeed);
+    ControlModule(double maxSpeed, double rotateSpeed, double moveSideSpeed);
     ~ControlModule();
-    void moveForward();
+    void adjustSpeed();
     void moveSide(int imageColumsMiddle, int contourColumsMiddle);
-    void rotate(int angle);
+    void rotate(double ratio);
+    void drive(double ratio);
     void unit();            //Brings the max Value to 1.0
+    double ratio(double angle);
 
-    void calcSpeeds(int imageColumsMiddle, int contourColumsMiddle, int angle); //Funktion to be called
+    void calcSpeeds(int imageColumsMiddle, int contourColumsMiddle, double angle); //Funktion to be called
-    std::vector<float> readMotors();
+    std::vector<double> readMotors();
 };

AutonomousMode/Processing/processing.cpp

@@ -82,58 +82,72 @@ void Processing::calcAngles(FrameData &data, int imageColums, int imageRows, boo
     
     if(data.contours.size() > 0)
     { 
-    int angle = 0;
         int index;
-            if(turnLeft){
         // get the most left/right contour
+        
         int leftmostEdge = imageColums;
-                
-                for (int i = 0; i < data.leftEdges.size(); i++)
-                {
-                    int edge = data.leftEdges[i].x;
-                                            
-                    if (edge <= leftmostEdge)
-                    {
-                        leftmostEdge = edge;
-                        index = i;
-                    }
-                }
-            }else
-            {
         int rightmostEdge = 0;
 
         for (int i = 0; i < data.leftEdges.size(); i++)
         {
             int edge = data.leftEdges[i].x;
                                     
-                    if (edge >= rightmostEdge)
+            if (edge <= leftmostEdge && turnLeft)
+            {
+                leftmostEdge = edge;
+                index = i;
+            }
+            else if(edge >= rightmostEdge && !turnLeft)
             {
                 rightmostEdge = edge;
                 index = i;
             }
         }
-            }    
+  
-            // Find the Top-Left and Buttom-Left Point of the Contour
+        // Find the Top-Left and Bottom-Left Point of the Contour
             
         std::vector<cv::Point> leftMostContour = data.contours[index];
-            int xleftButtom = imageColums; 
+
+        /*
+        Not going to dive into that part to fix it.
+        Trying to use centroid instead.
+        int xleftBottom = imageColums; 
         int xleftTop = imageColums;
-            int yButtom = data.boundingBoxes[index].y;
+        int yBottom = data.boundingBoxes[index].y;
         int yTop = data.boundingBoxes[index].height;
         
-            for (int i = 0; i < leftMostContour.size(); i++){
+        for (int i = 0; i < leftMostContour.size(); i++)
-                if(leftMostContour[i].y == 0 && leftMostContour[i].x < xleftButtom)
+        {
+            if(leftMostContour[i].y == 0 && leftMostContour[i].x < xleftBottom)
+            {
+                xleftBottom = leftMostContour[i].x;
+            }
+            else if(leftMostContour[i].y > yTop -4 && leftMostContour[i].x < xleftTop)
             {
-                    xleftButtom = leftMostContour[i].x;
-                }else if(leftMostContour[i].y > yTop -4 && leftMostContour[i].x < xleftTop){
                 xleftTop = leftMostContour[i].x;
             }
         }
 
         // calculate angle
-        int deltaX = abs(xleftButtom - xleftTop);
+        int deltaX = abs(xleftBottom - xleftTop);
         int deltaY = yTop;                
         angle = Calcs::calcAngle(deltaX, deltaY);
+        */
+
+        //Get the contours center using moments:
+        cv::Moments moments=cv::moments(leftMostContour);
+        LFRPoint contourCenter(moments.m10/moments.m00, moments.m01/moments.m00);
+        LFRPoint imageCenter(double(imageColums)/2.0, double(imageRows)/2.0);
+        LFRPoint focusPoint(imageCenter.x, imageCenter.y+1000.0);
+
+        LFRVector a(imageCenter-focusPoint);
+        LFRVector b(contourCenter-focusPoint);
+
+        double angle = a.angle(b);
+        if (b.x < 0.0)
+        {
+            angle *= -1.0;
+        }
 
         //Write to Data
         data.angle = angle;

AutonomousMode/Utils/CMakeLists.txt

@@ -1,6 +1,6 @@
 find_package( OpenCV REQUIRED )
 
-add_library(Utils SHARED utils.cpp)
+add_library(Utils utils.cpp)
 set_target_properties(Utils PROPERTIES VERSION ${PROJECT_VERSION})
 
 target_include_directories(Utils PRIVATE ${OpenCV_INCLUDE_DIRS})

AutonomousMode/Utils/utils.cpp

@@ -1,7 +1,5 @@
 #include "utils.h"
 
-#define PI 3.14159265359
-
 LFRPoint::LFRPoint(/* args */) : x(0.0), y(0.0)
 {
 }
@@ -31,6 +29,22 @@ LFRVector::~LFRVector()
 {
 }
 
+double LFRVector::angle(const LFRVector& other) const
+{
+    return acos(dot(other)/(this->norm()*other.norm())) * (180.0/M_PI);
+}
+
+double LFRVector::dot(const LFRVector& other) const
+{
+    return x*other.x + y*other.y;
+}
+
+double LFRVector::norm() const
+{
+    return sqrt(x*x+y*y);
+}
+
+
 LFRLine::LFRLine(/* args */) : start(), dir()
 {
 }
@@ -55,7 +69,7 @@ int Calcs::calcAngle(int deltaX, int deltaY){
         int refAngle = 0;
 
         if(deltaX > 10E-12){
-        refAngle = (int)((atan(deltaY/deltaX) * 180.0/PI) + 0.5 - (refAngle<0));  //Here 0.5 (or -0.5) is added to round a float number to int right
+        refAngle = (int)((atan(deltaY/deltaX) * 180.0/M_PI) + 0.5 - (refAngle<0));  //Here 0.5 (or -0.5) is added to round a float number to int right
 
         // convert from img coordinates to regbot coordinates
         refAngle = -(refAngle);

AutonomousMode/Utils/utils.h

@@ -1,6 +1,9 @@
 #pragma once
 
 #include <opencv2/opencv.hpp>
+#include <cmath>
+#define _USE_MATH_DEFINES
+#include <math.h>
 
 using namespace cv;
 using namespace std;
@@ -30,6 +33,10 @@ public:
     LFRVector(const LFRPoint& pt);
     ~LFRVector();
 
+    double angle(const LFRVector& other) const;
+    double dot(const LFRVector& other) const;
+    double norm() const;
+
 };
 
 class LFRLine
@@ -60,7 +67,7 @@ public:
     std::vector<cv::Rect> boundingBoxes;
     std::vector<cv::Point> leftEdges;
     std::vector<cv::Point> middlePoints;
-    int angle; //Angle of the contour the robot has to follow to
+    double angle; //Angle of the contour the robot has to follow to
     int index; //Index of the contour the robot has to follow to
 
     FrameData(): contours(), boundingBoxes(), leftEdges(), middlePoints() {}

AutonomousMode/autonomous_mode_main.cpp

@@ -10,10 +10,11 @@ int main(void)
     const int videoHeight = 720;
     const int videoWidth = 1280;
     const int gaussKernelSize = 11;
+    const double maxSpeed = 0.5;
 
     std::mutex mutex;
 
-    LFR lfr(videoHeight, videoWidth, thresholdBinary, gaussKernelSize, [&](std::exception const &ex)
+    LFR lfr(videoHeight, videoWidth, thresholdBinary, gaussKernelSize, maxSpeed, [&](std::exception const &ex)
     {
         std::unique_lock<std::mutex> lock(mutex);
         std::cerr<<"camera exception:"<<ex.what()<<std::endl;

AutonomousMode/lfr.cpp

@@ -3,11 +3,11 @@
 #define right false
 #define left true
 
-LFR::LFR(int videoHeight, int videoWidth, int thresholdBinary, int gaussKernelSize, ExceptionCallback cb):
+LFR::LFR(int videoHeight, int videoWidth, int thresholdBinary, int gaussKernelSize, double maxSpeed, ExceptionCallback cb):
     stop(false),
     input(videoHeight, videoWidth),
     processing(),
-    controlModule(),
+    controlModule(maxSpeed, 1.0, 1.0),
     interpreter(),
     intersectionHandler(),
     cb(cb)
@@ -139,12 +139,13 @@ cv::Mat LFR::provideOutput(Mat originalImage, Mat processedImage, const FrameDat
     {
         //Draw the Arrow for the check of the angle
         int length = 100;
-        Point P1 = frameData.middlePoints[frameData.index];
-        Point P2;
 
-        P2.x =  (int)round(P1.x + length * cos(frameData.angle * CV_PI / 180.0));
+        Point contourCenter = frameData.middlePoints[frameData.index];
-        P2.y =  (int)round(P1.y + length * sin(frameData.angle * CV_PI / 180.0));
+        Point imageCenter = Point(originalImage.size().width/2.0, originalImage.size().height/2.0);
-        cv::arrowedLine(originalImage, P1, P2, Scalar(0,0,255), 2, 8);
+        Point focalPoint = Point(imageCenter.x, imageCenter.y+1000.0);
+
+        cv::arrowedLine(originalImage, focalPoint, contourCenter, Scalar(0,0,255), 2, 8, 0, 0.03);
+        cv::arrowedLine(originalImage, focalPoint, imageCenter, Scalar(0,0,255), 2, 8, 0, 0.03);
     }
     return originalImage;
 }

AutonomousMode/lfr.h

@@ -22,7 +22,7 @@ struct LFR_Result
     cv::Mat rawImage;
     cv::Mat processedImage;
     FrameData data;
-    std::vector<float> motorSignals;
+    std::vector<double> motorSignals;
 };
 
 class LFR
@@ -57,7 +57,7 @@ private:
 
 public:
     LFR() = delete;
-    LFR(int videoHeight, int videoWidth, int thresholdBinary, int gaussKernelSize, ExceptionCallback cb);
+    LFR(int videoHeight, int videoWidth, int thresholdBinary, int gaussKernelSize, double maxSpeed, ExceptionCallback cb);
     ~LFR();
 
     void startLoop();

CMakeLists.txt

@@ -10,7 +10,7 @@ add_subdirectory(AutonomousMode)
 add_subdirectory(Communication)
 add_subdirectory(Socket)
 
-add_executable(lfr_central main.cpp)
+add_executable(lfr_central main.cpp lfr_state_machine.cpp lfr_states.cpp)
 
 target_include_directories(lfr_central PRIVATE ${lfr_image_processing_SOURCE_DIRS})
 target_include_directories(lfr_central PRIVATE ${lfr_uart_SOURCE_DIRS})

Communication/uart_communication.cpp

@@ -2,6 +2,7 @@
 
 
 LFR_UART::LFR_UART() : fileDescriptor(-1) {
+    this->last =std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch());
     this->openSerialPort();
     this->configureSerialPort();
 }
@@ -13,11 +14,13 @@ void LFR_UART::openFile(const char *fileName) {
     this->fileDescriptor = open(fileName, O_RDWR | O_NONBLOCK);
 }
 
-int LFR_UART::writeDataToFile(uint8_t *buff, uint32_t bufferLength) {
+int LFR_UART::writeDataToFile(int8_t *buff, uint32_t bufferLength) {
+    //std::cout << "Sending uart: " << std::bitset<8>(buff[0]) << std::endl;
+    std::cout << "Sending Uart: " << std::bitset<8>(buff[0]) << ", " << std::bitset<8>(buff[1]) << ", " << std::bitset<8>(buff[2]) << ", " << std::bitset<8>(buff[3]) << " csum:  "  << std::bitset<8>(buff[4]) << std::endl;
     return write(this->fileDescriptor, buff, bufferLength);
 }
 
-int LFR_UART::readFromFile(uint8_t *buff, uint32_t bufferLength) {
+int LFR_UART::readFromFile(int8_t *buff, uint32_t bufferLength) {
     return read(this->fileDescriptor, buff, bufferLength);
 }
 
@@ -25,37 +28,32 @@ int LFR_UART::closeFile() {
     return close(this->fileDescriptor);
 }
 
-uint8_t LFR_UART::doubleToByte(double in){
+int8_t LFR_UART::doubleToByte(double in){
     /*
-    * Map the range of -1.0 to 1.0 as double to the range of a byte.
+    * Map the range of -1.0 to 1.0 as double to the range of a byte. (int8 range)
-    * 
-    * -1 -> 0
-    * 0.0 -> 127
-    * 1.0 -> 254
-    * Not using the full range upto 254 to hit the 0.0
     */
     double minDouble = -1.0;
     double maxDouble = 1.0;
     double rangeDouble = maxDouble - minDouble;
 
-    double minByte = 0.0;
+    double minByte = -128.0;
-    double maxByte = 255.0;
+    double maxByte = 127.0;
     double rangeByte = maxByte - minByte;
 
     double progress = in - minDouble;
     double progressPercent = progress/rangeDouble;
 
     double inputInByteRange = minByte + progressPercent*rangeByte;
-    return uint8_t(inputInByteRange);
+    return int8_t(inputInByteRange);
 }
 
-double LFR_UART::byteToDouble(uint8_t in){
+double LFR_UART::byteToDouble(int8_t in){
     double minDouble = -1.0;
     double maxDouble = 1.0;
     double rangeDouble = maxDouble - minDouble;
 
-    double minByte = 0.0;
+    double minByte = -128.0;
-    double maxByte = 255.0;
+    double maxByte = 127.0;
     double rangeByte = maxByte - minByte;
 
     double progress = double(in) - minByte;
@@ -66,27 +64,43 @@ double LFR_UART::byteToDouble(uint8_t in){
 }
 
 void LFR_UART::sendTelegram(double wheel1, double wheel2, double wheel3, double wheel4){
+    std::chrono::milliseconds now = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch());
+    unsigned int deltaMs = static_cast<unsigned int>((now-last).count());
+    if (deltaMs < 50 && (std::fabs(wheel1)+std::fabs(wheel2)+std::fabs(wheel3)+std::fabs(wheel4)) > 0.0005)
+    {
+        std::cout << "Too fast" << std::endl;
+        return;
+    }
+    last = now;
+
     if(wheel1 > 1.0 || wheel2 > 1.0 || wheel3 > 1.0 || wheel4 > 1.0){
         throw CommunicatorException("Wheel value must not be greater than 1.0");
     }
     if(wheel1 < -1.0 || wheel2 < -1.0 || wheel3 < -1.0 || wheel4 < -1.0){
         throw CommunicatorException("Wheel value must not be smaller than -1.0");
     }
+    // Discrepancy between the numbering of the wheels in App/Pi and the µc
+    // Our - their
+    // 1 - 4
+    // 2 - 2
+    // 3 - 3
+    // 4 - 1
+    int8_t wheel1B = this->doubleToByte(wheel4);
+    //int8_t wheel1B = this->doubleToByte(wheel1);
+    int8_t wheel2B = this->doubleToByte(wheel2);
+    int8_t wheel3B = this->doubleToByte(wheel3);
+    int8_t wheel4B = this->doubleToByte(wheel1);
+    //int8_t wheel4B = this->doubleToByte(wheel4);
 
-    uint8_t wheel1B = this->doubleToByte(wheel1);
+    int8_t checksum = wheel1B^wheel2B^wheel3B^wheel4B;
-    uint8_t wheel2B = this->doubleToByte(wheel2);
-    uint8_t wheel3B = this->doubleToByte(wheel3);
-    uint8_t wheel4B = this->doubleToByte(wheel4);
 
-    uint8_t checksum = wheel1B^wheel2B^wheel3B^wheel4B;
+    int8_t telegram_buffer[5] = {wheel1B, wheel2B, wheel3B, wheel4B, checksum}; 
-
-    uint8_t telegram_buffer[5] = {wheel1B, wheel2B, wheel3B, wheel4B, checksum}; 
     uint32_t telegram_length = 5;
     this->writeDataToFile(telegram_buffer, telegram_length);
 }
 
 bool LFR_UART::readTelegram(double* buffer){
-    uint8_t tmp_buffer[5] = {0, 0, 0, 0, 0};
+    int8_t tmp_buffer[5] = {0, 0, 0, 0, 0};
     uint32_t telegram_length = 5;
     this->readFromFile(tmp_buffer, telegram_length);
 

Communication/uart_communication.h

@@ -10,12 +10,17 @@
 #include <termios.h>
 #include <iostream>
 #include <exception>
+#include <iostream>
+#include <chrono>
+#include <cmath>
 
 #include <bitset>
 
 
 class LFR_UART
 {
+public:
+	std::chrono::milliseconds last;
 	int fileDescriptor;
 	const char* serialPortPath = "/dev/ttyS0";
 	
@@ -28,13 +33,14 @@ class LFR_UART
 	void configureSerialPort();
 	void closeSerialPort();
 
-	int writeDataToFile(uint8_t *buff, uint32_t bufferLength);
+
-	int readFromFile(uint8_t *buff, uint32_t bufferLength);
+	int writeDataToFile(int8_t *buff, uint32_t bufferLength);
+	int readFromFile(int8_t *buff, uint32_t bufferLength);
 
 public:
 
-	uint8_t doubleToByte(double in);
+	int8_t doubleToByte(double in);
-	double byteToDouble(uint8_t in);
+	double byteToDouble(int8_t in);
 
 	void sendTelegram(double wheel1, double wheel2, double wheel3, double wheel4);
 	bool readTelegram(double* buffer);

Communication/uart_communication_main.cpp

@@ -1,5 +1,12 @@
 #include "uart_communication.h"
 
+/*
+* Usage: telnet [ip] [port]
+* eg.: telnet 192.158.4.1 8080
+* To quit: ctrl + +
+*           q
+*/
+
 int main(void) {
     printf("Starting the loopback application...\r\n");
     

MobileApp/.idea/deploymentTargetDropDown.xml

@@ -7,11 +7,11 @@
         <deviceKey>
           <Key>
             <type value="VIRTUAL_DEVICE_PATH" />
-            <value value="C:\Users\baran\.android\avd\Nexus_5_API_R.avd" />
+            <value value="C:\Users\baran\.android\avd\Pixel_2_API_R.avd" />
           </Key>
         </deviceKey>
       </Target>
     </targetSelectedWithDropDown>
-    <timeTargetWasSelectedWithDropDown value="2023-01-11T18:53:15.217760700Z" />
+    <timeTargetWasSelectedWithDropDown value="2023-01-12T21:16:30.648959800Z" />
   </component>
 </project>

MobileApp/.idea/misc.xml

@@ -5,13 +5,15 @@
       <map>
         <entry key="..\:/Line-Following-Robot/MobileApp/app/src/main/res/drawable-anydpi/manuell.xml" value="0.1" />
         <entry key="..\:/Line-Following-Robot/MobileApp/app/src/main/res/drawable-v24/ic_launcher_foreground.xml" value="0.1" />
+        <entry key="..\:/Line-Following-Robot/MobileApp/app/src/main/res/drawable/ic_baseline_rotate_left_24.xml" value="0.1" />
         <entry key="..\:/Line-Following-Robot/MobileApp/app/src/main/res/drawable/ic_dashboard_black_24dp.xml" value="0.1" />
         <entry key="..\:/Line-Following-Robot/MobileApp/app/src/main/res/drawable/ic_home_black_24dp.xml" value="0.1" />
         <entry key="..\:/Line-Following-Robot/MobileApp/app/src/main/res/drawable/ic_launcher_background.xml" value="0.1" />
         <entry key="..\:/Line-Following-Robot/MobileApp/app/src/main/res/drawable/ic_notifications_black_24dp.xml" value="0.1" />
         <entry key="..\:/Line-Following-Robot/MobileApp/app/src/main/res/drawable/my_btn_toggle.xml" value="0.1" />
+        <entry key="..\:/Line-Following-Robot/MobileApp/app/src/main/res/drawable/vorwarts.xml" value="0.1" />
         <entry key="..\:/Line-Following-Robot/MobileApp/app/src/main/res/layout/activity_main.xml" value="0.16666666666666666" />
-        <entry key="..\:/Line-Following-Robot/MobileApp/app/src/main/res/layout/fragment_automatik.xml" value="0.3333333333333333" />
+        <entry key="..\:/Line-Following-Robot/MobileApp/app/src/main/res/layout/fragment_automatik.xml" value="0.14666666666666667" />
         <entry key="..\:/Line-Following-Robot/MobileApp/app/src/main/res/layout/fragment_dashboard.xml" value="0.14666666666666667" />
         <entry key="..\:/Line-Following-Robot/MobileApp/app/src/main/res/layout/fragment_einstellungen.xml" value="0.25" />
         <entry key="..\:/Line-Following-Robot/MobileApp/app/src/main/res/layout/fragment_home.xml" value="0.16666666666666666" />
@@ -23,7 +25,7 @@
       </map>
     </option>
   </component>
-  <component name="ProjectRootManager" version="2" languageLevel="JDK_11" default="true" project-jdk-name="1.8" project-jdk-type="JavaSDK">
+  <component name="ProjectRootManager" version="2" languageLevel="JDK_11" default="true" project-jdk-name="11" project-jdk-type="JavaSDK">
     <output url="file://$PROJECT_DIR$/build/classes" />
   </component>
   <component name="ProjectType">

MobileApp/app/src/main/java/com/example/lfrmobileapp/Communication.java

@@ -1,22 +1,51 @@
 package com.example.lfrmobileapp;
 
 import android.accounts.NetworkErrorException;
+import android.util.Log;
 
 import java.io.DataOutputStream;
 import java.io.IOException;
 import java.net.Socket;
+import java.util.ArrayList;
+import java.util.Iterator;
+import java.util.Objects;
 
 public class Communication {
-    public boolean sendData(String data) {
+    public String telegram(int[] wheels, int strenght){
-        try {
+        String tel = "aa";
-            Socket socket = new Socket("192.168.0.1", 1755);
+        tel += "0"; //Manuel mode
-            DataOutputStream DOS = new DataOutputStream(socket.getOutputStream());
+
-            DOS.writeUTF(data);
+        for(int i = 0; i < wheels.length; i++){
-            socket.close();
+            if(wheels[i] == 1){
-        } catch (Exception e) {
+                tel += ";" + Float.toString(((float)strenght)/100)  ;
-            e.printStackTrace();
+            }else if(wheels[i] == -1)
+                tel += ";" + Float.toString(((float)-strenght)/100);
+            else{
+                tel += ";0.00";
             }
-        return true;
+        }
+        tel += "zz";
+        return tel;
+    }
+
+    public String telegram(boolean startStop, ArrayList<String> list){
+        String tel = "aa";
+        tel += "1"; //Autonomous mode
+        if(startStop){
+            tel += ";1";
+        }else{
+            return "aa0;0.0;0.0;0.0;0.0zz";
+        }
+
+        for (String i : list) {
+            if(i.equals("Links")){
+                tel += ";0";
+            }else if (i.equals("Rechts")){
+                tel += ";1";
+            }
+        }
+        tel += "zz";
+        return tel;
     }
 
 }

MobileApp/app/src/main/java/com/example/lfrmobileapp/DataTransferAsyncTask.java

@@ -0,0 +1,49 @@
+package com.example.lfrmobileapp;
+
+import android.os.AsyncTask;
+import android.util.Log;
+
+import java.io.DataOutputStream;
+import java.io.IOException;
+import java.io.OutputStream;
+import java.net.Socket;
+import java.util.concurrent.BlockingQueue;
+import java.util.concurrent.LinkedBlockingQueue;
+import java.util.concurrent.TimeUnit;
+
+public class DataTransferAsyncTask extends AsyncTask<Void, Void, Void> {
+    private static final double sendRatePerSecond = 4.0;
+    private boolean connected;
+    private final ExtendedSocket socket;
+    private BlockingQueue<String> blockingQueue;
+
+    public DataTransferAsyncTask(ExtendedSocket socket) {
+        this.socket = socket;
+        this.connected = true;
+        this.blockingQueue = new LinkedBlockingQueue<>();
+    }
+
+    public void writeTelegram(String telegram) {
+        // Schau mal hier bei Fehler
+        blockingQueue.offer(telegram);
+    }
+
+    @Override
+    protected Void doInBackground(Void... params) {
+        while(!isCancelled() && connected) {
+            try {
+                synchronized (socket) {
+                    String message = blockingQueue.poll((long) (1000.0/sendRatePerSecond), TimeUnit.MILLISECONDS);
+                    if(message != null){
+                        Log.v("TAG", message + "\n");
+                        socket.sendMessage(message);
+                    }
+                }
+            } catch (Exception e) {
+                e.printStackTrace();
+                connected = false;
+            }
+        }
+        return null;
+    }
+}

MobileApp/app/src/main/java/com/example/lfrmobileapp/ExtendedSocket.java

@@ -0,0 +1,25 @@
+package com.example.lfrmobileapp;
+
+import java.io.DataOutputStream;
+import java.io.IOException;
+import java.net.Socket;
+
+public class ExtendedSocket {
+    private final Socket socket;
+
+    public ExtendedSocket(Socket socket) {
+        this.socket = socket;
+    }
+
+    public void sendMessage(String message) throws IOException {
+        synchronized (socket){
+            DataOutputStream outputStream = new DataOutputStream(socket.getOutputStream());
+            outputStream.writeBytes(message);
+            outputStream.flush();
+        }
+    }
+
+    public void closeSocket() throws IOException {
+        this.socket.close();
+    }
+}

MobileApp/app/src/main/java/com/example/lfrmobileapp/MainActivity.java

@@ -1,6 +1,9 @@
 package com.example.lfrmobileapp;
 
+import android.content.Context;
+import android.content.SharedPreferences;
 import android.os.Bundle;
+import android.widget.Toast;
 
 import com.google.android.material.bottomnavigation.BottomNavigationView;
 
@@ -12,9 +15,21 @@ import androidx.navigation.ui.NavigationUI;
 
 import com.example.lfrmobileapp.databinding.ActivityMainBinding;
 
+import java.io.DataOutputStream;
+import java.io.IOException;
+import java.net.InetSocketAddress;
+import java.net.Socket;
+import java.util.concurrent.ExecutionException;
+import java.util.concurrent.ExecutorService;
+import java.util.concurrent.Executors;
+import java.util.concurrent.Future;
+
 public class MainActivity extends AppCompatActivity {
 
     private ActivityMainBinding binding;
+    private ExtendedSocket socket;
+    private DataTransferAsyncTask dataTransferAsyncTask;
+    public Boolean connected = false;
 
     @Override
     protected void onCreate(Bundle savedInstanceState) {
@@ -34,5 +49,43 @@ public class MainActivity extends AppCompatActivity {
         NavigationUI.setupWithNavController(binding.navView, navController);
     }
 
-
+    public boolean connectToWifi(String ipAddress, int port){
+        ExecutorService executor = Executors.newSingleThreadExecutor();
+        Future<Boolean> future = executor.submit(() -> {
+            Socket tempSocket = new Socket();
+            tempSocket.connect(new InetSocketAddress(ipAddress, port), 1000);
+            try {
+                socket = new ExtendedSocket(tempSocket);
+                socket.sendMessage("Verbindung aufbauen");
+            } catch (Exception e) {
+                socket.closeSocket();
+                return false;
             }
+            return true;
+        });
+
+        try {
+            connected = future.get();
+        } catch (InterruptedException e) {
+            System.err.println("InterruptedException: " + e.getMessage());
+        } catch (ExecutionException e) {
+            System.err.println("ExecutionException: " + e.getCause().getMessage());
+        }
+
+        executor.shutdown();
+
+        if(connected){
+            dataTransferAsyncTask = new DataTransferAsyncTask(socket);
+            dataTransferAsyncTask.execute();
+            return true;
+        }else{
+            return false;
+        }
+    }
+
+    public void sendTelegram(String telegram)
+    {
+        this.dataTransferAsyncTask.writeTelegram(telegram);
+    }
+}
+

MobileApp/app/src/main/java/com/example/lfrmobileapp/ui/dashboard/DashboardFragment.java

@@ -1,21 +1,50 @@
 package com.example.lfrmobileapp.ui.dashboard;
 
+import android.content.Context;
+import android.content.SharedPreferences;
+import android.graphics.Color;
 import android.os.Bundle;
 import android.view.LayoutInflater;
 import android.view.View;
 import android.view.ViewGroup;
+import android.widget.AdapterView;
+import android.widget.ArrayAdapter;
+import android.widget.CompoundButton;
+import android.widget.ListView;
 import android.widget.TextView;
 
 import androidx.annotation.NonNull;
 import androidx.fragment.app.Fragment;
 import androidx.lifecycle.ViewModelProvider;
 
+import com.example.lfrmobileapp.Communication;
+import com.example.lfrmobileapp.DataTransferAsyncTask;
+import com.example.lfrmobileapp.MainActivity;
+import com.example.lfrmobileapp.R;
 import com.example.lfrmobileapp.databinding.FragmentAutomatikBinding;
 
-public class DashboardFragment extends Fragment {
+import java.lang.reflect.Array;
+import java.util.ArrayList;
+import java.util.Arrays;
 
+import kotlin.collections.UArraySortingKt;
+
+// Autonomous Mode
+public class DashboardFragment extends Fragment {
+    Communication com = new Communication();
     private FragmentAutomatikBinding binding;
 
+    MainActivity mainActivity;
+
+    //Keys for Shared Preferences
+    String listKey = "listkey";
+
+    @Override
+    public void onAttach(Context context) {
+        super.onAttach(context);
+        mainActivity = (MainActivity) context;
+    }
+
     public View onCreateView(@NonNull LayoutInflater inflater,
                              ViewGroup container, Bundle savedInstanceState) {
         DashboardViewModel dashboardViewModel =
@@ -26,6 +55,104 @@ public class DashboardFragment extends Fragment {
 
         final TextView textView = binding.textDashboard;
         dashboardViewModel.getText().observe(getViewLifecycleOwner(), textView::setText);
+
+        //Shared Preferences
+        SharedPreferences sharedPref = getActivity().getPreferences(Context.MODE_PRIVATE);
+        SharedPreferences.Editor editor = sharedPref.edit();
+
+        //Get Arraylist from Shared Preferences String
+        ArrayList<String> autoList = stringToArraylist(sharedPref.getString(listKey, ""));
+
+        binding.startAutomatic.setOnCheckedChangeListener(new CompoundButton.OnCheckedChangeListener() {
+
+            @Override
+            public void onCheckedChanged(CompoundButton buttonView, boolean isChecked) {
+                if (mainActivity.connected){
+                    if(isChecked){
+                        mainActivity.sendTelegram(com.telegram(true, autoList));
+                    }else{
+                        mainActivity.sendTelegram(com.telegram(false, autoList));
+                    }
+                }
+            }
+        });
+
+
+        // on the below line we are initializing the adapter for our list view.
+        ArrayAdapter<String> adapter = new ArrayAdapter<String>(root.getContext(), android.R.layout.simple_list_item_1, autoList){
+            @Override
+            public View getView(int position, View convertView, ViewGroup parent) {
+                View view =super.getView(position, convertView, parent);
+                TextView textView=(TextView) view.findViewById(android.R.id.text1);
+                //Set Color to black
+                textView.setTextColor(Color.BLACK);
+                return view;
+            }
+
+            @Override
+            public void notifyDataSetChanged() {
+                super.notifyDataSetChanged();
+                //Save Arraylist in Shared Preferences as String
+                editor.putString(listKey, listToString(autoList));
+                editor.apply();
+            }
+        };
+
+
+
+        // on below line we are setting adapter for our list view.
+        binding.automaticList.setAdapter(adapter);
+
+        // on below line we are adding click listener for our button.
+        binding.addLeft.setOnClickListener(new View.OnClickListener() {
+            @Override
+            public void onClick(View v) {
+                // on below line we are getting text from edit text
+                String item = "Links";
+                Boolean automaticOn = binding.startAutomatic.isChecked();
+                // on below line we are checking if item is not empty
+                if (!item.isEmpty() && !automaticOn) {
+                    // on below line we are adding item to our list.
+                    autoList.add(item);
+                    // on below line we are notifying adapter
+                    // that data in list is updated to
+                    // update our list view.
+                    adapter.notifyDataSetChanged();
+                }
+            }
+        });
+
+        // on below line we are adding click listener for our button.
+        binding.addRight.setOnClickListener(new View.OnClickListener() {
+            @Override
+            public void onClick(View v) {
+                // on below line we are getting text from edit text
+                String item = "Rechts";
+
+                Boolean automaticOn = binding.startAutomatic.isChecked();
+                // on below line we are checking if item is not empty
+                if (!item.isEmpty() && !automaticOn) {
+                    // on below line we are adding item to our list.
+                    autoList.add(item);
+                    // on below line we are notifying adapter
+                    // that data in list is updated to
+                    // update our list view.
+                    adapter.notifyDataSetChanged();
+                }
+            }
+        });
+
+        //Deleting element in ListView
+        binding.automaticList.setOnItemClickListener(new AdapterView.OnItemClickListener() {
+            @Override
+            public void onItemClick(AdapterView<?> parent, View view, int position, long id) {
+                Boolean automaticOn = binding.startAutomatic.isChecked();
+                if(!automaticOn) {
+                    autoList.remove(position);
+                    adapter.notifyDataSetChanged();
+                }
+            }
+        });
         return root;
     }
 
@@ -34,4 +161,22 @@ public class DashboardFragment extends Fragment {
         super.onDestroyView();
         binding = null;
     }
+
+    public String listToString(ArrayList<String> list){
+        String result = "";
+        if(!list.isEmpty()) {
+            for (String s : list) {
+                result += s + ";";
             }
+        }
+        return result;
+    }
+
+    public ArrayList<String> stringToArraylist(String str){
+        ArrayList<String> list = null;
+        String[] stringArray = str.split(";");
+        list = new ArrayList<String>(Arrays.asList(stringArray));
+        return list;
+    }
+}
+

MobileApp/app/src/main/java/com/example/lfrmobileapp/ui/home/HomeFragment.java

@@ -1,8 +1,12 @@
 package com.example.lfrmobileapp.ui.home;
 
+import android.annotation.SuppressLint;
 import android.content.Context;
+import android.content.SharedPreferences;
 import android.os.Bundle;
+import android.util.Log;
 import android.view.LayoutInflater;
+import android.view.MotionEvent;
 import android.view.View;
 import android.view.ViewGroup;
 import android.widget.Button;
@@ -14,14 +18,38 @@ import androidx.fragment.app.Fragment;
 import androidx.lifecycle.ViewModelProvider;
 
 import com.example.lfrmobileapp.Communication;
+import com.example.lfrmobileapp.DataTransferAsyncTask;
+import com.example.lfrmobileapp.MainActivity;
+import com.example.lfrmobileapp.R;
 import com.example.lfrmobileapp.databinding.FragmentManuellBinding;
+import com.example.lfrmobileapp.ui.notifications.NotificationsFragment;
 
 import io.github.controlwear.virtual.joystick.android.JoystickView;
 
 public class HomeFragment extends Fragment {
 
+    //Keys for Shared Preferences
+    String ipKey = "ipkey";
+    String portKey = "portkey";
+
+    //Default Ip-Address and Port
+    String ipAddress = "192.168.0.1";
+    int port = 8000;
+
+    MainActivity mainActivity;
+
+    Communication com = new Communication();
+    long lastOnMoveCall = System.currentTimeMillis();
+
+
     private FragmentManuellBinding binding;
 
+    @Override
+    public void onAttach(Context context) {
+        super.onAttach(context);
+        mainActivity = (MainActivity) context;
+    }
+
     public View onCreateView(@NonNull LayoutInflater inflater,
                              ViewGroup container, Bundle savedInstanceState) {
         HomeViewModel homeViewModel =
@@ -30,16 +58,102 @@ public class HomeFragment extends Fragment {
         binding = FragmentManuellBinding.inflate(inflater, container, false);
         View root = binding.getRoot();
 
+        SharedPreferences sharedPref = getActivity().getPreferences(Context.MODE_PRIVATE);
+        SharedPreferences.Editor editor = sharedPref.edit();
+
+        //Get saved Ip-Adress and Port from Shared Preferences
+        ipAddress = sharedPref.getString(ipKey, "");
+        port = sharedPref.getInt(portKey, 0);
+
+        //Set text from Textview
         final TextView textView = binding.textHome;
         homeViewModel.getText().observe(getViewLifecycleOwner(), textView::setText);
 
+
+
+        binding.rotateLeft.setOnTouchListener(new View.OnTouchListener(){
+            @SuppressLint("ClickableViewAccessibility")
+            @Override
+                public boolean onTouch(View v, MotionEvent event) {
+                if (mainActivity.connected) {
+                    if (event.getAction() == MotionEvent.ACTION_UP) {
+                        binding.rotateLeft.performClick();
+                        mainActivity.sendTelegram(com.telegram(new int[]{0, 0, 0, 0}, 0));
+                        return true;
+                    } else {
+                        mainActivity.sendTelegram(com.telegram(new int[]{-1, 1, -1, 1}, 50));
+                    }
+                }
+                return false;
+            }
+        });
+
+        binding.rotateRight.setOnTouchListener(new View.OnTouchListener(){
+            @SuppressLint("ClickableViewAccessibility")
+            @Override
+            public boolean onTouch(View v, MotionEvent event) {
+                if (mainActivity.connected){
+                    if(event.getAction() == MotionEvent.ACTION_UP){
+                        binding.rotateRight.performClick();
+                        mainActivity.sendTelegram(com.telegram(new int[]{0, 0, 0, 0}, 0));
+                        return true;
+                    }else{
+                        mainActivity.sendTelegram(com.telegram(new int[]{1, -1, 1, -1}, 50));
+                    }
+                }
+                return false;
+            }
+        });
+
+
         JoystickView joystick = (JoystickView) binding.joystick;
-        Communication com = new Communication();
         joystick.setOnMoveListener(new JoystickView.OnMoveListener() {
             @Override
             public void onMove(int angle, int strength) {
+                boolean send = false;
+                if (System.currentTimeMillis() - lastOnMoveCall > 250){
+                    send = true;
+                    lastOnMoveCall = System.currentTimeMillis();
+                }
+                String telegram = "";
+
                 homeViewModel.setText(Integer.toString(angle), Integer.toString(strength));
-                com.sendData("Vanessa stinkt, Baran nicht");
+                //8 segments of the joystick
+                if((angle >= 338 || angle < 22) && strength != 0){
+                    joystick.setBackgroundResource(R.mipmap.right);
+                    telegram = com.telegram(new int[]{1, -1, -1, 1}, strength);
+                }else if((angle >= 22 && angle < 67) && strength != 0){
+                    joystick.setBackgroundResource(R.mipmap.right_forward);
+                    telegram = com.telegram(new int[]{1, 0, 0, 1}, strength);
+                }else if((angle >=67 && angle < 112) && strength != 0){
+                    joystick.setBackgroundResource(R.mipmap.forward);
+                    telegram = com.telegram(new int[]{1, 1, 1, 1}, strength);
+                }else if((angle >=112 && angle < 157) && strength != 0){
+                    joystick.setBackgroundResource(R.mipmap.left_forward);
+                    telegram = com.telegram(new int[]{0, 1, 1, 0}, strength);
+                }else if((angle >=157 && angle < 202) && strength != 0){
+                    joystick.setBackgroundResource(R.mipmap.left);
+                    telegram = com.telegram(new int[]{-1, 1, 1, -1}, strength);
+                }else if((angle >=202 && angle < 247) && strength != 0){
+                    joystick.setBackgroundResource(R.mipmap.left_back);
+                    telegram = com.telegram(new int[]{-1, 0, 0, -1}, strength);
+                }else if((angle >=247 && angle < 292) && strength != 0){
+                    joystick.setBackgroundResource(R.mipmap.back);
+                    telegram = com.telegram(new int[]{-1, -1, -1, -1}, strength);
+                }else if((angle >=292 && angle < 337) && strength != 0){
+                    joystick.setBackgroundResource(R.mipmap.right_back);
+                    telegram = com.telegram(new int[]{0, -1, -1, 0}, strength);
+                }
+                else{
+                    joystick.setBackgroundResource(R.mipmap.blank);
+                    telegram = com.telegram(new int[]{0, 0, 0, 0}, strength);
+                    send=true;
+                }
+                if (send && mainActivity.connected)
+                {
+                    mainActivity.sendTelegram(telegram);
+                    // Log.v("TAG", telegram + "\n");
+                }
             }
         });
 
@@ -49,8 +163,8 @@ public class HomeFragment extends Fragment {
     @Override
     public void onDestroyView() {
         super.onDestroyView();
+        // dataTransferAsyncTask.cancel(false);
         binding = null;
     }
-
-
 }
+

MobileApp/app/src/main/java/com/example/lfrmobileapp/ui/notifications/NotificationsFragment.java

@@ -1,21 +1,78 @@
 package com.example.lfrmobileapp.ui.notifications;
 
+import android.content.Context;
+import android.content.SharedPreferences;
 import android.os.Bundle;
 import android.view.LayoutInflater;
 import android.view.View;
 import android.view.ViewGroup;
-import android.widget.TextView;
+import android.widget.Button;
+import android.widget.Toast;
 
 import androidx.annotation.NonNull;
 import androidx.fragment.app.Fragment;
 import androidx.lifecycle.ViewModelProvider;
 
+import com.example.lfrmobileapp.Communication;
+import com.example.lfrmobileapp.DataTransferAsyncTask;
+import com.example.lfrmobileapp.MainActivity;
 import com.example.lfrmobileapp.databinding.FragmentEinstellungenBinding;
 
+import java.io.DataOutputStream;
+import java.net.Socket;
+
+
+/*class NetworkThread extends Thread {
+    String ip;
+    int port;
+    boolean connected;
+
+    NetworkThread(String ip,  int port) {
+        this.ip = ip;
+        this.port = port;
+        this.connected = false;
+    }
+
+    public void run() {
+        try {
+            Socket socket = new Socket(ip, port);
+            DataOutputStream outputStream = new DataOutputStream(socket.getOutputStream());
+            outputStream.writeBytes("Verbindung aufbauen");
+            outputStream.flush();
+            socket.close();
+            connected = true;
+
+        } catch (Exception e) {
+            e.printStackTrace();
+            connected = false;
+        }
+    }
+}*/
+
 public class NotificationsFragment extends Fragment {
 
+    Boolean connected = false;
+    MainActivity mainActivity;
+
+    String ipKey = "ipkey";
+    String portKey = "portkey";
+    String connectionFailed = "Verbindung fehlgeschlagen.";
+    String connectionSuccessful = "Verbindung erfolgreich.";
+
+    String ipAddress = "192.168.4.1";
+    int port = 8080;
+
+    int duration = Toast.LENGTH_LONG;
+    DataTransferAsyncTask dataTransferAsyncTask;
+
     private FragmentEinstellungenBinding binding;
 
+    @Override
+    public void onAttach(Context context) {
+        super.onAttach(context);
+        mainActivity = (MainActivity) context;
+    }
+
     public View onCreateView(@NonNull LayoutInflater inflater,
                              ViewGroup container, Bundle savedInstanceState) {
         NotificationsViewModel notificationsViewModel =
@@ -24,12 +81,55 @@ public class NotificationsFragment extends Fragment {
         binding = FragmentEinstellungenBinding.inflate(inflater, container, false);
         View root = binding.getRoot();
 
+        Communication com = new Communication();
+
+        SharedPreferences sharedPref = getActivity().getPreferences(Context.MODE_PRIVATE);
+        SharedPreferences.Editor editor = sharedPref.edit();
+
+        ipAddress = sharedPref.getString(ipKey, "");
+        binding.ipAdress.setText(ipAddress);
+        port = sharedPref.getInt(portKey, -1);
+        if(port != -1){
+            binding.port.setText(Integer.toString(port));
+        }else{
+            binding.port.setText("");
+        }
+
+        // dataTransferAsyncTask = new DataTransferAsyncTask(ipAddress, port);
+
+        final Button button = binding.connect;
+        button.setOnClickListener(new View.OnClickListener() {
+            public void onClick(View v) {
+                try {
+                    ipAddress = String.valueOf(binding.ipAdress.getText());
+                    port = Integer.parseInt(String.valueOf(binding.port.getText()));
+                } catch (Exception e){
+                    System.err.println("Parse Exception: " + e.getMessage());
+                }
+
+                connected = mainActivity.connectToWifi(ipAddress, port);
+
+                if(connected){
+                    editor.putString(ipKey, ipAddress);
+                    editor.putInt(portKey, port);
+                    editor.apply();
+                    Toast toast = Toast.makeText(v.getContext(), connectionSuccessful, duration);
+                    toast.show();
+                }else{
+                    Toast toast = Toast.makeText(v.getContext(), connectionFailed, duration);
+                    toast.show();
+                    // dataTransferAsyncTask.cancel(true);
+                }
+            }
+        });
+
         return root;
     }
 
     @Override
     public void onDestroyView() {
         super.onDestroyView();
+        // dataTransferAsyncTask.cancel(true);
         binding = null;
     }
 }

MobileApp/app/src/main/res/layout/fragment_automatik.xml

@@ -11,15 +11,16 @@
         android:layout_width="224dp"
         android:layout_height="38dp"
         android:layout_marginStart="8dp"
-        android:layout_marginTop="4dp"
+        android:layout_marginTop="16dp"
         android:layout_marginEnd="16dp"
         android:checked="false"
         android:fontFamily="sans-serif-medium"
         android:text="Roboter losfahren     "
         android:textSize="16sp"
         app:layout_constraintEnd_toEndOf="parent"
+        app:layout_constraintHorizontal_bias="0.147"
         app:layout_constraintStart_toStartOf="parent"
-        app:layout_constraintTop_toTopOf="parent"></Switch>
+        app:layout_constraintTop_toTopOf="parent" />
 
     <TextView
         android:id="@+id/text_dashboard"
@@ -39,34 +40,55 @@
 
     <TextView
         android:id="@+id/textTurnLeftRight"
-        android:layout_width="128dp"
+        android:layout_width="137dp"
-        android:layout_height="22dp"
+        android:layout_height="39dp"
         android:layout_marginStart="8dp"
-        android:layout_marginTop="68dp"
+        android:layout_marginTop="64dp"
         android:layout_marginEnd="8dp"
         android:fontFamily="sans-serif-medium"
-        android:text="Abbiegen nach "
+        android:text="Abzweigung hinzufügen:"
         android:textColor="#000000"
         android:textSize="16sp"
 
         app:layout_constraintEnd_toEndOf="parent"
-        app:layout_constraintHorizontal_bias="0.322"
+        app:layout_constraintHorizontal_bias="0.089"
         app:layout_constraintStart_toStartOf="parent"
         app:layout_constraintTop_toTopOf="parent" />
 
-    <ToggleButton
+    <Button
-        android:id="@+id/toggleButton"
+        android:id="@+id/addLeft"
-        android:layout_width="95dp"
+        android:layout_width="105dp"
         android:layout_height="40dp"
-        android:layout_marginTop="60dp"
+        android:layout_marginTop="64dp"
         android:layout_marginEnd="16dp"
         android:fontFamily="sans-serif-medium"
-        android:text="ToggleButton"
+        android:text="Links"
-        android:textOff="Links"
-        android:textOn="Rechts"
         android:textSize="16sp"
         app:layout_constraintEnd_toEndOf="parent"
-        app:layout_constraintHorizontal_bias="0.74"
+        app:layout_constraintHorizontal_bias="0.613"
         app:layout_constraintStart_toStartOf="parent"
         app:layout_constraintTop_toTopOf="parent" />
+
+    <Button
+        android:id="@+id/addRight"
+        android:layout_width="105dp"
+        android:layout_height="40dp"
+        android:layout_marginTop="64dp"
+        android:layout_marginEnd="16dp"
+        android:fontFamily="sans-serif-medium"
+        android:text="Rechts"
+        android:textSize="16sp"
+        app:layout_constraintEnd_toEndOf="parent"
+        app:layout_constraintHorizontal_bias="1.0"
+        app:layout_constraintStart_toStartOf="parent"
+        app:layout_constraintTop_toTopOf="parent" />
+
+    <ListView
+        android:id="@+id/automaticList"
+        android:layout_width="match_parent"
+        android:layout_height="470dp"
+        android:layout_marginTop="110dp"
+        app:layout_constraintEnd_toEndOf="parent"
+        app:layout_constraintStart_toStartOf="parent"
+        app:layout_constraintTop_toTopOf="@+id/startAutomatic" />
 </androidx.constraintlayout.widget.ConstraintLayout>

MobileApp/app/src/main/res/layout/fragment_einstellungen.xml

@@ -9,8 +9,8 @@
     <EditText
         android:id="@+id/ipAdress"
         android:layout_width="269dp"
-        android:layout_height="38dp"
+        android:layout_height="wrap_content"
-        android:layout_marginTop="64dp"
+        android:layout_marginTop="60dp"
         android:hint="IP-Adresse"
         android:digits="0123456789."
         android:textColor="#000000"
@@ -22,8 +22,9 @@
     <EditText
         android:id="@+id/port"
         android:layout_width="269dp"
-        android:layout_height="38dp"
+        android:layout_height="wrap_content"
         android:layout_marginTop="120dp"
+        android:ems="10"
         android:hint="Port"
         android:digits="0123456789"
         android:textColor="#000000"
@@ -31,4 +32,17 @@
         app:layout_constraintEnd_toEndOf="parent"
         app:layout_constraintStart_toStartOf="parent"
         app:layout_constraintTop_toTopOf="parent" />
+
+    <Button
+        android:id="@+id/connect"
+        android:layout_width="269dp"
+        android:layout_height="wrap_content"
+        android:layout_marginTop="180dp"
+        android:text="Verbinden"
+        android:background="@color/THblue"
+        app:layout_constraintEnd_toEndOf="parent"
+        app:layout_constraintStart_toStartOf="parent"
+        app:layout_constraintTop_toTopOf="parent"></Button>
+
+
 </androidx.constraintlayout.widget.ConstraintLayout>

MobileApp/app/src/main/res/layout/fragment_manuell.xml

@@ -10,13 +10,13 @@
         android:id="@+id/joystick"
         android:layout_width="308dp"
         android:layout_height="317dp"
-        android:layout_marginTop="12dp"
+        android:layout_marginTop="68dp"
-        android:background="@mipmap/kreis"
+        android:background="@mipmap/blank"
         app:JV_buttonColor="#FFFFFF"
         app:JV_buttonSizeRatio="11%"
         app:JV_fixedCenter="false"
         app:layout_constraintEnd_toEndOf="parent"
-        app:layout_constraintHorizontal_bias="0.481"
+        app:layout_constraintHorizontal_bias="0.495"
         app:layout_constraintStart_toStartOf="parent"
         app:layout_constraintTop_toTopOf="parent" />
 
@@ -25,6 +25,7 @@
         android:layout_width="match_parent"
         android:layout_height="wrap_content"
         android:layout_marginStart="8dp"
+        android:layout_marginTop="5dp"
         android:layout_marginEnd="8dp"
         android:textAlignment="center"
         android:textSize="15sp"
@@ -37,9 +38,9 @@
 
     <Button
         android:id="@+id/rotateLeft"
-        android:layout_width="140dp"
+        android:layout_width="120dp"
-        android:layout_height="140dp"
+        android:layout_height="120dp"
-        android:layout_marginTop="325dp"
+        android:layout_marginTop="350dp"
         android:background="@drawable/ic_baseline_rotate_left_24"
         app:layout_constraintEnd_toEndOf="parent"
         app:layout_constraintHorizontal_bias="0.150"
@@ -48,9 +49,9 @@
 
     <Button
         android:id="@+id/rotateRight"
-        android:layout_width="140dp"
+        android:layout_width="120dp"
-        android:layout_height="140dp"
+        android:layout_height="120dp"
-        android:layout_marginTop="325dp"
+        android:layout_marginTop="350dp"
         android:background="@drawable/ic_baseline_rotate_right_24"
         app:layout_constraintEnd_toEndOf="parent"
         app:layout_constraintHorizontal_bias="0.85"

Socket/lfr_socket.cpp

@@ -2,6 +2,7 @@
 
 LFR_Socket::LFR_Socket(ExceptionCallback cb): cb(cb)
 {
+    bufferIterator = std::begin(buffer);
     if ((server_fd = socket(AF_INET, SOCK_STREAM, 0)) == -1)
     {
         throw std::runtime_error("Failed opening the socket");
@@ -66,7 +67,7 @@ void LFR_Socket::createThread()
         bool connected = false;
         while(true)
         {
-            LFR_Socket::LFR_Telegram telegram;
+            LFR_Socket::LFR_Telegram telegram = {};
             bool received = false;
             if(!stop && !listeners.empty())
             {
@@ -91,32 +92,46 @@ void LFR_Socket::createThread()
                                 }
                                 else
                                 {
-                                    //std::cout << "accepted connection" << std::endl;
+                                    std::cout << "accepted connection" << std::endl;
                                     pollfds[1].fd = new_socket;
                                     connected  = true;
                                 }
                             }
                             else if (connected  && pollfds[0].revents & POLLIN)
                             {
-                                //std::cout << "second connection incoming" << std::endl;
+                                std::cout << "second connection incoming" << std::endl;
                                 int tmp_socket = accept(server_fd, (struct sockaddr *)&address, (socklen_t *)&addrlen);
                                 close(tmp_socket);
-                                //std::cout << "second connection closed" << std::endl;
+                                std::cout << "second connection closed" << std::endl;
                             }
                             else if (pollfds[1].revents & POLLIN)
                             {
-                                bytes_received = recv(new_socket, buffer, sizeof(buffer), 0);
+                                LFR_Telegram tmpBuffer = {0};
-                                if (bytes_received > 0)
+                                bytesReceived = recv(new_socket, tmpBuffer, sizeof(tmpBuffer), 0);
+                                if (bytesReceived > 0)
                                 {
-                                    received = true;
+                                    if(bufferIterator - std::begin(buffer) + bytesReceived > 1024)
-                                    std::copy(std::begin(buffer), std::end(buffer), std::begin(telegram));
+                                    {
-                                    //std::cout << "message received" << std::endl;
+                                        //Too long for buffer. Clear it.
+                                        std::fill(std::begin(buffer), std::end(buffer), 0);
+                                        bufferIterator = std::begin(buffer);
                                     }
-                                else if(bytes_received == 0)
+                                    bufferIterator = std::copy_if(
+                                        std::begin(tmpBuffer),
+                                        std::begin(tmpBuffer)+bytesReceived, 
+                                        bufferIterator,
+                                        [](char c){
+                                            return(c != '\n' && c != '\r');
+                                        });
+
+                                    //std::cout << "bytes received: " << bytesReceived << std::endl;
+                                    //received = true;
+                                }
+                                else if(bytesReceived == 0)
                                 {
                                     connected = false;
                                     close(pollfds[1].fd);
-                                    //std::cout << "connection closed by peer" << std::endl;
+                                    std::cout << "connection closed by peer" << std::endl;
                                 }
                             }
                         }
@@ -131,6 +146,32 @@ void LFR_Socket::createThread()
                     }
                 }
 
+                // Search the buffer for the start of a telegram
+                char *telegramStart = NULL;
+                telegramStart = strstr(buffer, "aa");
+                if(telegramStart && telegramStart != std::begin(buffer)) {
+                    //Move the content of the buffer
+                    auto delta = bufferIterator - telegramStart;
+                    std::copy(telegramStart, std::end(buffer), std::begin(buffer));
+                    bufferIterator = std::begin(buffer) + delta;
+                }
+
+                // Search the buffer for the end of a telegram
+                char *telegramEnd = NULL;
+                telegramEnd = strstr(buffer, "zz");
+                if(telegramEnd && telegramStart && telegramEnd-telegramStart>2) {
+                    std::copy(telegramStart+2, telegramEnd, telegram);
+                    received = true;
+                    std::copy(telegramEnd+2, std::end(buffer), std::begin(buffer));
+                    bufferIterator = std::begin(buffer);
+                }
+                else if(telegramEnd && telegramStart && telegramEnd-telegramStart<=2)
+                {
+                    //Got an empty telegram
+                    std::copy(telegramEnd, std::end(buffer), std::begin(buffer));
+                    bufferIterator = std::begin(buffer);
+                }
+                
                 //Invoke the callback method (ListenerPair second -> ListenerCallback)
                 if (received)
                 {

Socket/lfr_socket.h

@@ -13,6 +13,7 @@
 #include <string>
 #include <algorithm>
 #include <exception>
+#include <cstring>
 
 class LFR_Socket
 {
@@ -31,11 +32,12 @@ private:
     std::unique_ptr<std::thread> thread;
     mutable std::mutex mutex;
 
-    int server_fd, new_socket, bytes_received;
+    int server_fd, new_socket, bytesReceived;
     struct sockaddr_in address;
     int opt = 1;
     int addrlen = sizeof(address);
     char buffer[1024] = {0};    
+    char* bufferIterator;
 
     
     //void provideOutput(Mat originalImage, Mat processedImage, const FrameData& frameData, const Rect& roi);

lfr_state_interface.h

@@ -0,0 +1,11 @@
+#pragma once
+
+class LFR_StateMachine;
+
+class LFR_IState
+{
+ public:
+    virtual void enter(LFR_StateMachine* stateMachine) = 0;
+    virtual void exit(LFR_StateMachine* stateMachine) = 0;
+    virtual ~LFR_IState() {}
+};

lfr_state_machine.cpp

@@ -0,0 +1,186 @@
+#include "lfr_state_machine.h"
+
+// for string delimiter
+vector<string> LFR_StateMachine::split (string s, string delimiter) const {
+    size_t pos_start = 0, pos_end, delim_len = delimiter.length();
+    string token;
+    vector<string> res;
+
+    while ((pos_end = s.find (delimiter, pos_start)) != string::npos) {
+        token = s.substr (pos_start, pos_end - pos_start);
+        pos_start = pos_end + delim_len;
+        res.push_back (token);
+    }
+
+    res.push_back (s.substr (pos_start));
+    return res;
+}
+
+void LFR_StateMachine::sanitize (string& s) const
+{
+        char r = '\r', n = '\n';
+        s.erase(std::remove(s.begin(), s.end(), n), s.end());
+        s.erase(std::remove(s.begin(), s.end(), r), s.end());
+}
+
+bool LFR_StateMachine::checkStringValidity (const std::vector<std::string>& s) const
+{
+    try
+    {
+        int i = stoi(s[0]);
+        if(i == 0 && s.size() == 5)
+        {
+            for(int i = 0; i < 4; i ++)
+            {
+                if(stod(s[i]) > 1.0 || stod(s[i]) < -1.0)
+                {
+                    return false;
+                }
+            }
+        }
+        else if(i == 1)
+        {
+            return true;
+        }
+        else
+        {
+            return false;
+        }       
+    }
+    catch(const std::exception& e) {return false;}
+    return true;
+}
+
+void LFR_StateMachine::parseString(string s)
+{
+    sanitize(s);
+    std::vector<std::string> splitStr = split(s, ";");
+    if(!checkStringValidity(splitStr))
+    {
+        std::cout<<"Error: Invalid String: " << s << std::endl;
+        return;
+    }
+    else
+    {
+        std::cout<< s << std::endl;
+    }
+    double wheels[4] = {0.0, 0.0, 0.0, 0.0};
+    int mode = std::stoi(splitStr[0]);
+    if(mode == 0) {
+        for(int i = 1; i <= 4; i++)
+        {
+            wheels[i-1] = std::stod(splitStr[i]);
+        }
+        setState(State::Manual::getInstance());
+        uartCommunicator.sendTelegram(wheels[0], wheels[1], wheels[2], wheels[3]);
+    }
+    else if (mode == 1) {setState(State::Autonomous::getInstance());}
+    return;
+}
+
+LFR_StateMachine::LFR_StateMachine():    
+    autonomousMode(videoHeight, videoWidth, thresholdBinary, gaussKernelSize, maxSpeed, [&](std::exception const &ex)
+    {
+        std::unique_lock<std::mutex> lock(mutex);
+        std::cerr<<"camera exception:"<<ex.what()<<std::endl;
+        return false;
+    }),
+    uartCommunicator(),
+    socket([&](std::exception const &ex)
+    {
+        std::unique_lock<std::mutex> lock(mutex);
+        std::cerr<<"socket exception:"<<ex.what()<<std::endl;
+        return false;
+    })
+{
+    // Connect String parser to socket
+    socket.addListener([&](LFR_Socket::LFR_Telegram telegram)
+    {
+        std::unique_lock<std::mutex> lock(mutex);
+        parseString(std::string(telegram));
+    }, &mutex);
+    socket.startLoop();
+    
+    currentState = &State::Idle::getInstance();
+    currentState->enter(this);
+
+    cv::VideoWriter writer = cv::VideoWriter("video_200.avi", cv::VideoWriter::fourcc('M','J','P','G'), 4.0, cv::Size(videoWidth, videoHeight), true);
+    
+    auto t_start = std::chrono::high_resolution_clock::now();
+    auto t_end = std::chrono::high_resolution_clock::now();
+    double dur = std::chrono::duration<double, std::milli>(t_end-t_start).count();
+    
+    while(dur < 60000)
+    {
+        t_end = std::chrono::high_resolution_clock::now();
+        dur = std::chrono::duration<double, std::milli>(t_end-t_start).count();
+        {
+            this_thread::sleep_for(std::chrono::milliseconds(50));
+            std::unique_lock<std::mutex> lock(imgMutex);
+            if(!this->image.empty())
+            {
+                writer.write(this->image);
+            }
+        }
+    }
+    writer.release();
+    std::cout << "Exiting central" << std::endl;
+}
+
+void LFR_StateMachine::setState(LFR_IState& newState)
+{
+    if (&newState != currentState)
+    {
+        currentState->exit(this);
+        currentState = &newState;
+        currentState->enter(this);
+    }
+}
+
+void LFR_StateMachine::enterAutonomous()
+{
+    //cv::Mat img;
+    autonomousMode.addListener([&](LFR_Result result)
+    {
+        std::unique_lock<std::mutex> lock(mutex);
+        if (!result.rawImage.empty())
+        {
+            /*
+            img = result.rawImage;
+            //Resize to minimize latency using raspi over ssh/adapt to 3.5in screen
+            cv::resize(result.rawImage, img, cv::Size(480, 320));
+
+            //Calculate frame rate
+            now = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch());
+            unsigned int deltaMs = static_cast<unsigned int>((now-last).count());
+            double delta = static_cast<double>(deltaMs) / 1000.0;
+            double frameRate = 1.0 / static_cast<double>(delta);*/
+            double frameRate = -1.0;
+            {
+                std::unique_lock<std::mutex> lock(imgMutex);
+                image = result.rawImage;
+            }
+
+            if (result.validLane)
+            {
+                std::cout << "Frame rate: " << frameRate << " angle: " << result.data.angle
+                    << " motor 1: " << result.motorSignals[0] << " motor 2: " << result.motorSignals[1]
+                    << " motor 3: " << result.motorSignals[2] << " motor 4: " << result.motorSignals[3] <<std::endl;
+                uartCommunicator.sendTelegram(result.motorSignals[0], result.motorSignals[1], result.motorSignals[2], result.motorSignals[3]);
+            }
+            else
+            {
+                std::cout << "No lane found." << std::endl;;
+            }
+            //last = now;
+        }
+    }, &mutex);
+
+    autonomousMode.startLoop();
+}
+
+void LFR_StateMachine::exitAutonomous()
+{
+    autonomousMode.removeListener(&mutex);
+    autonomousMode.endLoop();
+}

lfr_state_machine.h

@@ -0,0 +1,43 @@
+#pragma once
+#include <iostream>
+#include <algorithm>
+
+#include <lfr.h>
+#include <lfr_socket.h>
+#include <uart_communication.h>
+#include "lfr_state_interface.h"
+#include "lfr_states.h"
+
+class LFR_StateMachine
+{
+    LFR_IState* currentState;
+
+    const int thresholdBinary = 140;
+    const int videoHeight = 720;
+    const int videoWidth = 1280;
+    const int gaussKernelSize = 11;
+    const double maxSpeed = 0.20;
+
+    std::mutex mutex;
+
+    LFR autonomousMode;
+    LFR_UART uartCommunicator;
+    LFR_Socket socket;
+
+    std::mutex imgMutex;
+    Mat image;
+
+    vector<string> split (string s, string delimiter) const;
+    void sanitize (string& s) const;
+    bool checkStringValidity (const std::vector<std::string>& s) const;
+    void parseString(string s);    
+
+public:
+    void enterAutonomous();
+    void exitAutonomous();
+    LFR_StateMachine();
+    inline LFR_IState* getCurrentState() const {return currentState;}
+    void setState(LFR_IState& newState);
+};
+
+

lfr_states.cpp

@@ -0,0 +1,52 @@
+#include "lfr_states.h"
+
+void State::Idle::enter(LFR_StateMachine* stateMachine)
+{
+
+}
+
+void State::Idle::exit(LFR_StateMachine* stateMachine)
+{
+    
+}
+
+LFR_IState& State::Idle::getInstance()
+{
+    static State::Idle singleton;
+    return singleton;
+}
+
+void State::Autonomous::enter(LFR_StateMachine* stateMachine)
+{
+    std::cout << "enter autonomous mode" << std::endl;
+    stateMachine->enterAutonomous();
+}
+
+void State::Autonomous::exit(LFR_StateMachine* stateMachine)
+{
+    std::cout << "exit autonomous mode" << std::endl;
+    stateMachine->exitAutonomous();
+}
+
+LFR_IState& State::Autonomous::getInstance()
+{
+    static State::Autonomous singleton;
+    return singleton;
+}
+
+void State::Manual::enter(LFR_StateMachine* stateMachine)
+{
+    std::cout << "enter manual mode" << std::endl;
+
+}
+
+void State::Manual::exit(LFR_StateMachine* stateMachine)
+{
+    std::cout << "exit manual mode" << std::endl;
+}
+
+LFR_IState& State::Manual::getInstance()
+{
+    static State::Manual singleton;
+    return singleton;
+}

lfr_states.h

@@ -0,0 +1,44 @@
+#pragma once
+#include "lfr_state_interface.h"
+#include "lfr_state_machine.h"
+
+namespace State
+{
+    class Idle: public LFR_IState
+    {
+        Idle() {}
+        Idle(const Idle& other) {};
+        Idle& operator= (const Idle& other);
+
+    public:
+        void enter(LFR_StateMachine*);
+        void exit(LFR_StateMachine*);
+        static LFR_IState& getInstance();
+    };
+
+
+    class Autonomous: public LFR_IState
+    {
+        Autonomous() {}
+        Autonomous(const Autonomous& other) {};
+        Autonomous& operator= (const Autonomous& other);
+
+    public:
+        void enter(LFR_StateMachine*);
+        void exit(LFR_StateMachine*);
+        static LFR_IState& getInstance();
+    };
+
+
+    class Manual: public LFR_IState
+    {
+        Manual() {}
+        Manual(const Manual& other) {};
+        Manual& operator= (const Manual& other);
+
+    public:
+        void enter(LFR_StateMachine*);
+        void exit(LFR_StateMachine*);
+        static LFR_IState& getInstance();
+    };
+}

main.cpp

@@ -1,37 +1,14 @@
 #include <iostream>
-#include <lfr.h>
+#include "lfr_state_machine.h"
-#include <lfr_socket.h>
+#include "lfr_states.h"
-#include <uart_communication.h>
+
+
 
 int main(void)
 {
-    std::cout << "hello central" << std::endl;
+    std::cout << "started central" << std::endl;
-
+    // Init State Machine;
-    const int thresholdBinary = 140;
+    std::cout << "create State Machine" << std::endl;
-    const int videoHeight = 720;
+    LFR_StateMachine stateMachine;
-    const int videoWidth = 1280;
-    const int gaussKernelSize = 11;
-
-    std::mutex mutex;
-
-    std::cout << "create lfr" << std::endl;
-    LFR lfr(videoHeight, videoWidth, thresholdBinary, gaussKernelSize, [&](std::exception const &ex)
-    {
-        std::unique_lock<std::mutex> lock(mutex);
-        std::cerr<<"camera exception:"<<ex.what()<<std::endl;
-        return false;
-    });
-
-    std::cout << "create uart" << std::endl;
-    LFR_UART uartCommunicator;
-
-    std::cout << "create socket" << std::endl;
-    LFR_Socket socket([&](std::exception const &ex)
-    {
-        std::unique_lock<std::mutex> lock(mutex);
-        std::cerr<<"socket exception:"<<ex.what()<<std::endl;
-        return false;
-    });
-
     return 0;
 }