From ba91da7ed3e94a48eb3667842dd0abd2bd098a1c Mon Sep 17 00:00:00 2001 From: Alexander Klein Date: Tue, 21 Feb 2023 11:18:49 +0100 Subject: [PATCH] done with all changes for new encoder --- include/moveControlConfig.h | 6 +-- lib/PulseCounter/pulseCounter.cpp | 0 lib/PulseCounter/pulseCounter.h | 27 ---------- lib/Speedometer/speedometer.cpp | 85 +++++++++++++++++++------------ lib/Speedometer/speedometer.h | 54 +++++++++++++++----- platformio.ini | 5 +- src/moveControl.cpp | 14 ++--- 7 files changed, 106 insertions(+), 85 deletions(-) delete mode 100644 lib/PulseCounter/pulseCounter.cpp delete mode 100644 lib/PulseCounter/pulseCounter.h diff --git a/include/moveControlConfig.h b/include/moveControlConfig.h index efd97da..d3cd3e7 100644 --- a/include/moveControlConfig.h +++ b/include/moveControlConfig.h @@ -13,14 +13,12 @@ #define M_DIR_11 12 #define M_DIR_12 27 #define M_PWM_1 13 - #define M_ENCODE_1A 32 - #define M_ENCODE_1B 33 + #define M_ENCODE_LEFT 26 //Right Motor #define M_DIR_21 23 #define M_DIR_22 14 #define M_PWM_2 22 - #define M_ENCODE_2A 25 - #define M_ENCODE_2B 26 + #define M_ENCODE_RIGHT 12 // PID config #define PID_OUT_MIN -100 diff --git a/lib/PulseCounter/pulseCounter.cpp b/lib/PulseCounter/pulseCounter.cpp deleted file mode 100644 index e69de29..0000000 diff --git a/lib/PulseCounter/pulseCounter.h b/lib/PulseCounter/pulseCounter.h deleted file mode 100644 index add3ab7..0000000 --- a/lib/PulseCounter/pulseCounter.h +++ /dev/null @@ -1,27 +0,0 @@ -/** - * @file pulseCounter.h - * @author Alexander Klein (alex@kleiax.de) - * @brief - * @version 0.1 - * @date 2023-02-18 - * - * @copyright Copyright (c) 2023 - * - */ - -#pragma once - -#include -#include -#include - -class PulseCounter { - public: - PulseCounter(uint8_t pin, uint8_t unit); - - private: - uint8_t pin; - uint8_t unit; - - int16_t counter = 0; -}; diff --git a/lib/Speedometer/speedometer.cpp b/lib/Speedometer/speedometer.cpp index 6963cfb..c63b2ca 100644 --- a/lib/Speedometer/speedometer.cpp +++ b/lib/Speedometer/speedometer.cpp @@ -11,25 +11,18 @@ */ #include "speedometer.h" -Speedometer::Speedometer() {} -Speedometer::~Speedometer() { - delete[] this->buf; -} - -void Speedometer::init(uint8_t pinA, uint8_t pinB, double diameter, uint16_t steps, uint8_t numOfValForAvg) { - this->init(pinA, pinB, diameter, steps); +Speedometer::Speedometer(uint8_t pin, double diameter, uint16_t steps, uint8_t numOfValForAvg) { + this->init(pin, 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; +Speedometer::Speedometer(uint8_t pin, double diameter, uint16_t steps) { + this->init(pin, diameter, steps); +} - initAvgBuf(); - - this->isInit = true; +Speedometer::~Speedometer() { + delete[] this->buf; + delete this->pulseCounter; } uint16_t Speedometer::loop() { @@ -54,9 +47,27 @@ void Speedometer::runSpeedometer() { uint16_t elapsed_time = time - last_millis_calc; last_millis_calc = time; - int16_t count = encoder.getCount(); - this->addValToBuf(count); - this->encoder.clearCount(); + int16_t count = this->pulseCounter->get_value(); + 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; + } + + 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 @@ -71,41 +82,49 @@ void Speedometer::runSpeedometer() { this->speed = 0; } - std::cout << "Speedometer::runSpeedometer speed: " << (int) this->speed << std::endl; + // std::cout << "Speedometer::runSpeedometer speed: " << (int) this->speed << std::endl; } void Speedometer::setNumOfValForAvg(uint8_t val) { this->bufSize = val; - if (this->isInit) - updateAvgBufSize(); + updateAvgBufSize(); } void Speedometer::setEncFilter(uint16_t val) { if (val > 1023) val = 1023; - this->encoder.setFilter(val); -} - -void Speedometer::setDelay(uint8_t delayLoop) { - this->delayLoop = delayLoop; -} - -double Speedometer::getSpeed() { - return this->speed; + this->pulseCounter->set_filter_value(val); } void Speedometer::calibrationMeasurementStart() { this->calibrationRunning = true; - this->encoder.clearCount(); + this->pulseCounter->clear(); + this->pulseCounter->resume(); } uint16_t Speedometer::calibrationMeasurementStop() { this->calibrationRunning = false; - uint16_t res = abs(this->encoder.getCount()); - this->encoder.clearCount(); + uint16_t res = abs(this->pulseCounter->get_value()); + this->pulseCounter->clear(); + this->pulseCounter->resume(); return res; } +void Speedometer::init(uint8_t pin, double diameter, uint16_t steps) { + this->diameter = diameter; + this->steps = steps; + + this->pulseCounter = new PulseCounter(); + this->pulseCounter->initialise(pin, PCNT_PIN_NOT_USED); + this->pulseCounter->set_mode(PCNT_COUNT_INC, PCNT_COUNT_DIS, PCNT_MODE_KEEP, PCNT_MODE_KEEP); + this->pulseCounter->set_filter_value(1000); // ignore pulses less than 1000 x 2.5ns + + this->pulseCounter->clear(); + this->pulseCounter->resume(); + + initAvgBuf(); +} + void Speedometer::initAvgBuf() { this->buf = new int16_t[bufSize]; for (uint8_t i = 0; i < bufSize; i++) diff --git a/lib/Speedometer/speedometer.h b/lib/Speedometer/speedometer.h index 8cfbe9b..835a651 100644 --- a/lib/Speedometer/speedometer.h +++ b/lib/Speedometer/speedometer.h @@ -15,7 +15,7 @@ #include #include #include -#include +#include /** * @brief The default size of numbers to be taken in account for the average. @@ -39,20 +39,31 @@ */ class Speedometer { public: - Speedometer(); - ~Speedometer(); + /** + * @brief Enum to control the direction. + * + * If the Direction is Forward, the internal counter counts up and a positiv speed will be returned. + * If the Direction is Backward, the internal counter counts down and a negativ speed will be returned. + * If the Dorection is None, no measurement will be taken. + */ + enum Direction { + None, + Forward, + Backward + }; /** - * @brief Initialize the speedometer + * @brief Construct a new Speedometer object * - * @param pinA Pin on the Esp from the encoder. - * @param pinB Pin on the Esp from the encoder. + * @param pin Pin on the Esp from the encoder. * @param diameter Diameter of the wheel in meters. * @param steps Encodersteps for a complete wheel rotation. * @param numOfValForAvg Number of last values ​​to be taken into account for the average. */ - void init(uint8_t pinA, uint8_t pinB, double diameter, uint16_t steps, uint8_t numOfValForAvg); - void init(uint8_t pinA, uint8_t pinB, double diameter, uint16_t steps); + Speedometer(uint8_t pin, double diameter, uint16_t steps, uint8_t numOfValForAvg); + Speedometer(uint8_t pin, double diameter, uint16_t steps); + + ~Speedometer(); /** * @brief Calls runSpeedometer() to update all Values. @@ -72,6 +83,13 @@ class Speedometer { */ void runSpeedometer(); + /** + * @brief Set the direction + * + * @param dir Direction + */ + void setDirection(Direction dir) { this->currentDirection = dir; } + /** * @brief Set the number of last values ​​to be taken into account for the average. * @@ -82,7 +100,9 @@ class Speedometer { /** * @brief Set the Enc Filter to prevent bouncing * - * @param val default = 250, max = 1023 + * ignore pulses less than val x 2.5ns + * + * @param val default = 1000, max = 1023 */ void setEncFilter(uint16_t val); @@ -91,14 +111,21 @@ class Speedometer { * * @param delayLoop time in Milliseconds */ - void setDelay(uint8_t delayLoop); + void setDelay(uint8_t delayLoop) { this->delayLoop = delayLoop; } + + /** + * @brief Get the Direction + * + * @return Direction + */ + Direction getDirection() { return this->currentDirection; } /** * @brief Get the calculated speed of the Wheel * * @return double speed in m/s */ - double getSpeed(); + double getSpeed() { return this->speed; } /** * @brief Start calibration @@ -120,14 +147,15 @@ class Speedometer { private: + void init(uint8_t pin, double diameter, uint16_t steps); void initAvgBuf(); void addValToBuf(int16_t val); void updateAvgBufSize(); int16_t calcAverage(); - ESP32Encoder encoder; + PulseCounter* pulseCounter; + Direction currentDirection = Direction::None; - bool isInit = false; bool calibrationRunning = false; double speed = 0; diff --git a/platformio.ini b/platformio.ini index 83da049..c45263c 100644 --- a/platformio.ini +++ b/platformio.ini @@ -16,9 +16,7 @@ framework = arduino monitor_speed = 115200 upload_speed = 921600 monitor_port = COM3 -monitor_filters = esp32_exception_decoder lib_deps = - madhephaestus/ESP32Encoder@^0.10.1 jvpernis/PS3 Controller Host@^1.1.0 sparkfun/SparkFun u-blox GNSS Arduino Library@^2.2.7 knolleary/PubSubClient@^2.8 @@ -27,15 +25,18 @@ lib_deps = marian-craciunescu/ESP32Ping@^1.7 bblanchon/ArduinoJson@^6.20.0 mprograms/QMC5883LCompass@^1.1.1 + mike-gofton/ESP32PulseCounter@^0.2.0 https://git.kleiax.de/PlatformIO-Libs/Menu.git#v1.0 upload_port = COM3 [env:release] build_type = release +lib_deps = mike-gofton/ESP32PulseCounter@^0.2.0 [env:debug] monitor_filters = esp32_exception_decoder build_type = debug +check_tool = clangtidy [platformio] description = A Rover who should be drive a route by gps. diff --git a/src/moveControl.cpp b/src/moveControl.cpp index 0e76e7e..23a2744 100644 --- a/src/moveControl.cpp +++ b/src/moveControl.cpp @@ -13,8 +13,8 @@ MoveControl::MoveControl() { this->left_motor = new MotorControl(); this->right_motor = new MotorControl(); - this->left_speedometer = new Speedometer; - this->right_speedometer = new Speedometer; + this->left_speedometer = new Speedometer(M_ENCODE_LEFT, WHEEL_DIAMETER, ENC_STEPS); + this->right_speedometer = new Speedometer(M_ENCODE_RIGHT, WHEEL_DIAMETER, ENC_STEPS); this->left_pid = new PID( &this->wheelspeed_left, &this->left_pid_out, @@ -36,15 +36,17 @@ MoveControl::MoveControl() { this->right_pid->SetMode(AUTOMATIC); this->left_motor->init(M_PWM_1, PWM_CHANNEL_M1, M_DIR_11, M_DIR_12); - this->right_motor->init(M_PWM_2, PWM_CHANNEL_M2, M_DIR_21, M_DIR_22); - - this->left_speedometer->init(M_ENCODE_1A, M_ENCODE_1B, WHEEL_DIAMETER, ENC_STEPS); - this->right_speedometer->init(M_ENCODE_2A, M_ENCODE_2B, WHEEL_DIAMETER, ENC_STEPS); + this->right_motor->init(M_PWM_2, PWM_CHANNEL_M2, M_DIR_21, M_DIR_22); } MoveControl::~MoveControl() { this->left_motor->emergencyStop(); this->right_motor->emergencyStop(); + + delete this->left_motor; + delete this->right_motor; + delete this->left_speedometer; + delete this->right_speedometer; } void MoveControl::loop() {