Start with Capture Route

src/main.cpp

@@ -3,7 +3,6 @@
 #include <iostream>
 #include <Wire.h>
 #include <LiquidCrystal_I2C.h>
-#include <TinyGPS++.h>
 
 #include "config.h"
 
@@ -17,13 +16,13 @@
 #include "menuAction.h"
 #include "SpecialMenus/menuPidSettings.h"
 #include "SpecialMenus/menuManualDrive.h"
+#include "SpecialMenus/menuCaptureRoute.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;}
@@ -33,12 +32,12 @@ void callbackControllerAction();
 void callbackControllerConnect();
 void controllerPrintBattery();
 void i2cScanner();
+void restart();
 
 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);
@@ -57,7 +56,7 @@ void setup() {
     Ps3.attach(callbackControllerAction);
     Ps3.attachOnConnect(callbackControllerConnect);
     std::cout << "\nReady to connect a PS3 Controller... \n";
-    Ps3.begin();
+    Ps3.begin(CONTROLLER_MAC);
 
     // Create Menu
     main_m = new Menu();
@@ -66,7 +65,8 @@ 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);
+    MenuCaptureRoute* cap_m = new MenuCaptureRoute(&driveManager);
+    MenuGPS* gps_m = new MenuGPS(driveManager.getNavigation()->getGPS());
 
     // Set Menus on LCD
     main_m->setLcd(lcd);
@@ -75,6 +75,7 @@ void setup() {
     pidl_m->setLcd(lcd);
     pidr_m->setLcd(lcd);
     man_m->setLcd(lcd);
+    cap_m->setLcd(lcd);
     gps_m->setLcd(lcd);
 
 
@@ -82,13 +83,15 @@ void setup() {
     MenuAction* mode_e = new MenuAction("Mode", mode_m);
     MenuAction* gps_e = new MenuAction("GPS", gps_m);
     MenuAction* pid_e = new MenuAction("PID", pid_m);
+    MenuAction* restart_e = new MenuAction("Restart", restart);
     main_m->addEntry(mode_e);
     main_m->addEntry(gps_e);
     main_m->addEntry(pid_e);
+    main_m->addEntry(restart_e);
 
     // Entry for the mode Menu
     MenuAction* autopilot_e = new MenuAction("Autopilot", dummy);
-    MenuAction* captureRoute_e = new MenuAction("Capture Route", dummy);
+    MenuAction* captureRoute_e = new MenuAction("Capture Route", cap_m);
     MenuAction* consolControl_e = new MenuAction("Consol Control", dummy);
     MenuAction* manualControl_e = new MenuAction("Manual Control", man_m);
     MenuAction* testMode_e = new MenuAction("Test Mode", dummy);
@@ -109,7 +112,6 @@ void setup() {
 void loop() {
     Network::checkMQTT();
     driveManager.loop();
-    gps.encode(Serial1.read());
 }
 
 bool isDelayReached(bool reset = false) {
@@ -200,3 +202,7 @@ void i2cScanner() {
     else
         Serial.println("done\n");
 }
+
+void restart() {
+    ESP.restart();
+}