131 lines
3.2 KiB
C++
131 lines
3.2 KiB
C++
/**
|
|
* @file speedometer.cpp
|
|
* @author Alexander Klein (alex@kleiax.de)
|
|
* @brief Implementation of the class speedometer.h
|
|
* @see speedometer.h
|
|
* @version 0.1
|
|
* @date 2021-12-13
|
|
*
|
|
* @copyright Copyright (c) 2021
|
|
*
|
|
*/
|
|
#include "speedometer.h"
|
|
|
|
Speedometer::Speedometer(uint8_t pin, double diameter, uint16_t steps) {
|
|
this->diameter = diameter;
|
|
this->steps = steps;
|
|
|
|
this->pulseCounter = new Counter(pin);
|
|
this->pulseCounter->setFilterValue(1023); // ignore pulses less than 1000 x 2.5ns
|
|
|
|
this->pulseCounter->clear();
|
|
this->pulseCounter->resume();
|
|
|
|
Component::loopDelay = Speedometer::loopDelay;
|
|
|
|
this->clearAvgBuf();
|
|
}
|
|
|
|
Speedometer::~Speedometer() {
|
|
delete this->pulseCounter;
|
|
}
|
|
|
|
void Speedometer::run() {
|
|
if (this->calibrationRunning)
|
|
return;
|
|
|
|
uint32_t time = millis();
|
|
|
|
uint16_t elapsedTime = time - this->lastMillisCalc;
|
|
this->lastMillisCalc = time;
|
|
|
|
int16_t pulse = this->pulseCounter->getValue();
|
|
this->pulseCounter->clear();
|
|
this->pulseCounter->resume();
|
|
|
|
|
|
double n = (double)pulse / this->steps; // Wheel revolutions in absolute time
|
|
double u = n / ((double)elapsedTime / 1000); // Wheel revolutions per second
|
|
double ms = u * (diameter * PI); // Speed in m/s
|
|
double rad = u * 2 * PI;
|
|
|
|
if (speed < 0.1) {
|
|
speed = 0;
|
|
rad = 0;
|
|
}
|
|
|
|
switch (this->currentDirection) {
|
|
case Direction::Forward :
|
|
this->speed = ms;
|
|
this->rad = rad;
|
|
break;
|
|
|
|
case Direction::Backward :
|
|
this->speed = -ms;
|
|
this->rad = -rad;
|
|
break;
|
|
|
|
case Direction::None :
|
|
this->speed = 0;
|
|
this->rad = 0;
|
|
break;
|
|
}
|
|
|
|
this->addValToBuf(static_cast<int16_t>(this->speed * Speedometer::conversionFactor));
|
|
}
|
|
|
|
void Speedometer::setDirection(Direction dir) {
|
|
if (this->currentDirection == dir)
|
|
return;
|
|
this->currentDirection = dir;
|
|
this->clearAvgBuf();
|
|
}
|
|
|
|
void Speedometer::setEncFilter(uint16_t val) {
|
|
if (val > 1023)
|
|
val = 1023;
|
|
this->pulseCounter->setFilterValue(val);
|
|
}
|
|
|
|
double Speedometer::getAvgSpeed() const {
|
|
int16_t avg = this->calcAverage();
|
|
return (float)avg / Speedometer::conversionFactor;
|
|
}
|
|
|
|
void Speedometer::calibrationMeasurementStart() {
|
|
std::cout << "Start" << std::endl;
|
|
this->calibrationRunning = true;
|
|
this->pulseCounter->clear();
|
|
this->pulseCounter->resume();
|
|
}
|
|
|
|
uint16_t Speedometer::calibrationMeasurementStop() {
|
|
std::cout << "Ende" << std::endl;
|
|
this->calibrationRunning = false;
|
|
uint16_t res = abs(this->pulseCounter->getValue());
|
|
this->pulseCounter->clear();
|
|
this->pulseCounter->resume();
|
|
std::cout << "Result: " << res << std::endl;
|
|
return res;
|
|
}
|
|
|
|
void Speedometer::clearAvgBuf() {
|
|
for (uint8_t i = 0; i < bufSize; i++)
|
|
this->buf[i] = 0;
|
|
}
|
|
|
|
void Speedometer::addValToBuf(int16_t val) {
|
|
this->buf[this->bufPos] = val;
|
|
this->bufPos++;
|
|
|
|
if (bufPos == bufSize)
|
|
bufPos = 0;
|
|
}
|
|
|
|
int16_t Speedometer::calcAverage() const {
|
|
int16_t sum = 0;
|
|
for (int i = 0; i < this->bufSize; i++)
|
|
sum += this->buf[i];
|
|
return sum / this->bufSize;
|
|
}
|