Added consolControl & PID

src/main.cpp

@@ -11,6 +11,7 @@
 #include "driveModi/autopilot.h"
 #include "driveModi/captureRoute.h"
 #include "driveModi/manualControl.h"
+#include "driveModi/consolControl.h"
 #include "moveControl.h"
 #include "route.h"
 #include "debugMqtt.h"
@@ -27,11 +28,12 @@ Speedometer speedometer_left;
 Speedometer speedometer_right;
 MoveControl moveController;
 Route route;
-DebugMqtt debug("main");
+DebugMqtt debugger("main");
 
 ManualControl manualControl;
 CaptureRoute captureRoute;
 Autopilot autopilot;
+ConsolControl consolControl;
 
 IPAddress local_IP(WLAN_IP);
 IPAddress gateway(WLAN_GATEWAY);
@@ -42,7 +44,7 @@ WiFiClient wifi_client;
 PubSubClient mqtt_client(wifi_client);
 
 int controller_battery = -1;
-enum DriveMode {autopilot_e, captureRoute_e, manualControl_e};
+enum DriveMode {autopilot_e, captureRoute_e, manualControl_e, consolControl_e};
 DriveMode driveMode = manualControl_e;
 
 
@@ -57,7 +59,7 @@ void setup() {
     // Connect to Wi-Fi network with SSID and password
     Serial.print("Connecting to ");
     Serial.println(WLAN_SSID);
-    WiFi.begin("Kleiax2", "Punica-699");
+    WiFi.begin(WLAN_SSID, WLAN_PASSWORD);
     while (WiFi.status() != WL_CONNECTED) {
         delay(500);
         Serial.print(".");
@@ -96,6 +98,7 @@ void setup() {
     manualControl.init(&moveController, &left_motor, &right_motor);
     captureRoute.init(&route, &moveController, &left_motor, &right_motor);
     autopilot.init(&route, &moveController);
+    consolControl.init(&moveController);
 }
 
 void loop() {
@@ -122,6 +125,10 @@ void loop() {
             autopilot.runAutopilot();
             break;
 
+        case consolControl_e:
+            consolControl.run();
+            break;
+
         default:
             break;
     }
@@ -154,17 +161,22 @@ void callbackControllerAction() {
         switch (driveMode) {
             case manualControl_e:
             driveMode = DriveMode::captureRoute_e;
-            debug.sendMsg(Loglevel::info, "New driveMode = CaptureRoute");
+            debugger.sendMsg(Loglevel::info, "New driveMode = CaptureRoute");
             break;
 
         case captureRoute_e:
             driveMode = DriveMode::autopilot_e;
-            debug.sendMsg(Loglevel::info, "New driveMode = Autopilot");
+            debugger.sendMsg(Loglevel::info, "New driveMode = Autopilot");
             break;
 
         case autopilot_e:
+            driveMode = DriveMode::consolControl_e;
+            debugger.sendMsg(Loglevel::info, "New driveMode = ConsolControl");
+            break;
+
+        case consolControl_e:
             driveMode = DriveMode::manualControl_e;
-            debug.sendMsg(Loglevel::info, "New driveMode = ManualControl");
+            debugger.sendMsg(Loglevel::info, "New driveMode = ManualControl");
             break;
 
         default:
@@ -175,7 +187,7 @@ void callbackControllerAction() {
 
 void callbackControllerConnect() {
     Serial.println("Controller connected to ESP32");
-    debug.sendMsg(Loglevel::info, "Controller connected to ESP32");
+    debugger.sendMsg(Loglevel::info, "Controller connected to ESP32");
 }
 
 void controllerPrintBattery() {