Start with Capture Route

src/driveModi/Modi/CaptureRoute/captureRoute.cpp

@@ -1,22 +1,14 @@
 #include "driveModi/Modi/CaptureRoute/captureRoute.h"
 
-CaptureRoute::CaptureRoute() {
+CaptureRoute::CaptureRoute(MoveControl* moveControl, Navigation* navigation) 
+    : ManualControl(moveControl) {
+        this->navigation = navigation;
 }
 
-void CaptureRoute::init() {
-    
-    
+void CaptureRoute::loop() {
+    ManualControl::loop();
 }
 
 void CaptureRoute::runCaptureRoute() {
-    // this->runManualControl();
 
-    // static uint64_t last_millis = 0;
-    // if (millis() - last_millis < RUN_CAPTuRE_ROUTE_DELAY) {
-    //     return;
-    // }
-    // last_millis = millis();
-
-    //Add Point to route
-    // this->route->addCurrentLocationToRoute();
 }

src/driveModi/Modi/CaptureRoute/captureRoute.h

@@ -1,21 +1,20 @@
 #ifndef CAPTURE_ROUTE_H
 #define CAPTURE_ROUTE_H
 
+#include <iostream>
+
 #include "driveModi/Modi/ManualControl/manualControl.h"
-// #include "route.h"
+#include "navigation.h"
 
-#include "config.h"
-
-class CaptureRoute : ManualControl {
+class CaptureRoute : public ManualControl {
     public:
-        CaptureRoute();
+        CaptureRoute(MoveControl* moveControl, Navigation* navigation);
 
-        void init();
+        void loop();
         void runCaptureRoute();
 
     private:
-        // Route *route;
-        // DebugMqtt *debug;
+        Navigation* navigation;
 };
 
 #endif // CAPTURE_ROUTE_H

src/driveModi/Modi/ManualControl/manualControl.cpp

@@ -10,7 +10,10 @@
  */
 #include "manualControl.h"
 
-ManualControl::ManualControl() { }
+ManualControl::ManualControl(MoveControl *moveControl) { 
+    this->moveControl = moveControl;
+    this->moveControl->setDrivingStatus(DrivingStatus::drive);
+}
 
 ManualControl::~ManualControl() {
     this->moveControl->setSpeed(0);
@@ -18,11 +21,6 @@ ManualControl::~ManualControl() {
     this->moveControl->setDrivingStatus(DrivingStatus::stop);
 }
 
-void ManualControl::init(MoveControl *moveControl) {
-    this->moveControl = moveControl;
-    this->moveControl->setDrivingStatus(DrivingStatus::drive);
-}
-
 void ManualControl::loop() {
     static uint32_t last_millis = 0;
     if (millis() - last_millis < delay) {

src/driveModi/Modi/ManualControl/manualControl.h

@@ -26,7 +26,7 @@
  */
 class ManualControl : public DriveModi{
     public:
-        ManualControl();
+        ManualControl(MoveControl *moveControl);
 
         /**
          * @brief Destroy the Manual Control object
@@ -34,15 +34,6 @@ class ManualControl : public DriveModi{
          */
         ~ManualControl();
 
-        /**
-         * @brief Initalize ManualControl
-         * 
-         * set DrivingStatus::drive
-         * 
-         * @param moveControl 
-         */
-        void init(MoveControl *moveControl);
-
         /**
          * @brief Calls runManualControl() to update all values.
          * 

src/driveModi/driveManager.cpp

@@ -11,28 +11,30 @@
 
 #include "driveModi/driveManager.h"
 
+Modi& operator++(Modi& m, int) {
+    return m = (m == Modi::TestMode) ? Modi::Off : static_cast<Modi>(static_cast<int>(m)+1);
+}
+
 DriveManager::DriveManager(MoveControl *moveControl) {
     this->moveControl = moveControl;
 }
 
 void DriveManager::loop() {
     this->moveControl->loop();
-    if (currentModus)
-        this->currentModus->loop();
-}
-
-void DriveManager::runDriveManager() {
-    
+    this->navigation->loop();
+    if (currentModusPtr)
+        this->currentModusPtr->loop();
 }
 
 void DriveManager::nextModus() {
-    
+    changeModus(this->currentModus++);
 }
 
 void DriveManager::changeModus(Modi modus) {
+    this->currentModus = modus;
 
     //Set moveControl to a safe state
-    delete this->currentModus;
+    delete this->currentModusPtr;
 
     this->moveControl->setSpeed(0);
     this->moveControl->setRotationspeed(0);
@@ -40,34 +42,33 @@ void DriveManager::changeModus(Modi modus) {
     
     switch (modus) {
         case Modi::Off:
-            this->currentModus = nullptr;
+            this->currentModusPtr = nullptr;
             break;
 
         case Modi::ManualControl: {
-            ManualControl *ptr = new ManualControl;
-            ptr->init(this->moveControl);
-            this->currentModus = ptr;
+            this->currentModusPtr = new ManualControl(this->moveControl);
             }
             break;
 
-        case Modi::CaptureRoute:
-            this->currentModus = nullptr;
+        case Modi::CaptureRoute: {
+            this->currentModusPtr = new CaptureRoute(this->moveControl, this->navigation);
+            }
             break;
 
         case Modi::Autopilot:
-            this->currentModus = nullptr;
+            this->currentModusPtr = nullptr;
             break;
 
         case Modi::ConsolControl:
-            this->currentModus = nullptr;
+            this->currentModusPtr = nullptr;
             break;
 
         case Modi::TestMode:
-            this->currentModus = nullptr;
+            this->currentModusPtr = nullptr;
             break;
         
         default:
-            this->currentModus = nullptr;
+            this->currentModusPtr = nullptr;
             break;
     }
 }

src/driveModi/driveManager.h

@@ -12,8 +12,16 @@
 #ifndef DRIVE_MANAGER_H
 #define DRIVE_MANAGER_H
 
+//GPS Serial Pins
+#define GPS_RX 17
+#define GPS_TX 16
+#define GPS_BAUD 9600
+
+#include <Arduino.h>
+
 #include "moveControl.h"
 #include "driveModi/driveModi.h"
+#include "navigation.h"
 
 // All Drive Modi
 #include "driveModi/Modi/ManualControl/manualControl.h"
@@ -23,28 +31,31 @@
 #include "driveModi/Modi/TestMode/testMode.h"
 
 enum class Modi {   
-                    Off,
-                    ManualControl,
-                    CaptureRoute,
-                    Autopilot,
-                    ConsolControl,
-                    TestMode
-                };
+    Off,
+    ManualControl,
+    CaptureRoute,
+    Autopilot,
+    ConsolControl,
+    TestMode
+};
 
 class DriveManager {
     public:
         DriveManager(MoveControl *moveControl);
-
         void loop();
-        void runDriveManager();
 
         void nextModus();
         void changeModus(Modi modus);
 
-    private:
-        MoveControl *moveControl;
-        DriveModi *currentModus;
+        Navigation* getNavigation() {return this->navigation;}
+        DriveModi* getDriveModiPtr() { return this->currentModusPtr; }
+        Modi getDriveModi() { return this->currentModus; }
 
+    private:
+        Modi currentModus = Modi::Off;
+        MoveControl *moveControl;
+        DriveModi *currentModusPtr;
+        Navigation* navigation;
 };