restructerd projekt

lib/Debug/MQTT/debugMqtt.cpp

@@ -0,0 +1,102 @@
+#include "debugMqtt.h"
+
+PubSubClient* DebugMqtt::client;
+Loglevel DebugMqtt::loglevel;
+bool DebugMqtt::isInit = false;
+char DebugMqtt::msg[MQTT_BUFFER_SITE];
+char DebugMqtt::topic[MQTT_BUFFER_SITE];
+unsigned long long int DebugMqtt::real_millis = 0;
+unsigned long int DebugMqtt::last_millis = 0;
+
+
+DebugMqtt::DebugMqtt(const char* name) {
+    this->name = name;
+}
+
+void DebugMqtt::sendMsg(Loglevel loglevel, String topic, String msg) {
+    snprintf (DebugMqtt::msg, MQTT_BUFFER_SITE, "%s: %s",this->name ,msg.c_str());
+    this->sendData(loglevel, topic, DebugMqtt::msg);
+}
+
+void DebugMqtt::sendMsg(Loglevel loglevel, String msg) {
+    this->sendMsg(loglevel, "", msg);
+}
+
+void DebugMqtt::sendData(Loglevel loglevel, String topic, String data) {
+    if (!DebugMqtt::isInit) {return;}
+
+    if (loglevel <= DebugMqtt::loglevel && loglevel > Loglevel::none) {
+        snprintf (DebugMqtt::topic, MQTT_BUFFER_SITE, "%s%s", DebugMqtt::enum_to_string(loglevel).c_str(), topic.c_str());
+        snprintf (DebugMqtt::msg, MQTT_BUFFER_SITE, "%s", data.c_str());
+        client->publish(DebugMqtt::topic, DebugMqtt::msg);
+    }
+}
+
+void DebugMqtt::sendData(Loglevel loglevel, String data){
+    this->sendData(loglevel, "", data);
+}
+
+void DebugMqtt::writeToInflux(String measurement_name, String field_set, float measurement) {
+    // Example String: "weather temperature=82 1465839830100400200";
+
+    unsigned long long int nanos = DebugMqtt::getUpdatedRealMillis() * 1000000;
+    snprintf(DebugMqtt::msg, MQTT_BUFFER_SITE, "%s %s=%f %llu", measurement_name.c_str(), field_set.c_str(), measurement, nanos);
+    this->sendData(Loglevel::influx, DebugMqtt::msg);
+}
+
+void DebugMqtt::sendTime() {
+    struct tm timeinfo;
+    if(!getLocalTime(&timeinfo)){
+        snprintf(DebugMqtt::msg, MQTT_BUFFER_SITE, "Failed to obtain time");
+    }
+    snprintf(DebugMqtt::msg, MQTT_BUFFER_SITE, "%d %d %d %d %d %d %llu", timeinfo.tm_year, timeinfo.tm_mon, timeinfo.tm_mday, timeinfo.tm_hour, timeinfo.tm_min, timeinfo.tm_sec, DebugMqtt::getUpdatedRealMillis());
+    this->sendMsg(Loglevel::info, DebugMqtt::msg);
+}
+
+void DebugMqtt::init(PubSubClient *client, Loglevel loglevel) {
+    DebugMqtt::client = client;
+    DebugMqtt::loglevel = loglevel;
+    DebugMqtt::isInit = true;
+}
+
+void DebugMqtt::changeLoglevel(Loglevel loglevel) {
+    DebugMqtt::loglevel = loglevel;
+}
+
+void DebugMqtt::initRealMillis() {
+    time_t now;
+    struct tm timeinfo;
+    if(!getLocalTime(&timeinfo)){
+        snprintf(DebugMqtt::msg, MQTT_BUFFER_SITE, "Failed to obtain time in initRealMillis()");
+        DebugMqtt::client->publish(DebugMqtt::enum_to_string(Loglevel::error).c_str(), DebugMqtt::msg);
+        return;
+    }
+    time(&now);
+    DebugMqtt::last_millis = millis();
+    DebugMqtt::real_millis = now;
+    DebugMqtt::real_millis = DebugMqtt::real_millis * 1000 + DebugMqtt::last_millis;
+}
+
+unsigned long long int DebugMqtt::getUpdatedRealMillis() {
+    DebugMqtt::real_millis = DebugMqtt::real_millis + (millis() - DebugMqtt::last_millis);
+    DebugMqtt::last_millis = millis();
+    return DebugMqtt::real_millis;
+}
+
+String DebugMqtt::enum_to_string(Loglevel loglevel) {
+      switch(loglevel){
+        case Loglevel::error :
+            return "Rover/Error";
+        case Loglevel::warn :
+            return "Rover/Warn";
+        case Loglevel::info :
+            return "Rover/Info";
+        case Loglevel::debug :
+            return "Rover/Debug";
+        case Loglevel::influx :
+            return "Rover/Influx";
+         default:
+            return "INVALID ENUM";
+      }
+ }
+

lib/Debug/MQTT/debugMqtt.h

@@ -0,0 +1,47 @@
+#ifndef DEBUG_MQTT_H
+#define DEBUG_MQTT_H
+
+#include <PubSubClient.h>
+
+#include "config.h"
+
+enum Loglevel { none,
+                error,
+                warn,
+                info,
+                debug,
+                influx};
+
+class DebugMqtt {
+    public:
+        DebugMqtt(const char* name);
+        void sendMsg(Loglevel loglevel, String topic, String msg);
+        void sendMsg(Loglevel loglevel, String msg);
+        void sendData(Loglevel loglevel, String topic, String data);
+        void sendData(Loglevel loglevel, String data);
+        void writeToInflux(String measurement_name, String field_set, float measurement);
+
+        void sendTime();
+
+        static void init(PubSubClient *client, Loglevel loglevel);
+        static void changeLoglevel(Loglevel loglevel);
+        static void initRealMillis();
+        static unsigned long long int getUpdatedRealMillis();
+
+    private:
+        const char* name;
+
+        static String enum_to_string(Loglevel loglevel);
+
+        static PubSubClient* client;
+        static Loglevel loglevel;
+        static bool isInit;
+        static char msg[MQTT_BUFFER_SITE];
+        static char topic[MQTT_BUFFER_SITE];
+
+        static unsigned long long int real_millis;
+        static unsigned long int last_millis;
+
+};
+
+#endif // DEBUG_MQTT_H

lib/Debug/Times/debugTimes.cpp

@@ -0,0 +1,11 @@
+#include "debugTimes.h"
+
+DebugTimes::DebugTimes() {
+    this->startTime = millis();
+}
+
+void DebugTimes::stop(const char* name, uint16_t minTime) {
+    uint64_t time = millis() - this->startTime;
+    if (time > minTime)
+        Serial.printf("%s needs %llu ms\n", name, time);
+}

lib/Debug/Times/debugTimes.h

@@ -0,0 +1,18 @@
+#ifndef DEBUG_TIMES_H
+#define DEBUG_TIMES_H
+
+#include <list>
+#include <cstdint>
+
+#include <Arduino.h>
+
+class DebugTimes {
+    public:
+        DebugTimes();
+        void stop(const char* name, uint16_t minTime = 0);
+
+    private:
+        uint64_t startTime;
+};
+
+#endif //DEBUG_TIMES_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