From 07f09d691f439e491ea075100abf6f58a0a743f9 Mon Sep 17 00:00:00 2001 From: Alexander Klein Date: Thu, 20 Apr 2023 12:31:48 +0200 Subject: [PATCH] fix speedometer for new encoder --- include/moveControl.h | 8 ++++ lib/Speedometer/speedometer.cpp | 81 +++++++++++++++++---------------- lib/Speedometer/speedometer.h | 8 ++-- src/moveControl.cpp | 41 ++++++++++++++--- 4 files changed, 91 insertions(+), 47 deletions(-) diff --git a/include/moveControl.h b/include/moveControl.h index c812917..08510e7 100644 --- a/include/moveControl.h +++ b/include/moveControl.h @@ -80,6 +80,12 @@ class MoveControl { */ void setDrivingStatus(DrivingStatus status); + /** + * @brief Stops the engine immediately + * + */ + void emergencyStop(); + /** * @brief Set the target speed * @@ -156,6 +162,8 @@ class MoveControl { void setDelay(uint8_t delay) { this->delay = delay; } private: + void setSpeedometerDirection(Speedometer *speedometer, double value); + /** * @brief Converts values into wheel speeds * diff --git a/lib/Speedometer/speedometer.cpp b/lib/Speedometer/speedometer.cpp index 0d24b95..9cb7976 100644 --- a/lib/Speedometer/speedometer.cpp +++ b/lib/Speedometer/speedometer.cpp @@ -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() { diff --git a/lib/Speedometer/speedometer.h b/lib/Speedometer/speedometer.h index d1fc5cf..b0108e9 100644 --- a/lib/Speedometer/speedometer.h +++ b/lib/Speedometer/speedometer.h @@ -89,7 +89,7 @@ class Speedometer { * * @param dir Direction */ - void setDirection(Direction dir) { this->currentDirection = dir; } + void setDirection(Direction dir); /** * @brief Set the number of last values ​​to be taken into account for the average. @@ -150,6 +150,7 @@ class Speedometer { private: void init(uint8_t pin, double diameter, uint16_t steps); void initAvgBuf(); + void clearAvgBuf(); void addValToBuf(int16_t val); void updateAvgBufSize(); int16_t calcAverage(); @@ -162,6 +163,7 @@ class Speedometer { double speed = 0; double diameter; + uint8_t printCounter = 0; uint8_t bufSize = BUFSIZE; uint8_t delayLoop = DELAY_SPEEDOMETER; uint8_t bufPos = 0; @@ -169,8 +171,8 @@ class Speedometer { int16_t *buf = nullptr; - uint32_t last_millis_loop = 0; - uint32_t last_millis_calc = 0; + uint32_t lastMillisLoop = 0; + uint32_t lastMillisCalc = 0; }; #endif // SPEEDOMETER_H diff --git a/src/moveControl.cpp b/src/moveControl.cpp index c315d27..8992dc4 100644 --- a/src/moveControl.cpp +++ b/src/moveControl.cpp @@ -112,18 +112,32 @@ void MoveControl::setDrivingStatus(DrivingStatus status) { // } } +void MoveControl::emergencyStop() { + this->left_motor->emergencyStop(); + this->right_motor->emergencyStop(); + this->setSpeed(0); + this->setRotationSpeed(0); + this->setRawPowerLeft(0); + this->setRawPowerRight(0); + this->driving_status = DrivingStatus::stop; +} + void MoveControl::setSpeed(double speed) { - if (speed < 0.1 && speed > -0.1) + if (speed < 0.1 && speed > -0.1) { this->x_speed = 0; - else - this->x_speed = speed; + return; + } + + this->x_speed = speed; } void MoveControl::setRotationSpeed(double speed) { - if (speed < 0.1 && speed > -0.1) + if (speed < 0.1 && speed > -0.1){ this->rotation_speed = 0; - else - this->rotation_speed = speed; + return; + } + + this->rotation_speed = speed; } void MoveControl::setRawPowerLeft(int16_t power) { @@ -155,6 +169,15 @@ PID* MoveControl::getPID(uint8_t side) { return nullptr; } +void MoveControl::setSpeedometerDirection(Speedometer *speedometer, double value) { + if (value > 0) + speedometer->setDirection(Speedometer::Direction::Forward); + else if (value < 0) + speedometer->setDirection(Speedometer::Direction::Backward); + else + speedometer->setDirection(Speedometer::Direction::None); +} + void MoveControl::calcTargetWheelSpeed() { /* original formula: (1 / r) / 1 b \ / x \ = / Xl \ @@ -174,16 +197,22 @@ void MoveControl::regulateMotors() { case DrivingStatus::stop : this->left_motor->setTargetPower(0); this->right_motor->setTargetPower(0); + this->setSpeedometerDirection(this->left_speedometer, 0); + this->setSpeedometerDirection(this->right_speedometer, 0); break; case DrivingStatus::drive : this->left_motor->setTargetPower( (int8_t) this->left_pid_out); this->right_motor->setTargetPower( (int8_t) this->right_pid_out); + this->setSpeedometerDirection(this->left_speedometer, this->left_pid_out); + this->setSpeedometerDirection(this->right_speedometer, this->right_pid_out); break; case DrivingStatus::raw : this->left_motor->setTargetPower(this->rawPowerLeft); this->right_motor->setTargetPower(this->rawPowerRight); + this->setSpeedometerDirection(this->left_speedometer, this->rawPowerLeft); + this->setSpeedometerDirection(this->right_speedometer, this->rawPowerRight); break; default: