diff --git a/lib/Navigation/navigation.cpp b/lib/Navigation/navigation.cpp
index ad12832..e5a5c00 100644
--- a/lib/Navigation/navigation.cpp
+++ b/lib/Navigation/navigation.cpp
@@ -12,18 +12,30 @@
 #include "navigation.h"
 
 Navigation::Navigation(uint8_t rx, uint8_t tx) {
-    this->gps = new TinyGPSPlus();
+    this->gps = new TinyGPSPlus;
     Serial1.begin(9600, SERIAL_8N1, rx, tx);
 }
 
-void Navigation::loop() {
-    gps->encode(Serial1.read());
+Navigation::~Navigation() {
+    delete this->gps;
 }
 
-void Navigation::addCurrentPosToRoute() {
+void Navigation::loop() {
+    while (Serial1.available())
+        gps->encode(Serial1.read());
+}
+
+Point 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();
+        if (MIN_DISTANCE_BETWEEN_POINTS <= 
+            TinyGPSPlus::distanceBetween(p.lat, p.lon, this->lastPoint.lat, this->lastPoint.lon))
+            this->route->addPointToRoute(p);
     }
+    return p;
 }
diff --git a/lib/Navigation/navigation.h b/lib/Navigation/navigation.h
index b859434..9c59047 100644
--- a/lib/Navigation/navigation.h
+++ b/lib/Navigation/navigation.h
@@ -14,20 +14,26 @@
 
 #include <TinyGPS++.h>
 #include <Arduino.h>
+#include <iostream>
 
 #include "route.h"
+
+#define MIN_DISTANCE_BETWEEN_POINTS 1
 class Navigation {
     public:
         Navigation(uint8_t rx, uint8_t tx);
+        ~Navigation();
         void loop();
 
-        void addCurrentPosToRoute();
+        Point addCurrentPosToRoute();
 
         TinyGPSPlus* getGPS() { return this->gps; }
 
     private:
         TinyGPSPlus* gps;
         Route* route;
+
+        Point lastPoint;
 };
 
 #endif // NAVIGATION_H
\ No newline at end of file
diff --git a/lib/Navigation/route.h b/lib/Navigation/route.h
index 485029b..4442f09 100644
--- a/lib/Navigation/route.h
+++ b/lib/Navigation/route.h
@@ -15,17 +15,20 @@
 #include <cstdint>
 #include <list>    
 
-        
-    
 
 class Point{
     public:
-        Point(double lat, double lon) {this->lat = lat; this->lon = lon;}
+        Point(double lat, double lon) { this->lat = lat; this->lon = lon; }
+        Point(){ this->lat = 0; this->lon = 0; }
+
         double lat = 0;
         double lon = 0;
+
         bool operator==(const Point& rhs) const {
             return this->lat == rhs.lat && this->lon == rhs.lon;
         }
+
+        bool isValid() const { return this->lat + this->lon; }
 };
 
 class Route {
diff --git a/src/SpecialMenus/menuDriveMode.h b/src/SpecialMenus/menuDriveMode.h
index 6fd3b99..1f27151 100644
--- a/src/SpecialMenus/menuDriveMode.h
+++ b/src/SpecialMenus/menuDriveMode.h
@@ -26,6 +26,7 @@ class MenuDriveMode : public MenuControl {
         void yes(){}
 
         virtual void printMenu() = 0;
+        void update(){}
 
     protected:
         DriveManager* driveManager;
diff --git a/src/driveModi/Modi/CaptureRoute/captureRoute.cpp b/src/driveModi/Modi/CaptureRoute/captureRoute.cpp
index a8e4d47..67a0f3b 100644
--- a/src/driveModi/Modi/CaptureRoute/captureRoute.cpp
+++ b/src/driveModi/Modi/CaptureRoute/captureRoute.cpp
@@ -7,8 +7,16 @@ CaptureRoute::CaptureRoute(MoveControl* moveControl, Navigation* navigation)
 
 void CaptureRoute::loop() {
     ManualControl::loop();
+
+    if (millis() - this->last_millis < delay) {
+        return;
+    }
+    this->runCaptureRoute();
+    this->last_millis = millis();
 }
 
 void CaptureRoute::runCaptureRoute() {
-
+    if (Ps3.data.button.triangle) {
+        this->navigation->addCurrentPosToRoute();
+    }
 }
\ No newline at end of file
diff --git a/src/driveModi/Modi/CaptureRoute/captureRoute.h b/src/driveModi/Modi/CaptureRoute/captureRoute.h
index 10374fd..0086e18 100644
--- a/src/driveModi/Modi/CaptureRoute/captureRoute.h
+++ b/src/driveModi/Modi/CaptureRoute/captureRoute.h
@@ -6,6 +6,7 @@
 #include "driveModi/Modi/ManualControl/manualControl.h"
 #include "navigation.h"
 
+
 class CaptureRoute : public ManualControl {
     public:
         CaptureRoute(MoveControl* moveControl, Navigation* navigation);
@@ -15,6 +16,9 @@ class CaptureRoute : public ManualControl {
 
     private:
         Navigation* navigation;
+
+        uint32_t last_millis = 0;
+        uint16_t delay = 200;
 };
 
 #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 f9740af..767d927 100644
--- a/src/driveModi/Modi/ManualControl/manualControl.cpp
+++ b/src/driveModi/Modi/ManualControl/manualControl.cpp
@@ -22,12 +22,11 @@ ManualControl::~ManualControl() {
 }
 
 void ManualControl::loop() {
-    static uint32_t last_millis = 0;
-    if (millis() - last_millis < delay) {
+    if (millis() - this->last_millis < delay) {
         return;
     }
     this->runManualControl();
-    last_millis = millis();
+    this->last_millis = millis();
 }
 
 void ManualControl::runManualControl() {
diff --git a/src/driveModi/Modi/ManualControl/manualControl.h b/src/driveModi/Modi/ManualControl/manualControl.h
index bb4e205..9c52b69 100644
--- a/src/driveModi/Modi/ManualControl/manualControl.h
+++ b/src/driveModi/Modi/ManualControl/manualControl.h
@@ -11,8 +11,6 @@
 #ifndef MANUAL_CONTROL_H
 #define MANUAL_CONTROL_H
 
-#include <Ps3Controller.h>
-
 #include "moveControl.h"
 #include "driveModi/driveModi.h"
 
@@ -74,6 +72,7 @@ class ManualControl : public DriveModi{
     private:
         MoveControl *moveControl;
 
+        uint32_t last_millis = 0;
         uint8_t delay = 10;
         double max_speed = 1;
         double max_rotation = 7;
diff --git a/src/driveModi/driveManager.cpp b/src/driveModi/driveManager.cpp
index 21f9fa6..ee5da36 100644
--- a/src/driveModi/driveManager.cpp
+++ b/src/driveModi/driveManager.cpp
@@ -17,6 +17,11 @@ Modi& operator++(Modi& m, int) {
 
 DriveManager::DriveManager(MoveControl *moveControl) {
     this->moveControl = moveControl;
+    this->navigation = new Navigation(17, 16);
+}
+
+DriveManager::~DriveManager() {
+    delete this->navigation;
 }
 
 void DriveManager::loop() {
diff --git a/src/driveModi/driveManager.h b/src/driveModi/driveManager.h
index 1edcc5a..bd87af7 100644
--- a/src/driveModi/driveManager.h
+++ b/src/driveModi/driveManager.h
@@ -42,6 +42,7 @@ enum class Modi {
 class DriveManager {
     public:
         DriveManager(MoveControl *moveControl);
+        ~DriveManager();
         void loop();
 
         void nextModus();
@@ -54,7 +55,7 @@ class DriveManager {
     private:
         Modi currentModus = Modi::Off;
         MoveControl *moveControl;
-        DriveModi *currentModusPtr;
+        DriveModi *currentModusPtr = nullptr;
         Navigation* navigation;
 };
 
diff --git a/src/driveModi/driveModi.h b/src/driveModi/driveModi.h
index 74ac982..221bd5e 100644
--- a/src/driveModi/driveModi.h
+++ b/src/driveModi/driveModi.h
@@ -12,6 +12,8 @@
 #ifndef DRIVEMODI_H
 #define DRIVEMODI_H
 
+#include <Ps3Controller.h>
+
 class DriveModi {
     public:
         DriveModi(){}
diff --git a/src/main.cpp b/src/main.cpp
index 442dd0b..87095bc 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -39,6 +39,7 @@ void exitDriveMode() {driveManager.changeModus(Modi::Off);}
 void setup() {
     Serial.begin(115200);
     std::cout << "Welcome to Kleiax-Rover" << std::endl;
+    std::cout << "All actions from the main programm run on" << xPortGetCoreID() << std::endl;
 
     Wire.begin(21, 19);
     // i2cScanner();
@@ -146,10 +147,13 @@ void callbackControllerAction() {
 
         isDelayReached(true);        
     }
+    // This have to be stand here because it have to be run on the other Core
+    main_m->update();
 }
 
 void callbackControllerConnect() {
     std::cout << "Controller connected to ESP32" << std::endl;
+    std::cout << "All actions from the Controller run on" << xPortGetCoreID() << std::endl;
 
     // TODO: Display is not functional without this seconde init, first init is in setup()
     lcd->init();