Start with Capture Route

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;
     }
 }