a lot of bullshit

src/driveModi/Modi/Autopilot/autopilot.cpp

@@ -0,0 +1,14 @@
+#include "driveModi/Modi/Autopilot/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/Modi/Autopilot/autopilot.h

@@ -0,0 +1,27 @@
+#ifndef AUTOPILOT_H
+#define AUTOPILOT_H
+
+#include <TinyGPS++.h>
+
+#include "route.h"
+#include "moveControl.h"
+#include "debugMqtt.h"
+#include "driveModi/driveModi.h"
+
+class Autopilot : DriveModi{
+    public:
+        Autopilot();
+
+        void init(Route *route, MoveControl *moveControl);
+        void loop();
+        void runAutopilot();
+
+
+    private:
+        Route *route;
+        MoveControl *moveControl;
+        DebugMqtt *debug;
+
+};
+
+#endif // AUTOPILOT_H

src/driveModi/Modi/CaptureRoute/captureRoute.cpp

@@ -0,0 +1,22 @@
+#include "driveModi/Modi/CaptureRoute/captureRoute.h"
+
+CaptureRoute::CaptureRoute() {
+}
+
+void CaptureRoute::init() {
+    
+    
+}
+
+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
+    // this->route->addCurrentLocationToRoute();
+}

src/driveModi/Modi/CaptureRoute/captureRoute.h

@@ -0,0 +1,21 @@
+#ifndef CAPTURE_ROUTE_H
+#define CAPTURE_ROUTE_H
+
+#include "driveModi/Modi/ManualControl/manualControl.h"
+// #include "route.h"
+
+#include "config.h"
+
+class CaptureRoute : ManualControl {
+    public:
+        CaptureRoute();
+
+        void init();
+        void runCaptureRoute();
+
+    private:
+        // Route *route;
+        // DebugMqtt *debug;
+};
+
+#endif // CAPTURE_ROUTE_H

src/driveModi/Modi/ConsolControl/consolControl.cpp

@@ -0,0 +1,43 @@
+#include "consolControl.h"
+
+ConsolControl::ConsolControl() {
+    this->debug = new DebugMqtt("ConsolControl");
+}
+
+void ConsolControl::init(MoveControl *MoveControl) {
+    this->moveControl = moveControl;
+}
+
+void ConsolControl::run() {
+    uint8_t side = -1;
+
+    if (Serial.available()) {
+        char cmd = Serial.read();
+        if (cmd == 'l') {
+            side = 0;
+        } else if (cmd == 'r') {
+            side = 1;
+        }
+
+        double p = Serial.parseFloat();
+        double i = Serial.parseFloat();
+        double d = Serial.parseFloat();
+        double s = Serial.parseFloat();
+        double r = Serial.parseFloat();
+
+        if (p && i && d && s && r) {
+            Serial.println("ConsolControl changeDate");
+            this->moveControl->setPidTunings(side, p, i, d);
+            this->moveControl->setSpeed(s);
+            this->moveControl->setRotationspeed(r);
+        } else {
+            this->moveControl->setDrivingStatus(DrivingStatus::stop);
+            printManual();
+            debug->sendMsg(Loglevel::error, "not all double are not zero");
+        }
+    }
+}
+
+void ConsolControl::printManual() {
+    Serial.println("r 12.0 13.0 4.0 1.0 1.0 // site p i d s r");
+}

src/driveModi/Modi/ConsolControl/consolControl.h

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

src/driveModi/Modi/ManualControl/manualControl.cpp

@@ -0,0 +1,42 @@
+/**
+ * @file manualControl.cpp
+ * @author Alexander Klein (alex@kleiax.de)
+ * @brief Implementation of the class manualControl.h
+ * @version 0.1
+ * @date 2021-12-13
+ * 
+ * @copyright Copyright (c) 2021
+ * 
+ */
+#include "manualControl.h"
+
+ManualControl::ManualControl() { }
+
+ManualControl::~ManualControl() {
+    this->moveControl->setSpeed(0);
+    this->moveControl->setRotationspeed(0);
+    this->moveControl->setDrivingStatus(DrivingStatus::stop);
+}
+
+void ManualControl::init(MoveControl *moveControl) {
+    this->moveControl = moveControl;
+    this->moveControl->setDrivingStatus(DrivingStatus::drive);
+}
+
+void ManualControl::loop() {
+    static uint32_t last_millis = 0;
+    if (millis() - last_millis < delay) {
+        return;
+    }
+}
+
+void ManualControl::runManualControl() {
+    int8_t x = Ps3.data.analog.stick.lx;
+    int8_t y = Ps3.data.analog.stick.ly;
+
+    double value_per_step = this->max_speed * 2 / 256;
+    this->moveControl->setSpeed((y * -1) * value_per_step);
+
+    value_per_step = this->max_rotation * 2 / 256;
+    this->moveControl->setRotationspeed(-x * value_per_step);
+}

src/driveModi/Modi/ManualControl/manualControl.h

@@ -0,0 +1,91 @@
+/**
+ * @file manualControl.h
+ * @author Alexander Klein (alex@kleiax.de)
+ * @brief A small class to drive the Rover by the controller joystick.
+ * @version 0.1
+ * @date 2021-12-13
+ * 
+ * @copyright Copyright (c) 2021
+ * 
+ */
+#ifndef MANUAL_CONTROL_H
+#define MANUAL_CONTROL_H
+
+#include <Ps3Controller.h>
+
+#include "moveControl.h"
+#include "driveModi/driveModi.h"
+
+/**
+ * @brief Drive the Rover with a Joystick
+ * 
+ * This class gets the x and y value from the PS3 controller
+ * and map the values to moveControl
+ * 
+ * @see MoveControl
+ */
+class ManualControl : public DriveModi{
+    public:
+        ManualControl();
+
+        /**
+         * @brief Destroy the Manual Control object
+         * Set in moveControl speed and rotation to 0 and set DrivingStatus::stop
+         */
+        ~ManualControl();
+
+        /**
+         * @brief Initalize ManualControl
+         * 
+         * set DrivingStatus::drive
+         * 
+         * @param moveControl 
+         */
+        void init(MoveControl *moveControl);
+
+        /**
+         * @brief Calls runManualControl() to update all values.
+         * 
+         * This function should be called every mainloop. If the delay is not reached, than the
+         * functions returns immediately.
+         * @see runSpeedometer()
+         * @see setDelay()
+         */
+        void loop();
+
+        /**
+         * @brief Noramly called repeatedly by loop() to calcluate new values.
+         * Set new values for speed and rotation in moveControl
+         */
+        void runManualControl();
+
+        /**
+         * @brief Set the min delay between each loop
+         * 
+         * @param delay time in Milliseconds
+         */
+        void setDelay(uint8_t delay_) { delay = delay_; }
+
+        /**
+         * @brief Set the max speed
+         * 
+         * @param maxSpeed in m/s 
+         */
+        void setMaxSpeed(double maxSpeed) { max_speed = maxSpeed; }
+        
+        /**
+         * @brief Set the max rotation
+         * 
+         * @param maxRotation in rad/s (maybe) 
+         */
+        void setMaxRotation(double maxRotation) { max_rotation = maxRotation; }
+
+    private:
+        MoveControl *moveControl;
+
+        uint8_t delay = 10;
+        double max_speed = 1;
+        double max_rotation = 7;
+};
+
+#endif // MANUAL_CONTROL_H