diff --git a/debugMqtt/hw1/main.py b/debugMqtt/hw1/main.py
index d83007f..a11416c 100644
--- a/debugMqtt/hw1/main.py
+++ b/debugMqtt/hw1/main.py
@@ -9,7 +9,6 @@ def on_connect(client, userdata, flags, rc):
     client.subscribe("Rover/Info")
     client.subscribe("Rover/Warn")
     client.subscribe("Rover/Error")
-    client.subscribe("Rover/Info")
 
 
 def on_message(client, userdata, msg):
@@ -21,6 +20,6 @@ if __name__ == '__main__':
     client.on_connect = on_connect
     client.on_message = on_message
 
-    client.connect("dz-pi.hw1.fb4.fh", 1883, 60)
+    client.connect("172.22.64.216", 1883, 60)
 
     client.loop_forever()
diff --git a/debugMqtt/hw1/main_debug.py b/debugMqtt/hw1/main_debug.py
index b8e61f7..70957cd 100644
--- a/debugMqtt/hw1/main_debug.py
+++ b/debugMqtt/hw1/main_debug.py
@@ -18,6 +18,6 @@ if __name__ == '__main__':
     client.on_connect = on_connect
     client.on_message = on_message
 
-    client.connect("dz-pi.hw1.fb4.fh", 1883, 60)
+    client.connect("172.22.64.216", 1883, 60)
 
     client.loop_forever()
diff --git a/include/moveControl.h b/include/moveControl.h
index 8ed24cb..b726103 100644
--- a/include/moveControl.h
+++ b/include/moveControl.h
@@ -12,6 +12,7 @@
 #define MOVE_CONTROL_H
 
 #include <cstdint>
+#include <iostream>
 #include <PID_v1.h>
 #include <Arduino.h>
 
@@ -166,4 +167,4 @@ class MoveControl {
 
         uint8_t delay = 30;
 };
-#endif // MOVE_CONTROL_H
\ No newline at end of file
+#endif // MOVE_CONTROL_H
diff --git a/include/network.h b/include/network.h
index f2a64c0..141893f 100644
--- a/include/network.h
+++ b/include/network.h
@@ -12,6 +12,7 @@
 #ifndef NETWORK_H
 #define NETWORK_H
 
+#include <iostream>
 #include <WiFi.h>
 #include <PubSubClient.h>
 
@@ -39,4 +40,4 @@ class Network {
         static PubSubClient* mqtt_client;
 };
 
-#endif // NETWORK_H
\ No newline at end of file
+#endif // NETWORK_H
diff --git a/include/networkConfig.h b/include/networkConfig.h
index a36accf..403018b 100644
--- a/include/networkConfig.h
+++ b/include/networkConfig.h
@@ -1,4 +1,4 @@
-#define RHEDE
+#define HW1
 
 //Network config RHEDE
 #ifdef RHEDE
@@ -15,8 +15,6 @@
 #define NTP_SERVER "2.de.pool.ntp.org"
 #endif //RHEDE
 
-
-
 //Network config HW1
 #ifdef HW1
 #define WLAN_SSID "hw1_gast"
@@ -51,4 +49,4 @@
 #define MQTT_SERVER 0x0201a8c0      //192.168.1.2
 #define MQTT_PORT 1883
 #define NTP_SERVER 0x0201a8c0       //192.168.1.2
-#endif //TPMOBIL 
\ No newline at end of file
+#endif //TPMOBIL 
diff --git a/lib/MQTT/debugMqtt.cpp b/lib/MQTT/debugMqtt.cpp
index e198c14..a58d001 100644
--- a/lib/MQTT/debugMqtt.cpp
+++ b/lib/MQTT/debugMqtt.cpp
@@ -20,6 +20,11 @@ char DebugMqtt::topic[MQTT_BUFFER_SIZE];
 
 DebugMqtt::DebugMqtt(const char* name) {
     this->name = name;
+    this->buf = new char[this->bufSitze];
+}
+
+DebugMqtt::~DebugMqtt() {
+    delete this->buf;
 }
 
 void DebugMqtt::sendMsg(Loglevel loglevel, String topic, String msg) {
@@ -32,7 +37,9 @@ void DebugMqtt::sendMsg(Loglevel loglevel, String msg) {
 }
 
 void DebugMqtt::sendData(Loglevel loglevel, String topic, String data) {
-    if (!DebugMqtt::isInit) {return;}
+    if (!DebugMqtt::isInit) {
+        return;
+    }
 
     if (loglevel <= DebugMqtt::loglevel && loglevel > Loglevel::none) {
         snprintf (DebugMqtt::topic, MQTT_BUFFER_SIZE, "%s%s", DebugMqtt::enum_to_string(loglevel).c_str(), topic.c_str());
@@ -52,9 +59,19 @@ void DebugMqtt::writeToInflux(String measurement_name, String field_set, float m
     this->sendData(Loglevel::influx, DebugMqtt::msg);
 }
 
+void DebugMqtt::addCharacter(char c) {
+    this->buf[this->bufPos] = c;
+    this->bufPos++;
+    if (c == '\n' || this->bufPos >= this->bufSitze - 1) {
+        this->buf[this->bufPos - 1] = '\0';
+        this->sendMsg(Loglevel::info, buf);
+        this->bufPos = 0;
+    }
+}
+
 void DebugMqtt::init(PubSubClient *client, Loglevel max_loglevel) {
     DebugMqtt::client = client;
-    DebugMqtt::loglevel = loglevel;
+    DebugMqtt::loglevel = max_loglevel;
     DebugMqtt::isInit = true;
 }
 
diff --git a/lib/MQTT/debugMqtt.h b/lib/MQTT/debugMqtt.h
index 9e8f7c3..af97b48 100644
--- a/lib/MQTT/debugMqtt.h
+++ b/lib/MQTT/debugMqtt.h
@@ -11,6 +11,7 @@
 #ifndef DEBUG_MQTT_H
 #define DEBUG_MQTT_H
 
+#include <iostream>
 #include <PubSubClient.h>
 
 /**
@@ -71,6 +72,8 @@ class DebugMqtt {
          */
         DebugMqtt(const char* name);
 
+        ~DebugMqtt();
+
         /**
          * @brief Send a Message via MQTT
          * 
@@ -120,6 +123,8 @@ class DebugMqtt {
          */
         void writeToInflux(String measurement_name, String field_set, float measurement, uint64_t nanos);
 
+        void addCharacter(char c);
+
         /**
          * @brief Initalize debugMQTT for all instances
          * 
@@ -133,7 +138,7 @@ class DebugMqtt {
          * @param client PubSubClient
          * @param max_loglevel Max loglevel to send.
          */
-        static void init(PubSubClient *client, Loglevel max_loglevel);
+        static void init(PubSubClient* client, Loglevel max_loglevel);
 
         /**
          * @brief Change loglevel
@@ -146,6 +151,9 @@ class DebugMqtt {
 
     private:
         const char* name;
+        char* buf;
+        uint8_t bufPos = 0;
+        uint8_t bufSitze = 100;
 
         static String enum_to_string(Loglevel loglevel);
 
@@ -156,4 +164,4 @@ class DebugMqtt {
         static char topic[MQTT_BUFFER_SIZE];
 };
 
-#endif // DEBUG_MQTT_H
\ No newline at end of file
+#endif // DEBUG_MQTT_H
diff --git a/lib/MQTT/outputBufMqtt.cpp b/lib/MQTT/outputBufMqtt.cpp
new file mode 100644
index 0000000..ab3bdff
--- /dev/null
+++ b/lib/MQTT/outputBufMqtt.cpp
@@ -0,0 +1,30 @@
+/**
+ * @file outputStreamMqtt.cpp
+ * @author Alexander Klein (alex@kleiax.de)
+ * @brief 
+ * @version 0.1
+ * @date 2022-02-14
+ * 
+ * @copyright Copyright (c) 2022
+ * 
+ */
+
+#include "outputBufMqtt.h"
+
+OutputBufMqtt::OutputBufMqtt(DebugMqtt* debugMqtt) 
+    : std::streambuf() {
+    this->debugMqtt = debugMqtt;
+}
+
+std::streambuf::int_type OutputBufMqtt::overflow(std::streambuf::int_type c) {
+    if (c != EOF) {
+        // c = std::toupper(static_cast<char>(c), getloc());
+
+        this->debugMqtt->addCharacter((char) c);
+        Serial.print((char) c);
+        // if (putchar(c) == EOF) {
+        //     return EOF;
+        // }
+    }
+    return c;
+}
diff --git a/lib/MQTT/outputBufMqtt.h b/lib/MQTT/outputBufMqtt.h
new file mode 100644
index 0000000..a63477a
--- /dev/null
+++ b/lib/MQTT/outputBufMqtt.h
@@ -0,0 +1,33 @@
+/**
+ * @file outputStreamMqtt.h
+ * @author Alexander Klein (alex@kleiax.de)
+ * @brief 
+ * @version 0.1
+ * @date 2022-02-14
+ * 
+ * @copyright Copyright (c) 2022
+ * 
+ */
+
+#ifndef OUTPUT_STREAM_MQTT_H
+#define OUTPUT_STREAM_MQTT_H
+
+#include <iostream>
+#include <streambuf>
+#include <locale>
+#include <cstdio>
+
+#include "debugMqtt.h"
+
+class OutputBufMqtt : public std::streambuf {
+    public:
+        OutputBufMqtt(DebugMqtt* debugMqtt);
+
+    protected:
+        virtual std::streambuf::int_type overflow(std::streambuf::int_type c);
+
+    public:
+        DebugMqtt* debugMqtt;
+};
+
+#endif // OUTPUT_STREAM_MQTT_H
diff --git a/lib/Menu/menu.cpp b/lib/Menu/menu.cpp
index ceffbf5..7a22f6f 100644
--- a/lib/Menu/menu.cpp
+++ b/lib/Menu/menu.cpp
@@ -34,14 +34,13 @@ void Menu::printMenu() {
             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++) {
-            if (this->selectedEntry == iter)
-                std::cout << "  " << (**iter).getName() << std::endl;
-            else
-                std::cout << (**iter).getName() << std::endl;
-        }
+    } 
+    std::cout << std::endl;
+    for (std::list<MenuAction*>::iterator iter = this->entrys.begin(); iter != this->entrys.end(); iter++) {
+        if (this->selectedEntry == iter)
+            std::cout << "  " << (**iter).getName() << std::endl;
+        else
+            std::cout << (**iter).getName() << std::endl;
     }
     this->inSubmenu = false;
 }
@@ -124,4 +123,4 @@ void Menu::no() {
     }
 
     this->left();
-}
\ No newline at end of file
+}
diff --git a/src/SpecialMenus/Akku/menuAkku.cpp b/src/SpecialMenus/Akku/menuAkku.cpp
index ce8aeff..04cac88 100644
--- a/src/SpecialMenus/Akku/menuAkku.cpp
+++ b/src/SpecialMenus/Akku/menuAkku.cpp
@@ -55,9 +55,8 @@ void MenuAkku::printMenu() {
         lcd->printf("Controller: %s", buf);
         lcd->setCursor(0, 1);
         lcd->printf("Main: %u%%", mainBattery);
-    } else {
-        std::cout << "Controller: " << buf << "Main: " << mainBattery << "%" << std::endl;
     }
+    std::cout << "Controller: " << buf << " Main: " << (int) mainBattery << "%" << std::endl;
 }
 
 void MenuAkku::update() {
diff --git a/src/SpecialMenus/GPS/menuGPS.cpp b/src/SpecialMenus/GPS/menuGPS.cpp
index 7e5faed..b77db59 100644
--- a/src/SpecialMenus/GPS/menuGPS.cpp
+++ b/src/SpecialMenus/GPS/menuGPS.cpp
@@ -76,13 +76,12 @@ void MenuGPS::printPage(uint8_t page) const {
                     lcd->setCursor(0, 1);
                     lcd->printf("or no GPS signal");
                 }
-            } else {
+            }
                 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 or no GPS signal" << std::endl;
-            }
+                                << "HDOP: " << gps->hdop.hdop() << std::endl; 
+                else
+                    std::cout << "Data isn't valid or no GPS signal" << std::endl;
             break;
         
         case 1:
@@ -98,13 +97,12 @@ void MenuGPS::printPage(uint8_t page) const {
                     lcd->setCursor(0, 1);
                     lcd->printf("or NO GPS signal");
                 }
-            } else {
-                if (gps->satellites.isValid() && gps->satellites.value() > 2)
-                    std::cout   << "Lat: " << gps->location.lat()
-                                << "Lng: " << gps->location.lng() << std::endl;
-                else 
-                    std::cout << "Date isn't valid or NO GPS signal" << std::endl;
             }
+            if (gps->satellites.isValid() && gps->satellites.value() > 2)
+                std::cout   << "Lat: " << gps->location.lat()
+                            << "Lng: " << gps->location.lng() << std::endl;
+            else 
+                std::cout << "Data isn't valid or NO GPS signal" << std::endl;
             break;
 
         case 2:
@@ -114,9 +112,8 @@ void MenuGPS::printPage(uint8_t page) const {
                 lcd->printf("Chars processed");
                 lcd->setCursor(0, 1);
                 lcd->printf("-> %u", gps->charsProcessed());
-            } else {
-                std::cout << "Chars processed: " << gps->charsProcessed() << std::endl;
             }
+            std::cout << "Chars processed: " << gps->charsProcessed() << std::endl;
             break;
 
         default:
@@ -124,9 +121,8 @@ void MenuGPS::printPage(uint8_t page) const {
                 lcd->clear();
                 lcd->setCursor(0, 0);
                 lcd->printf("Page: %u", page);  
-            } else {
-                printf("Page: %u", page);
             }
+            std::cout << "Page: " << (int) page << std::endl;
             break;
     }
 }
diff --git a/src/SpecialMenus/PID/menuPidSettings.cpp b/src/SpecialMenus/PID/menuPidSettings.cpp
index 86d0957..4774c2b 100644
--- a/src/SpecialMenus/PID/menuPidSettings.cpp
+++ b/src/SpecialMenus/PID/menuPidSettings.cpp
@@ -85,10 +85,9 @@ void MenuPidSettings::printMenu() {
         lcd->print("  P    I    D  ");
         lcd->setCursor(0, 1);
         lcd->print(buf);
-    } else {
-        std::cout << "  P    I    D  " << std::endl;
-        std::cout << buf << std::endl;
     }
+    std::cout << "  P    I    D  " << std::endl;
+    std::cout << buf << std::endl;
 }
 
 
diff --git a/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp b/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp
index 6e51d64..47b6be7 100644
--- a/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp
+++ b/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp
@@ -69,8 +69,8 @@ void MenuAutopilot::printMenu() {
             this->lcd->setCursor(0, 1);
             this->lcd->print(buf2);
 
-        } else
-            std::cout << buf1 << " " << buf2 << std::endl;
+        }
+        std::cout << buf1 << " " << buf2 << std::endl;
 }
 
 void MenuAutopilot::update() {
diff --git a/src/SpecialMenus/driveModi/CaptureRoute/menuCaptureRoute.cpp b/src/SpecialMenus/driveModi/CaptureRoute/menuCaptureRoute.cpp
index 562d043..a99743e 100644
--- a/src/SpecialMenus/driveModi/CaptureRoute/menuCaptureRoute.cpp
+++ b/src/SpecialMenus/driveModi/CaptureRoute/menuCaptureRoute.cpp
@@ -21,8 +21,8 @@ void MenuCaptureRoute::printMenu() {
         this->lcd->setCursor(0, 1);
         this->lcd->print("du fahren. :-)");
 
-    } else
-        std::cout << "Capture du\n fahren. :-)" << std::endl;
+    } 
+    std::cout << "Points: " << this->routeInfo.totalPoints << " du fahren. :-)" << std::endl;
 }
 
 void MenuCaptureRoute::update() {
@@ -37,4 +37,4 @@ void MenuCaptureRoute::init() {
     this->driveManager->changeModus(Modi::CaptureRoute);
     this->captureRoute = (CaptureRoute*) this->driveManager->getDriveModiPtr();
     this->routeInfo = this->captureRoute->getRouteInfo();
-}
\ No newline at end of file
+}
diff --git a/src/SpecialMenus/driveModi/ManualDrive/menuManualDrive.cpp b/src/SpecialMenus/driveModi/ManualDrive/menuManualDrive.cpp
index 09eeee6..6ec8654 100644
--- a/src/SpecialMenus/driveModi/ManualDrive/menuManualDrive.cpp
+++ b/src/SpecialMenus/driveModi/ManualDrive/menuManualDrive.cpp
@@ -19,6 +19,6 @@ void MenuManualControl::printMenu() {
         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
+    }
+    std::cout << "Jetzt kannst du\n fahren. :-)" << std::endl;
+}
diff --git a/src/main.cpp b/src/main.cpp
index 1ce9607..9d14ff9 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -9,6 +9,8 @@
 #include "driveModi/driveManager.h"
 #include "moveControl.h"
 #include "network.h"
+#include "outputBufMqtt.h"
+#include "debugMqtt.h"
 
 #include "menu.h"
 #include "menuAction.h"
@@ -23,23 +25,38 @@ MoveControl moveController;
 DriveManager driveManager(&moveController);
 Menu* main_m;
 LiquidCrystal_I2C* lcd;
+OutputBufMqtt* bufMqtt;
+DebugMqtt* debugMqtt;
 
-// TODO: Platzhalter, muss irgendwann weg
-void dummy(){std::cout << "Dummy in Action" <<std::endl;}
-
-
-void callbackControllerAction();
-void callbackControllerConnect();
-void controllerPrintBattery();
-void i2cScanner();
-void restart();
+void callbackControllerAction(void);
+void callbackControllerConnect(void);
+void i2cScanner(void);
+void restart(void);
+void makeMenu(void);
 
 
 void setup() {
     Serial.begin(115200);
+
+    Network::setIps();
+    Network::connectWifi();
+    Network::setupMQTT();
+    Network::checkMQTT();
+    DebugMqtt::init(Network::getMqtttClient(), Loglevel::debug);
+
+    std::cout << "Activate additional output via MQTT..." << std::endl;
+    debugMqtt = new DebugMqtt("Console");
+    bufMqtt = new OutputBufMqtt(debugMqtt);
+    std::cout.rdbuf(bufMqtt);
+
     std::cout << "Welcome to Kleiax-Rover" << std::endl;
     std::cout << "All actions from the main programm run on Core -> " << xPortGetCoreID() << std::endl;
 
+    Ps3.attach(callbackControllerAction);
+    Ps3.attachOnConnect(callbackControllerConnect);
+    std::cout << "\nReady to connect a PS3 Controller... \n";
+    Ps3.begin(CONTROLLER_MAC);
+
     Wire.begin(21, 19);
     // i2cScanner();
     lcd = new LiquidCrystal_I2C(0x3F,16,2);
@@ -49,14 +66,97 @@ void setup() {
     lcd->setCursor(0, 1);
     lcd->print("Press PS-Button");
 
-    Network::setIps();
-    Network::connectWifi();
-    Network::setupMQTT();
+    makeMenu();
+}
 
-    Ps3.attach(callbackControllerAction);
-    Ps3.attachOnConnect(callbackControllerConnect);
-    std::cout << "\nReady to connect a PS3 Controller... \n";
-    Ps3.begin(CONTROLLER_MAC);
+void loop() {
+    Network::checkMQTT();
+    driveManager.loop();
+}
+
+void callbackControllerAction() {
+    auto isDelayReached = [](bool reset = false) -> bool {
+        static uint32_t lastMillis = 0;
+        if (reset)
+            lastMillis = millis();
+        static const uint16_t delay = 300;
+        bool res = false;
+
+        if (millis() - lastMillis > delay)
+            res = true;    
+        return res;
+    };
+
+    // Menu controlling
+    if (isDelayReached()) {
+        if (Ps3.data.button.up)
+            main_m->up();
+        else if (Ps3.data.button.down)
+            main_m->down();
+        else if (Ps3.data.button.left)
+            main_m->left();
+        else if (Ps3.data.button.right)
+            main_m->right();
+        else if (Ps3.data.button.circle)
+            main_m->yes();
+        else if (Ps3.data.button.cross)
+            main_m->no();
+        else if (Ps3.data.button.select)
+            debugMqtt->sendMsg(Loglevel::info, "Here we are: callbackController..");
+
+        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 Core -> " << xPortGetCoreID() << std::endl;
+
+    // TODO: Display is not functional without this seconde init, first init is in setup()
+    lcd->init();
+    main_m->printMenu();
+}
+
+void i2cScanner() {
+    std::cout << "\nI2C Scanner" << std::endl;
+
+    byte error, address;
+    int nDevices;
+    Serial.println("Scanning...");
+    nDevices = 0;
+    for(address = 1; address < 127; address++ ) {
+        Wire.beginTransmission(address);
+        error = Wire.endTransmission();
+        if (error == 0) {
+            Serial.print("I2C device found at address 0x");
+            if (address<16)
+                Serial.print("0");
+            Serial.println(address,HEX);
+            nDevices++;
+        }
+        else if (error==4) {
+            Serial.print("Unknow error at address 0x");
+            if (address<16)
+                Serial.print("0");
+            Serial.println(address,HEX);
+        }    
+    }
+    if (nDevices == 0)
+        Serial.println("No I2C devices found\n");
+    else
+        Serial.println("done\n");
+}
+
+void restart() {
+    ESP.restart();
+}
+
+void makeMenu() {
+    auto dummy = []() {
+        std::cout << "Dummy in Action" <<std::endl;
+    };
 
     // Create Menu
     main_m = new Menu();
@@ -112,87 +212,4 @@ void setup() {
     MenuAction* pidr_e = new MenuAction("Right", pidr_m);
     pid_m->addEntry(pidl_e);
     pid_m->addEntry(pidr_e);
-
-}
-
-void loop() {
-    Network::checkMQTT();
-    driveManager.loop();
-}
-
-bool isDelayReached(bool reset = false) {
-    static uint32_t lastMillis = 0;
-    if (reset)
-        lastMillis = millis();
-    static const uint16_t delay = 300;
-    bool res = false;
-
-    if (millis() - lastMillis > delay)
-        res = true;    
-    return res;
-}
-
-void callbackControllerAction() {
-    // Menu controlling
-    if (isDelayReached()) {
-        if (Ps3.data.button.up)
-            main_m->up();
-        else if (Ps3.data.button.down)
-            main_m->down();
-        else if (Ps3.data.button.left)
-            main_m->left();
-        else if (Ps3.data.button.right)
-            main_m->right();
-        else if (Ps3.data.button.circle)
-            main_m->yes();
-        else if (Ps3.data.button.cross)
-            main_m->no();
-
-        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 Core -> " << xPortGetCoreID() << std::endl;
-
-    // TODO: Display is not functional without this seconde init, first init is in setup()
-    lcd->init();
-    main_m->printMenu();
-}
-
-void i2cScanner() {
-    std::cout << "\nI2C Scanner" << std::endl;
-
-    byte error, address;
-    int nDevices;
-    Serial.println("Scanning...");
-    nDevices = 0;
-    for(address = 1; address < 127; address++ ) {
-        Wire.beginTransmission(address);
-        error = Wire.endTransmission();
-        if (error == 0) {
-            Serial.print("I2C device found at address 0x");
-            if (address<16)
-                Serial.print("0");
-            Serial.println(address,HEX);
-            nDevices++;
-        }
-        else if (error==4) {
-            Serial.print("Unknow error at address 0x");
-            if (address<16)
-                Serial.print("0");
-            Serial.println(address,HEX);
-        }    
-    }
-    if (nDevices == 0)
-        Serial.println("No I2C devices found\n");
-    else
-        Serial.println("done\n");
-}
-
-void restart() {
-    ESP.restart();
 }
diff --git a/src/moveControl.cpp b/src/moveControl.cpp
index 16c60bf..2c71362 100644
--- a/src/moveControl.cpp
+++ b/src/moveControl.cpp
@@ -47,7 +47,9 @@ void MoveControl::loop() {
         ||  right_motor_time > 50
         ||  left_speed_time > 50
         ||  right_speed_time > 50) {
-        Serial.printf("left M: %d, right M %d, left S %d, right S %d in moveControl::loop\n", left_motor_time, right_motor_time, left_speed_time, right_speed_time);
+        char buf[128];
+        sprintf(buf, "left M: %d, right M %d, left S %d, right S %d in moveControl::loop\n", left_motor_time, right_motor_time, left_speed_time, right_speed_time);
+        std::cout << buf;
     }
 
     static uint64_t last_millis = 0;
@@ -162,4 +164,4 @@ void MoveControl::regulateMotors() {
 void MoveControl::updateCurrentWheelSpeed() {
     this->wheelspeed_left = this->left_speedometer->getSpeed();
     this->wheelspeed_right = this->right_speedometer->getSpeed();
-}
\ No newline at end of file
+}
diff --git a/src/network.cpp b/src/network.cpp
index 8a3788a..4c31c72 100644
--- a/src/network.cpp
+++ b/src/network.cpp
@@ -27,12 +27,11 @@ void Network::setIps() {
 }
 
 void Network::setupMQTT() {
-    mqtt_client = new PubSubClient(wifi_client);
-    mqtt_client->setServer(mqtt_server, MQTT_PORT);
-    // mqtt_client.setServer(MQTT_SERVER, MQTT_PORT);
+    Network::mqtt_client = new PubSubClient(wifi_client);
+    // mqtt_client->setServer(mqtt_server, MQTT_PORT);
+    Network::mqtt_client->setServer(MQTT_SERVER, MQTT_PORT);
     Network::mqtt_client->setSocketTimeout(1);
-    mqtt_client->setSocketTimeout(1);
-    connectMQTT();
+    Network::connectMQTT();
 }
 
 bool Network::connectMQTT() {
@@ -55,24 +54,22 @@ bool Network::connectMQTT() {
 }
 void Network::connectWifi() {
    // Configures static IP address
-    if (!WiFi.config(local_IP, gateway, subnet)) {
-        Serial.println("STA Failed to configure");
-    }
+    if (!WiFi.config(local_IP, gateway, subnet))
+        std::cout << "STA Failed to configure" << std::endl;
 
     // Connect to Wi-Fi network with SSID and password
-    Serial.print("Connecting to ");
-    Serial.println(WLAN_SSID);
+    std::cout << "Connecting to " << WLAN_SSID << std::endl;
     WiFi.begin(WLAN_SSID, WLAN_PASSWORD);
     while (WiFi.status() != WL_CONNECTED) {
         delay(500);
-        Serial.print(".");
+        std::cout << "." << std::flush;
     }
 
     // Print local IP address and start web server
-    Serial.println("");
-    Serial.println("WiFi connected.");
-    Serial.println("IP address: ");
-    Serial.println(WiFi.localIP()); 
+    std::cout << std::endl;
+    std::cout << "WiFi connected." << std::endl;
+    std::cout << "IP address: " << std::endl;
+    std::cout << WLAN_IP << std::endl;
 }
 
 void Network::checkMQTT() {