clean main.cpp, clean moveControl.cpp

added classdiagramm and ManualControl

src/driveModi/manualControl.cpp

@@ -0,0 +1,83 @@
+#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;
+    }
+    this->last_millis = millis();
+
+    switch (this->drive_mode) {
+        case DriveMode::stop:
+            this->reset();
+            break;
+
+        case DriveMode::joystick:
+            this->driveWithControllerJoystick();
+            this->moveControl->runMoveControl();
+            break;
+
+        case DriveMode::trigger:
+            this->driveWithControllerShoulderTrigger();
+            break;
+
+        default:
+            break;
+    }
+}
+
+void ManualControl::driveWithControllerJoystick() {
+    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);
+
+    value_per_step = MANUALCONTROL_MAX_ROTATION * 2 / 256;
+    this->moveControl->setRotationspeed(x * value_per_step);
+}
+
+void ManualControl::changeDriveMode(DriveMode drive_mode) {
+    this->drive_mode = drive_mode;
+}
+
+void ManualControl::driveWithControllerShoulderTrigger() {
+    uint8_t nowL = Ps3.data.analog.button.l2;
+    uint8_t nowR = Ps3.data.analog.button.r2;
+
+    map(nowL, 0, 255, 0, 100);
+    map(nowR, 0, 255, 0, 100);
+
+    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.h

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