From a1d59c18ade1800dd853e6987c410a546c0b6a18 Mon Sep 17 00:00:00 2001 From: Alexander Klein Date: Sun, 12 Dec 2021 16:53:19 +0100 Subject: [PATCH] restructerd projekt --- .gitignore | 4 +- .vscode/settings.json | 6 +- doc/{ => Diagrams}/classdiagramm.uxf | 0 doc/{ => Diagrams}/statesAutopilot.uxf | 0 doc/Notes and TODOs/TODO allgemein.txt | 6 + include/README | 39 ---- {src => include}/config.h | 0 {src => include}/moveControl.h | 4 +- {src => include}/route.h | 0 {src => lib/Debug/MQTT}/debugMqtt.cpp | 0 {src => lib/Debug/MQTT}/debugMqtt.h | 0 {src => lib/Debug/Times}/debugTimes.cpp | 0 {src => lib/Debug/Times}/debugTimes.h | 0 .../Illumination}/underfloorLighting.cpp | 0 .../Illumination}/underfloorLighting.h | 0 lib/MotorControl/motorControl.cpp | 167 ++++++++++++++++ lib/MotorControl/motorControl.h | 86 ++++++++ lib/README | 46 ----- lib/Speedometer/speedometer.cpp | 103 ++++++++++ lib/Speedometer/speedometer.h | 120 ++++++++++++ old/wheelEncoder.cpp | 19 -- old/wheelEncoder.h | 24 --- src/hardware/motorControl.cpp | 183 ------------------ src/hardware/motorControl.h | 45 ----- src/hardware/speedometer.cpp | 92 --------- src/hardware/speedometer.h | 35 ---- 26 files changed, 492 insertions(+), 487 deletions(-) rename doc/{ => Diagrams}/classdiagramm.uxf (100%) rename doc/{ => Diagrams}/statesAutopilot.uxf (100%) create mode 100644 doc/Notes and TODOs/TODO allgemein.txt delete mode 100644 include/README rename {src => include}/config.h (100%) rename {src => include}/moveControl.h (95%) rename {src => include}/route.h (100%) rename {src => lib/Debug/MQTT}/debugMqtt.cpp (100%) rename {src => lib/Debug/MQTT}/debugMqtt.h (100%) rename {src => lib/Debug/Times}/debugTimes.cpp (100%) rename {src => lib/Debug/Times}/debugTimes.h (100%) rename {src/hardware => lib/Illumination}/underfloorLighting.cpp (100%) rename {src/hardware => lib/Illumination}/underfloorLighting.h (100%) create mode 100644 lib/MotorControl/motorControl.cpp create mode 100644 lib/MotorControl/motorControl.h delete mode 100644 lib/README create mode 100644 lib/Speedometer/speedometer.cpp create mode 100644 lib/Speedometer/speedometer.h delete mode 100644 old/wheelEncoder.cpp delete mode 100644 old/wheelEncoder.h delete mode 100644 src/hardware/motorControl.cpp delete mode 100644 src/hardware/motorControl.h delete mode 100644 src/hardware/speedometer.cpp delete mode 100644 src/hardware/speedometer.h diff --git a/.gitignore b/.gitignore index f6d85db..a87a354 100644 --- a/.gitignore +++ b/.gitignore @@ -4,4 +4,6 @@ .vscode/launch.json .vscode/ipch -workspace.code-workspace \ No newline at end of file +.debugMqtt/doxygen + +workspace.code-workspace diff --git a/.vscode/settings.json b/.vscode/settings.json index f363801..247209a 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -59,5 +59,9 @@ "**/.next/**" ], "todohighlight.maxFilesForSearch": 5120, - "todohighlight.toggleURI": false + "todohighlight.toggleURI": false, + + + "doxdocgen.generic.authorEmail": "alex@kleiax.de", + "doxdocgen.generic.authorName": "Alexander Klein" } \ No newline at end of file diff --git a/doc/classdiagramm.uxf b/doc/Diagrams/classdiagramm.uxf similarity index 100% rename from doc/classdiagramm.uxf rename to doc/Diagrams/classdiagramm.uxf diff --git a/doc/statesAutopilot.uxf b/doc/Diagrams/statesAutopilot.uxf similarity index 100% rename from doc/statesAutopilot.uxf rename to doc/Diagrams/statesAutopilot.uxf diff --git a/doc/Notes and TODOs/TODO allgemein.txt b/doc/Notes and TODOs/TODO allgemein.txt new file mode 100644 index 0000000..71ec4bb --- /dev/null +++ b/doc/Notes and TODOs/TODO allgemein.txt @@ -0,0 +1,6 @@ +Meine Libs von andren Abhängigkeiten lösen + - Die loop funmktions mit eigenem Timer oder ohne Timer + - Alle config.h und nicht allgemeinen Header entfernen + +Speedometer: + - Mitrechnen durchschnittliche aufrufzeit \ No newline at end of file diff --git a/include/README b/include/README deleted file mode 100644 index 194dcd4..0000000 --- a/include/README +++ /dev/null @@ -1,39 +0,0 @@ - -This directory is intended for project header files. - -A header file is a file containing C declarations and macro definitions -to be shared between several project source files. You request the use of a -header file in your project source file (C, C++, etc) located in `src` folder -by including it, with the C preprocessing directive `#include'. - -```src/main.c - -#include "header.h" - -int main (void) -{ - ... -} -``` - -Including a header file produces the same results as copying the header file -into each source file that needs it. Such copying would be time-consuming -and error-prone. With a header file, the related declarations appear -in only one place. If they need to be changed, they can be changed in one -place, and programs that include the header file will automatically use the -new version when next recompiled. The header file eliminates the labor of -finding and changing all the copies as well as the risk that a failure to -find one copy will result in inconsistencies within a program. - -In C, the usual convention is to give header files names that end with `.h'. -It is most portable to use only letters, digits, dashes, and underscores in -header file names, and at most one dot. - -Read more about using header files in official GCC documentation: - -* Include Syntax -* Include Operation -* Once-Only Headers -* Computed Includes - -https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html diff --git a/src/config.h b/include/config.h similarity index 100% rename from src/config.h rename to include/config.h diff --git a/src/moveControl.h b/include/moveControl.h similarity index 95% rename from src/moveControl.h rename to include/moveControl.h index 5b02f71..deffcad 100644 --- a/src/moveControl.h +++ b/include/moveControl.h @@ -4,8 +4,8 @@ #include #include -#include "hardware/motorControl.h" -#include "hardware/speedometer.h" +#include "motorControl.h" +#include "speedometer" #include "config.h" #include "debugMqtt.h" #include "debugTimes.h" diff --git a/src/route.h b/include/route.h similarity index 100% rename from src/route.h rename to include/route.h diff --git a/src/debugMqtt.cpp b/lib/Debug/MQTT/debugMqtt.cpp similarity index 100% rename from src/debugMqtt.cpp rename to lib/Debug/MQTT/debugMqtt.cpp diff --git a/src/debugMqtt.h b/lib/Debug/MQTT/debugMqtt.h similarity index 100% rename from src/debugMqtt.h rename to lib/Debug/MQTT/debugMqtt.h diff --git a/src/debugTimes.cpp b/lib/Debug/Times/debugTimes.cpp similarity index 100% rename from src/debugTimes.cpp rename to lib/Debug/Times/debugTimes.cpp diff --git a/src/debugTimes.h b/lib/Debug/Times/debugTimes.h similarity index 100% rename from src/debugTimes.h rename to lib/Debug/Times/debugTimes.h diff --git a/src/hardware/underfloorLighting.cpp b/lib/Illumination/underfloorLighting.cpp similarity index 100% rename from src/hardware/underfloorLighting.cpp rename to lib/Illumination/underfloorLighting.cpp diff --git a/src/hardware/underfloorLighting.h b/lib/Illumination/underfloorLighting.h similarity index 100% rename from src/hardware/underfloorLighting.h rename to lib/Illumination/underfloorLighting.h diff --git a/lib/MotorControl/motorControl.cpp b/lib/MotorControl/motorControl.cpp new file mode 100644 index 0000000..d71d43c --- /dev/null +++ b/lib/MotorControl/motorControl.cpp @@ -0,0 +1,167 @@ +#include "motorControl.h" + +MotorControl::MotorControl() {} + +void MotorControl::init(uint8_t pwm_pin, uint8_t pwm_channel, uint8_t dir_1, uint8_t dir_2) { + this->pwm_pin = pwm_pin; + this->pwm_channel = pwm_channel; + this->dir_1 = dir_1; + this->dir_2 = dir_2; + + pinMode(this->dir_1, OUTPUT); + pinMode(this->dir_2, OUTPUT); + + digitalWrite(this->dir_1, LOW); + digitalWrite(this->dir_2, LOW); + + ledcSetup(this->pwm_channel, PWMFREQ, PWMRES); + ledcAttachPin(this->pwm_pin, this->pwm_channel); + ledcWrite(this->pwm_channel, 0); +} + +void MotorControl::loop() { + static uint32_t last_millis = 0; + uint32_t time = millis(); + + //Cancel if delay is not reached + if (time - last_millis < delay) { + return; + } + runMotorControl(); + last_millis = time; +} + +void MotorControl::runMotorControl() { + // Absolute difference between target_power and speed + uint8_t abs_difference = abs(this->target_power - this->power); + + // Difference between target_power and speed + int16_t difference = this->target_power - this->power; + + // Check that the target speed is close to 0 and that the abs_difference is lower than powersteps + if (abs(this->target_power) < powersteps && abs_difference < powersteps) { + this->setRealPower(0); + return; + } + + // Correct speed + if (abs_difference < powersteps) { + return; + } + + // Positive or negative tagret speed + if (this->target_power >= 0) { + // Positive or negative speed + if (this->power >= 0) { + if (difference > 0) { + this->increasePower(powersteps); + } else { + this->increasePower(-powersteps); + } + } else { + this->increasePower(powersteps); + } + + } else { + // Positive or negative speed + if (this->power >= 0) { + this->increasePower(-powersteps); + } else { + if (difference > 0) { + this->increasePower(powersteps); + } else { + this->increasePower(-powersteps); + } + } + } +} + +void MotorControl::setTargetPower(int8_t power) { + if (power <= 100 && power >= -100) { + this->target_power = power; + } else { + char str[64]; + sprintf(str, "Invalid Argument (power: %d) in setTargetPower!", power); + } +} + +void MotorControl::stop() { + this->target_power = 0; +} + +void MotorControl::emergencyStop() { + setRealPower(0); + while(1) +} + +void MotorControl::toString() { + Serial.printf("Target power: %d, power: %d, direction: %d \n", this->target_power, this->power, this->direction); +} + +int8_t MotorControl::getPower() { + return this->power; +} + +int8_t MotorControl::getTargetPower() { + return this->target_power; +} + +bool MotorControl::isTargetPowerReached() { + if (this->target_power == this->power) + return true; + return false; +} + +bool MotorControl::isAccelerationPositive() { + if (power < target_power) + return true; + return false; +} + +bool MotorControl::isAccelerationNegative() { + if (power > target_power) + return true; + return false; +} + +void MotorControl::setRealPower(int8_t power) { + //TODO: Exceptionhandling + if (power <= 100 || power <= -100) { + this->power = power; + } else { + return; + } + + if (power == 0) { + this->direction = 0; + digitalWrite(this->dir_1, LOW); + digitalWrite(this->dir_2, LOW); + ledcWrite(this->pwm_channel, 0); + return; + } + + uint8_t pwm_val = map(abs(power), 0, 100, this->pwm_min, this->pwm_max); + + if ((this->direction == 1 || this->direction == 0) && power < 0){ // new direction backward + this->direction = 2; + digitalWrite(this->dir_1, LOW); + digitalWrite(this->dir_2, HIGH); + } else if ((this->direction == 2 || this->direction == 0) && power > 0){ // new direction forward + this->direction = 1; + digitalWrite(this->dir_1, HIGH); + digitalWrite(this->dir_2, LOW); + } + + ledcWrite(this->pwm_channel, pwm_val); +} + +void MotorControl::increasePower(int8_t power) { + //TODO: Exceptionhandling + //TODO: make a stop befor a direction change + if (abs(power) > 2 * powersteps) { + Serial.println("Invalid Argument in MotorControl::increasePower"); + return; + } + + this->setRealPower(this->power + power); +} \ No newline at end of file diff --git a/lib/MotorControl/motorControl.h b/lib/MotorControl/motorControl.h new file mode 100644 index 0000000..d6d5ac1 --- /dev/null +++ b/lib/MotorControl/motorControl.h @@ -0,0 +1,86 @@ +/** + * @file motorControl.h + * @author Alexander Klein (alex@kleiax.de) + * @brief + * @version 0.1 + * @date 2021-12-09 + * + * @copyright Copyright (c) 2021 + * + */ +#ifndef MOTOR_CONTROL_H +#define MOTOR_CONTROL_H + +#include +#include +#include + +#define DELAY 10 +#define PWMFREQ 16000 +#define PWMRES 8 +#define POWERSTEPS 2 // A total of 20 levels ( 100 / SPEED_STEPS ) * RUN_MOTOR_CONTROL_DELAY = 500ms +#define PWMMIN 50 +#define PWMMAX 95 // Max 98% of 2^PWM_RES + +/** + * @brief + * + */ +class MotorControl { + public: + MotorControl(); + void init(uint8_t pwm_pin, uint8_t pwm_channel, uint8_t dir_1, uint8_t dir_2); + void loop(); + void runMotorControl(); + + /** + * @brief Set the minimum duty cycle + * + * @param min duty cycle in percent + */ + void setMinPwm(uint8_t min); + + /** + * @brief Set the maximum duty cycle + * + * @param max duty cycle in percent + */ + void setMaxPwm(uint8_t max); + + void setPowerSteps(uint8_t steps); // Min anfahrkurve einbauen + void setTargetPower(int8_t power); + void setDelay(uint8_t delay); // Min anfahrkurve einbauen + void stop(); + + /** + * @brief + * + */ + void emergencyStop(); + + int8_t getPower(); + int8_t getTargetPower(); + bool isTargetPowerReached(); + bool isAccelerationPositive(); + bool isAccelerationNegative(); + + private: + void setRealPower(int8_t power); + void increasePower(int8_t power); + + int8_t target_power = 0; + int8_t power = 0; + uint8_t direction = 0; // 0 = stop, 1 = forward, 2 = backward + + uint8_t pwm_pin; + uint8_t pwm_channel; + uint8_t pwm_min = PWMMIN; + uint8_t pwm_max = PWMMAX; + uint8_t dir_1; + uint8_t dir_2; + uint8_t delay = DELAY; + uint8_t powersteps = POWERSTEPS; + +}; + +#endif // MOTOR_CONTROL_H \ No newline at end of file diff --git a/lib/README b/lib/README deleted file mode 100644 index 6debab1..0000000 --- a/lib/README +++ /dev/null @@ -1,46 +0,0 @@ - -This directory is intended for project specific (private) libraries. -PlatformIO will compile them to static libraries and link into executable file. - -The source code of each library should be placed in a an own separate directory -("lib/your_library_name/[here are source files]"). - -For example, see a structure of the following two libraries `Foo` and `Bar`: - -|--lib -| | -| |--Bar -| | |--docs -| | |--examples -| | |--src -| | |- Bar.c -| | |- Bar.h -| | |- library.json (optional, custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html -| | -| |--Foo -| | |- Foo.c -| | |- Foo.h -| | -| |- README --> THIS FILE -| -|- platformio.ini -|--src - |- main.c - -and a contents of `src/main.c`: -``` -#include -#include - -int main (void) -{ - ... -} - -``` - -PlatformIO Library Dependency Finder will find automatically dependent -libraries scanning project source files. - -More information about PlatformIO Library Dependency Finder -- https://docs.platformio.org/page/librarymanager/ldf.html diff --git a/lib/Speedometer/speedometer.cpp b/lib/Speedometer/speedometer.cpp new file mode 100644 index 0000000..2bdd66e --- /dev/null +++ b/lib/Speedometer/speedometer.cpp @@ -0,0 +1,103 @@ +#include "speedometer.h" + +Speedometer::Speedometer() {} + +void Speedometer::init(uint8_t pinA, uint8_t pinB, double diameter, uint16_t steps, uint8_t numOfValForAvg) { + this->init(pinA, pinB, 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; + + initAvgBuf(); + + this->isInit = true; +} + +void Speedometer::loop() { + static uint32_t last_millis = 0; + uint32_t time = millis(); + + //Cancel if delay is not reached + if (time - last_millis < delay) { + return; + } + runSpeedometer(); + last_millis = time; +} + +void Speedometer::runSpeedometer() { + static uint32_t last_millis = 0; + uint32_t time = millis(); + + uint16_t elapsed_time = time - last_millis; + last_millis = time; + + int16_t count = encoder.getCount(); + this->addValToBuf(count); + encoder.clearCount(); + + 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 + + if (count > 0) { + this->speed = ms; + } else if (count < 0) { + this->speed = ms * (-1); + } else { + this->speed = 0; + } +} + +void Speedometer::setNumOfValForAvg(uint8_t val) { + this->bufSize = val; + if (this->isInit) + updateAvgBufSize(); +} + +void Speedometer::setEncFilter(uint16_t val) { + if (val > 1023) val = 1023; + this->encoder.setFilter(val); +} + +void Speedometer::setDelay(uint8_t delay) { + this->delay = delay; +} + +double Speedometer::getSpeed() { + return this->speed; +} + +void Speedometer::initAvgBuf() { + this->buf = new int16_t[bufSize]; + for (uint8_t i = 0; i < bufSize; i++) + this->buf[i] = 0; +} + +void Speedometer::addValToBuf(int16_t val) { + static uint8_t bufPos = 0; + this->buf[bufPos] = val; + bufPos++; + + if (bufPos == bufSize) { + bufPos = 0; + } +} + +void Speedometer::updateAvgBufSize() { + delete[] this->buf; + initAvgBuf(); +} + +int16_t Speedometer::calcAverage() { + int16_t sum = 0; + for (int i = 0; i < bufSize; i++) + sum += this->buf[i]; + return sum / bufSize; +} \ No newline at end of file diff --git a/lib/Speedometer/speedometer.h b/lib/Speedometer/speedometer.h new file mode 100644 index 0000000..264ee22 --- /dev/null +++ b/lib/Speedometer/speedometer.h @@ -0,0 +1,120 @@ +/** + * @file speedometer.h + * @author Alexander Klein (alex@kleiax.de) + * @brief A complete implementation to measure wheel speeds with an encoder. + * @version 0.1 + * @date 2021-12-09 + * + * @copyright Copyright (c) 2021 + * + */ + +#ifndef SPEEDOMETER_H +#define SPEEDOMETER_H + +#include +#include + +/** + * @brief The default size of numbers to be taken in account for the average. + * + */ +#define BUFSIZE 10 + +/** + * @brief Default value for min Millisseconds between each loop + * @see setDelay(uint8_t val) + */ +#define DELAY 30 +#define PI 3.1415926535897932384626433832795 + +/** + * @brief A class which use a encoder to calc the speed + * + * This class use ESP32 pulse counter hardware peripheral. + * The calclutaed speed is the average of an amount of last measurments. + * + */ +class Speedometer { + public: + Speedometer(); + + /** + * @brief Initalize the speedometer + * + * @param pinA Pin on the Esp from the encoder. + * @param pinB 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); + + /** + * @brief Calls runSpeedometer() to update all Values. + * + * This function should be called every mainloop. If the delay is not reached, than the + * functions returns immediately. + * @see runSpeedometer() + * @see DELAY + */ + void loop(); + + /** + * @brief Noramly called repeatedly by loop to calcluate new values. + * + * Add a new Value to the average and update the speed. + */ + void runSpeedometer(); + + /** + * @brief Set the number of last values ​​to be taken into account for the average. + * + * @param val length of the array + */ + void setNumOfValForAvg(uint8_t val); + + /** + * @brief Set the Enc Filter to prevent bouncing + * + * @param val default = 250, max = 1023 + */ + void setEncFilter(uint16_t val); + + /** + * @brief Set the min delay between each loop + * + * @param val time in Milliseconds + */ + void setDelay(uint8_t delay); + + /** + * @brief Get the calculated speed of the Wheel + * + * @return double speed in m/s + */ + double getSpeed(); + + + private: + void initAvgBuf(); + void addValToBuf(int16_t val); + void updateAvgBufSize(); + int16_t calcAverage(); + + ESP32Encoder encoder; + + bool isInit = false; + + double speed = 0; + double diameter; + + uint8_t bufSize = BUFSIZE; + uint16_t steps; + uint8_t delay = DELAY; + + int16_t *buf; +}; + +#endif // SPEEDOMETER_H \ No newline at end of file diff --git a/old/wheelEncoder.cpp b/old/wheelEncoder.cpp deleted file mode 100644 index aa9cd89..0000000 --- a/old/wheelEncoder.cpp +++ /dev/null @@ -1,19 +0,0 @@ -#include "wheelEncoder.h" - -WheelEncoder::WheelEncoder() { - -} - -void WheelEncoder::initEncoder(uint8_t pinA, uint8_t pinB) { - this->pinA = pinA; - this->pinB = pinB; - - pinMode(this->pinA, INPUT_PULLDOWN); - pinMode(this->pinB, INPUT_PULLDOWN); - - -} - -int64_t WheelEncoder::getCount() { - return this->count; -} \ No newline at end of file diff --git a/old/wheelEncoder.h b/old/wheelEncoder.h deleted file mode 100644 index 6c20dd2..0000000 --- a/old/wheelEncoder.h +++ /dev/null @@ -1,24 +0,0 @@ -#ifndef WHEEL_ENCODER_H -#define WHEEL_ENCODER_H - -#include -#include -#include -#include - -class WheelEncoder { - public: - WheelEncoder(); - void initEncoder(uint8_t pinA, uint8_t pinB); - int64_t getCount(); - - private: - uint8_t pinA; - uint8_t pinB; - bool init = false; - - volatile int64_t count = 0; - -}; - -#endif // WHEEL_ENCODER_H \ No newline at end of file diff --git a/src/hardware/motorControl.cpp b/src/hardware/motorControl.cpp deleted file mode 100644 index b00ad37..0000000 --- a/src/hardware/motorControl.cpp +++ /dev/null @@ -1,183 +0,0 @@ -#include "motorControl.h" - -#include - -#include "config.h" - -MotorControl::MotorControl() { - this->debug = new DebugMqtt("MotorControl"); -} - -void MotorControl::init(uint8_t pwm_pin, uint8_t pwm_channel, uint8_t direction_pin_1, uint8_t direction_pin_2, String name) { - debug->sendMsg(Loglevel::info, "Init..."); - this->name = name; - - this->pwm_pin = pwm_pin; - this->pwm_channel = pwm_channel; - this->direction_pin_1 = direction_pin_1; - this->direction_pin_2 = direction_pin_2; - - pinMode(this->direction_pin_1, OUTPUT); - pinMode(this->direction_pin_2, OUTPUT); - - digitalWrite(this->direction_pin_1, LOW); - digitalWrite(this->direction_pin_2, LOW); - - ledcSetup(this->pwm_channel, PWM_FREQ, PWM_RES); - ledcAttachPin(this->pwm_pin, this->pwm_channel); - ledcWrite(this->pwm_channel, 0); - - debug->sendMsg(Loglevel::info, "Init finished!"); -} - -void MotorControl::runMotorControl() { - if (millis() - this->last_millis > RUN_MOTOR_CONTROL_DELAY) { - - // Absolute difference between target_power and speed - uint8_t abs_difference = abs(this->target_power - this->power); - - // Difference between target_power and speed - int16_t difference = this->target_power - this->power; - - // Check that the target speed is close to 0 and that the abs_difference is lower than POWER_STEPS - if (abs(this->target_power) < POWER_STEPS && abs_difference < POWER_STEPS) { - this->setRealPower(0); - this->last_millis = millis(); - return; - } - - // Correct speed - if (abs_difference < POWER_STEPS) { - this->last_millis = millis(); - return; - } - - // Positive or negative tagret speed - if (this->target_power >= 0) { - // Positive or negative speed - if (this->power >= 0) { - if (difference > 0) { - this->increasePower(POWER_STEPS); - } else { - this->increasePower(-POWER_STEPS); - } - } else { - this->increasePower(POWER_STEPS); - } - - } else { - // Positive or negative speed - if (this->power >= 0) { - this->increasePower(-POWER_STEPS); - } else { - if (difference > 0) { - this->increasePower(POWER_STEPS); - } else { - this->increasePower(-POWER_STEPS); - } - } - } - this->last_millis = millis(); - } -} - -void MotorControl::setTargetPower(int8_t power) { - if (power <= 100 && power >= -100) { - this->target_power = power; - char str[32]; - sprintf(str, "target_power_%s", this->name.c_str()); - debug->writeToInflux("motorControl", str, power); - } else { - char str[64]; - sprintf(str, "Invalid Argument (power: %d) in setTargetPower!", power); - debug->sendMsg(Loglevel::error, str); - } -} - -void MotorControl::stop() { - this->target_power = 0; - char str[32]; - sprintf(str, "target_power_%s", this->name.c_str()); - debug->writeToInflux("motorControl", str, this->target_power); -} - -void MotorControl::emergencyStop() { - setRealPower(0); -} - -void MotorControl::toString() { - Serial.printf("Target power: %d, power: %d, direction: %d \n", this->target_power, this->power, this->direction); -} - -int8_t MotorControl::getPower() { - return this->power; -} - -int8_t MotorControl::getTargetPower() { - return this->target_power; -} - -bool MotorControl::isTargetPowerReached() { - if (this->target_power == this->power) - return true; - return false; -} - -bool MotorControl::isAccelerationPositive() { - if (power < target_power) - return true; - return false; -} - -bool MotorControl::isAccelerationNegative() { - if (power > target_power) - return true; - return false; -} - -void MotorControl::setRealPower(int8_t power) { - //TODO: Exceptionhandling - if (power <= 100 || power <= -100) { - this->power = power; - } else { - debug->sendMsg(Loglevel::error, "Invalid Argument in setRealPower!"); - return; - } - - if (power == 0) { - this->direction = 0; - digitalWrite(this->direction_pin_1, LOW); - digitalWrite(this->direction_pin_2, LOW); - ledcWrite(this->pwm_channel, 0); - return; - } - - uint8_t pwm_val = map(abs(power), 0, 100, PWM_MIN, PWM_MAX); - - if ((this->direction == 1 || this->direction == 0) && power < 0){ // new direction backward - this->direction = 2; - digitalWrite(this->direction_pin_1, LOW); - digitalWrite(this->direction_pin_2, HIGH); - } else if ((this->direction == 2 || this->direction == 0) && power > 0){ // new direction forward - this->direction = 1; - digitalWrite(this->direction_pin_1, HIGH); - digitalWrite(this->direction_pin_2, LOW); - } - - ledcWrite(this->pwm_channel, pwm_val); - char str[32]; - sprintf(str, "real_power_%s", this->name.c_str()); - debug->writeToInflux("motorControl", str, power); - // Serial.printf("pwm_val: %d, ", pwm_val); -} - -void MotorControl::increasePower(int8_t power) { - //TODO: Exceptionhandling - //TODO: make a stop befor a direction change - if (abs(power) > 2 * POWER_STEPS) { - Serial.println("Invalid Argument in MotorControl::increasePower"); - return; - } - - this->setRealPower(this->power + power); -} \ No newline at end of file diff --git a/src/hardware/motorControl.h b/src/hardware/motorControl.h deleted file mode 100644 index 6dbd608..0000000 --- a/src/hardware/motorControl.h +++ /dev/null @@ -1,45 +0,0 @@ -#ifndef MOTOR_CONTROL_H -#define MOTOR_CONTROL_H - -#include -#include - -#include "debugMqtt.h" - -class MotorControl { - public: - MotorControl(); - void init(uint8_t pwm_pin, uint8_t pwm_channel, uint8_t direction_1, uint8_t direction_2, String name); - void runMotorControl(); - void setTargetPower(int8_t power); - void stop(); - void emergencyStop(); - void toString(); - - int8_t getPower(); - int8_t getTargetPower(); - bool isTargetPowerReached(); - bool isAccelerationPositive(); - bool isAccelerationNegative(); - - private: - void setRealPower(int8_t power); - void increasePower(int8_t power); - - String name; - - DebugMqtt *debug; - - int8_t target_power = 0; - int8_t power = 0; - uint8_t direction = 0; // 0 = stop, 1 = forward, 2 = backward - - uint64_t last_millis = 0; - - uint8_t pwm_pin; - uint8_t pwm_channel; - uint8_t direction_pin_1; - uint8_t direction_pin_2; -}; - -#endif // MOTOR_CONTROL_H \ No newline at end of file diff --git a/src/hardware/speedometer.cpp b/src/hardware/speedometer.cpp deleted file mode 100644 index 0f07972..0000000 --- a/src/hardware/speedometer.cpp +++ /dev/null @@ -1,92 +0,0 @@ -#include "speedometer.h" - -Speedometer::Speedometer() { - for (int i = 0; i < BUF_SIZE; i++) { - this->buf[i] = 0; - } - this->debug = new DebugMqtt("Speedometer"); -} - -void Speedometer::init(uint8_t pinA, uint8_t pinB, String name) { - debug->sendMsg(Loglevel::info, "Init..."); - this->name = name; - ESP32Encoder::useInternalWeakPullResistors=DOWN; - this->encoder.attachFullQuad(pinA, pinB); - this->encoder.setFilter(ENC_FILTER); - debug->sendMsg(Loglevel::info, "Init finished!"); -} - -void Speedometer::runSpeedometer() { - uint32_t time = millis(); - - //Cancel if delay is not reached - if (time - this->last_millis < RUN_SPEEDOMETER_DELAY) { - return; - } - - uint16_t elapsed_time = time - this->last_millis; - this->last_millis = time; - - int16_t count = encoder.getCount(); - this->addValToBuf(count); - encoder.clearCount(); - - uint8_t direction = STOP; - if (count > 0) { - direction = FORWARD; - } else if (count < 0) { - direction = BACKWARD; - } - - count = abs(this->getAverage()); // Absolute time in milliseconds - double n = (double)count / ENC_STEPS; // Wheel revolutions in absolute time - double u = (double)n / ((double)elapsed_time / 1000); // Wheel revolutions per second - double ms = u * (WHEEL_DIAMETER * PI); // Speed in m/s - - if (direction == FORWARD) { - this->speed = ms; - } else if (direction == BACKWARD) { - this->speed = ms * (-1); - } else { - this->speed = 0; - } - char str[32]; - sprintf(str, "speed_%s", this->name.c_str()); - debug->writeToInflux("speedometer", str, this->speed); - - // Serial.printf("time: %d, ", time); - // Serial.printf("count: %d, ",count); - // Serial.printf("n: %f, u: %f, ms: %f \n", n, u, ms); -} - -double Speedometer::getSpeed() { - return this->speed; -} - -uint8_t Speedometer::getDirection() { - if (this->speed > 0) { - return FORWARD; - } else if (this->speed < 0) { - return BACKWARD; - } else { - return STOP; - } -} - -void Speedometer::addValToBuf(int16_t val) { - this->buf[this->bufPos] = val; - this->bufPos++; - - if (this->bufPos == BUF_SIZE) { - this->bufPos = 0; - } -} - -int16_t Speedometer::getAverage() { - int16_t sum = 0; - for (int i = 0; i < BUF_SIZE; i++) { - sum += this->buf[i]; - } - - return sum / BUF_SIZE; -} \ No newline at end of file diff --git a/src/hardware/speedometer.h b/src/hardware/speedometer.h deleted file mode 100644 index 9a7b096..0000000 --- a/src/hardware/speedometer.h +++ /dev/null @@ -1,35 +0,0 @@ -#ifndef SPEEDOMETER_H -#define SPEEDOMETER_H - -#include -#include -#include - -#include "config.h" -#include "debugMqtt.h" - -class Speedometer { - public: - Speedometer(); - void init(uint8_t pinA, uint8_t pinB, String name); - void runSpeedometer(); - double getSpeed(); - uint8_t getDirection(); - - - private: - void addValToBuf(int16_t val); - int16_t getAverage(); - - ESP32Encoder encoder; - DebugMqtt *debug; - - String name; - double speed = 0; - uint32_t last_millis = 0; - - uint8_t bufPos = 0; - int16_t buf[BUF_SIZE]; -}; - -#endif // SPEEDOMETER_H \ No newline at end of file