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

src/main.cpp

@@ -12,6 +12,7 @@
 #include "driveModi/captureRoute.h"
 #include "driveModi/manualControl.h"
 #include "moveControl.h"
+#include "route.h"
 #include "debugMqtt.h"
 
 void callbackControllerAction();
@@ -25,9 +26,12 @@ MotorControl right_motor;
 Speedometer speedometer_left;
 Speedometer speedometer_right;
 MoveControl moveController;
-DebugMqtt debugger("main");
+Route route;
+DebugMqtt debug("main");
 
 ManualControl manualControl;
+CaptureRoute captureRoute;
+Autopilot autopilot;
 
 IPAddress local_IP(WLAN_IP);
 IPAddress gateway(WLAN_GATEWAY);
@@ -90,6 +94,8 @@ void setup() {
 
     // Init driveModi
     manualControl.init(&moveController, &left_motor, &right_motor);
+    captureRoute.init(&route, &moveController, &left_motor, &right_motor);
+    autopilot.init(&route, &moveController);
 }
 
 void loop() {
@@ -101,6 +107,7 @@ void loop() {
     right_motor.runMotorControl();
     speedometer_left.runSpeedometer();
     speedometer_right.runSpeedometer();
+    route.runRoute();
 
     switch (driveMode) {
         case manualControl_e:
@@ -108,11 +115,11 @@ void loop() {
             break;
 
         case captureRoute_e:
-            /* code */
+            captureRoute.runCaptureRoute();
             break;
 
         case autopilot_e:
-            /* code */
+            autopilot.runAutopilot();
             break;
 
         default:
@@ -143,14 +150,32 @@ void reconnectMqtt() {
 }
 
 void callbackControllerAction() {
-    if (Ps3.event.button_down.start) {
+    if (Ps3.event.button_down.ps) {
-        
+        switch (driveMode) {
+            case manualControl_e:
+            driveMode = DriveMode::captureRoute_e;
+            debug.sendMsg(Loglevel::info, "New driveMode = CaptureRoute");
+            break;
+
+        case captureRoute_e:
+            driveMode = DriveMode::autopilot_e;
+            debug.sendMsg(Loglevel::info, "New driveMode = Autopilot");
+            break;
+
+        case autopilot_e:
+            driveMode = DriveMode::manualControl_e;
+            debug.sendMsg(Loglevel::info, "New driveMode = ManualControl");
+            break;
+
+        default:
+            break;
+        }
     }
 }
 
 void callbackControllerConnect() {
     Serial.println("Controller connected to ESP32");
-    debugger.sendMsg(Loglevel::info, "Controller connected to ESP32");
+    debug.sendMsg(Loglevel::info, "Controller connected to ESP32");
 }
 
 void controllerPrintBattery() {

src/moveControl.cpp

@@ -25,6 +25,10 @@ void MoveControl::runMoveControl() {
 
     this->calcWheelSpeed();
     this->regulateMotors();
+
+    char str[64];
+    sprintf(str, "x_speed: %3.2f, rotation_speed: %3.2f, tar_left: %3.2f, tar_right: %3.2f", this->x_speed, this->rotation_speed, this->wheelspeed_left_target, this->wheelspeed_right_target);
+    debug->sendMsg(Loglevel::debug, str);
 }
 
 void MoveControl::setDrivingStatus(DrivingStatus status) {

src/route.cpp

@@ -1,14 +1,39 @@
 #include "route.h"
 
-Route::Route(){
+Route::Route() {
-
+    Serial1.begin(GPS_BAUD, SERIAL_8N1, GPS_RX, GPS_TX);
+    this->debug = new DebugMqtt("Route");
 }
 
-void Route::addPoint(Point point) {
+void Route::runRoute() {
+    static uint64_t last_millis = 0;
+    if (millis() - last_millis < RUN_ROUTE_DELAY) {
+        return;
+    }
+    last_millis = millis();
+
+    while (Serial1.available() > 0)
+        gps.encode(Serial1.read());
+
+    if (gps.location.isUpdated() && gps.location.isValid()) {
+        this->currentLocation.lat = gps.location.lat();
+        this->currentLocation.lon = gps.location.lng();
+    }
+}
+
+void Route::addPointToRoute(Point point) {
     this->points.push_back(point);
     this->count_points++;
 }
 
+void Route::addCurrentLocationToRoute() {
+    if (*this->points.end() == this->currentLocation  &&
+        !this->nearlySameLocation(*this->points.end(), this->currentLocation)) {
+        return;
+    }
+    this->addPointToRoute(currentLocation);
+}
+
 void Route::delRoute() {
     this->points.clear();
     this->count_points = 0;
@@ -45,4 +70,10 @@ double Route::getDis(Point point_1, Point point_2) {
     double dx = 111.3 * cos(lat) * (point_1.lon - point_2.lon);
     double dy = 111.3 * (point_1.lat - point_2.lat);
     return sqrt(dx * dx + dy * dy);
+}
+
+bool Route::nearlySameLocation(Point p1, Point p2) {
+    if (this->getDis(p1, p2) < DISTANCE_BETWEEN_POINTS)
+        return true;
+    return false;
 }

src/route.h

@@ -3,21 +3,30 @@
 
 #include <cstdint>
 #include <list>
+#include <TinyGPS++.h>
+#include <Arduino.h>
 
 #include "hardware/motorControl.h"
 #include "hardware/speedometer.h"
+#include "debugMqtt.h"
 #include "config.h"
 
 struct Point{
-    float lat = 0;
+    double lat = 0;
-    float lon = 0;
+    double lon = 0;
+    bool operator==(const Point& rhs) const {
+        return this->lat == rhs.lat && this->lon == rhs.lon;
+    }
 };
 
 class Route {
     public:
         Route();
 
-        void addPoint(Point point);
+        void runRoute();
+
+        void addPointToRoute(Point point);
+        void addCurrentLocationToRoute();
         void delRoute();
         void resetRoute();
 
@@ -29,14 +38,21 @@ class Route {
 
 
     private:
+        bool nearlySameLocation(Point p1, Point p2);
+
         std::list<Point> points;
         std::list<Point>::iterator it;
+        Point currentLocation;
 
         uint16_t count_points = 0;
 
         bool route_started = false;
         bool route_finished = false;
 
+        TinyGPSPlus gps;
+
+        DebugMqtt *debug;
+
 };
 
 #endif // ROUTE_H