restructerd projekt

.gitignore

@@ -4,4 +4,6 @@
 .vscode/launch.json
 .vscode/ipch
 
-workspace.code-workspace
+.debugMqtt/doxygen
+
+workspace.code-workspace

.vscode/settings.json

@@ -59,5 +59,9 @@
         "**/.next/**"
     ],
     "todohighlight.maxFilesForSearch": 5120,
-    "todohighlight.toggleURI": false
+    "todohighlight.toggleURI": false,
+
+
+    "doxdocgen.generic.authorEmail": "alex@kleiax.de",
+    "doxdocgen.generic.authorName": "Alexander Klein"
 }

doc/Notes and TODOs/TODO allgemein.txt

@@ -0,0 +1,6 @@
+Meine Libs von andren Abhängigkeiten lösen
+    - Die loop funmktions mit eigenem Timer oder ohne Timer
+    - Alle config.h und nicht allgemeinen Header entfernen
+
+Speedometer:
+    - Mitrechnen durchschnittliche aufrufzeit

include/README

@@ -1,39 +0,0 @@
-
-This directory is intended for project header files.
-
-A header file is a file containing C declarations and macro definitions
-to be shared between several project source files. You request the use of a
-header file in your project source file (C, C++, etc) located in `src` folder
-by including it, with the C preprocessing directive `#include'.
-
-```src/main.c
-
-#include "header.h"
-
-int main (void)
-{
- ...
-}
-```
-
-Including a header file produces the same results as copying the header file
-into each source file that needs it. Such copying would be time-consuming
-and error-prone. With a header file, the related declarations appear
-in only one place. If they need to be changed, they can be changed in one
-place, and programs that include the header file will automatically use the
-new version when next recompiled. The header file eliminates the labor of
-finding and changing all the copies as well as the risk that a failure to
-find one copy will result in inconsistencies within a program.
-
-In C, the usual convention is to give header files names that end with `.h'.
-It is most portable to use only letters, digits, dashes, and underscores in
-header file names, and at most one dot.
-
-Read more about using header files in official GCC documentation:
-
-* Include Syntax
-* Include Operation
-* Once-Only Headers
-* Computed Includes
-
-https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html

include/moveControl.h

@@ -4,8 +4,8 @@
 #include <cstdint>
 #include <PID_v1.h>
 
-#include "hardware/motorControl.h"
+#include "motorControl.h"
-#include "hardware/speedometer.h"
+#include "speedometer"
 #include "config.h"
 #include "debugMqtt.h"
 #include "debugTimes.h"

lib/MotorControl/motorControl.cpp

@@ -0,0 +1,167 @@
+#include "motorControl.h"
+
+MotorControl::MotorControl() {}
+
+void MotorControl::init(uint8_t pwm_pin, uint8_t pwm_channel, uint8_t dir_1, uint8_t dir_2) {
+    this->pwm_pin = pwm_pin;
+    this->pwm_channel = pwm_channel;
+    this->dir_1 = dir_1;
+    this->dir_2 = dir_2;
+
+    pinMode(this->dir_1, OUTPUT);
+    pinMode(this->dir_2, OUTPUT);
+
+    digitalWrite(this->dir_1, LOW);
+    digitalWrite(this->dir_2, LOW);
+
+    ledcSetup(this->pwm_channel, PWMFREQ, PWMRES);
+    ledcAttachPin(this->pwm_pin, this->pwm_channel);
+    ledcWrite(this->pwm_channel, 0);
+}
+
+void MotorControl::loop() {
+    static uint32_t last_millis = 0;
+    uint32_t time = millis();
+
+    //Cancel if delay is not reached
+    if (time - last_millis < delay) {
+        return;
+    }
+    runMotorControl();
+    last_millis = time;
+}
+
+void MotorControl::runMotorControl() {
+    // Absolute difference between target_power and speed
+    uint8_t abs_difference = abs(this->target_power - this->power);
+
+    // Difference between target_power and speed
+    int16_t difference = this->target_power - this->power;
+
+    // Check that the target speed is close to 0 and that the abs_difference is lower than powersteps
+    if (abs(this->target_power) <  powersteps && abs_difference < powersteps) {
+        this->setRealPower(0);
+        return;
+    }
+
+    // Correct speed
+    if (abs_difference < powersteps) {
+        return;
+    }
+
+    // Positive or negative tagret speed
+    if (this->target_power >= 0) {
+        // Positive or negative speed
+        if (this->power >= 0) {
+            if (difference > 0) {
+                this->increasePower(powersteps);
+            } else {
+                this->increasePower(-powersteps);
+            }
+        } else {
+            this->increasePower(powersteps);
+        }
+
+    } else {
+        // Positive or negative speed
+        if (this->power >= 0) {
+            this->increasePower(-powersteps);
+        } else {
+            if (difference > 0) {
+                this->increasePower(powersteps);
+            } else {
+                this->increasePower(-powersteps);
+            }
+        }
+    }
+}
+
+void MotorControl::setTargetPower(int8_t power) {
+    if (power <= 100 && power >= -100) {
+        this->target_power = power;
+    } else {
+        char str[64];
+        sprintf(str, "Invalid Argument (power: %d) in setTargetPower!", power);
+    }
+}
+
+void MotorControl::stop() {
+    this->target_power = 0;
+}
+
+void MotorControl::emergencyStop() {
+    setRealPower(0);
+    while(1)
+}
+
+void MotorControl::toString() {
+    Serial.printf("Target power: %d, power: %d, direction: %d \n", this->target_power, this->power, this->direction);
+}
+
+int8_t MotorControl::getPower() {
+    return this->power;
+}
+
+int8_t MotorControl::getTargetPower() {
+    return this->target_power;
+}
+
+bool MotorControl::isTargetPowerReached() {
+    if (this->target_power == this->power)
+        return true;
+    return false;
+}
+
+bool MotorControl::isAccelerationPositive() {
+    if (power < target_power)
+        return true;
+    return false;
+}
+
+bool MotorControl::isAccelerationNegative() {
+    if (power > target_power)
+        return true;
+    return false;
+}
+
+void MotorControl::setRealPower(int8_t power) {
+    //TODO: Exceptionhandling
+    if (power <= 100 || power <= -100) {
+        this->power = power;
+    } else {
+        return;
+    }
+
+    if (power == 0) {
+        this->direction = 0;
+        digitalWrite(this->dir_1, LOW);
+        digitalWrite(this->dir_2, LOW);
+        ledcWrite(this->pwm_channel, 0);
+        return;
+    }
+
+    uint8_t pwm_val = map(abs(power), 0, 100, this->pwm_min, this->pwm_max);
+
+    if ((this->direction == 1 || this->direction == 0) && power < 0){  // new direction backward
+        this->direction = 2;
+        digitalWrite(this->dir_1, LOW);
+        digitalWrite(this->dir_2, HIGH);
+    } else if ((this->direction == 2 || this->direction == 0) && power > 0){  // new direction forward
+        this->direction = 1;
+        digitalWrite(this->dir_1, HIGH);
+        digitalWrite(this->dir_2, LOW);
+    }
+
+    ledcWrite(this->pwm_channel, pwm_val);
+}
+
+void MotorControl::increasePower(int8_t power) {
+    //TODO: Exceptionhandling
+    //TODO: make a stop befor a direction change
+    if (abs(power) > 2 * powersteps) {
+        Serial.println("Invalid Argument in MotorControl::increasePower");
+        return;
+    }
+
+    this->setRealPower(this->power + power);
+}

lib/MotorControl/motorControl.h

@@ -0,0 +1,86 @@
+/**
+ * @file motorControl.h
+ * @author Alexander Klein (alex@kleiax.de)
+ * @brief 
+ * @version 0.1
+ * @date 2021-12-09
+ * 
+ * @copyright Copyright (c) 2021
+ * 
+ */
+#ifndef MOTOR_CONTROL_H
+#define MOTOR_CONTROL_H
+
+#include <cstdint>
+#include <string>
+#include <Arduino.h>
+
+#define DELAY 10
+#define PWMFREQ 16000
+#define PWMRES 8
+#define POWERSTEPS 2       //  A total of 20 levels ( 100 / SPEED_STEPS ) * RUN_MOTOR_CONTROL_DELAY = 500ms
+#define PWMMIN 50
+#define PWMMAX 95         // Max 98% of 2^PWM_RES
+
+/**
+ * @brief 
+ * 
+ */
+class MotorControl {
+    public:
+        MotorControl();
+        void init(uint8_t pwm_pin, uint8_t pwm_channel, uint8_t dir_1, uint8_t dir_2);
+        void loop();
+        void runMotorControl();
+
+        /**
+         * @brief Set the minimum duty cycle
+         * 
+         * @param min duty cycle in percent
+         */
+        void setMinPwm(uint8_t min);
+
+        /**
+         * @brief Set the maximum duty cycle
+         * 
+         * @param max duty cycle in percent
+         */
+        void setMaxPwm(uint8_t max);
+
+        void setPowerSteps(uint8_t steps); // Min anfahrkurve einbauen 
+        void setTargetPower(int8_t power);
+        void setDelay(uint8_t delay); // Min anfahrkurve einbauen
+        void stop();
+
+        /**
+         * @brief 
+         * 
+         */
+        void emergencyStop();
+
+        int8_t getPower();
+        int8_t getTargetPower();
+        bool isTargetPowerReached();
+        bool isAccelerationPositive();
+        bool isAccelerationNegative();
+
+    private:
+        void setRealPower(int8_t power);
+        void increasePower(int8_t power);
+
+        int8_t target_power = 0;
+        int8_t power = 0;
+        uint8_t direction = 0; // 0 = stop, 1 = forward, 2 = backward
+
+        uint8_t pwm_pin;
+        uint8_t pwm_channel;
+        uint8_t pwm_min = PWMMIN;
+        uint8_t pwm_max = PWMMAX;
+        uint8_t dir_1;
+        uint8_t dir_2;
+        uint8_t delay = DELAY;
+        uint8_t powersteps = POWERSTEPS;
+        
+};
+
+#endif // MOTOR_CONTROL_H

lib/README

@@ -1,46 +0,0 @@
-
-This directory is intended for project specific (private) libraries.
-PlatformIO will compile them to static libraries and link into executable file.
-
-The source code of each library should be placed in a an own separate directory
-("lib/your_library_name/[here are source files]").
-
-For example, see a structure of the following two libraries `Foo` and `Bar`:
-
-|--lib
-|  |
-|  |--Bar
-|  |  |--docs
-|  |  |--examples
-|  |  |--src
-|  |     |- Bar.c
-|  |     |- Bar.h
-|  |  |- library.json (optional, custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html
-|  |
-|  |--Foo
-|  |  |- Foo.c
-|  |  |- Foo.h
-|  |
-|  |- README --> THIS FILE
-|
-|- platformio.ini
-|--src
-   |- main.c
-
-and a contents of `src/main.c`:
-```
-#include <Foo.h>
-#include <Bar.h>
-
-int main (void)
-{
-  ...
-}
-
-```
-
-PlatformIO Library Dependency Finder will find automatically dependent
-libraries scanning project source files.
-
-More information about PlatformIO Library Dependency Finder
-- https://docs.platformio.org/page/librarymanager/ldf.html

lib/Speedometer/speedometer.cpp

@@ -0,0 +1,103 @@
+#include "speedometer.h"
+
+Speedometer::Speedometer() {}
+
+void Speedometer::init(uint8_t pinA, uint8_t pinB, double diameter, uint16_t steps, uint8_t numOfValForAvg) {
+    this->init(pinA, pinB, diameter, steps);
+    this->bufSize = numOfValForAvg;
+}
+
+void Speedometer::init(uint8_t pinA, uint8_t pinB, double diameter, uint16_t steps) {
+    ESP32Encoder::useInternalWeakPullResistors=DOWN;
+    this->encoder.attachFullQuad(pinA, pinB);
+    this->diameter = diameter;
+    this->steps = steps;
+
+    initAvgBuf();
+
+    this->isInit = true;
+}
+
+void Speedometer::loop() {
+    static uint32_t last_millis = 0;
+    uint32_t time = millis();
+
+    //Cancel if delay is not reached
+    if (time - last_millis < delay) {
+        return;
+    }
+    runSpeedometer();
+    last_millis = time;
+}
+
+void Speedometer::runSpeedometer() {
+    static uint32_t last_millis = 0;
+    uint32_t time = millis();
+
+    uint16_t elapsed_time = time - last_millis;
+    last_millis = time;
+
+    int16_t count = encoder.getCount();
+    this->addValToBuf(count);
+    encoder.clearCount();
+
+    uint16_t count_abs = abs(this->calcAverage());          // Absolute time in milliseconds
+    double n = (double)count_abs / steps;                   // Wheel revolutions in absolute time
+    double u = (double)n / ((double)elapsed_time / 1000);   // Wheel revolutions per second
+    double ms = u * (diameter * PI);                        // Speed in m/s
+
+    if (count > 0) {
+        this->speed = ms;
+    } else if (count < 0) {
+        this->speed = ms * (-1);
+    } else {
+        this->speed = 0;
+    }
+}
+
+void Speedometer::setNumOfValForAvg(uint8_t val) {
+    this->bufSize = val;
+    if (this->isInit) 
+        updateAvgBufSize();
+}
+
+void Speedometer::setEncFilter(uint16_t val) {
+    if (val > 1023) val = 1023;
+    this->encoder.setFilter(val);
+}
+
+void Speedometer::setDelay(uint8_t delay) {
+    this->delay = delay;
+}
+
+double Speedometer::getSpeed() {
+    return this->speed;
+}
+
+void Speedometer::initAvgBuf() {
+    this->buf = new int16_t[bufSize];
+    for (uint8_t i = 0; i < bufSize; i++) 
+        this->buf[i] = 0;
+}
+
+void Speedometer::addValToBuf(int16_t val) {
+    static uint8_t bufPos = 0;
+    this->buf[bufPos] = val;
+    bufPos++;
+
+    if (bufPos == bufSize) {
+        bufPos = 0;
+    }
+}
+
+void Speedometer::updateAvgBufSize() {
+    delete[] this->buf;
+    initAvgBuf();
+}
+
+int16_t Speedometer::calcAverage() {
+    int16_t sum = 0;
+    for (int i = 0; i < bufSize; i++)
+        sum += this->buf[i];
+    return sum / bufSize;
+}

lib/Speedometer/speedometer.h

@@ -0,0 +1,120 @@
+/**
+ * @file speedometer.h
+ * @author Alexander Klein (alex@kleiax.de)
+ * @brief A complete implementation to measure wheel speeds with an encoder.
+ * @version 0.1
+ * @date 2021-12-09
+ * 
+ * @copyright Copyright (c) 2021
+ * 
+ */
+
+#ifndef SPEEDOMETER_H
+#define SPEEDOMETER_H
+
+#include <cstdint>
+#include <ESP32Encoder.h>
+
+/**
+ * @brief The default size of numbers to be taken in account for the average.
+ * 
+ */
+#define BUFSIZE 10
+
+/**
+ * @brief Default value for min Millisseconds between each loop
+ * @see setDelay(uint8_t val)
+ */
+#define DELAY 30
+#define PI 3.1415926535897932384626433832795
+
+/**
+ * @brief A class which use a encoder to calc the speed
+ * 
+ * This class use ESP32 pulse counter hardware peripheral.
+ * The calclutaed speed is the average of an amount of last measurments.
+ * 
+ */
+class Speedometer {
+    public:
+        Speedometer();
+
+        /**
+         * @brief Initalize the speedometer
+         * 
+         * @param pinA Pin on the Esp from the encoder.
+         * @param pinB Pin on the Esp from the encoder.
+         * @param diameter Diameter of the wheel in meters.
+         * @param steps Encodersteps for a complete wheel rotation.
+         * @param numOfValForAvg Number of last values ​​to be taken into account for the average.
+         */
+        void init(uint8_t pinA, uint8_t pinB, double diameter, uint16_t steps, uint8_t numOfValForAvg);
+        void init(uint8_t pinA, uint8_t pinB, double diameter, uint16_t steps);
+
+        /**
+         * @brief Calls runSpeedometer() 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 DELAY
+         */
+        void loop();
+
+        /**
+         * @brief Noramly called repeatedly by loop to calcluate new values.
+         * 
+         * Add a new Value to the average and update the speed.
+         */
+        void runSpeedometer();
+
+        /**
+         * @brief Set the number of last values ​​to be taken into account for the average.
+         * 
+         * @param val length of the array
+         */
+        void setNumOfValForAvg(uint8_t val);
+
+        /**
+         * @brief Set the Enc Filter to prevent bouncing
+         * 
+         * @param val default = 250, max = 1023
+         */
+        void setEncFilter(uint16_t val);
+
+        /**
+         * @brief Set the min delay between each loop
+         * 
+         * @param val time in Milliseconds
+         */
+        void setDelay(uint8_t delay);
+
+        /**
+         * @brief Get the calculated speed of the Wheel
+         * 
+         * @return double speed in m/s
+         */
+        double getSpeed();
+
+
+    private:
+        void initAvgBuf();
+        void addValToBuf(int16_t val);
+        void updateAvgBufSize();
+        int16_t calcAverage();
+
+        ESP32Encoder encoder;
+
+        bool isInit = false;
+
+        double speed = 0;
+        double diameter;
+
+        uint8_t bufSize = BUFSIZE;
+        uint16_t steps;
+        uint8_t delay = DELAY;
+
+        int16_t *buf;
+};
+
+#endif // SPEEDOMETER_H

old/wheelEncoder.cpp

@@ -1,19 +0,0 @@
-#include "wheelEncoder.h"
-
-WheelEncoder::WheelEncoder() {
-
-}
-
-void WheelEncoder::initEncoder(uint8_t pinA, uint8_t pinB) {
-    this->pinA = pinA;
-    this->pinB = pinB;
-
-    pinMode(this->pinA, INPUT_PULLDOWN);
-    pinMode(this->pinB, INPUT_PULLDOWN);
-
-    
-}
-
-int64_t WheelEncoder::getCount() {
-    return this->count;
-}

old/wheelEncoder.h

@@ -1,24 +0,0 @@
-#ifndef WHEEL_ENCODER_H
-#define WHEEL_ENCODER_H
-
-#include <cstdint>
-#include <Arduino.h>
-#include <driver/gpio.h>
-#include <driver/pcnt.h>
-
-class WheelEncoder {
-    public:
-        WheelEncoder();
-        void initEncoder(uint8_t pinA, uint8_t pinB);
-        int64_t getCount();
-
-    private:
-        uint8_t pinA;
-        uint8_t pinB;
-        bool init = false;
-
-        volatile int64_t count = 0;
-
-};
-
-#endif // WHEEL_ENCODER_H

src/hardware/motorControl.cpp

@@ -1,183 +0,0 @@
-#include "motorControl.h"
-
-#include <Arduino.h>
-
-#include "config.h"
-
-MotorControl::MotorControl() {
-    this->debug = new DebugMqtt("MotorControl");
-}
-
-void MotorControl::init(uint8_t pwm_pin, uint8_t pwm_channel, uint8_t direction_pin_1, uint8_t direction_pin_2, String name) {
-    debug->sendMsg(Loglevel::info, "Init...");
-    this->name = name;
-
-    this->pwm_pin = pwm_pin;
-    this->pwm_channel = pwm_channel;
-    this->direction_pin_1 = direction_pin_1;
-    this->direction_pin_2 = direction_pin_2;
-
-    pinMode(this->direction_pin_1, OUTPUT);
-    pinMode(this->direction_pin_2, OUTPUT);
-
-    digitalWrite(this->direction_pin_1, LOW);
-    digitalWrite(this->direction_pin_2, LOW);
-
-    ledcSetup(this->pwm_channel, PWM_FREQ, PWM_RES);
-    ledcAttachPin(this->pwm_pin, this->pwm_channel);
-    ledcWrite(this->pwm_channel, 0);
-
-    debug->sendMsg(Loglevel::info, "Init finished!");
-}
-
-void MotorControl::runMotorControl() {
-    if (millis() - this->last_millis > RUN_MOTOR_CONTROL_DELAY) {
-
-        // Absolute difference between target_power and speed
-        uint8_t abs_difference = abs(this->target_power - this->power);
-
-        // Difference between target_power and speed
-        int16_t difference = this->target_power - this->power;
-
-        // Check that the target speed is close to 0 and that the abs_difference is lower than POWER_STEPS
-        if (abs(this->target_power) <  POWER_STEPS && abs_difference < POWER_STEPS) {
-            this->setRealPower(0);
-            this->last_millis = millis();
-            return;
-        }
-
-        // Correct speed
-        if (abs_difference < POWER_STEPS) {
-            this->last_millis = millis();
-            return;
-        }
-
-        // Positive or negative tagret speed
-        if (this->target_power >= 0) {
-            // Positive or negative speed
-            if (this->power >= 0) {
-                if (difference > 0) {
-                    this->increasePower(POWER_STEPS);
-                } else {
-                    this->increasePower(-POWER_STEPS);
-                }
-            } else {
-                this->increasePower(POWER_STEPS);
-            }
-
-        } else {
-            // Positive or negative speed
-            if (this->power >= 0) {
-                this->increasePower(-POWER_STEPS);
-            } else {
-                if (difference > 0) {
-                    this->increasePower(POWER_STEPS);
-                } else {
-                    this->increasePower(-POWER_STEPS);
-                }
-            }
-        }
-        this->last_millis = millis();
-    }
-}
-
-void MotorControl::setTargetPower(int8_t power) {
-    if (power <= 100 && power >= -100) {
-        this->target_power = power;
-        char str[32];
-        sprintf(str, "target_power_%s", this->name.c_str());
-        debug->writeToInflux("motorControl", str, power);
-    } else {
-        char str[64];
-        sprintf(str, "Invalid Argument (power: %d) in setTargetPower!", power);
-        debug->sendMsg(Loglevel::error, str);
-    }
-}
-
-void MotorControl::stop() {
-    this->target_power = 0;
-    char str[32];
-    sprintf(str, "target_power_%s", this->name.c_str());
-    debug->writeToInflux("motorControl", str, this->target_power);
-}
-
-void MotorControl::emergencyStop() {
-    setRealPower(0);
-}
-
-void MotorControl::toString() {
-    Serial.printf("Target power: %d, power: %d, direction: %d \n", this->target_power, this->power, this->direction);
-}
-
-int8_t MotorControl::getPower() {
-    return this->power;
-}
-
-int8_t MotorControl::getTargetPower() {
-    return this->target_power;
-}
-
-bool MotorControl::isTargetPowerReached() {
-    if (this->target_power == this->power)
-        return true;
-    return false;
-}
-
-bool MotorControl::isAccelerationPositive() {
-    if (power < target_power)
-        return true;
-    return false;
-}
-
-bool MotorControl::isAccelerationNegative() {
-    if (power > target_power)
-        return true;
-    return false;
-}
-
-void MotorControl::setRealPower(int8_t power) {
-    //TODO: Exceptionhandling
-    if (power <= 100 || power <= -100) {
-        this->power = power;
-    } else {
-        debug->sendMsg(Loglevel::error, "Invalid Argument in setRealPower!");
-        return;
-    }
-
-    if (power == 0) {
-        this->direction = 0;
-        digitalWrite(this->direction_pin_1, LOW);
-        digitalWrite(this->direction_pin_2, LOW);
-        ledcWrite(this->pwm_channel, 0);
-        return;
-    }
-
-    uint8_t pwm_val = map(abs(power), 0, 100, PWM_MIN, PWM_MAX);
-
-    if ((this->direction == 1 || this->direction == 0) && power < 0){  // new direction backward
-        this->direction = 2;
-        digitalWrite(this->direction_pin_1, LOW);
-        digitalWrite(this->direction_pin_2, HIGH);
-    } else if ((this->direction == 2 || this->direction == 0) && power > 0){  // new direction forward
-        this->direction = 1;
-        digitalWrite(this->direction_pin_1, HIGH);
-        digitalWrite(this->direction_pin_2, LOW);
-    }
-
-    ledcWrite(this->pwm_channel, pwm_val);
-    char str[32];
-    sprintf(str, "real_power_%s", this->name.c_str());
-    debug->writeToInflux("motorControl", str, power);
-    // Serial.printf("pwm_val: %d, ", pwm_val);
-}
-
-void MotorControl::increasePower(int8_t power) {
-    //TODO: Exceptionhandling
-    //TODO: make a stop befor a direction change
-    if (abs(power) > 2 * POWER_STEPS) {
-        Serial.println("Invalid Argument in MotorControl::increasePower");
-        return;
-    }
-
-    this->setRealPower(this->power + power);
-}

src/hardware/motorControl.h

@@ -1,45 +0,0 @@
-#ifndef MOTOR_CONTROL_H
-#define MOTOR_CONTROL_H
-
-#include <cstdint>
-#include <string>
-
-#include "debugMqtt.h"
-
-class MotorControl {
-    public:
-        MotorControl();
-        void init(uint8_t pwm_pin, uint8_t pwm_channel, uint8_t direction_1, uint8_t direction_2, String name);
-        void runMotorControl();
-        void setTargetPower(int8_t power);
-        void stop();
-        void emergencyStop();
-        void toString();
-
-        int8_t getPower();
-        int8_t getTargetPower();
-        bool isTargetPowerReached();
-        bool isAccelerationPositive();
-        bool isAccelerationNegative();
-
-    private:
-        void setRealPower(int8_t power);
-        void increasePower(int8_t power);
-
-        String name;
-
-        DebugMqtt *debug;
-
-        int8_t target_power = 0;
-        int8_t power = 0;
-        uint8_t direction = 0; // 0 = stop, 1 = forward, 2 = backward
-
-        uint64_t last_millis = 0;
-
-        uint8_t pwm_pin;
-        uint8_t pwm_channel;
-        uint8_t direction_pin_1;
-        uint8_t direction_pin_2;
-};
-
-#endif // MOTOR_CONTROL_H

src/hardware/speedometer.cpp

@@ -1,92 +0,0 @@
-#include "speedometer.h"
-
-Speedometer::Speedometer() {
-    for (int i = 0; i < BUF_SIZE; i++) {
-        this->buf[i] = 0;
-    }
-    this->debug = new DebugMqtt("Speedometer");
-}
-
-void Speedometer::init(uint8_t pinA, uint8_t pinB, String name) {
-    debug->sendMsg(Loglevel::info, "Init...");
-    this->name = name;
-    ESP32Encoder::useInternalWeakPullResistors=DOWN;
-    this->encoder.attachFullQuad(pinA, pinB);
-    this->encoder.setFilter(ENC_FILTER);
-    debug->sendMsg(Loglevel::info, "Init finished!");
-}
-
-void Speedometer::runSpeedometer() {
-    uint32_t time = millis();
-
-    //Cancel if delay is not reached
-    if (time - this->last_millis < RUN_SPEEDOMETER_DELAY) {
-        return;
-    }
-
-    uint16_t elapsed_time = time - this->last_millis;
-    this->last_millis = time;
-
-    int16_t count = encoder.getCount();
-    this->addValToBuf(count);
-    encoder.clearCount();
-
-    uint8_t direction = STOP;
-    if (count > 0) {
-        direction = FORWARD;
-    } else if (count < 0) {
-        direction = BACKWARD;
-    }
-
-    count = abs(this->getAverage());                                     // Absolute time in milliseconds
-    double n = (double)count / ENC_STEPS;                           // Wheel revolutions in absolute time
-    double u = (double)n / ((double)elapsed_time / 1000);   // Wheel revolutions per second
-    double ms = u * (WHEEL_DIAMETER * PI);                // Speed in m/s
-
-    if (direction == FORWARD) {
-        this->speed = ms;
-    } else if (direction == BACKWARD) {
-        this->speed = ms * (-1);
-    } else {
-        this->speed = 0;
-    }
-    char str[32];
-    sprintf(str, "speed_%s", this->name.c_str());
-    debug->writeToInflux("speedometer", str, this->speed);
-
-    // Serial.printf("time: %d, ", time);
-    // Serial.printf("count: %d, ",count);
-    // Serial.printf("n: %f, u: %f, ms: %f \n", n, u, ms);
-}
-
-double Speedometer::getSpeed() {
-    return this->speed;
-}
-
-uint8_t Speedometer::getDirection() {
-    if (this->speed > 0) {
-        return FORWARD;
-    } else if (this->speed < 0) {
-        return BACKWARD;
-    } else {
-        return STOP;
-    }
-}
-
-void Speedometer::addValToBuf(int16_t val) {
-    this->buf[this->bufPos] = val;
-    this->bufPos++;
-
-    if (this->bufPos == BUF_SIZE) {
-        this->bufPos = 0;
-    }
-}
-
-int16_t Speedometer::getAverage() {
-    int16_t sum = 0;
-    for (int i = 0; i < BUF_SIZE; i++) {
-        sum += this->buf[i];
-    }
-
-    return sum / BUF_SIZE;
-}

src/hardware/speedometer.h

@@ -1,35 +0,0 @@
-#ifndef SPEEDOMETER_H
-#define SPEEDOMETER_H
-
-#include <cstdint>
-#include <ESP32Encoder.h>
-#include <math.h>
-
-#include "config.h"
-#include "debugMqtt.h"
-
-class Speedometer {
-    public:
-        Speedometer();
-        void init(uint8_t pinA, uint8_t pinB, String name);
-        void runSpeedometer();
-        double getSpeed();
-        uint8_t getDirection();
-
-
-    private:
-        void addValToBuf(int16_t val);
-        int16_t getAverage();
-
-        ESP32Encoder encoder;
-        DebugMqtt *debug;
-
-        String name;
-        double speed = 0;
-        uint32_t last_millis = 0;
-
-        uint8_t bufPos = 0;
-        int16_t buf[BUF_SIZE];
-};
-
-#endif // SPEEDOMETER_H