155 lines
3.4 KiB
C++
155 lines
3.4 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)
|
|
: pulseCounter{new Counter(pin)}, diameter{diameter}, steps{steps}, buf{}
|
|
{
|
|
this->pulseCounter->setFilterValue(Speedometer::maxFilterValue); // 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()
|
|
{
|
|
static constexpr float minimalSpeed = 0.1;
|
|
|
|
if (this->calibrationRunning)
|
|
{
|
|
return;
|
|
}
|
|
|
|
const uint32_t time = millis();
|
|
|
|
const uint16_t elapsedTime = time - this->lastMillisCalc;
|
|
this->lastMillisCalc = time;
|
|
|
|
const double pulse = this->pulseCounter->getValue();
|
|
this->pulseCounter->clear();
|
|
this->pulseCounter->resume();
|
|
|
|
const double wheelRevolutionsAbsolute = pulse / this->steps;
|
|
const double wheelRevolutionsRelativ = wheelRevolutionsAbsolute / (elapsedTime / 1000.0);
|
|
|
|
double meterPerSecond = wheelRevolutionsRelativ * (diameter * PI);
|
|
double radPerSecond = wheelRevolutionsRelativ * 2 * PI;
|
|
|
|
if (meterPerSecond < minimalSpeed)
|
|
{
|
|
meterPerSecond = 0;
|
|
radPerSecond = 0;
|
|
}
|
|
|
|
switch (this->currentDirection)
|
|
{
|
|
case Direction::Forward:
|
|
this->speed = meterPerSecond;
|
|
this->rad = radPerSecond;
|
|
break;
|
|
|
|
case Direction::Backward:
|
|
this->speed = -meterPerSecond;
|
|
this->rad = -radPerSecond;
|
|
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 > Speedometer::maxFilterValue)
|
|
{
|
|
val = Speedometer::maxFilterValue;
|
|
}
|
|
this->pulseCounter->setFilterValue(val);
|
|
}
|
|
|
|
double Speedometer::getAvgSpeed() const
|
|
{
|
|
const double avg = this->calcAverage();
|
|
return 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;
|
|
const 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 < Speedometer::bufSize; i++)
|
|
{
|
|
sum += this->buf[i];
|
|
}
|
|
return sum / Speedometer::bufSize;
|
|
}
|