diff --git a/include/moveControl.h b/include/moveControl.h
index fc20a79..da82fee 100644
--- a/include/moveControl.h
+++ b/include/moveControl.h
@@ -125,7 +125,7 @@ class MoveControl : public Component {
          * @param side 0 -> left, 1 -> right
          * @return PID* 
          */
-        PID* getPID(uint8_t side);
+        PID* getPID(uint8_t side) const;
 
         /**
          * @brief Get the Speedometer Left object
diff --git a/lib/Battery/battery.cpp b/lib/Battery/battery.cpp
index 84237fd..d13e496 100644
--- a/lib/Battery/battery.cpp
+++ b/lib/Battery/battery.cpp
@@ -39,12 +39,12 @@ void Battery::run() {
     return;
 }
 
-double Battery::getBatteryVoltage() {
+double Battery::getBatteryVoltage() const {
     double res = this->batteryVoltage;
     return (int)(res*100+0.5)/100.0;
 }
 
-bool Battery::isBatteryLow(double voltage) {
+bool Battery::isBatteryLow(double voltage) const {
     if (this->getBatteryVoltage() <= voltage && this->batteryVoltage > this->absurdLowVoltage)
         return true;
     return false;
@@ -136,7 +136,7 @@ void Battery::initBuffer() {
         this->adcBuffer[i] = 0;
 }
 
-uint16_t Battery::getBufAvg() {
+uint16_t Battery::getBufAvg() const {
     uint32_t res = 0;
     uint8_t emptyPos = 0;
     for (uint8_t i = 0; i < Battery::bufferSize; i++) {
diff --git a/lib/Battery/battery.h b/lib/Battery/battery.h
index 12841f5..f6837be 100644
--- a/lib/Battery/battery.h
+++ b/lib/Battery/battery.h
@@ -48,14 +48,14 @@ class Battery : public Component {
          * 
          * @return double in Volt
          */
-        double getBatteryVoltage();
+        double getBatteryVoltage() const;
 
         /**
          * @brief Get the charge level of the battery
          * 
          * @return uint8_t charge level in percent
          */
-        uint8_t getBatteryPercent() { return this->batteryPercent; }
+        uint8_t getBatteryPercent() const { return this->batteryPercent; }
 
         /**
          * @brief Checks if the battery is low.
@@ -68,7 +68,7 @@ class Battery : public Component {
          * @return true if the battery is low
          * @return false if the battery is high
          */
-        bool isBatteryLow(double voltage);
+        bool isBatteryLow(double voltage) const;
 
         bool isNewValue();
 
@@ -79,7 +79,7 @@ class Battery : public Component {
         void calculateBatteryPercent();
         void readAdcToBuf();
         void initBuffer();
-        uint16_t getBufAvg();
+        uint16_t getBufAvg() const;
 
         static const uint8_t bufferSize = 30;
 
diff --git a/lib/Counter/counter.cpp b/lib/Counter/counter.cpp
index ce18500..a4918e0 100644
--- a/lib/Counter/counter.cpp
+++ b/lib/Counter/counter.cpp
@@ -36,7 +36,7 @@ void Counter::clear() {
     pcnt_counter_clear(this->unit);
 }
 
-int16_t Counter::getValue() {
+int16_t Counter::getValue() const {
     int16_t res;
     pcnt_get_counter_value(this->unit, &res);
     return res;
diff --git a/lib/Counter/counter.h b/lib/Counter/counter.h
index d84f15a..240895a 100644
--- a/lib/Counter/counter.h
+++ b/lib/Counter/counter.h
@@ -14,7 +14,7 @@ class Counter {
         void resume();
         void clear();
 
-        int16_t getValue();
+        int16_t getValue() const;
 
         void setFilterValue(uint16_t value);
         void filterEnable();
diff --git a/lib/MotorControl/motorControl.cpp b/lib/MotorControl/motorControl.cpp
index 22b3ad5..ab815e7 100644
--- a/lib/MotorControl/motorControl.cpp
+++ b/lib/MotorControl/motorControl.cpp
@@ -114,27 +114,19 @@ void MotorControl::emergencyStop() {
     setRealPower(0);
 }
 
-int8_t MotorControl::getPower() {
-    return this->power;
-}
-
-int8_t MotorControl::getTargetPower() {
-    return this->targetPower;
-}
-
-bool MotorControl::isTargetPowerReached() {
+bool MotorControl::isTargetPowerReached() const {
     if (this->targetPower == this->power)
         return true;
     return false;
 }
 
-bool MotorControl::isAccelerationPositive() {
+bool MotorControl::isAccelerationPositive() const {
     if (power < targetPower)
         return true;
     return false;
 }
 
-bool MotorControl::isAccelerationNegative() {
+bool MotorControl::isAccelerationNegative() const {
     if (power > targetPower)
         return true;
     return false;
diff --git a/lib/MotorControl/motorControl.h b/lib/MotorControl/motorControl.h
index 9565f82..46ef407 100644
--- a/lib/MotorControl/motorControl.h
+++ b/lib/MotorControl/motorControl.h
@@ -103,19 +103,19 @@ class MotorControl : public Component {
          * 
          * @return int8_t percent of power (-100 to 100)
          */
-        int8_t getPower();
+        int8_t getPower() const { return this->power; };
 
         /**
          * @brief Get the target power
          * 
          * @return int8_t percent of power (-100 to 100)
          */
-        int8_t getTargetPower();
+        int8_t getTargetPower() const { return this->targetPower; };
 
         uint16_t getDutycycle() const { return this->dutycycle; }
-        bool isTargetPowerReached();
-        bool isAccelerationPositive();
-        bool isAccelerationNegative();
+        bool isTargetPowerReached() const;
+        bool isAccelerationPositive() const;
+        bool isAccelerationNegative() const;
 
     private:
         void run() override;
diff --git a/lib/Route/route.h b/lib/Route/route.h
index e358bfb..a2e2418 100644
--- a/lib/Route/route.h
+++ b/lib/Route/route.h
@@ -128,7 +128,7 @@ class Point{
          * 
          * @return Accuracy 
          */
-        Accuracy getAccuracy() { return this->accuracy; }
+        Accuracy getAccuracy() const { return this->accuracy; }
 
     private:
         void init(uint32_t horizontalAccuracy, uint32_t creationTime);
diff --git a/lib/Speedometer/speedometer.h b/lib/Speedometer/speedometer.h
index d5b8a33..1277f1a 100644
--- a/lib/Speedometer/speedometer.h
+++ b/lib/Speedometer/speedometer.h
@@ -95,7 +95,7 @@ class Speedometer : public Component {
          * 
          * @return Direction 
          */
-        Direction getDirection() { return this->currentDirection; }
+        Direction getDirection() const { return this->currentDirection; }
 
         /**
          * @brief Get the calculated speed of the Wheel
diff --git a/src/driveModi/Modi/Autopilot/autopilot.h b/src/driveModi/Modi/Autopilot/autopilot.h
index bc04a86..c3697f6 100644
--- a/src/driveModi/Modi/Autopilot/autopilot.h
+++ b/src/driveModi/Modi/Autopilot/autopilot.h
@@ -65,18 +65,6 @@ class Autopilot : public ManualControl {
          */
         ~Autopilot();
 
-        /**
-         * @brief Calls runAutopilot or ManualControl loop
-         * 
-         * This function should be called every main loop. If self
-         * driving is activated the function call run Autopilot. If
-         * the loopDelayMillis is not reached the function returns immediately.
-         * 
-         * If self driving is not activated this functions calls the 
-         * ManualControl loop additionally.
-         */
-        void loop();
-
         void restart();
 
         /**
diff --git a/src/driveModi/Modi/CaptureRoute/captureRoute.cpp b/src/driveModi/Modi/CaptureRoute/captureRoute.cpp
index c31da6e..b334e19 100644
--- a/src/driveModi/Modi/CaptureRoute/captureRoute.cpp
+++ b/src/driveModi/Modi/CaptureRoute/captureRoute.cpp
@@ -35,7 +35,7 @@ void CaptureRoute::run() {
     }
 }
 
-double CaptureRoute::getDistanceToLastPoint() {
+double CaptureRoute::getDistanceToLastPoint() const {
     if (!this->lastSavedPoint.isInit())
         return 0;
     return this->lastSavedPoint.distanceTo(this->navigation->getCurrentPosition());
diff --git a/src/driveModi/Modi/CaptureRoute/captureRoute.h b/src/driveModi/Modi/CaptureRoute/captureRoute.h
index 6679267..c262a98 100644
--- a/src/driveModi/Modi/CaptureRoute/captureRoute.h
+++ b/src/driveModi/Modi/CaptureRoute/captureRoute.h
@@ -62,7 +62,7 @@ class CaptureRoute : public ManualControl {
          * 
          * @return double in meters
          */
-        double getDistanceToLastPoint();
+        double getDistanceToLastPoint() const;
 
         /**
          * @brief Tells if there are new informations to display
diff --git a/src/driveModi/Modi/TestMode/testMode.cpp b/src/driveModi/Modi/TestMode/testMode.cpp
index e32a8ee..333bcf0 100644
--- a/src/driveModi/Modi/TestMode/testMode.cpp
+++ b/src/driveModi/Modi/TestMode/testMode.cpp
@@ -120,7 +120,7 @@ void TestMode::abortManeuver() {
     this->abort = true;
 }
 
-uint8_t TestMode::getRemainingManeuverTime() {
+uint8_t TestMode::getRemainingManeuverTime() const {
     if (busy)
         return (uint8_t) ((this->maneuverTime - (millis() - this->actionStart)) / 1000);
     return 0;
diff --git a/src/driveModi/Modi/TestMode/testMode.h b/src/driveModi/Modi/TestMode/testMode.h
index f0d2f96..43a7b10 100644
--- a/src/driveModi/Modi/TestMode/testMode.h
+++ b/src/driveModi/Modi/TestMode/testMode.h
@@ -89,7 +89,7 @@ class TestMode : public DriveModi {
          */
         void abortManeuver();
 
-        uint8_t getRemainingManeuverTime();
+        uint8_t getRemainingManeuverTime() const;
 
         /**
          * @brief Returns true if a maneuver is running
@@ -99,8 +99,8 @@ class TestMode : public DriveModi {
          */
         bool getBusy() const { return this->busy; }
         Maneuver getManeuver() const { return this->maneuver; }
-        Speedometer* getSpeedometerLeft() { return this->moveControl->getSpeedometerLeft(); }
-        Speedometer* getSpeedometerRight() { return this->moveControl->getSpeedometerRight(); }
+        Speedometer* getSpeedometerLeft() const { return this->moveControl->getSpeedometerLeft(); }
+        Speedometer* getSpeedometerRight() const { return this->moveControl->getSpeedometerRight(); }
 
     private:
         void run() override;
diff --git a/src/driveModi/driveManager.h b/src/driveModi/driveManager.h
index 07edfa4..3841c75 100644
--- a/src/driveModi/driveManager.h
+++ b/src/driveModi/driveManager.h
@@ -84,7 +84,7 @@ class DriveManager : public Component {
          * 
          * @return Navigation* 
          */
-        Navigation* getNavigation() {return this->navigation;}
+        Navigation* getNavigation() const {return this->navigation;}
 
         /**
          * @brief Get the DriveModi Ptr object
@@ -96,14 +96,14 @@ class DriveManager : public Component {
          * @see getDriveModi
          * @return DriveModi* 
          */
-        DriveModi* getDriveModiPtr() { return this->currentModusPtr; }
+        DriveModi* getDriveModiPtr() const { return this->currentModusPtr; }
 
         /**
          * @brief Get the DriveModi enum
          * 
          * @return Modi 
          */
-        Modi getDriveModi() { return this->currentModus; }
+        Modi getDriveModi() const { return this->currentModus; }
 
     private:
         void run() override;
diff --git a/src/driveModi/driveModi.h b/src/driveModi/driveModi.h
index 0cd7089..a78f0f4 100644
--- a/src/driveModi/driveModi.h
+++ b/src/driveModi/driveModi.h
@@ -34,7 +34,7 @@ class DriveModi : public Component {
         void setMaxSpeed(double maxForwardSpeed) { maxForwardSpeed = maxForwardSpeed; }
         void increaseMaxSpeed(double increase = 0.1) { maxForwardSpeed += increase; }
         void decreaseMaxSpeed(double increase = 0.1) { maxForwardSpeed -= increase; }
-        double getMaxSpeed() { return this->maxForwardSpeed; }
+        double getMaxSpeed() const { return this->maxForwardSpeed; }
 
         /**
          * @brief Set the max rotation
@@ -44,7 +44,7 @@ class DriveModi : public Component {
         void setMaxRotation(double maxRotation) { maxRotationSpeed = maxRotation; }
         void increaseMaxRotation(double increase = 0.1) { maxRotationSpeed += increase; }
         void decreaseMaxRotation(double increase = 0.1) { maxRotationSpeed -= increase; }
-        double getMaxRotation() { return this->maxRotationSpeed; }
+        double getMaxRotation() const { return this->maxRotationSpeed; }
 
     protected:
         MoveControl *moveControl;
diff --git a/src/main.cpp b/src/main.cpp
index 93083b3..54a05b7 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -228,6 +228,7 @@ void makeMenu() {
     man_m->setLcd(lcdWrapper);
     cap_m->setLcd(lcdWrapper);
     auto_m->setLcd(lcdWrapper);
+    auto_m->setUpdateDelay(1000);
     testM_m->setLcd(lcdWrapper);
     comp_m->setLcd(lcdWrapper);
     gps_m->setLcd(lcdWrapper);
diff --git a/src/moveControl.cpp b/src/moveControl.cpp
index e21fcfd..c4f21ec 100644
--- a/src/moveControl.cpp
+++ b/src/moveControl.cpp
@@ -137,7 +137,7 @@ void MoveControl::setPidTunings(uint8_t side, double p, double i, double d) {
     selectedPID->SetTunings(p, i, d);
 }
 
-PID* MoveControl::getPID(uint8_t side) {
+PID* MoveControl::getPID(uint8_t side) const {
     if (side == 0)
         return this->left_pid;
     else if (side == 1)