moving files

lib/Speedometer/speedometer.cpp

@@ -0,0 +1,114 @@
+/**
+ * @file speedometer.cpp
+ * @author Alexander Klein (alex@kleiax.de)
+ * @brief Implemention of the class speedometer.h
+ * @see speedometer.h
+ * @version 0.1
+ * @date 2021-12-13
+ * 
+ * @copyright Copyright (c) 2021
+ * 
+ */
+#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_SPEEDOMETER 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_SPEEDOMETER
+         */
+        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_SPEEDOMETER;
+
+        int16_t *buf;
+};
+
+#endif // SPEEDOMETER_H