kleiax/Bachelorarbeit-Rover
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases 1 Wiki Activity

fix speedometer for new encoder

Browse Source
This commit is contained in:
kleiax
2023-04-20 12:31:48 +02:00
parent 3fab7c7f83
commit 07f09d691f
4 changed files with 91 additions and 47 deletions
Download Patch File Download Diff File Expand all files Collapse all files
lib/Speedometer/speedometer.cpp
+43 -38
View File
@@ -30,59 +30,59 @@ uint16_t Speedometer::loop() {
return -1;
uint32_t time = millis();
uint16_t elapsed_time = time - this->last_millis_loop;
uint16_t elapsedTime = time - this->lastMillisLoop;
//Cancel if delayLoop is not reached
if (elapsed_time < this->delayLoop)
return elapsed_time;
if (elapsedTime < this->delayLoop)
return elapsedTime;
runSpeedometer();
this->last_millis_loop = time;
return elapsed_time;
this->lastMillisLoop = time;
return elapsedTime;
}
void Speedometer::runSpeedometer() {
uint32_t time = millis();
uint16_t elapsed_time = time - last_millis_calc;
last_millis_calc = time;
int16_t count = this->pulseCounter->getValue();
switch (this->currentDirection) {
case Direction::Forward :
this->addValToBuf(count);
break;
case Direction::Backward :
this->addValToBuf(-count);
break;
case Direction::None :
this->addValToBuf(0);
break;
default:
std::cout << "Wrong value in Speedometer::runSpeedometer" << std::endl;
break;
}
uint16_t elapsedTime = time - this->lastMillisCalc;
this->lastMillisCalc = time;
this->addValToBuf(this->pulseCounter->getValue());
this->pulseCounter->clear();
this->pulseCounter->resume();
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
double n = (double)this->calcAverage() / 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
if (count > 0) {
this->speed = ms;
} else if (count < 0) {
this->speed = ms * (-1);
} else {
this->speed = 0;
switch (this->currentDirection) {
case Direction::Forward :
this->speed = ms;
break;
case Direction::Backward :
this->speed = -ms;
break;
case Direction::None :
this->speed = 0;
break;
}
// std::cout << "Speedometer::runSpeedometer speed: " << (int) this->speed << std::endl;
// if (this->speed)
// this->printCounter++;
// if (this->printCounter > 10) {
// this->printCounter = 0;
// std::cout << "Speedometer::runSpeedometer speed: " << (int) this->speed << std::endl;
// }
}
void Speedometer::setDirection(Direction dir) {
if (this->currentDirection == dir)
return;
this->currentDirection = dir;
this->clearAvgBuf();
}
void Speedometer::setNumOfValForAvg(uint8_t val) {
@@ -132,12 +132,17 @@ void Speedometer::initAvgBuf() {
this->buf[i] = 0;
}
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;
bufPos = 0;
}
void Speedometer::updateAvgBufSize() {