implement untested captureRoute

src/driveModi/autopilot.cpp

@@ -0,0 +1,14 @@
+#include "driveModi/autopilot.h"
+
+Autopilot::Autopilot() {
+    this->debug = new DebugMqtt("Autopilot");
+}
+
+void Autopilot::init(Route *route, MoveControl *moveControl) {
+    this->route = route;
+    this->moveControl = moveControl;
+}
+
+void Autopilot::runAutopilot() {
+
+}

src/driveModi/autopilot.h

@@ -5,15 +5,20 @@
 
 #include "route.h"
 #include "moveControl.h"
+#include "debugMqtt.h"
 
 class Autopilot {
     public:
         Autopilot();
 
+        void init(Route *route, MoveControl *moveControl);
+        void runAutopilot();
+
+
     private:
         Route *route;
         MoveControl *moveControl;
-        TinyGPSPlus *gps;
+        DebugMqtt *debug;
 
 };
 

src/driveModi/captureRoute.cpp

@@ -0,0 +1,26 @@
+#include "driveModi/captureRoute.h"
+
+CaptureRoute::CaptureRoute() {
+    this->debug = new DebugMqtt("CaptureRoute");
+}
+
+void CaptureRoute::init(Route *route, MoveControl *moveControl, MotorControl *motorControlLeft, MotorControl *motorControlRight) {
+    this->route = route;
+    ManualControl::init(moveControl, motorControlLeft, motorControlRight);
+    
+}
+
+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
+    Point p;
+    p.lat = this.
+    this->route->addPoint()
+}

src/driveModi/captureRoute.h

@@ -0,0 +1,22 @@
+#ifndef CAPTURE_ROUTE_H
+#define CAPTURE_ROUTE_H
+
+#include "driveModi/manualControl.h"
+#include "debugMqtt.h"
+#include "route.h"
+
+#include "config.h"
+
+class CaptureRoute : ManualControl {
+    public:
+        CaptureRoute();
+
+        void init(Route *route, MoveControl *moveControl, MotorControl *motorControlLeft, MotorControl *motorControlRight);
+        void runCaptureRoute();
+
+    private:
+        Route *route;
+        DebugMqtt *debug;
+};
+
+#endif // CAPTURE_ROUTE_H