diff --git a/src/SpecialMenus/driveModi/TestMode/menuTestMode.cpp b/src/SpecialMenus/driveModi/TestMode/menuTestMode.cpp
index d2c9a07..c4d4ec8 100644
--- a/src/SpecialMenus/driveModi/TestMode/menuTestMode.cpp
+++ b/src/SpecialMenus/driveModi/TestMode/menuTestMode.cpp
@@ -78,6 +78,8 @@ void MenuTestMode::init() {
     this->driveManager->changeModus(Modi::TestMode);
     this->testMode = (TestMode*) this->driveManager->getDriveModiPtr();
 
+    testMode->setSpeed(15);
+
     auto dummy = []() {
         std::cout << "Dummy in Action" <<std::endl;
     };
@@ -183,15 +185,16 @@ bool MenuTestMode::checkInput() {
 }
 
 void MenuTestMode::printManeuverTime() {
-    std::cout << "MenuTestMode::printManeuverTime: 1" << std::endl;
-    String lineTwo = "left ";
-    std::cout << "MenuTestMode::printManeuverTime: 2" << std::endl;
-    lineTwo.concat(this->testMode->getRemainingManeuverTime());
-    std::cout << "MenuTestMode::printManeuverTime: 3" << std::endl;
-    lineTwo.concat("secs");
-    std::cout << "MenuTestMode::printManeuverTime: 4" << std::endl;
-    this->print("Maneuver time", lineTwo);
-    std::cout << "MenuTestMode::printManeuverTime: 5" << std::endl;
+    uint8_t maneuverTime = this->testMode->getRemainingManeuverTime();
+    if (maneuverTime) {
+        String lineTwo = "";
+        lineTwo.concat(maneuverTime);
+        lineTwo.concat(" secs");
+        this->print("Maneuver time:", lineTwo);
+    } else {
+        this->maneuverInAction = false;
+        this->printMenu();
+    }
 }
 
 MenuTestModeWrapper::MenuTestModeWrapper(MenuTestMode* menu, TestModeFunctionSingle testModeFunction) {
@@ -201,7 +204,8 @@ MenuTestModeWrapper::MenuTestModeWrapper(MenuTestMode* menu, TestModeFunctionSin
 }
 
 MenuTestModeWrapper::MenuTestModeWrapper(MenuTestMode* menu, TestModeFunctionDouble testModeFunction) {
-    this->testMode = testMode;
+    this->menu = menu;
+    this->testMode = this->menu->getTestMode();
     this->testModeFunctionDouble = testModeFunction;
 }
 
@@ -209,13 +213,16 @@ void MenuTestModeWrapper::action(int16_t* values, uint8_t length) {
     // This function calls a member function with a pointer
     // Very helpful site:
     // https://isocpp.org/wiki/faq/pointers-to-members#fnptr-vs-memfnptr-types
-    std::cout << "MenuTestModeWrapper::action: 0" << std::endl;
-    if (length == 2 && this->testModeFunctionDouble)
-        (this->testMode->*this->testModeFunctionDouble)(values[0], values[1]);
-    else if (length == 1 && this->testModeFunctionSingle)
-        (this->testMode->*this->testModeFunctionSingle)(values[0]);
 
-    std::cout << "MenuTestModeWrapper::action: 1" << std::endl;
-    this->menu->printManeuverTime();
-    std::cout << "MenuTestModeWrapper::action: 2" << std::endl;
+    bool res = false;
+
+    if (length == 2 && this->testModeFunctionDouble)
+        res = (this->testMode->*this->testModeFunctionDouble)(values[0], values[1]);
+    else if (length == 1 && this->testModeFunctionSingle)
+        res = (this->testMode->*this->testModeFunctionSingle)(values[0]);
+
+    if (res) {
+        this->menu->printManeuverTime();
+        this->menu->maneuverStarted();
+    }
 }   
diff --git a/src/SpecialMenus/driveModi/TestMode/menuTestMode.h b/src/SpecialMenus/driveModi/TestMode/menuTestMode.h
index fd9bc5c..89011fa 100644
--- a/src/SpecialMenus/driveModi/TestMode/menuTestMode.h
+++ b/src/SpecialMenus/driveModi/TestMode/menuTestMode.h
@@ -34,6 +34,7 @@ class MenuTestMode : public MenuDriveMode {
         void update() override;
 
         TestMode* getTestMode() const { return this->testMode; }
+        void maneuverStarted() { this->maneuverInAction = true; }
 
     private:
         void init();
diff --git a/src/driveModi/Modi/TestMode/testMode.cpp b/src/driveModi/Modi/TestMode/testMode.cpp
index c5ba056..d8b8161 100644
--- a/src/driveModi/Modi/TestMode/testMode.cpp
+++ b/src/driveModi/Modi/TestMode/testMode.cpp
@@ -10,8 +10,9 @@
  */
 #include "testMode.h"
 
-TestMode::TestMode(MoveControl *moveControl) {
+TestMode::TestMode(MoveControl *moveControl,  Navigation* navigation) {
     this->moveControl = moveControl;
+    this->navigation = navigation;
 }
 
 TestMode::~TestMode() {
@@ -22,11 +23,20 @@ void TestMode::loop() {
     if (millis() - this->lastMillis < this->delay)
         return;
 
+    if (this->maneuver == Maneuver::Turn) {
+        uint16_t delta = abs(this->azimuth - this->navigation->getAzimuth());
+        if (delta > this->degree)
+            this->abort = true;
+    }
+
     if (millis() - this->actionStart > this->maneuverTime || this->abort) {
         this->busy = false;
         this->abort = false;
         this->moveControl->setDrivingStatus(DrivingStatus::stop);
+        this->maneuver = Maneuver::None;
     }
+
+    this->lastMillis = millis();
 }
 
 void TestMode::setSpeed(int16_t speed) {
@@ -41,13 +51,9 @@ bool TestMode::drive(int16_t cm, int16_t degree) {
     if (this->busy)
         return false;
 
-    if (true) {
-        std::cout << "It works... cm: " << cm << " degree: " << degree << std::endl;
-        return false;
-    }
-
     this->actionStart = millis();
     this->moveControl->setDrivingStatus(DrivingStatus::drive);
+
     if (cm == 0) {
         //Only left or right
         this->moveControl->setSpeed(0);
@@ -57,8 +63,10 @@ bool TestMode::drive(int16_t cm, int16_t degree) {
         else if (degree > 0)
             this->moveControl->setRotationSpeed(this->rotationSpeed);
         
-        //TODO: calc this shit
-        this->maneuverTime = 10;
+        this->azimuth = this->navigation->getAzimuth();
+        this->degree = degree;
+
+        this->maneuverTime = 5 * 1000;
         this->busy = true;
         return true;
 
@@ -73,9 +81,20 @@ bool TestMode::drive(int16_t cm, int16_t degree) {
         
         this->maneuverTime = (uint32_t) (((double) cm / 100.0) / this->speed) * 1000;
         this->busy = true;
+        this->maneuver = Maneuver::Drive;
         return true;
     } else {
         //forward or backward and left or right
+
+        if (cm < 0)
+            this->moveControl->setSpeed(-this->speed);
+        else if (cm > 0)
+            this->moveControl->setSpeed(this->speed);
+        
+        this->maneuverTime = (uint32_t) (((double) cm / 100.0) / this->speed) * 1000;
+        this->busy = true;
+        this->maneuver = Maneuver::Drive;
+        return true;
     }
 
     this->moveControl->setDrivingStatus(DrivingStatus::stop);    
@@ -83,9 +102,9 @@ bool TestMode::drive(int16_t cm, int16_t degree) {
 }
 
 bool TestMode::leftEngine(int16_t powerPercentage, int16_t seconds) {
-    std::cout << "TestMode::leftEngine: 1" << std::endl;
     if (this->engineInit(powerPercentage, seconds)) {
         this->moveControl->setRawPowerLeft(powerPercentage);
+        this->maneuver = Maneuver::LeftEngine;
         return true;
     }
     return false;
@@ -94,6 +113,7 @@ bool TestMode::leftEngine(int16_t powerPercentage, int16_t seconds) {
 bool TestMode::rightEngine(int16_t powerPercentage, int16_t seconds) {
     if (this->engineInit(powerPercentage, seconds)) {
         this->moveControl->setRawPowerRight(powerPercentage);
+        this->maneuver = Maneuver::RightEngine;
         return true;
     }
     return false;
@@ -102,7 +122,8 @@ bool TestMode::rightEngine(int16_t powerPercentage, int16_t seconds) {
 bool TestMode::bothEngine(int16_t powerPercentage, int16_t seconds) {
     if (this->engineInit(powerPercentage, seconds)) {
         this->moveControl->setRawPowerLeft(powerPercentage);
-        this->moveControl->setRawPowerRight(powerPercentage);;
+        this->moveControl->setRawPowerRight(powerPercentage);
+        this->maneuver = Maneuver::BothEngine;
         return true;
     }
     return false;
@@ -121,17 +142,11 @@ uint8_t TestMode::getRemainingManeuverTime() {
 bool TestMode::engineInit(int16_t powerPercentage, int16_t seconds) {
     if (this->busy)
         return false;
-
     if (powerPercentage >= 100 || powerPercentage <= -100)
         return false;
-
     if (seconds < 0)
         return false;
 
-    if (true) {
-        std::cout << "It works... %: " << powerPercentage << " s: " << seconds << std::endl;
-    }
-
     this->actionStart = millis();
     this->moveControl->setDrivingStatus(DrivingStatus::raw);
     this->maneuverTime = seconds * 1000;
diff --git a/src/driveModi/Modi/TestMode/testMode.h b/src/driveModi/Modi/TestMode/testMode.h
index 736e60f..262c938 100644
--- a/src/driveModi/Modi/TestMode/testMode.h
+++ b/src/driveModi/Modi/TestMode/testMode.h
@@ -11,12 +11,24 @@
 #ifndef TEST_MODE_H 
 #define TEST_MODE_H
 
+#include <iostream>
+
 #include "moveControl.h"
+#include "navigation.h"
 #include "driveModi/driveModi.h"
 
 class TestMode : public DriveModi {
     public:
-        TestMode(MoveControl *MoveControl);
+        enum Maneuver {
+            None,
+            LeftEngine,
+            RightEngine,
+            BothEngine,
+            Turn,
+            Drive
+        };
+
+        TestMode(MoveControl *moveControl, Navigation* navigation);
         ~TestMode();
 
         void loop() override;
@@ -34,11 +46,16 @@ class TestMode : public DriveModi {
 
         uint8_t getRemainingManeuverTime();
         bool getBusy() const { return this->busy; }
+        Maneuver getManeuver() const { return this->maneuver; }
 
     private:
         bool engineInit(int16_t powerPercentage, int16_t seconds);
 
         MoveControl *moveControl;
+        Navigation* navigation;
+        Maneuver maneuver = Maneuver::None;
+        int16_t maneuverValueOne = 0;
+        int16_t maneuverValueTwo = 0;
 
         double speed = 0;
         double rotationSpeed = 0;
@@ -47,11 +64,13 @@ class TestMode : public DriveModi {
         bool abort = false;
         
         uint8_t delay = 10;
-
+        uint16_t azimuth;
+        int16_t degree;
         uint32_t maneuverTime = 0;
         uint32_t lastMillis = 0;
         uint32_t actionStart = 0;
 
+
 };
 
 #endif // TEST_MODE_H
diff --git a/src/driveModi/driveManager.cpp b/src/driveModi/driveManager.cpp
index 8214043..653e33b 100644
--- a/src/driveModi/driveManager.cpp
+++ b/src/driveModi/driveManager.cpp
@@ -85,7 +85,7 @@ void DriveManager::changeModus(Modi modus) {
             break;
 
         case Modi::TestMode: {
-                this->currentModusPtr = new TestMode(this->moveControl);
+                this->currentModusPtr = new TestMode(this->moveControl,  this->navigation);
             }
             break;
         
diff --git a/src/main.cpp b/src/main.cpp
index 8550836..4d455a5 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -66,6 +66,8 @@ void callbackControllerConnect(void);
 void callbackControllerDisconnect(void);
 void i2cScanner(void);
 void makeMenu(void);
+void restart(void);
+void disconnectController(void);
 
 
 void setup() {
@@ -139,9 +141,19 @@ void loop() {
     driveManager->loop();
     lcdWrapper->loop();
 
-    DebugTimes mainBatteryTime;
     mainBattery->loop();
-    mainBatteryTime.stopConsol("MainBatteryTime", 5);
+    if (mainBattery->getBatteryPercent() <= 10) {
+        moveController.setDrivingStatus(DrivingStatus::stop);
+        moveController.setSpeed(0);
+        moveController.setRotationSpeed(0);
+
+        lcd->setCursor(0, 0);
+        lcd->printf("Low Battery...");
+        lcd->setCursor(0, 1);
+        lcd->print("Please turn off.");
+        
+        while (1);        
+    }
 
     if (disconnectPs3) {
         DebugTimes disconnectPS3Time;
@@ -249,22 +261,6 @@ void makeMenu() {
         std::cout << "Dummy in Action" <<std::endl;
     };
 
-    auto restart = [&]() {
-        lcdWrapper->clear();
-        lcdWrapper->setCursor(0, 0);
-        lcdWrapper->print("Rebooting ...");
-        ESP.restart();
-    };
-
-    auto disconnectController = [&]() {
-        disconnectPs3 = true;
-        lcdWrapper->clear();
-        lcdWrapper->setCursor(0, 0);
-        lcdWrapper->print("Controller is");
-        lcdWrapper->setCursor(0, 1);
-        lcdWrapper->print("disconnected.");
-    };
-
     // Create Menu
     main_m = new Menu();
     Menu* mode_m = new Menu();
@@ -339,3 +335,19 @@ void makeMenu() {
     pidr_m->setEntry(1, "I-Part", (uint8_t) moveController.getPID(1)->GetKi());
     pidr_m->setEntry(2, "D-Part", (uint8_t) moveController.getPID(1)->GetKd());
 }
+
+void restart(void) {
+    lcdWrapper->clear();
+    lcdWrapper->setCursor(0, 0);
+    lcdWrapper->print("Rebooting ...");
+    ESP.restart();
+}
+
+void disconnectController(void) {
+    disconnectPs3 = true;
+    lcdWrapper->clear();
+    lcdWrapper->setCursor(0, 0);
+    lcdWrapper->print("Controller is");
+    lcdWrapper->setCursor(0, 1);
+    lcdWrapper->print("disconnected.");
+}