diff --git a/lib/Navigation/navigation.cpp b/lib/Navigation/navigation.cpp
index d119589..3a8d6e3 100644
--- a/lib/Navigation/navigation.cpp
+++ b/lib/Navigation/navigation.cpp
@@ -141,7 +141,9 @@ CourseCorrection Navigation::getCourseCorrection() {
 }
 
 bool Navigation::addCurrentPosToRoute() {
+    std::cout << "Navigation::addCurrentPosToRoute: 1" << std::endl;
     if (!this->currentPosition.isValid()) { return false; }
+    std::cout << "Navigation::addCurrentPosToRoute: 2" << std::endl;
 
     // First Point
     if (this->route->getRouteInfo().totalPoints == 0) {
@@ -149,6 +151,7 @@ bool Navigation::addCurrentPosToRoute() {
         this->lastPoint = this->currentPosition;
         return true;
     }
+    std::cout << "Navigation::addCurrentPosToRoute: 3" << std::endl;
 
     // Every Point after the first
     if (MIN_DISTANCE_BETWEEN_POINTS <= this->currentPosition.distanceTo(this->lastPoint)) {
@@ -156,6 +159,7 @@ bool Navigation::addCurrentPosToRoute() {
         this->lastPoint = this->currentPosition;
         return true;
     }
+    std::cout << "Navigation::addCurrentPosToRoute: 4" << std::endl;
     return false;
 }
 
diff --git a/src/driveModi/Modi/Autopilot/autopilot.cpp b/src/driveModi/Modi/Autopilot/autopilot.cpp
index 2bbc421..9b5ebd1 100644
--- a/src/driveModi/Modi/Autopilot/autopilot.cpp
+++ b/src/driveModi/Modi/Autopilot/autopilot.cpp
@@ -11,8 +11,8 @@
 
 #include "driveModi/Modi/Autopilot/autopilot.h"
 
-Autopilot::Autopilot(MoveControl* moveControl, Navigation* navigation)
-    : ManualControl(moveControl) {
+Autopilot::Autopilot(MoveControl* moveControl, Ps3Controller *gamepad, Navigation* navigation)
+    : ManualControl(moveControl, gamepad) {
         this->navigation = navigation;
         this->navigationStarted = this->navigation->startNavigation();
         this->routeInfo = this->navigation->getRouteInfo();
diff --git a/src/driveModi/Modi/Autopilot/autopilot.h b/src/driveModi/Modi/Autopilot/autopilot.h
index 1593619..2d63802 100644
--- a/src/driveModi/Modi/Autopilot/autopilot.h
+++ b/src/driveModi/Modi/Autopilot/autopilot.h
@@ -35,7 +35,7 @@ class Autopilot : public ManualControl {
          * @param moveControl for ManualControl
          * @param navigation for route instructions
          */
-        Autopilot(MoveControl* moveControl, Navigation* navigation);
+        Autopilot(MoveControl* moveControl, Ps3Controller *gamepad, Navigation* navigation);
 
         /**
          * @brief Calls runAutopilot or ManualControl loop
diff --git a/src/driveModi/Modi/CaptureRoute/captureRoute.cpp b/src/driveModi/Modi/CaptureRoute/captureRoute.cpp
index d3361b9..746376c 100644
--- a/src/driveModi/Modi/CaptureRoute/captureRoute.cpp
+++ b/src/driveModi/Modi/CaptureRoute/captureRoute.cpp
@@ -11,8 +11,8 @@
 
 #include "driveModi/Modi/CaptureRoute/captureRoute.h"
 
-CaptureRoute::CaptureRoute(MoveControl* moveControl, Navigation* navigation) 
-    : ManualControl(moveControl) {
+CaptureRoute::CaptureRoute(MoveControl* moveControl, Ps3Controller *gamepad, Navigation* navigation) 
+    : ManualControl(moveControl, gamepad) {
         this->navigation = navigation;
         this->routeInfo = RouteInfo{0, 0};
 }
@@ -28,7 +28,7 @@ void CaptureRoute::loop() {
 }
 
 void CaptureRoute::runCaptureRoute() {
-    if (Ps3.data.button.triangle) {
+    if (this->gamepad->data.button.triangle) {
         if (this->navigation->addCurrentPosToRoute()) {
             this->routeInfo = navigation->getRouteInfo();
             this->updateDisplay = true;
diff --git a/src/driveModi/Modi/CaptureRoute/captureRoute.h b/src/driveModi/Modi/CaptureRoute/captureRoute.h
index 344b962..efb0f4e 100644
--- a/src/driveModi/Modi/CaptureRoute/captureRoute.h
+++ b/src/driveModi/Modi/CaptureRoute/captureRoute.h
@@ -33,7 +33,7 @@ class CaptureRoute : public ManualControl {
          * @param moveControl for ManualControl
          * @param navigation to add Points
          */
-        CaptureRoute(MoveControl* moveControl, Navigation* navigation);
+        CaptureRoute(MoveControl* moveControl, Ps3Controller *gamepad, Navigation* navigation);
 
         /**
          * @brief Calls runCaptureRoute and ManualControl::loop
@@ -50,12 +50,12 @@ class CaptureRoute : public ManualControl {
          */
         void runCaptureRoute();
 
-        /**
-         * @brief Get the Navigation object
-         * 
-         * @return Navigation* 
-         */
-        Navigation* getNavigation() const { return this->navigation; }
+        // /**
+        //  * @brief Get the Navigation object
+        //  * 
+        //  * @return Navigation* 
+        //  */
+        // Navigation* getNavigation() const { return this->navigation; }
 
         /**
          * @brief Get the Route Info object
diff --git a/src/driveModi/Modi/ManualControl/manualControl.cpp b/src/driveModi/Modi/ManualControl/manualControl.cpp
index 9ed622d..5afc56a 100644
--- a/src/driveModi/Modi/ManualControl/manualControl.cpp
+++ b/src/driveModi/Modi/ManualControl/manualControl.cpp
@@ -10,8 +10,9 @@
  */
 #include "manualControl.h"
 
-ManualControl::ManualControl(MoveControl *moveControl) { 
+ManualControl::ManualControl(MoveControl *moveControl, Ps3Controller *gamepad) { 
     this->moveControl = moveControl;
+    this->gamepad = gamepad;
     this->moveControl->setDrivingStatus(DrivingStatus::drive);
 }
 
diff --git a/src/driveModi/Modi/ManualControl/manualControl.h b/src/driveModi/Modi/ManualControl/manualControl.h
index 8448eb7..60bf312 100644
--- a/src/driveModi/Modi/ManualControl/manualControl.h
+++ b/src/driveModi/Modi/ManualControl/manualControl.h
@@ -24,7 +24,7 @@
  */
 class ManualControl : public DriveModi {
     public:
-        ManualControl(MoveControl *moveControl);
+        ManualControl(MoveControl *moveControl, Ps3Controller *gamepad);
 
         /**
          * @brief Destroy the Manual Control object
@@ -71,6 +71,7 @@ class ManualControl : public DriveModi {
 
     protected:
         MoveControl *moveControl;
+        Ps3Controller *gamepad;
 
     private:
         uint32_t lastMillis = 0;
diff --git a/src/driveModi/driveManager.cpp b/src/driveModi/driveManager.cpp
index d0ee340..8214043 100644
--- a/src/driveModi/driveManager.cpp
+++ b/src/driveModi/driveManager.cpp
@@ -15,8 +15,9 @@ Modi& operator++(Modi& m, int) {
     return m = (m == Modi::TestMode) ? Modi::Off : static_cast<Modi>(static_cast<int>(m)+1);
 }
 
-DriveManager::DriveManager(MoveControl *moveControl, bool wifi) {
+DriveManager::DriveManager(MoveControl *moveControl, Ps3Controller *gamepad, bool wifi) {
     this->moveControl = moveControl;
+    this->gamepad = gamepad;
 
     this->spiPort = new SPIClass(HSPI);
     spiPort->begin(UBLOX_GNSS_SPI_SCK, UBLOX_GNSS_SPI_CIPO, UBLOX_GNSS_SPI_COPI, UBLOX_GNSS_SPI_CS);
@@ -65,17 +66,17 @@ void DriveManager::changeModus(Modi modus) {
             break;
 
         case Modi::ManualControl: {
-            this->currentModusPtr = new ManualControl(this->moveControl);
+            this->currentModusPtr = new ManualControl(this->moveControl, this->gamepad);
             }
             break;
 
         case Modi::CaptureRoute: {
-            this->currentModusPtr = new CaptureRoute(this->moveControl, this->navigation);
+            this->currentModusPtr = new CaptureRoute(this->moveControl, this->gamepad,  this->navigation);
             }
             break;
 
         case Modi::Autopilot: {
-            this->currentModusPtr = new Autopilot(this->moveControl, this->navigation);
+            this->currentModusPtr = new Autopilot(this->moveControl, this->gamepad, this->navigation);
             }
             break;
 
diff --git a/src/driveModi/driveManager.h b/src/driveModi/driveManager.h
index 59154fb..96219c9 100644
--- a/src/driveModi/driveManager.h
+++ b/src/driveModi/driveManager.h
@@ -51,7 +51,7 @@ class DriveManager {
          * 
          * @param moveControl 
          */
-        DriveManager(MoveControl *moveControl, bool wifi = false);
+        DriveManager(MoveControl *moveControl, Ps3Controller *gamepad, bool wifi = false);
 
         /**
          * @brief Destroy the Drive Manager object
@@ -117,6 +117,7 @@ class DriveManager {
     private:
         Modi currentModus = Modi::Off;
         MoveControl *moveControl;
+        Ps3Controller *gamepad;
         DriveModi *currentModusPtr = nullptr;
         Navigation* navigation;
         SPIClass* spiPort;
diff --git a/src/main.cpp b/src/main.cpp
index 9208009..03d1697 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -84,7 +84,7 @@ void setup() {
     Network::setIps();
     Network::setupMQTT();
     wifiIsActive = Network::connectWifi();
-    driveManager = new DriveManager(&moveController, wifiIsActive);
+    driveManager = new DriveManager(&moveController, gamepad, wifiIsActive);
     if (wifiIsActive) {
         
         // const char* remoteHost = "www.google.com";