restart the work

src/main.cpp

@@ -5,9 +5,13 @@
 #include <time.h>
 
 #include "config.h"
+
 #include "hardware/motorControl.h"
-#include "moveControl.h"
 #include "hardware/speedometer.h"
+#include "driveModi/autopilot.h"
+#include "driveModi/captureRoute.h"
+#include "driveModi/manualControl.h"
+#include "moveControl.h"
 #include "debugMqtt.h"
 
 void callbackControllerAction();
@@ -23,6 +27,8 @@ Speedometer speedometer_right;
 MoveControl moveController;
 DebugMqtt debugger("main");
 
+ManualControl manualControl;
+
 IPAddress local_IP(WLAN_IP);
 IPAddress gateway(WLAN_GATEWAY);
 IPAddress subnet(WLAN_SUBNETMASK);
@@ -32,6 +38,9 @@ WiFiClient wifi_client;
 PubSubClient mqtt_client(wifi_client);
 
 int controller_battery = -1;
+enum DriveMode {autopilot_e, captureRoute_e, manualControl_e};
+DriveMode driveMode = manualControl_e;
+
 
 void setup() {
     Serial.begin(115200);
@@ -66,10 +75,11 @@ void setup() {
 
     Ps3.attach(callbackControllerAction);
     Ps3.attachOnConnect(callbackControllerConnect);
-    Ps3.attachOnDisconnect(callbackControllerDisconnect);
+    // Ps3.attachOnDisconnect(callbackControllerDisconnect);
     Serial.println("\nReady to connect a PS3 Controller... \n");
     Ps3.begin();
 
+    // Init hardware
     left_motor.init(M_PWM_1, PWM_CHANNEL_M1, M_DIR_11, M_DIR_12, "left");
     right_motor.init(M_PWM_2, PWM_CHANNEL_M2, M_DIR_21, M_DIR_22, "right");
 
@@ -77,6 +87,9 @@ void setup() {
     speedometer_right.init(M_ENCODE_2A, M_ENCODE_2B, "right");
 
     moveController.init(&left_motor, &right_motor, &speedometer_left, &speedometer_right);
+
+    // Init driveModi
+    manualControl.init(&moveController, &left_motor, &right_motor);
 }
 
 void loop() {
@@ -88,6 +101,23 @@ void loop() {
     right_motor.runMotorControl();
     speedometer_left.runSpeedometer();
     speedometer_right.runSpeedometer();
+
+    switch (driveMode) {
+        case manualControl_e:
+            manualControl.runManualControl();
+            break;
+
+        case captureRoute_e:
+            /* code */
+            break;
+
+        case autopilot_e:
+            /* code */
+            break;
+
+        default:
+            break;
+    }
 }
 
 void reconnectMqtt() {
@@ -120,7 +150,7 @@ void callbackControllerAction() {
 
 void callbackControllerConnect() {
     Serial.println("Controller connected to ESP32");
-    debugger.sendMsg(Loglevel::info, "Controller connected to ESP32")
+    debugger.sendMsg(Loglevel::info, "Controller connected to ESP32");
 }
 
 void controllerPrintBattery() {