From e4cdc5f98dc30291a5f69ca321fff4750a9bd273 Mon Sep 17 00:00:00 2001 From: Alexander Klein Date: Thu, 23 Mar 2023 19:31:02 +0100 Subject: [PATCH] finished new encoder --- include/moveControlConfig.h | 2 +- lib/Counter/counter.cpp | 56 +++++++++++++++++++ lib/Counter/counter.h | 30 ++++++++++ lib/Speedometer/speedometer.cpp | 15 ++--- lib/Speedometer/speedometer.h | 5 +- platformio.ini | 4 +- .../driveModi/TestMode/speedometerTest.cpp | 1 + src/moveControl.cpp | 4 ++ 8 files changed, 104 insertions(+), 13 deletions(-) create mode 100644 lib/Counter/counter.cpp create mode 100644 lib/Counter/counter.h diff --git a/include/moveControlConfig.h b/include/moveControlConfig.h index d3cd3e7..42c6637 100644 --- a/include/moveControlConfig.h +++ b/include/moveControlConfig.h @@ -18,7 +18,7 @@ #define M_DIR_21 23 #define M_DIR_22 14 #define M_PWM_2 22 - #define M_ENCODE_RIGHT 12 + #define M_ENCODE_RIGHT 33 // PID config #define PID_OUT_MIN -100 diff --git a/lib/Counter/counter.cpp b/lib/Counter/counter.cpp new file mode 100644 index 0000000..ce18500 --- /dev/null +++ b/lib/Counter/counter.cpp @@ -0,0 +1,56 @@ +#include "counter.h" +#include + +uint8_t Counter::amountOfCounter = 0; + +Counter::Counter(uint8_t pin) { + this->pulsePin = pin; + + this->unit = static_cast(Counter::amountOfCounter); + if (Counter::amountOfCounter < 7) + Counter::amountOfCounter++; + + pcnt_config_t config; + config.unit = this->unit; + config.channel = PCNT_CHANNEL_0; + config.counter_h_lim = COUNTER_HIGH_LIMIT; + config.counter_l_lim = COUNTER_LOW_LIMIT; + config.ctrl_gpio_num = PCNT_PIN_NOT_USED; + config.hctrl_mode = PCNT_CHANNEL_LEVEL_ACTION_KEEP; + config.lctrl_mode = PCNT_CHANNEL_LEVEL_ACTION_KEEP; + config.pulse_gpio_num = this->pulsePin; + config.pos_mode = PCNT_CHANNEL_EDGE_ACTION_INCREASE; + config.neg_mode = PCNT_CHANNEL_EDGE_ACTION_HOLD; + pcnt_unit_config(&config); +} + +void Counter::pause() { + pcnt_counter_pause(this->unit); +} + +void Counter::resume() { + pcnt_counter_resume(this->unit); +} + +void Counter::clear() { + pcnt_counter_clear(this->unit); +} + +int16_t Counter::getValue() { + int16_t res; + pcnt_get_counter_value(this->unit, &res); + return res; +} + +void Counter::setFilterValue(uint16_t value) { + pcnt_set_filter_value(this->unit, value); + this->filterEnable(); +} + +void Counter::filterEnable() { + pcnt_filter_enable(this->unit); +} + +void Counter::filterDisable() { + pcnt_filter_disable(this->unit); +} diff --git a/lib/Counter/counter.h b/lib/Counter/counter.h new file mode 100644 index 0000000..d84f15a --- /dev/null +++ b/lib/Counter/counter.h @@ -0,0 +1,30 @@ +#pragma once + +#include +#include + +#define COUNTER_HIGH_LIMIT INT16_MAX +#define COUNTER_LOW_LIMIT 0 + +class Counter { + public: + Counter(uint8_t pin); + + void pause(); + void resume(); + void clear(); + + int16_t getValue(); + + void setFilterValue(uint16_t value); + void filterEnable(); + void filterDisable(); + + private: + static uint8_t amountOfCounter; + + bool initalised = false; + + uint8_t pulsePin; + pcnt_unit_t unit; +}; diff --git a/lib/Speedometer/speedometer.cpp b/lib/Speedometer/speedometer.cpp index c63b2ca..0d24b95 100644 --- a/lib/Speedometer/speedometer.cpp +++ b/lib/Speedometer/speedometer.cpp @@ -47,7 +47,7 @@ void Speedometer::runSpeedometer() { uint16_t elapsed_time = time - last_millis_calc; last_millis_calc = time; - int16_t count = this->pulseCounter->get_value(); + int16_t count = this->pulseCounter->getValue(); switch (this->currentDirection) { case Direction::Forward : this->addValToBuf(count); @@ -93,20 +93,23 @@ void Speedometer::setNumOfValForAvg(uint8_t val) { void Speedometer::setEncFilter(uint16_t val) { if (val > 1023) val = 1023; - this->pulseCounter->set_filter_value(val); + this->pulseCounter->setFilterValue(val); } 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->get_value()); + uint16_t res = abs(this->pulseCounter->getValue()); this->pulseCounter->clear(); this->pulseCounter->resume(); + std::cout << "Result: " << res << std::endl; return res; } @@ -114,10 +117,8 @@ 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 = new Counter(pin); + this->pulseCounter->setFilterValue(1000); // ignore pulses less than 1000 x 2.5ns this->pulseCounter->clear(); this->pulseCounter->resume(); diff --git a/lib/Speedometer/speedometer.h b/lib/Speedometer/speedometer.h index 835a651..d1fc5cf 100644 --- a/lib/Speedometer/speedometer.h +++ b/lib/Speedometer/speedometer.h @@ -15,7 +15,8 @@ #include #include #include -#include + +#include "counter.h" /** * @brief The default size of numbers to be taken in account for the average. @@ -153,7 +154,7 @@ class Speedometer { void updateAvgBufSize(); int16_t calcAverage(); - PulseCounter* pulseCounter; + Counter* pulseCounter; Direction currentDirection = Direction::None; bool calibrationRunning = false; diff --git a/platformio.ini b/platformio.ini index c45263c..55d53f4 100644 --- a/platformio.ini +++ b/platformio.ini @@ -25,17 +25,15 @@ 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 +monitor_filters = esp32_exception_decoder check_tool = clangtidy [platformio] diff --git a/src/SpecialMenus/driveModi/TestMode/speedometerTest.cpp b/src/SpecialMenus/driveModi/TestMode/speedometerTest.cpp index bbd7c8f..cf06a1b 100644 --- a/src/SpecialMenus/driveModi/TestMode/speedometerTest.cpp +++ b/src/SpecialMenus/driveModi/TestMode/speedometerTest.cpp @@ -13,6 +13,7 @@ SpeedometerTest::SpeedometerTest(Speedometer* speedometer) { this->speedometer = speedometer; + this->speedometer->setDirection(Speedometer::Forward); } diff --git a/src/moveControl.cpp b/src/moveControl.cpp index 23a2744..c315d27 100644 --- a/src/moveControl.cpp +++ b/src/moveControl.cpp @@ -60,6 +60,10 @@ void MoveControl::loop() { || left_speed_time > 50 || right_speed_time > 50) { + // Dont count overTimeCounter if one speedometer is in TestMode. + if (left_speed_time == UINT16_MAX || right_speed_time == UINT16_MAX) + this->overTimeCounter--; + this->overTimeCounter++; if (this->overTimeCounter >= this->overTimeMax) { char buf[128];