implement untested captureRoute

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() {