diff --git a/AutonomousMode/ControlModule/control_module.cpp b/AutonomousMode/ControlModule/control_module.cpp
index 1f235b3..a80bea4 100644
--- a/AutonomousMode/ControlModule/control_module.cpp
+++ b/AutonomousMode/ControlModule/control_module.cpp
@@ -4,7 +4,7 @@ ControlModule::ControlModule(): ControlModule(0.0, 1.0, 1.0)
 {    
 }
 
-ControlModule::ControlModule(float maxSpeed, float rotateSpeed, float moveSideSpeed)
+ControlModule::ControlModule(double maxSpeed, double rotateSpeed, double moveSideSpeed)
 {
     motors[0] = 0.0;
     motors[1] = 0.0;
@@ -41,15 +41,15 @@ void ControlModule::adjustSpeed(){
 };
 
 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){
-    float speed = rotateSpeed * (static_cast<float>(angle) + 90.0f)/90.0f;
+void ControlModule::rotate(double angle){
+    double speed = rotateSpeed * (angle + 90.0)/90.0;
     motors[0] += speed;
     motors[1] -= speed;
     motors[2] += speed;
@@ -57,7 +57,7 @@ void ControlModule::rotate(int angle){
 }
 
 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];
@@ -73,7 +73,7 @@ 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);
     //moveSide(imageColumsMiddle, contourColumsMiddle);
     rotate(angle);
@@ -81,8 +81,8 @@ void ControlModule::calcSpeeds(int imageColumsMiddle, int contourColumsMiddle, i
     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]};
 } 
\ No newline at end of file
diff --git a/AutonomousMode/ControlModule/control_module.h b/AutonomousMode/ControlModule/control_module.h
index 4022ff5..33aee68 100644
--- a/AutonomousMode/ControlModule/control_module.h
+++ b/AutonomousMode/ControlModule/control_module.h
@@ -7,19 +7,19 @@ class ControlModule
 {
 private:
     mutable std::mutex mtx;
-    float motors[4]; //LeftFront; RightFront; LeftBack; RightBack
-    float maxSpeed;
-    float rotateSpeed;
-    float moveSideSpeed;
+    double motors[4]; //LeftFront; RightFront; LeftBack; RightBack
+    double maxSpeed;
+    double rotateSpeed;
+    double moveSideSpeed;
 public:
     ControlModule(/* args */);
-    ControlModule(float maxSpeed, float rotateSpeed, float moveSideSpeed);
+    ControlModule(double maxSpeed, double rotateSpeed, double moveSideSpeed);
     ~ControlModule();
     void adjustSpeed();
     void moveSide(int imageColumsMiddle, int contourColumsMiddle);
-    void rotate(int angle);
+    void rotate(double angle);
     void unit();            //Brings the max Value to 1.0
 
-    void calcSpeeds(int imageColumsMiddle, int contourColumsMiddle, int angle); //Funktion to be called
-    std::vector<float> readMotors();
+    void calcSpeeds(int imageColumsMiddle, int contourColumsMiddle, double angle); //Funktion to be called
+    std::vector<double> readMotors();
 };
\ No newline at end of file
diff --git a/AutonomousMode/Utils/CMakeLists.txt b/AutonomousMode/Utils/CMakeLists.txt
index 196ca0d..33158ab 100644
--- a/AutonomousMode/Utils/CMakeLists.txt
+++ b/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})
diff --git a/AutonomousMode/Utils/utils.cpp b/AutonomousMode/Utils/utils.cpp
index f23524c..7ffbea4 100644
--- a/AutonomousMode/Utils/utils.cpp
+++ b/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);
diff --git a/AutonomousMode/Utils/utils.h b/AutonomousMode/Utils/utils.h
index e0e0809..ec847b0 100644
--- a/AutonomousMode/Utils/utils.h
+++ b/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() {}
diff --git a/AutonomousMode/lfr.h b/AutonomousMode/lfr.h
index 4dfb5bc..e2ad730 100644
--- a/AutonomousMode/lfr.h
+++ b/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