Added consolControl & PID

src/driveModi/captureRoute.cpp

@@ -20,7 +20,5 @@ void CaptureRoute::runCaptureRoute() {
     last_millis = millis();
 
     //Add Point to route
-    Point p;
-    p.lat = this.
-    this->route->addPoint()
+    this->route->addCurrentLocationToRoute();
 }

src/driveModi/consolControl.cpp

@@ -0,0 +1,40 @@
+#include "consolControl.h"
+#include <string>
+
+ConsolControl::ConsolControl() {
+    this->debug = new DebugMqtt("ConsolControl");
+}
+
+void ConsolControl::init(MoveControl *MoveControl) {
+    this->moveControl = moveControl;
+    this->left_pid = this->moveControl->getPID(0);
+    this->right_pid = this->moveControl->getPID(1);
+}
+
+void ConsolControl::run() {
+    PID *selectedPID;
+
+    if (Serial.available()) {
+        char cmd = Serial.read();
+        if (cmd == 'l') {
+            selectedPID = this->left_pid;
+        } else if (cmd == 'r') {
+            selectedPID = this->right_pid;
+        }
+
+        double p = Serial.parseFloat();
+        double i = Serial.parseFloat();
+        double d = Serial.parseFloat();
+
+        if (p && i && d) {
+            Serial.println("ConsolControl changeDate");
+            selectedPID->SetTunings(p, i, d);
+        } else {
+            printManual();
+        }
+    }
+}
+
+void printManual() {
+    Serial.println("r 12.0 13.0 4.0 // site p i d");
+}

src/driveModi/consolControl.h

@@ -0,0 +1,24 @@
+#ifndef CONSOL_CONTROL_H
+#define CONSOL_CONTROL_H
+
+#include "moveControl.h"
+#include "debugMqtt.h"
+
+class ConsolControl {
+    public:
+        ConsolControl();
+        void init(MoveControl *moveControl);
+        void run();
+
+    private:
+        void printManual();
+
+        MoveControl *moveControl;
+        DebugMqtt *debug;
+        PID *left_pid;
+        PID *right_pid;
+
+
+};
+
+#endif // CONSOL_CONTROL_H

src/driveModi/manualControl.cpp

@@ -48,7 +48,8 @@ void ManualControl::runManualControl() {
 
     switch (this->controlMode) {
         case ControMode::halt:
-            this->reset();
+            this->moveControl->setDrivingStatus(DrivingStatus::stop);
+            //this->reset();
             break;
 
         case ControMode::joystick:
@@ -66,14 +67,16 @@ void ManualControl::runManualControl() {
 }
 
 void ManualControl::driveWithControllerJoystick() {
+    this->moveControl->setDrivingStatus(DrivingStatus::drive);
     int8_t x = Ps3.data.analog.stick.lx;
     int8_t y = Ps3.data.analog.stick.ly;
 
     double value_per_step = MANUALCONTROL_MAX_SPEED * 2 / 256;
     this->moveControl->setSpeed((y * -1) * value_per_step);
-    char str[64];
-    sprintf(str, "%f", (y * -1) * value_per_step);
-    debug->sendMsg(Loglevel::debug, str);
+    this->moveControl->setDrivingStatus(DrivingStatus::drive);
+    // char str[64];
+    // sprintf(str, "driveWithJoy: %f", (y * -1) * value_per_step);
+    // debug->sendMsg(Loglevel::debug, str); // Kommt 1 raus bei vollausschlag
 
     value_per_step = MANUALCONTROL_MAX_ROTATION * 2 / 256;
     this->moveControl->setRotationspeed(x * value_per_step);