103 lines
2.4 KiB
C++
103 lines
2.4 KiB
C++
#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;
|
|
} |