a lot of bullshit

src/driveModi/CaptureRoute/captureRoute.h

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

src/driveModi/ManualControl/manualControl.cpp

@@ -1,125 +0,0 @@
-#include "manualControl.h"
-
-ManualControl::ManualControl() {
-    this->debug = new DebugMqtt("ManualControl");
-}
-
-void ManualControl::init(MoveControl *moveControl, MotorControl *motorControlLeft, MotorControl *motorControlRight) {
-    debug->sendMsg(Loglevel::info, "Init...");
-
-    this->moveControl = moveControl;
-    this->motorControlLeft = motorControlLeft;
-    this->motorControlRight = motorControlRight;
-
-    debug->sendMsg(Loglevel::info, "Init finished!");
-}
-
-void ManualControl::runManualControl() {
-    //Cancel if delay is not reached
-    if (millis() - this->last_millis < RUN_MANUALCONTROL_DELAY) {
-        return;
-    }
-    //Reset if this function was called a long time ago
-    if (millis() - this->last_millis > 1000) {
-        this->reset();
-        controlMode = ControMode::halt;
-        debug->sendMsg(Loglevel::info, "Changed ControlMode to halt by Timereset");
-    }
-    this->last_millis = millis();
-
-    //Change controlMode
-    if (this->controlMode == ControMode::halt && Ps3.data.button.l3) {
-        this->reset();
-        controlMode = ControMode::joystick;
-        debug->sendMsg(Loglevel::info, "Changed ControlMode to Joystick by Gamepad");
-    } else if (this->controlMode == ControMode::halt && Ps3.data.button.r3) {
-        this->reset();
-        controlMode = ControMode::trigger;
-        debug->sendMsg(Loglevel::info, "Changed ControlMode to ShoulderTrigger by Gamepad");
-    } else if (this->controlMode == ControMode::joystick && Ps3.data.button.r3) {
-        this->reset();
-        controlMode = ControMode::trigger;
-        debug->sendMsg(Loglevel::info, "Changed ControlMode to ShoulderTrigger by Gamepad");
-    } else if (this->controlMode == ControMode::trigger && Ps3.data.button.l3) {
-        this->reset();
-        controlMode = ControMode::joystick;
-        debug->sendMsg(Loglevel::info, "Changed ControlMode to Joystick by Gamepad");
-    }
-
-    switch (this->controlMode) {
-        case ControMode::halt:
-            this->moveControl->setDrivingStatus(DrivingStatus::stop);
-            break;
-
-        case ControMode::joystick: {
-            // DebugTimes driveWithJoyDebug;
-            this->driveWithControllerJoystick();
-            // driveWithJoyDebug.stop("driveWithControllerJoystick", 100);
-
-            // DebugTimes moveControllDebug;
-            this->moveControl->runMoveControl();
-            // moveControllDebug.stop("moveControl", 100);
-            }
-            break;
-
-        case ControMode::trigger:
-            this->driveWithControllerShoulderTrigger();
-            break;
-
-        default:
-            break;
-    }
-}
-
-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);
-    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);
-}
-
-void ManualControl::changeDriveMode(ControMode drive_mode) {
-    this->reset();
-    controlMode = drive_mode;
-    debug->sendMsg(Loglevel::info, "Changed ControlMode by function");
-}
-
-void ManualControl::driveWithControllerShoulderTrigger() {
-    uint8_t nowL = Ps3.data.analog.button.l2;
-    uint8_t nowR = Ps3.data.analog.button.r2;;
-
-    nowL = map(nowL, 0, 255, 0, 100);
-    nowR = map(nowR, 0, 255, 0, 100);
-
-    // char str[64];
-    // sprintf(str, "nachher: nowL: %u, nowR: %u", nowL, nowR);
-    // debug->sendMsg(Loglevel::debug, str);
-
-    if (nowL != this->oldL) {
-        this->motorControlLeft->setTargetPower(nowL);
-        this->oldL = nowL;
-    }
-
-    if (nowR != this->oldR) {
-        this->motorControlRight->setTargetPower(nowR);
-        this->oldR = nowR;
-    }
-}
-
-void ManualControl::reset() {
-    this->oldL = 0;
-    this->oldR = 0;
-    motorControlLeft->setTargetPower(0);
-    motorControlRight->setTargetPower(0);
-    moveControl->setRotationspeed(0);
-    moveControl->setSpeed(0);
-}

src/driveModi/ManualControl/manualControl.h

@@ -1,37 +0,0 @@
-#ifndef MANUAL_CONTROL_H
-#define MANUAL_CONTROL_H
-
-#include <Ps3Controller.h>
-
-#include "moveControl.h"
-#include "motorControl.h"
-#include "debugMqtt.h"
-#include "debugTimes.h"
-
-enum ControMode {halt, joystick, trigger};
-class ManualControl {
-    public:
-        ManualControl();
-        void init(MoveControl *moveControl, MotorControl *motorControlLeft, MotorControl *motorControlRight);
-        void runManualControl();
-        void changeDriveMode(ControMode drive_mode);
-
-    private:
-        void driveWithControllerJoystick();
-        void driveWithControllerShoulderTrigger();
-        void reset();
-
-        MoveControl *moveControl;
-        MotorControl *motorControlLeft;
-        MotorControl *motorControlRight;
-        DebugMqtt *debug;
-
-        ControMode controlMode = ControMode::halt;
-        uint32_t last_millis = 0;
-
-        // Is needed for driveWithControllerShoulderTrigger()
-        uint8_t oldL = 0;
-        uint8_t oldR = 0;
-};
-
-#endif // MANUAL_CONTROL_H

src/driveModi/Modi/Autopilot/autopilot.cpp

@@ -1,4 +1,4 @@
-#include "driveModi/autopilot.h"
+#include "driveModi/Modi/Autopilot/autopilot.h"
 
 Autopilot::Autopilot() {
     this->debug = new DebugMqtt("Autopilot");

src/driveModi/Modi/Autopilot/autopilot.h

@@ -6,12 +6,14 @@
 #include "route.h"
 #include "moveControl.h"
 #include "debugMqtt.h"
+#include "driveModi/driveModi.h"
 
-class Autopilot {
+class Autopilot : DriveModi{
     public:
         Autopilot();
 
         void init(Route *route, MoveControl *moveControl);
+        void loop();
         void runAutopilot();
 
 

src/driveModi/Modi/CaptureRoute/captureRoute.cpp

@@ -1,12 +1,10 @@
-#include "driveModi/captureRoute.h"
+#include "driveModi/Modi/CaptureRoute/captureRoute.h"
 
 CaptureRoute::CaptureRoute() {
-    this->debug = new DebugMqtt("CaptureRoute");
 }
 
-void CaptureRoute::init(Route *route, MoveControl *moveControl, MotorControl *motorControlLeft, MotorControl *motorControlRight) {
-    this->route = route;
-    ManualControl::init(moveControl, motorControlLeft, motorControlRight);
+void CaptureRoute::init() {
+    
     
 }
 

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.h

@@ -4,9 +4,10 @@
 #include <string>
 
 #include "moveControl.h"
+#include "driveModi/driveModi.h"
 #include "debugMqtt.h"
 
-class ConsolControl {
+class ConsolControl : DriveModi{
     public:
         ConsolControl();
         void init(MoveControl *moveControl);

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

src/driveModi/driveManager.cpp

@@ -0,0 +1,72 @@
+/**
+ * @file driveManager.cpp
+ * @author Alexander Klein (alex@kleiax.de)
+ * @brief Implemention of the class driveManager.h.
+ * @version 0.1
+ * @date 2021-12-14
+ * 
+ * @copyright Copyright (c) 2021
+ * 
+ */
+
+#include "driveModi/driveManager.h"
+
+DriveManager::DriveManager(MoveControl *moveControl) {
+    this->moveControl = moveControl;
+}
+
+void DriveManager::loop() {
+    this->currentModus->loop();
+    this->moveControl->loop();
+}
+
+void DriveManager::runDriveManager() {
+    
+}
+
+void DriveManager::nextModus() {
+    
+}
+
+void DriveManager::changeModus(Modi modus) {
+
+    //Set moveControl to a safe state
+    this->moveControl->setSpeed(0);
+    this->moveControl->setRotationspeed(0);
+    this->moveControl->setDrivingStatus(DrivingStatus::stop);
+    delete this->currentModus;
+
+    switch (modus) {
+        case Modi::Off:
+            this->currentModus = nullptr;
+            break;
+
+        case Modi::ManualControl: {
+            ManualControl *ptr = new ManualControl;
+            ptr->init(this->moveControl);
+            this->currentModus = ptr;
+            }
+            break;
+
+        case Modi::CaptureRoute:
+            this->currentModus = nullptr;
+            break;
+
+        case Modi::Autopilot:
+            this->currentModus = nullptr;
+            break;
+
+        case Modi::ConsolControl:
+            this->currentModus = nullptr;
+            break;
+
+        case Modi::TestMode:
+            this->currentModus = nullptr;
+            break;
+        
+        default:
+            this->currentModus = nullptr;
+            break;
+    }
+}
+

src/driveModi/driveManager.h

@@ -0,0 +1,51 @@
+/**
+ * @file driveManager.h
+ * @author Alexander Klein (alex@kleiax.de)
+ * @brief Contains a class to switch the DriveModi
+ * @version 0.1
+ * @date 2021-12-14
+ * 
+ * @copyright Copyright (c) 2021
+ * 
+ */
+
+#ifndef DRIVE_MANAGER_H
+#define DRIVE_MANAGER_H
+
+#include "moveControl.h"
+#include "driveModi/driveModi.h"
+
+// All Drive Modi
+#include "driveModi/Modi/ManualControl/manualControl.h"
+#include "driveModi/Modi/CaptureRoute/captureRoute.h"
+#include "driveModi/Modi/Autopilot/autopilot.h"
+#include "driveModi/Modi/ConsolControl/consolControl.h"
+#include "driveModi/Modi/TestMode/testMode.h"
+
+enum class Modi {   
+                    Off,
+                    ManualControl,
+                    CaptureRoute,
+                    Autopilot,
+                    ConsolControl,
+                    TestMode
+                };
+
+class DriveManager {
+    public:
+        DriveManager(MoveControl *moveControl);
+
+        void loop();
+        void runDriveManager();
+
+        void nextModus();
+        void changeModus(Modi modus);
+
+    private:
+        MoveControl *moveControl;
+        DriveModi *currentModus;
+
+};
+
+
+#endif // DRIVE_MANAGER_H

src/driveModi/driveModi.h

@@ -0,0 +1,22 @@
+/**
+ * @file driveModi.h
+ * @author Alexander Klein (alex@kleiax.de)
+ * @brief Contains a virtual class for all Modi
+ * @version 0.1
+ * @date 2021-12-14
+ * 
+ * @copyright Copyright (c) 2021
+ * 
+ */
+
+#ifndef DRIVEMODI_H
+#define DRIVEMODI_H
+
+class DriveModi {
+    public:
+        DriveModi(){}
+        virtual ~DriveModi(){}
+        virtual void loop() = 0;
+        
+};
+#endif // DRIVEMODI_H