added compass amd displayWrapper

src/main.cpp

@@ -27,6 +27,7 @@
 
 #include "driveModi/driveManager.h"
 #include "OutputBuf/outputBuf.h"
+#include "displayWrapper.h"
 #include "moveControl.h"
 #include "network.h"
 #include "debugMqtt.h"
@@ -48,6 +49,7 @@ MoveControl moveController;
 DriveManager* driveManager;
 Menu* main_m;
 LiquidCrystal_I2C* lcd;
+DisplayWrapper* lcdWrapper;
 OutputBuf* outputBuf;
 DebugMqtt* debugMqtt = nullptr;
 Battery* mainBattery;
@@ -120,6 +122,7 @@ void setup() {
     lcd->printf("%c Kleiax-Rover %c", wifiIndicator, wifiIndicator);
     lcd->setCursor(0, 1);
     lcd->print("Press PS-Button");
+    lcdWrapper = new DisplayWrapper(lcd);
 
     makeMenu();
 
@@ -134,6 +137,7 @@ void loop() {
         wifiTime.stopConsol("WiFi-Time", 10);
     }
     driveManager->loop();
+    lcdWrapper->loop();
 
     DebugTimes mainBatteryTime;
     mainBattery->loop();
@@ -198,18 +202,9 @@ void callbackControllerAction() {
 }
 
 void callbackControllerConnect() {
-    static bool firstConnect = true;
-
     std::cout << "Controller connected to ESP32" << std::endl;
     std::cout << "All actions from the Controller run on Core -> " << xPortGetCoreID() << std::endl;
 
-    // Display is not functional without this seconde init, first init is in setup(),
-    // because after the setup the other core control the display.
-    if (firstConnect) {
-        lcd->init();
-        firstConnect = false;
-    }
-
     main_m->printMenu();
 }
 
@@ -255,19 +250,19 @@ void makeMenu() {
     };
 
     auto restart = [&]() {
-        lcd->clear();
-        lcd->setCursor(0, 0);
-        lcd->print("Rebooting ...");
+        lcdWrapper->clear();
+        lcdWrapper->setCursor(0, 0);
+        lcdWrapper->print("Rebooting ...");
         ESP.restart();
     };
 
     auto disconnectController = [&]() {
         disconnectPs3 = true;
-        lcd->clear();
-        lcd->setCursor(0, 0);
-        lcd->print("Controller is");
-        lcd->setCursor(0, 1);
-        lcd->print("disconnected.");
+        lcdWrapper->clear();
+        lcdWrapper->setCursor(0, 0);
+        lcdWrapper->print("Controller is");
+        lcdWrapper->setCursor(0, 1);
+        lcdWrapper->print("disconnected.");
     };
 
     // Create Menu
@@ -276,8 +271,6 @@ void makeMenu() {
     Menu* pid_m = new Menu();
     MenuIntInput* pidl_m = new MenuIntInput(3, new MenuPidSettings(moveController.getPID(0)));
     MenuIntInput* pidr_m = new MenuIntInput(3, new MenuPidSettings(moveController.getPID(1)));
-    // MenuPidSettings* pidl_m = new MenuPidSettings(moveController.getPID(0));
-    // MenuPidSettings* pidr_m = new MenuPidSettings(moveController.getPID(1));
     MenuManualControl* man_m = new MenuManualControl(driveManager);
     MenuCaptureRoute* cap_m = new MenuCaptureRoute(driveManager);
     MenuAutopilot* auto_m = new MenuAutopilot(driveManager);
@@ -287,18 +280,18 @@ void makeMenu() {
     MenuRoute* rout_m = new MenuRoute(driveManager->getNavigation()->getRoute());
 
     // Set Menus on LCD
-    main_m->setLcd(lcd);
-    mode_m->setLcd(lcd);
-    pid_m->setLcd(lcd);
-    pidl_m->setLcd(lcd);
-    pidr_m->setLcd(lcd);
-    man_m->setLcd(lcd);
-    cap_m->setLcd(lcd);
-    auto_m->setLcd(lcd);
-    test_m->setLcd(lcd);
-    gps_m->setLcd(lcd);
-    sys_m->setLcd(lcd);
-    rout_m->setLcd(lcd);
+    main_m->setLcd(lcdWrapper);
+    mode_m->setLcd(lcdWrapper);
+    pid_m->setLcd(lcdWrapper);
+    pidl_m->setLcd(lcdWrapper);
+    pidr_m->setLcd(lcdWrapper);
+    man_m->setLcd(lcdWrapper);
+    cap_m->setLcd(lcdWrapper);
+    auto_m->setLcd(lcdWrapper);
+    test_m->setLcd(lcdWrapper);
+    gps_m->setLcd(lcdWrapper);
+    sys_m->setLcd(lcdWrapper);
+    rout_m->setLcd(lcdWrapper);
 
 
     // Entry for the main menu