diff --git a/include/config.h b/include/config.h
index 7fa1074..9217138 100644
--- a/include/config.h
+++ b/include/config.h
@@ -1,10 +1,2 @@
-    //GPS Serial Pins
-    #define GPS_RX 17
-    #define GPS_TX 16
-    #define GPS_BAUD 9600
-
-
-
-
 // PS3 Controller
-// ESP32 MAC BL 24:62:AB:F2:4B:3A
+#define CONTROLLER_MAC "24:62:AB:F2:4B:3A"
diff --git a/include/route.h b/include/route.h
deleted file mode 100644
index 29f90ac..0000000
--- a/include/route.h
+++ /dev/null
@@ -1,53 +0,0 @@
-// #ifndef ROUTE_H
-// #define ROUTE_H
-
-// #include <cstdint>
-// #include <list>
-// #include <TinyGPS++.h>
-// #include <Arduino.h>
-
-// #include "motorControl.h"
-// #include "speedometer.h"
-// #include "debugMqtt.h"
-// #include "config.h"
-
-// struct Point{
-//     double lat = 0;
-//     double lon = 0;
-//     bool operator==(const Point& rhs) const {
-//         return this->lat == rhs.lat && this->lon == rhs.lon;
-//     }
-// };
-
-// class Route {
-//     public:
-//         Route();
-
-//         void runRoute();
-
-//         void addPointToRoute(Point point);
-//         void addCurrentLocationToRoute();
-//         void delRoute();
-//         void resetRoute();
-
-//         Point startRoute();
-//         Point getNextPoint();
-//         uint16_t getNumberOfPoints();
-
-//         static double getDis(Point point_1, Point point_2);
-
-
-//     private:
-//         bool nearlySameLocation(Point p1, Point p2);
-
-//         std::list<Point> points;
-//         std::list<Point>::iterator it;
-//         Point currentLocation;
-
-//         uint16_t count_points = 0;
-
-//         bool route_started = false;
-//         bool route_finished = false;
-// };
-
-// #endif // ROUTE_H
\ No newline at end of file
diff --git a/lib/Menu/menu.cpp b/lib/Menu/menu.cpp
index 3a9c768..ee33e34 100644
--- a/lib/Menu/menu.cpp
+++ b/lib/Menu/menu.cpp
@@ -23,29 +23,24 @@ void Menu::addEntry(MenuAction* entry) {
 
 void Menu::printMenu() {
     if (this->lcd) {
-        MenuAction* selectedEntry = *(this->it);
         lcd->clear();
         lcd->setCursor(0, 0);
         lcd->print("-> ");
-        lcd->print(selectedEntry->getName());
+        lcd->print((**this->it).getName());
         lcd->setCursor(0, 1);
         std::list<MenuAction*>::iterator iter = this->it;
         iter++;
         if (iter != this->entrys.end()) {
-            selectedEntry = *iter;
-            lcd->print(selectedEntry->getName());
-        } else {
-            selectedEntry = *this->entrys.begin();
-            lcd->print(selectedEntry->getName());
-        }
+            lcd->print((**iter).getName());
+        } else
+            lcd->print((*this->entrys.begin())->getName());
     } else {
         std::cout << std::endl;
         for (std::list<MenuAction*>::iterator iter = this->entrys.begin(); iter != this->entrys.end(); iter++) {
-            MenuAction* selectedEntry = *(iter);
             if (this->it == iter)
-                std::cout << "  " << selectedEntry->getName() << std::endl;
+                std::cout << "  " << (**iter).getName() << std::endl;
             else
-                std::cout << selectedEntry->getName() << std::endl;
+                std::cout << (**iter).getName() << std::endl;
         }
     }
     this->inSubmenu = false;
@@ -53,8 +48,7 @@ void Menu::printMenu() {
 
 void Menu::down() {
     if (this->inSubmenu) {
-        MenuAction* selectedEntry = *(this->it);
-        selectedEntry->getMenu()->down();
+        (**this->it).getMenu()->down();
         return;
     }
 
@@ -70,8 +64,7 @@ void Menu::down() {
 
 void Menu::up() {
     if (this->inSubmenu) {
-        MenuAction* selectedEntry = *(this->it);
-        selectedEntry->getMenu()->up();
+        (**this->it).getMenu()->up();
         return;
     }
     std::list<MenuAction*>::iterator iter = this->entrys.end();
@@ -86,25 +79,21 @@ void Menu::up() {
 }
 
 void Menu::right() {
-    // TODO: find out why i cannot call runAction() directly from the iterator
-    MenuAction* selectedEntry = *(this->it);
-
     if (this->inSubmenu) {
-        selectedEntry->getMenu()->right();
+        (**this->it).getMenu()->right();
         return;
     }
 
-    if (selectedEntry->getIsMenu()) {
+    if ((**this->it).getIsMenu()) {
         this->inSubmenu = true;
-        selectedEntry->getMenu()->setParentMenu(this);
+        (**this->it).getMenu()->setParentMenu(this);
     } 
-    selectedEntry->runAction();
+    (**this->it).runAction();
 }
 
 void Menu::left() {
     if (this->inSubmenu) {
-        MenuAction* selectedEntry = *(this->it);
-        selectedEntry->getMenu()->left();
+        (**this->it).getMenu()->left();
         return;
     }
 
@@ -114,8 +103,7 @@ void Menu::left() {
 
 void Menu::yes() {
     if (this->inSubmenu) {
-        MenuAction* selectedEntry = *(this->it);
-        selectedEntry->getMenu()->yes();
+        (**this->it).getMenu()->yes();
         return;
     }
 
@@ -124,8 +112,7 @@ void Menu::yes() {
 
 void Menu::no() {
     if (this->inSubmenu) {
-        MenuAction* selectedEntry = *(this->it);
-        selectedEntry->getMenu()->no();
+        (**this->it).getMenu()->no();
         return;
     }
 
diff --git a/lib/Navigation/coordinate.cpp b/lib/Navigation/coordinate.cpp
deleted file mode 100644
index 13051f3..0000000
--- a/lib/Navigation/coordinate.cpp
+++ /dev/null
@@ -1,2 +0,0 @@
-#include "coordinate.h"
-
diff --git a/lib/Navigation/coordinate.h b/lib/Navigation/coordinate.h
deleted file mode 100644
index 683707b..0000000
--- a/lib/Navigation/coordinate.h
+++ /dev/null
@@ -1,31 +0,0 @@
-/**
- * @file coordinate.h
- * @author Alexander Klein (alex@kleiax.de)
- * @brief 
- * @version 0.1
- * @date 2022-01-10
- * 
- * @copyright Copyright (c) 2022
- * 
- */
-
-#ifndef COORDINATE_H
-#define COORDINATE_H
-
-#include <stdint.h>
-
-class Coordinate {
-    public:
-        Coordinate(/* args */);
-        ~Coordinate();
-
-        double getDistance(Coordinate coordinate);
-        uint16_t getCompassDirection(Coordinate coordinate);
-    
-    private:
-        double lon;
-        double lat;
-};
-
-
-#endif // COORDINATE_H
\ No newline at end of file
diff --git a/lib/Navigation/navigation.cpp b/lib/Navigation/navigation.cpp
index a352ad6..ad12832 100644
--- a/lib/Navigation/navigation.cpp
+++ b/lib/Navigation/navigation.cpp
@@ -1,2 +1,29 @@
+/**
+ * @file navigation.cpp
+ * @author Alexander Klein (alex@kleiax.de)
+ * @brief 
+ * @version 0.1
+ * @date 2022-01-31
+ * 
+ * @copyright Copyright (c) 2022
+ * 
+ */
+
 #include "navigation.h"
 
+Navigation::Navigation(uint8_t rx, uint8_t tx) {
+    this->gps = new TinyGPSPlus();
+    Serial1.begin(9600, SERIAL_8N1, rx, tx);
+}
+
+void Navigation::loop() {
+    gps->encode(Serial1.read());
+}
+
+void Navigation::addCurrentPosToRoute() {
+    Point p;
+    if (this->gps->location.isUpdated() && this->gps->location.isValid()) {
+        p.lat = this->gps->location.lat();
+        p.lon = this->gps->location.lng();
+    }
+}
diff --git a/lib/Navigation/navigation.h b/lib/Navigation/navigation.h
index 68a36b8..b859434 100644
--- a/lib/Navigation/navigation.h
+++ b/lib/Navigation/navigation.h
@@ -12,22 +12,22 @@
 #ifndef NAVIGATION_H
 #define NAVIGATION_H
 
+#include <TinyGPS++.h>
 #include <Arduino.h>
 
-#include "coordinate.h"
-
+#include "route.h"
 class Navigation {
     public:
-        Navigation(uint32_t baud, uint8_t rx, uint8_t tx);
+        Navigation(uint8_t rx, uint8_t tx);
+        void loop();
 
-        void addCoordinateToRoute(Coordinate coordinate);
-        void addCurrentCoordinateToRoute();
-        void delRoute();
+        void addCurrentPosToRoute();
 
-        double getDistanceToLastCoordinate();    
+        TinyGPSPlus* getGPS() { return this->gps; }
 
     private:
-        Coordinate *lastCoordinate;
+        TinyGPSPlus* gps;
+        Route* route;
 };
 
 #endif // NAVIGATION_H
\ No newline at end of file
diff --git a/lib/Navigation/route.cpp b/lib/Navigation/route.cpp
new file mode 100644
index 0000000..a2d9135
--- /dev/null
+++ b/lib/Navigation/route.cpp
@@ -0,0 +1,32 @@
+/**
+ * @file route.cpp
+ * @author Alexander Klein (alex@kleiax.de)
+ * @brief 
+ * @version 0.1
+ * @date 2022-01-31
+ * 
+ * @copyright Copyright (c) 2022
+ * 
+ */
+
+#include "route.h"
+
+Route::Route() {
+
+}
+
+void Route::addPointToRoute(Point point) {
+    this->points.push_back(point);
+}
+
+Point Route::startRoute() {
+    *this->it = this->points.begin();
+    return **this->it;
+}
+
+Point Route::getNextPoint() {
+    if (*this->it != this->points.end())
+        return **this->it;
+    else
+        return Point(0, 0);
+}
\ No newline at end of file
diff --git a/lib/Navigation/route.h b/lib/Navigation/route.h
new file mode 100644
index 0000000..485029b
--- /dev/null
+++ b/lib/Navigation/route.h
@@ -0,0 +1,48 @@
+/**
+ * @file route.h
+ * @author Alexander Klein (alex@kleiax.de)
+ * @brief 
+ * @version 0.1
+ * @date 2022-01-31
+ * 
+ * @copyright Copyright (c) 2022
+ * 
+ */
+
+#ifndef ROUTE_H
+#define ROUTE_H
+
+#include <cstdint>
+#include <list>    
+
+        
+    
+
+class Point{
+    public:
+        Point(double lat, double lon) {this->lat = lat; this->lon = lon;}
+        double lat = 0;
+        double lon = 0;
+        bool operator==(const Point& rhs) const {
+            return this->lat == rhs.lat && this->lon == rhs.lon;
+        }
+};
+
+class Route {
+    public:
+        Route();
+        void addPointToRoute(Point point);
+        Point startRoute();
+        Point getNextPoint();
+
+        uint16_t getNumberOfPoints() { return this->count_points; }
+
+    private:
+        uint16_t count_points = 0;
+
+        std::list<Point> points;
+        std::list<Point>::iterator* it;
+
+};
+
+#endif // ROUTE_H
\ No newline at end of file
diff --git a/src/SpecialMenus/menuCaptureRoute.cpp b/src/SpecialMenus/menuCaptureRoute.cpp
new file mode 100644
index 0000000..8fb6928
--- /dev/null
+++ b/src/SpecialMenus/menuCaptureRoute.cpp
@@ -0,0 +1,24 @@
+/**
+ * @file menuCaptureRoute.cpp
+ * @author Alexander Klein (alex@kleiax.de)
+ * @brief 
+ * @version 0.1
+ * @date 2022-01-31
+ * 
+ * @copyright Copyright (c) 2022
+ * 
+ */
+#include "menuCaptureRoute.h"
+
+void MenuCaptureRoute::printMenu() {
+    this->driveManager->changeModus(Modi::CaptureRoute);
+    if (this->lcd) {
+        this->lcd->clear();
+        this->lcd->setCursor(0, 0);
+        this->lcd->print("Jetzt kannst");
+        this->lcd->setCursor(0, 1);
+        this->lcd->print("du fahren. :-)");
+
+    } else
+        std::cout << "Jetzt kannst du\n fahren. :-)" << std::endl;
+}
\ No newline at end of file
diff --git a/src/SpecialMenus/menuCaptureRoute.h b/src/SpecialMenus/menuCaptureRoute.h
new file mode 100644
index 0000000..a0ff636
--- /dev/null
+++ b/src/SpecialMenus/menuCaptureRoute.h
@@ -0,0 +1,22 @@
+/**
+ * @file menuCaptureRoute.h
+ * @author Alexander Klein (alex@kleiax.de)
+ * @brief 
+ * @version 0.1
+ * @date 2022-01-31
+ * 
+ * @copyright Copyright (c) 2022
+ * 
+ */
+#ifndef MENU_CAPTURE_ROUTE_H
+#define MENU_MANUAL_DRIVE_H
+
+#include "menuDriveMode.h"
+
+class MenuCaptureRoute : public MenuDriveMode {
+    public:
+        MenuCaptureRoute(DriveManager* driveManager) : MenuDriveMode(driveManager) {}
+        void printMenu();
+};
+
+#endif // MENU_MANUAL_DRIVE_H
\ No newline at end of file
diff --git a/src/SpecialMenus/menuDriveMode.cpp b/src/SpecialMenus/menuDriveMode.cpp
new file mode 100644
index 0000000..c8361ca
--- /dev/null
+++ b/src/SpecialMenus/menuDriveMode.cpp
@@ -0,0 +1,26 @@
+/**
+ * @file menuDriveMode.cpp
+ * @author Alexander Klein (alex@kleiax.de)
+ * @brief 
+ * @version 0.1
+ * @date 2022-01-31
+ * 
+ * @copyright Copyright (c) 2022
+ * 
+ */
+
+#include "menuDriveMode.h"
+
+MenuDriveMode::MenuDriveMode(DriveManager* driveManager) {
+    this->driveManager = driveManager;
+}
+
+void MenuDriveMode::left() {
+    this->driveManager->changeModus(Modi::Off);
+    if (parentMenu)
+        this->parentMenu->printMenu();
+}
+
+void MenuDriveMode::no() {
+    this->left();
+}
\ No newline at end of file
diff --git a/src/SpecialMenus/menuDriveMode.h b/src/SpecialMenus/menuDriveMode.h
new file mode 100644
index 0000000..6fd3b99
--- /dev/null
+++ b/src/SpecialMenus/menuDriveMode.h
@@ -0,0 +1,34 @@
+/**
+ * @file menuDriveMode.h
+ * @author Alexander Klein (alex@kleiax.de)
+ * @brief 
+ * @version 0.1
+ * @date 2022-01-31
+ * 
+ * @copyright Copyright (c) 2022
+ * 
+ */
+#ifndef MENU_DRIVE_MODI_H
+#define MENU_DRIVE_MODI_H
+
+#include "menuControl.h"
+#include "driveModi/driveManager.h"
+
+class MenuDriveMode : public MenuControl {
+    public:
+        MenuDriveMode(DriveManager* driveManager);
+
+        void down(){}
+        void up(){}
+        void right(){}
+        void left();
+        void no();
+        void yes(){}
+
+        virtual void printMenu() = 0;
+
+    protected:
+        DriveManager* driveManager;
+};
+
+#endif // MENU_DRIVE_MODI_H
\ No newline at end of file
diff --git a/src/SpecialMenus/menuGPS.cpp b/src/SpecialMenus/menuGPS.cpp
index ce22ae3..45796fc 100644
--- a/src/SpecialMenus/menuGPS.cpp
+++ b/src/SpecialMenus/menuGPS.cpp
@@ -36,7 +36,7 @@ void MenuGPS::printMenu() {
     if (this->lcd) {
         lcd->clear();
         lcd->setCursor(0, 0);
-        if (gps->satellites.isValid()) {
+        if (gps->satellites.isValid() && gps->satellites.value() > 2) {
             lcd->printf("Sats: %3.d", gps->satellites.value());
             lcd->setCursor(0, 1);
             lcd->printf("HDOP: %f", gps->hdop.hdop());
@@ -44,12 +44,14 @@ void MenuGPS::printMenu() {
             lcd->clear();
             lcd->setCursor(0, 0);
             lcd->printf("Data isn't valid");
+            lcd->setCursor(0, 1);
+            lcd->printf("or no GPS signal");
         }
     } else {
-        if (gps->satellites.isValid())
+        if (gps->satellites.isValid() && gps->satellites.value() > 2)
             std::cout   << "Sats: " << gps->satellites.value()
                         << "HDOP: " << gps->hdop.hdop() << std::endl;
         else 
-            std::cout << "Date isn't valid" << std::endl;
+            std::cout << "Date isn't valid or no GPS signal" << std::endl;
     }
 }
\ No newline at end of file
diff --git a/src/SpecialMenus/menuManualDrive.cpp b/src/SpecialMenus/menuManualDrive.cpp
index 2cad2a0..09eeee6 100644
--- a/src/SpecialMenus/menuManualDrive.cpp
+++ b/src/SpecialMenus/menuManualDrive.cpp
@@ -1,19 +1,15 @@
+/**
+ * @file menuManualDrive.cpp
+ * @author Alexander Klein (alex@kleiax.de)
+ * @brief 
+ * @version 0.1
+ * @date 2022-01-20
+ * 
+ * @copyright Copyright (c) 2022
+ * 
+ */
 #include "menuManualDrive.h"
 
-MenuManualControl::MenuManualControl(DriveManager* driveManager) {
-    this->driveManager = driveManager;
-}
-
-void MenuManualControl::left() {
-    this->driveManager->changeModus(Modi::Off);
-    if (parentMenu)
-        this->parentMenu->printMenu();
-}
-
-void MenuManualControl::no() {
-    this->left();
-}
-
 void MenuManualControl::printMenu() {
     this->driveManager->changeModus(Modi::ManualControl);
     if (this->lcd) {
diff --git a/src/SpecialMenus/menuManualDrive.h b/src/SpecialMenus/menuManualDrive.h
index 8b388e5..6ee2ecc 100644
--- a/src/SpecialMenus/menuManualDrive.h
+++ b/src/SpecialMenus/menuManualDrive.h
@@ -12,27 +12,12 @@
 #ifndef MENU_MANUAL_DRIVE_H
 #define MENU_MANUAL_DRIVE_H
 
-#include "menuControl.h"
-#include "driveModi/driveManager.h"
+#include "menuDriveMode.h"
 
-// TODO: delete after installtion of display
-#include <Arduino.h>
-
-class MenuManualControl : public MenuControl {
+class MenuManualControl : public MenuDriveMode {
     public:
-        MenuManualControl(DriveManager* driveManager);
-
-        void down(){}
-        void up(){}
-        void right(){}
-        void left();
-        void no();
-        void yes(){}
-
+        MenuManualControl(DriveManager* driveManager) : MenuDriveMode(driveManager) {}
         void printMenu();
-
-    private:
-        DriveManager* driveManager;
 };
 
 #endif // MENU_MANUAL_DRIVE_H
\ No newline at end of file
diff --git a/src/driveModi/Modi/CaptureRoute/captureRoute.cpp b/src/driveModi/Modi/CaptureRoute/captureRoute.cpp
index d161a93..a8e4d47 100644
--- a/src/driveModi/Modi/CaptureRoute/captureRoute.cpp
+++ b/src/driveModi/Modi/CaptureRoute/captureRoute.cpp
@@ -1,22 +1,14 @@
 #include "driveModi/Modi/CaptureRoute/captureRoute.h"
 
-CaptureRoute::CaptureRoute() {
+CaptureRoute::CaptureRoute(MoveControl* moveControl, Navigation* navigation) 
+    : ManualControl(moveControl) {
+        this->navigation = navigation;
 }
 
-void CaptureRoute::init() {
-    
-    
+void CaptureRoute::loop() {
+    ManualControl::loop();
 }
 
 void CaptureRoute::runCaptureRoute() {
-    // this->runManualControl();
 
-    // static uint64_t last_millis = 0;
-    // if (millis() - last_millis < RUN_CAPTuRE_ROUTE_DELAY) {
-    //     return;
-    // }
-    // last_millis = millis();
-
-    //Add Point to route
-    // this->route->addCurrentLocationToRoute();
 }
\ No newline at end of file
diff --git a/src/driveModi/Modi/CaptureRoute/captureRoute.h b/src/driveModi/Modi/CaptureRoute/captureRoute.h
index c94d670..10374fd 100644
--- a/src/driveModi/Modi/CaptureRoute/captureRoute.h
+++ b/src/driveModi/Modi/CaptureRoute/captureRoute.h
@@ -1,21 +1,20 @@
 #ifndef CAPTURE_ROUTE_H
 #define CAPTURE_ROUTE_H
 
+#include <iostream>
+
 #include "driveModi/Modi/ManualControl/manualControl.h"
-// #include "route.h"
+#include "navigation.h"
 
-#include "config.h"
-
-class CaptureRoute : ManualControl {
+class CaptureRoute : public ManualControl {
     public:
-        CaptureRoute();
+        CaptureRoute(MoveControl* moveControl, Navigation* navigation);
 
-        void init();
+        void loop();
         void runCaptureRoute();
 
     private:
-        // Route *route;
-        // DebugMqtt *debug;
+        Navigation* navigation;
 };
 
 #endif // CAPTURE_ROUTE_H
\ No newline at end of file
diff --git a/src/driveModi/Modi/ManualControl/manualControl.cpp b/src/driveModi/Modi/ManualControl/manualControl.cpp
index 31d9767..f9740af 100644
--- a/src/driveModi/Modi/ManualControl/manualControl.cpp
+++ b/src/driveModi/Modi/ManualControl/manualControl.cpp
@@ -10,7 +10,10 @@
  */
 #include "manualControl.h"
 
-ManualControl::ManualControl() { }
+ManualControl::ManualControl(MoveControl *moveControl) { 
+    this->moveControl = moveControl;
+    this->moveControl->setDrivingStatus(DrivingStatus::drive);
+}
 
 ManualControl::~ManualControl() {
     this->moveControl->setSpeed(0);
@@ -18,11 +21,6 @@ ManualControl::~ManualControl() {
     this->moveControl->setDrivingStatus(DrivingStatus::stop);
 }
 
-void ManualControl::init(MoveControl *moveControl) {
-    this->moveControl = moveControl;
-    this->moveControl->setDrivingStatus(DrivingStatus::drive);
-}
-
 void ManualControl::loop() {
     static uint32_t last_millis = 0;
     if (millis() - last_millis < delay) {
diff --git a/src/driveModi/Modi/ManualControl/manualControl.h b/src/driveModi/Modi/ManualControl/manualControl.h
index e23df23..bb4e205 100644
--- a/src/driveModi/Modi/ManualControl/manualControl.h
+++ b/src/driveModi/Modi/ManualControl/manualControl.h
@@ -26,7 +26,7 @@
  */
 class ManualControl : public DriveModi{
     public:
-        ManualControl();
+        ManualControl(MoveControl *moveControl);
 
         /**
          * @brief Destroy the Manual Control object
@@ -34,15 +34,6 @@ class ManualControl : public DriveModi{
          */
         ~ManualControl();
 
-        /**
-         * @brief Initalize ManualControl
-         * 
-         * set DrivingStatus::drive
-         * 
-         * @param moveControl 
-         */
-        void init(MoveControl *moveControl);
-
         /**
          * @brief Calls runManualControl() to update all values.
          * 
diff --git a/src/driveModi/driveManager.cpp b/src/driveModi/driveManager.cpp
index e2ee626..21f9fa6 100644
--- a/src/driveModi/driveManager.cpp
+++ b/src/driveModi/driveManager.cpp
@@ -11,28 +11,30 @@
 
 #include "driveModi/driveManager.h"
 
+Modi& operator++(Modi& m, int) {
+    return m = (m == Modi::TestMode) ? Modi::Off : static_cast<Modi>(static_cast<int>(m)+1);
+}
+
 DriveManager::DriveManager(MoveControl *moveControl) {
     this->moveControl = moveControl;
 }
 
 void DriveManager::loop() {
     this->moveControl->loop();
-    if (currentModus)
-        this->currentModus->loop();
-}
-
-void DriveManager::runDriveManager() {
-    
+    this->navigation->loop();
+    if (currentModusPtr)
+        this->currentModusPtr->loop();
 }
 
 void DriveManager::nextModus() {
-    
+    changeModus(this->currentModus++);
 }
 
 void DriveManager::changeModus(Modi modus) {
+    this->currentModus = modus;
 
     //Set moveControl to a safe state
-    delete this->currentModus;
+    delete this->currentModusPtr;
 
     this->moveControl->setSpeed(0);
     this->moveControl->setRotationspeed(0);
@@ -40,34 +42,33 @@ void DriveManager::changeModus(Modi modus) {
     
     switch (modus) {
         case Modi::Off:
-            this->currentModus = nullptr;
+            this->currentModusPtr = nullptr;
             break;
 
         case Modi::ManualControl: {
-            ManualControl *ptr = new ManualControl;
-            ptr->init(this->moveControl);
-            this->currentModus = ptr;
+            this->currentModusPtr = new ManualControl(this->moveControl);
             }
             break;
 
-        case Modi::CaptureRoute:
-            this->currentModus = nullptr;
+        case Modi::CaptureRoute: {
+            this->currentModusPtr = new CaptureRoute(this->moveControl, this->navigation);
+            }
             break;
 
         case Modi::Autopilot:
-            this->currentModus = nullptr;
+            this->currentModusPtr = nullptr;
             break;
 
         case Modi::ConsolControl:
-            this->currentModus = nullptr;
+            this->currentModusPtr = nullptr;
             break;
 
         case Modi::TestMode:
-            this->currentModus = nullptr;
+            this->currentModusPtr = nullptr;
             break;
         
         default:
-            this->currentModus = nullptr;
+            this->currentModusPtr = nullptr;
             break;
     }
 }
diff --git a/src/driveModi/driveManager.h b/src/driveModi/driveManager.h
index a3c8386..1edcc5a 100644
--- a/src/driveModi/driveManager.h
+++ b/src/driveModi/driveManager.h
@@ -12,8 +12,16 @@
 #ifndef DRIVE_MANAGER_H
 #define DRIVE_MANAGER_H
 
+//GPS Serial Pins
+#define GPS_RX 17
+#define GPS_TX 16
+#define GPS_BAUD 9600
+
+#include <Arduino.h>
+
 #include "moveControl.h"
 #include "driveModi/driveModi.h"
+#include "navigation.h"
 
 // All Drive Modi
 #include "driveModi/Modi/ManualControl/manualControl.h"
@@ -23,28 +31,31 @@
 #include "driveModi/Modi/TestMode/testMode.h"
 
 enum class Modi {   
-                    Off,
-                    ManualControl,
-                    CaptureRoute,
-                    Autopilot,
-                    ConsolControl,
-                    TestMode
-                };
+    Off,
+    ManualControl,
+    CaptureRoute,
+    Autopilot,
+    ConsolControl,
+    TestMode
+};
 
 class DriveManager {
     public:
         DriveManager(MoveControl *moveControl);
-
         void loop();
-        void runDriveManager();
 
         void nextModus();
         void changeModus(Modi modus);
 
-    private:
-        MoveControl *moveControl;
-        DriveModi *currentModus;
+        Navigation* getNavigation() {return this->navigation;}
+        DriveModi* getDriveModiPtr() { return this->currentModusPtr; }
+        Modi getDriveModi() { return this->currentModus; }
 
+    private:
+        Modi currentModus = Modi::Off;
+        MoveControl *moveControl;
+        DriveModi *currentModusPtr;
+        Navigation* navigation;
 };
 
 
diff --git a/src/main.cpp b/src/main.cpp
index be602e6..442dd0b 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -3,7 +3,6 @@
 #include <iostream>
 #include <Wire.h>
 #include <LiquidCrystal_I2C.h>
-#include <TinyGPS++.h>
 
 #include "config.h"
 
@@ -17,13 +16,13 @@
 #include "menuAction.h"
 #include "SpecialMenus/menuPidSettings.h"
 #include "SpecialMenus/menuManualDrive.h"
+#include "SpecialMenus/menuCaptureRoute.h"
 #include "SpecialMenus/menuGPS.h"
 
 MoveControl moveController;
 DriveManager driveManager(&moveController);
 Menu* main_m;
 LiquidCrystal_I2C* lcd;
-TinyGPSPlus gps;
 
 // TODO: Platzhalter, muss irgendwann weg
 void dummy(){std::cout << "Dummy in Action" <<std::endl;}
@@ -33,12 +32,12 @@ void callbackControllerAction();
 void callbackControllerConnect();
 void controllerPrintBattery();
 void i2cScanner();
+void restart();
 
 void exitDriveMode() {driveManager.changeModus(Modi::Off);}
 
 void setup() {
     Serial.begin(115200);
-    Serial1.begin(GPS_BAUD, SERIAL_8N1, GPS_RX, GPS_TX);
     std::cout << "Welcome to Kleiax-Rover" << std::endl;
 
     Wire.begin(21, 19);
@@ -57,7 +56,7 @@ void setup() {
     Ps3.attach(callbackControllerAction);
     Ps3.attachOnConnect(callbackControllerConnect);
     std::cout << "\nReady to connect a PS3 Controller... \n";
-    Ps3.begin();
+    Ps3.begin(CONTROLLER_MAC);
 
     // Create Menu
     main_m = new Menu();
@@ -66,7 +65,8 @@ void setup() {
     MenuPidSettings* pidl_m = new MenuPidSettings(moveController.getPID(0));
     MenuPidSettings* pidr_m = new MenuPidSettings(moveController.getPID(1));
     MenuManualControl* man_m = new MenuManualControl(&driveManager);
-    MenuGPS* gps_m = new MenuGPS(&gps);
+    MenuCaptureRoute* cap_m = new MenuCaptureRoute(&driveManager);
+    MenuGPS* gps_m = new MenuGPS(driveManager.getNavigation()->getGPS());
 
     // Set Menus on LCD
     main_m->setLcd(lcd);
@@ -75,6 +75,7 @@ void setup() {
     pidl_m->setLcd(lcd);
     pidr_m->setLcd(lcd);
     man_m->setLcd(lcd);
+    cap_m->setLcd(lcd);
     gps_m->setLcd(lcd);
 
 
@@ -82,13 +83,15 @@ void setup() {
     MenuAction* mode_e = new MenuAction("Mode", mode_m);
     MenuAction* gps_e = new MenuAction("GPS", gps_m);
     MenuAction* pid_e = new MenuAction("PID", pid_m);
+    MenuAction* restart_e = new MenuAction("Restart", restart);
     main_m->addEntry(mode_e);
     main_m->addEntry(gps_e);
     main_m->addEntry(pid_e);
+    main_m->addEntry(restart_e);
 
     // Entry for the mode Menu
     MenuAction* autopilot_e = new MenuAction("Autopilot", dummy);
-    MenuAction* captureRoute_e = new MenuAction("Capture Route", dummy);
+    MenuAction* captureRoute_e = new MenuAction("Capture Route", cap_m);
     MenuAction* consolControl_e = new MenuAction("Consol Control", dummy);
     MenuAction* manualControl_e = new MenuAction("Manual Control", man_m);
     MenuAction* testMode_e = new MenuAction("Test Mode", dummy);
@@ -109,7 +112,6 @@ void setup() {
 void loop() {
     Network::checkMQTT();
     driveManager.loop();
-    gps.encode(Serial1.read());
 }
 
 bool isDelayReached(bool reset = false) {
@@ -200,3 +202,7 @@ void i2cScanner() {
     else
         Serial.println("done\n");
 }
+
+void restart() {
+    ESP.restart();
+}
\ No newline at end of file
diff --git a/src/route.cpp b/src/route.cpp
deleted file mode 100644
index 7516c42..0000000
--- a/src/route.cpp
+++ /dev/null
@@ -1,80 +0,0 @@
-// #include "route.h"
-// #include "config.h"
-
-// Route::Route() {
-//     Serial1.begin(GPS_BAUD, SERIAL_8N1, GPS_RX, GPS_TX);
-//     this->debug = new DebugMqtt("Route");
-// }
-
-// void Route::runRoute() {
-//     static uint64_t last_millis = 0;
-//     if (millis() - last_millis < 123456789) {
-//         return;
-//     }
-//     last_millis = millis();
-
-//     while (Serial1.available() > 0)
-//         gps.encode(Serial1.read());
-
-//     if (gps.location.isUpdated() && gps.location.isValid()) {
-//         this->currentLocation.lat = gps.location.lat();
-//         this->currentLocation.lon = gps.location.lng();
-//     }
-// }
-
-// void Route::addPointToRoute(Point point) {
-//     this->points.push_back(point);
-//     this->count_points++;
-// }
-
-// void Route::addCurrentLocationToRoute() {
-//     if (*this->points.end() == this->currentLocation  &&
-//         !this->nearlySameLocation(*this->points.end(), this->currentLocation)) {
-//         return;
-//     }
-//     this->addPointToRoute(currentLocation);
-// }
-
-// void Route::delRoute() {
-//     this->points.clear();
-//     this->count_points = 0;
-// }
-
-// Point Route::startRoute() {
-//     this->route_started = true;
-//     this->route_finished = false;
-
-//     this->it = this->points.begin();
-//     return *it;
-// }
-
-// Point Route::getNextPoint() {
-//     if (this->route_started && !route_finished) {
-//         it++;
-//         return *it;
-//     }
-
-//     if (it == this->points.end()) {
-//         this->route_finished = true;
-//     }
-
-//     Point fail;
-//     return fail;
-// }
-
-// uint16_t Route::getNumberOfPoints() {
-//     return this->count_points;
-// }
-
-// double Route::getDis(Point point_1, Point point_2) {
-//     double lat = (point_1.lat + point_2.lat) / 2 * 0.1745;
-//     double dx = 111.3 * cos(lat) * (point_1.lon - point_2.lon);
-//     double dy = 111.3 * (point_1.lat - point_2.lat);
-//     return sqrt(dx * dx + dy * dy);
-// }
-
-// bool Route::nearlySameLocation(Point p1, Point p2) {
-//     if (this->getDis(p1, p2) < 0.5)
-//         return true;
-//     return false;
-// }
\ No newline at end of file