begin with gps, can drive now

src/main.cpp

@@ -1,6 +1,9 @@
 #include <Arduino.h>
 #include <Ps3Controller.h>
 #include <iostream>
+#include <Wire.h>
+#include <LiquidCrystal_I2C.h>
+#include <TinyGPS++.h>
 
 #include "config.h"
 
@@ -14,10 +17,13 @@
 #include "menuAction.h"
 #include "SpecialMenus/menuPidSettings.h"
 #include "SpecialMenus/menuManualDrive.h"
+#include "SpecialMenus/menuGPS.h"
 
 MoveControl moveController;
 DriveManager driveManager(&moveController);
 Menu* main_m;
+LiquidCrystal_I2C* lcd;
+TinyGPSPlus gps;
 
 // TODO: Platzhalter, muss irgendwann weg
 void dummy(){std::cout << "Dummy in Action" <<std::endl;}
@@ -26,13 +32,24 @@ void dummy(){std::cout << "Dummy in Action" <<std::endl;}
 void callbackControllerAction();
 void callbackControllerConnect();
 void controllerPrintBattery();
+void i2cScanner();
 
 void exitDriveMode() {driveManager.changeModus(Modi::Off);}
 
 void setup() {
     Serial.begin(115200);
+    Serial1.begin(GPS_BAUD, SERIAL_8N1, GPS_RX, GPS_TX);
     std::cout << "Welcome to Kleiax-Rover" << std::endl;
 
+    Wire.begin(21, 19);
+    // i2cScanner();
+    lcd = new LiquidCrystal_I2C(0x3F,16,2);
+    lcd->init();
+    lcd->backlight();
+    lcd->print("- Kleiax-Rover -");
+    lcd->setCursor(0, 1);
+    lcd->print("Press PS-Button");
+
     Network::setIps();
     Network::connectWifi();
     Network::setupMQTT();
@@ -42,8 +59,6 @@ void setup() {
     std::cout << "\nReady to connect a PS3 Controller... \n";
     Ps3.begin();
 
-    Serial1.begin(GPS_BAUD, SERIAL_8N1, GPS_RX, GPS_TX);
-
     // Create Menu
     main_m = new Menu();
     Menu* mode_m = new Menu();
@@ -51,10 +66,21 @@ void setup() {
     MenuPidSettings* pidl_m = new MenuPidSettings(moveController.getPID(0));
     MenuPidSettings* pidr_m = new MenuPidSettings(moveController.getPID(1));
     MenuManualControl* man_m = new MenuManualControl(&driveManager);
+    MenuGPS* gps_m = new MenuGPS(&gps);
+
+    // 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);
+    gps_m->setLcd(lcd);
+
 
     // Entry for the main menu
     MenuAction* mode_e = new MenuAction("Mode", mode_m);
-    MenuAction* gps_e = new MenuAction("GPS", dummy);
+    MenuAction* gps_e = new MenuAction("GPS", gps_m);
     MenuAction* pid_e = new MenuAction("PID", pid_m);
     main_m->addEntry(mode_e);
     main_m->addEntry(gps_e);
@@ -83,21 +109,18 @@ void setup() {
 void loop() {
     Network::checkMQTT();
     driveManager.loop();
-    while (Serial1.available() > 0)
-        std::cout << "In while von gps in loop in main.cpp" << std::endl;
-        // Serial.print(Serial1.read());
+    gps.encode(Serial1.read());
 }
 
-bool isDelayReached() {
+bool isDelayReached(bool reset = false) {
     static uint32_t lastMillis = 0;
-    const uint16_t delay = 200;
+    if (reset)
+        lastMillis = millis();
+    static const uint16_t delay = 300;
     bool res = false;
 
-    if (millis() - lastMillis > delay) {
-        res = true;
-        lastMillis = millis();
-    }
-    
+    if (millis() - lastMillis > delay)
+        res = true;    
     return res;
 }
 
@@ -118,11 +141,16 @@ void callbackControllerAction() {
             main_m->no();   
         else if (Ps3.data.button.ps)
             controllerPrintBattery();
+
+        isDelayReached(true);        
     }
 }
 
 void callbackControllerConnect() {
     std::cout << "Controller connected to ESP32" << std::endl;
+
+    // TODO: Display is not functional without this seconde init, first init is in setup()
+    lcd->init();
     main_m->printMenu();
 }
 
@@ -141,3 +169,34 @@ void controllerPrintBattery() {
     else if( controller_battery == ps3_status_battery_shutdown ) printf("SHUTDOWN\n");
     else printf("UNDEFINED\n");
 }
+
+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");
+}