diff --git a/.gitignore b/.gitignore index 72abbae..c8ced1f 100644 --- a/.gitignore +++ b/.gitignore @@ -3,11 +3,10 @@ .vscode/c_cpp_properties.json .vscode/launch.json .vscode/ipch +rover-cppcheck-build-dir doc/Doxygen/html* doc/Doxygen/generated* - - workspace.code-workspace projektarbeit.code-workspace diff --git a/.vscode/settings.json b/.vscode/settings.json index 3370206..d055b55 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -123,11 +123,14 @@ "Ahnung", "akku", "ardui", + "Baseclass", "blox", "bluedroid", "carr", "CIPO", "COPI", + "Doxygen", + "Dutycycle", "gast", "GNSS", "GPGGA", @@ -136,9 +139,12 @@ "Keine", "kleiax", "Lebennig", + "microcontroller", + "MQTT", "NMEA", "NMEAGPGGA", "NTRIP", + "pcnt", "pidl", "pidr", "Punica", @@ -146,8 +152,10 @@ "Rtcm", "Schalke", "Soln", + "Systeminformation", "TPMOBIL", "UBLOX", + "wheelspeed", "ZLPJ" ], "cmake.configureOnOpen": false diff --git a/doc/Doxygen/Doxyfile b/doc/Doxygen/Doxyfile index 3ad1597..b80b40a 100644 --- a/doc/Doxygen/Doxyfile +++ b/doc/Doxygen/Doxyfile @@ -51,7 +51,7 @@ PROJECT_BRIEF = "A small verhicle which should be drive a route with GP # pixels and the maximum width should not exceed 200 pixels. Doxygen will copy # the logo to the output directory. -PROJECT_LOGO = C:/Users/alex1/git/projektarbeit/doc/Doxygen/logo.png +PROJECT_LOGO = logo.png # The OUTPUT_DIRECTORY tag is used to specify the (relative or absolute) path # into which the generated documentation will be written. If a relative path is @@ -606,7 +606,7 @@ HIDE_COMPOUND_REFERENCE= NO # will show which file needs to be included to use the class. # The default value is: YES. -SHOW_HEADERFILE = YES +# SHOW_HEADERFILE = YES # If the SHOW_INCLUDE_FILES tag is set to YES then doxygen will put a list of # the files that are included by a file in the documentation of that file. @@ -824,7 +824,7 @@ WARN_IF_DOC_ERROR = YES # parameters have no documentation without warning. # The default value is: YES. -WARN_IF_INCOMPLETE_DOC = YES +# WARN_IF_INCOMPLETE_DOC = YES # This WARN_NO_PARAMDOC option can be enabled to get warnings for functions that # are documented, but have no documentation for their parameters or return @@ -1608,7 +1608,7 @@ GENERATE_TREEVIEW = YES # The default value is: NO. # This tag requires that the tag GENERATE_HTML is set to YES. -FULL_SIDEBAR = NO +# FULL_SIDEBAR = NO # The ENUM_VALUES_PER_LINE tag can be used to set the number of enum values that # doxygen will group on one line in the generated HTML documentation. @@ -1691,7 +1691,7 @@ USE_MATHJAX = NO # The default value is: MathJax_2. # This tag requires that the tag USE_MATHJAX is set to YES. -MATHJAX_VERSION = MathJax_2 +# MATHJAX_VERSION = MathJax_2 # When MathJax is enabled you can set the default output format to be used for # the MathJax output. For more details about the output format see MathJax diff --git a/doc/TODO allgemein.txt b/doc/TODO allgemein.txt index 0719e08..a3fc74a 100644 --- a/doc/TODO allgemein.txt +++ b/doc/TODO allgemein.txt @@ -6,39 +6,42 @@ Do later: -> Program underfloorLighting -> Add an beeper -> Program the beeper - -> Update GNSS Lib to v3 -> Engine slow down without curve in motorControl -> Network clean up (Mqtt remove?) -> Extra class for maneuver, autopilot should inherit from int16_t - -> Better remote Control with Leds for gnss rtk etc + -> Remote Control + - Leds for gnss rtk etc + - what happens exactly when no data is arriving -> Test Menu for big curve driving -> Api with Names and show on Maps in Browser -> Menu structure mit add functions for each menu mit pointer return to config (additional not replace) -> Menu Display from parent as run() to make Menu as Component -> Racing Mode -> ConsolControl + -> Menü für Einstellungen + - WiFi (save in Flash) + -> Battery + - tabelle mit eigenen Werten übergeben und nicht in header (wiederverwendbarkeit) + - kalibrierungsmethode mit Menü + - Daten in flash speichern können + -> Check speration between Ui and Route (RouteMenu) + -> update sparkfun gnss auf v3 für SPI korrekturdatenüvbetragung + -> Rover Objekt mit Error zustand freeze, damit das wenn möglich auch auf dem Display angezeigt wird. + -> ESP und Sensoren in den DeepSleep für Auschalten oder Akkuschutz + -> Doxygen comments + - navigation + - autopilot + Do now: Code: - Doxygen Kommentare aktualisieren - Geschwindigkeiten außerhalb von Modi einstellen, Manager kann die Werte einstellen da Interface - Automat in MoveControl weil jetzt in Arbeit beschrieben - Liste mit Betriebsmodi, automatisch in Menü einfügen - Betriebmodi dem Drivemanger ohne switchCase geben, aus Liste oder so - bettery managment checken, buffer in battery und in Main - battery tabelle mit eigenen Werten übergeben und nicht in header (wiederverwendbarkeit) - battery methode für daten erzeugen und in eeporm speichern - battery bruch mit R werten nur einmal berechnen - lange kein daten von fernbedienung dann? - Input pointer von fernbedienung nicht veränderbar doppel const - trennen funktion und ui route menü - DriveManager Mode als Template übergeben + Fernbedienung! Latex: - Genaue Beschreibung von PulseCounter HardwareUnit wenn möglich Anhang: Liste mit allen Komponenten und Kurzbeschreibung + Unterschied funktionale und nicht funktionale Anforderungen + Overfull H-Boxen + Test Motor, Zeiten kontrolieren. Wenn einfach so dann bezug auf fehler bei Timing und freeRtos + Was mit den nicht erfüllten Anforderungen machen? + Hardware doppelt erklärt -3D: - Oberrahmen soll nur aussehen wie ein A - - diff --git a/doc/latexSnippet.cpp b/doc/latexSnippet.cpp deleted file mode 100644 index ac4c65b..0000000 --- a/doc/latexSnippet.cpp +++ /dev/null @@ -1,9 +0,0 @@ -void KomponenteA::loop() { - this->komponenteB->loop(); - this->komponenteC->loop(); - - if (millis() - this->lastMillis < this->delayMillis) - return; - - this->doSomething(); -} diff --git a/include/Version.h b/include/Version.h index 109f133..99c9ad6 100644 --- a/include/Version.h +++ b/include/Version.h @@ -1,9 +1,8 @@ - // AUTO GENERATED FILE, DO NOT EDIT - #ifndef VERSION - #define VERSION "0.8.36" - #endif - #ifndef BUILD_TIMESTAMP - #define BUILD_TIMESTAMP "2023-08-13 15:13:34.491991" - #endif - \ No newline at end of file +// AUTO GENERATED FILE, DO NOT EDIT +#ifndef VERSION +#define VERSION "1.8.36" +#endif +#ifndef BUILD_TIMESTAMP +#define BUILD_TIMESTAMP "2023-10-13 15:13:34.491991" +#endif diff --git a/include/config.h b/include/config.h index 0a205f9..5ea15ec 100644 --- a/include/config.h +++ b/include/config.h @@ -1,52 +1,74 @@ /** * @file config.h * @author Alexander Klein (alex@kleiax.de) - * @brief Some defines to configere the project. + * @brief Some defines to configure the project. * @version 0.1 * @date 2022-02-15 - * + * * @copyright Copyright (c) 2022 - * + * */ #pragma once -namespace PinNumbers { +/** + * @brief The numbers of pins to be used + * + */ +namespace PinNumbers +{ constexpr uint8_t spiCopi = 16; constexpr uint8_t spiCipo = 4; constexpr uint8_t spiSck = 5; constexpr uint8_t gnssSpiCs = 17; + constexpr uint8_t sda = 21; + constexpr uint8_t scl = 19; + constexpr uint8_t battery = 35; - namespace LeftMotor { + namespace LeftMotor + { constexpr uint8_t dir1 = 27; constexpr uint8_t dir2 = 12; - constexpr uint8_t pwm = 13; - constexpr uint8_t encoder = 26; + constexpr uint8_t pwm = 13; + constexpr uint8_t encoder = 32; constexpr uint8_t pmwChannel = 0; } - namespace RightMotor { + namespace RightMotor + { constexpr uint8_t dir1 = 14; constexpr uint8_t dir2 = 23; - constexpr uint8_t pwm = 22; + constexpr uint8_t pwm = 22; constexpr uint8_t encoder = 33; - constexpr uint8_t pmwChannel = 0; + constexpr uint8_t pmwChannel = 1; } } -namespace Settings { - constexpr float wheelDiameter = 0.1263; - constexpr uint16_t encoderSteps = 360; +/** + * @brief Default values for different components + * + */ +namespace Settings +{ + constexpr uint32_t baudRate = 115200; + constexpr uint32_t i2cSpeed = 400000; - namespace Pid { - namespace Left { - constexpr uint8_t P = 75; + constexpr float wheelDiameter = 0.105; + constexpr float wheelDistance = 0.255; + constexpr uint16_t encoderSteps = 384; + + namespace Pid + { + namespace Left + { + constexpr uint8_t P = 5; constexpr uint8_t I = 0; constexpr uint8_t D = 0; } - namespace Right { - constexpr uint8_t P = 75; + namespace Right + { + constexpr uint8_t P = 5; constexpr uint8_t I = 0; constexpr uint8_t D = 0; } diff --git a/include/moveControl.h b/include/moveControl.h index 8c6d7b8..a10c8a2 100644 --- a/include/moveControl.h +++ b/include/moveControl.h @@ -4,9 +4,9 @@ * @brief Contains the MoveControl class * @version 0.1 * @date 2021-12-14 - * + * * @copyright Copyright (c) 2021 - * + * */ #ifndef MOVE_CONTROL_H #define MOVE_CONTROL_H @@ -22,157 +22,182 @@ #include "debugTimes.h" #include "component.h" +/** + * @brief A struct to easy set the speed + * + */ +struct DrivingSpeeds +{ + double x; + double rot; +}; /** * @brief This class manages the motors and the encoders - * + * * The class uses two PIDs to control the motors. The PIDs * use the Speedometer class to get the current speed and the * given target speed to calculate a new duty cycle for the motors. - * + * */ -class MoveControl : public Component { - public: - /** - * @brief used to set driving status - * - * When set to stop all motors are set to halt - * - */ - enum Status {Stop, - Drive, - Raw}; +class MoveControl : public Component +{ +public: + /** + * @brief Used to set driving status + * + * When set to stop all motors are set to halt + * + * When set to Raw the PIDs are not used. The target + * values must be given withe setRawPowerLeft() and + * setRawPowerRight(). + * + * Drive is the normal working mode. + * + */ + enum Status + { + Stop, + Drive, + Raw + }; + /** + * @brief Construct a new Move Control object + * + * Initialize the motors, encoders and PIDs with given + * values in config.h + */ + MoveControl(); - /** - * @brief Construct a new Move Control object - * - * Initialize the motors, encoders and PIDs with given - * values in moveControlConfig.h - */ - MoveControl(); + /** + * @brief Destroy the Move Control object + * + * Stops the motors with emergencyStop() + */ + ~MoveControl(); - /** - * @brief Destroy the Move Control object - * - * Stops the motors with emergencyStop() - */ - ~MoveControl(); + /** + * @brief Set the Status + * + * Control if this component should work normally or with + * raw values for the motors. + * + * @see Status + * @param status + */ + void setDrivingStatus(Status status); - /** - * @brief Set the Status - * - * @see Status - * @param status - */ - void setDrivingStatus(Status status); + /** + * @brief Stops the engine immediately + * + */ + void emergencyStop(); - /** - * @brief Stops the engine immediately - * - */ - void emergencyStop(); + /** + * @brief Set the target speed + * + * If the given speed is close to zero, then the + * target speed is set to zero. + * @param speed in m/s + */ + void setSpeed(double speed); - /** - * @brief Set the target speed - * - * If the given speed is close to zero, then the - * target speed is set to zero. - * @param speed in m/s - */ - void setSpeed(double speed); + /** + * @brief Set the rotationSpeed + * + * If the given rotationSpeed is close to zero, then the + * target rotationSpeed is set to zero. + * @param speed rad/s + */ + void setRotationSpeed(double speed); - /** - * @brief Set the rotationspeed - * - * If the given rotationspeed is close to zero, then the - * target rotationspeed is set to zero. - * @param speed rad/s - */ - void setRotationSpeed(double speed); + /** + * @brief Set the speeds + * + * This function is a combination of the functions setSpeed() and + * setRotationSpeed(). The struct DrivingSpeeds + * is used for this. + * + * + * @param drivingSpeeds + */ + void setSpeeds(DrivingSpeeds drivingSpeeds); - /** - * @brief Set Raw Power Left - * - * This value has only an effect if Status is raw. - * - * @param power between -100 and 100 - */ - void setRawPowerLeft(int16_t power); + /** + * @brief Set raw power left + * + * This value has only an effect if the status is raw. + * @see setDrivingStatus + * + * @param power between -100 and 100 + */ + void setRawPowerLeft(int16_t power); - /** - * @brief Set the Raw Power Right - * - * This value has only an effect if Status is raw. - * - * @param power between -100 and 100 - */ - void setRawPowerRight(int16_t power); + /** + * @brief Set the raw power right + * + * This value has only an effect if status is raw. + * + * @param power between -100 and 100 + */ + void setRawPowerRight(int16_t power); - /** - * @brief Set the pid tunings - * - * @param side 0 -> left, 1 -> right - * @param p - * @param i - * @param d - */ - void setPidTunings(uint8_t side, double p, double i, double d); + /** + * @brief Set the pid tunings + * + * @param side 0 -> left, 1 -> right + * @param pPart + * @param iPart + * @param dPart + */ + void setPidTunings(uint8_t side, double pPart, double iPart, double dPart); - /** - * @brief Returns the PID object of the choosen side. - * - * @param side 0 -> left, 1 -> right - * @return PID* - */ - PID* getPID(uint8_t side) const; + /** + * @brief Returns the PID object of the choosen side. + * + * @param side 0 -> left, 1 -> right + * @return PID* + */ + PID *getPID(uint8_t side) const; - /** - * @brief Get the Speedometer Left object - * - * @return Speedometer* - */ - Speedometer* getSpeedometerLeft() const { return this->left_speedometer; } + Speedometer *getSpeedometerLeft() const { return this->leftSpeedometer; } + Speedometer *getSpeedometerRight() const { return this->rightSpeedometer; } - /** - * @brief Get the Speedometer Right object - * - * @return Speedometer* - */ - Speedometer* getSpeedometerRight() const { return this->right_speedometer; } + uint16_t getDutycycleLeft() const { return this->leftMotor->getDutycycle(); } + uint16_t getDutycycleRight() const { return this->rightMotor->getDutycycle(); } - uint16_t getDutycycleLeft() const { return this->left_motor->getDutycycle(); } - uint16_t getDutycycleRight() const { return this->right_motor->getDutycycle(); } +private: + void run() override; + static void setSpeedometerDirection(Speedometer *speedometer, double value); + void calcTargetWheelSpeed(); + void regulateMotors(); + void updateCurrentWheelSpeed(); - private: - void run() override; - void setSpeedometerDirection(Speedometer *speedometer, double value); - void calcTargetWheelSpeed(); - void regulateMotors(); - void updateCurrentWheelSpeed(); + MotorControl *leftMotor; + MotorControl *rightMotor; + Speedometer *leftSpeedometer; + Speedometer *rightSpeedometer; + PID *leftPid; + PID *rightPid; - MotorControl *left_motor; - MotorControl *right_motor; - Speedometer *left_speedometer; - Speedometer *right_speedometer; - PID *left_pid; - PID *right_pid; + Status driving_status = Status::Stop; + DrivingSpeeds drivingSpeeds = {0, 0}; - Status driving_status = Status::Stop; + double wheelspeedLeftTarget = 0; + double wheelspeedRightTarget = 0; + double wheelspeedLeft = 0; + double wheelspeedRight = 0; + double leftPidOut = 0; + double rightPidOut = 0; - double x_speed = 0; - double rotation_speed = 0; + uint8_t overTimeCounter = 0; + uint8_t overTimeMax = 100; + int8_t rawPowerLeft = 0; + int8_t rawPowerRight = 0; - double wheelspeed_left_target = 0; - double wheelspeed_right_target = 0; - double wheelspeed_left = 0; - double wheelspeed_right = 0; - double left_pid_out; - double right_pid_out; - - uint8_t overTimeCounter = 0; - uint8_t overTimeMax = 100; - int8_t rawPowerLeft = 0; - int8_t rawPowerRight = 0; + static constexpr int8_t maxPercentage = 100; + static constexpr float minSpeed = 0.2; + static constexpr uint8_t loopDelay = 20; }; #endif // MOVE_CONTROL_H diff --git a/include/network.h b/include/network.h deleted file mode 100644 index cf2cd9b..0000000 --- a/include/network.h +++ /dev/null @@ -1,107 +0,0 @@ -/** - * @file network.h - * @author Alexander Klein (alex@kleiax.de) - * @brief Contains the Network class. - * @version 0.1 - * @date 2021-12-14 - * - * @copyright Copyright (c) 2021 - * - */ - -#ifndef NETWORK_H -#define NETWORK_H - -#include -#include -#include -#include -#include - -#include "networkConfig.h" - -typedef void (*recieveCallbackPtr) (const uint8_t * mac, const uint8_t *incomingData, int len); -typedef void (*sendCallbackPtr) (const uint8_t *mac_addr, esp_now_send_status_t status); - -/** - * @brief This class handles all network stuff - * - * This class only inherits static functions and members and use - * only the configuration data is given by networkConfig.h - * - */ -class Network { - public: - /** - * @brief Set the all IPs needed by this class - */ - static void setIps(); - - /** - * @brief Establish a connection to the WiFi. - * - * @return true Wifi is connected - * @return false Timeout - */ - static bool connectWifi(); - static bool connectEspNow(recieveCallbackPtr reci, sendCallbackPtr send); - static uint8_t getCurrentChannel(); - - /** - * @brief Set all general settings to connect to a broker. - */ - static void setupMQTT(); - - /** - * @brief Checks if mqtt is still connected - * - * If mqtt is not connect, this function try a reconnect - * with the function connectMQTT. This function should be - * called every mainloop. - * - * @return true if connected - * @return false if not connected - */ - static bool checkMQTT(); - static bool initMQTT(); - - /** - * @brief Checks if WiFi is still connected - * - * If WiFi is not connected, this function try a reconnect. - * This function should be called every mainloop. - */ - static void checkWiFi(); - - /** - * @brief Get the Mqtt Client object - * - * @return PubSubClient* - */ - static PubSubClient* getMqttClient(); - static const uint8_t * getBroadcastAddress() { return broadcastAddress; } - - private: - /** - * @brief Tries a connect to the broker - * - * @return true if the connect attemp was successful - * @return false if the connect attemp was unsuccessful - */ - static bool connectMQTT(); - static int32_t getWiFiChannel(const char *ssid); - - static esp_now_peer_info_t peerInfo; - static uint8_t broadcastAddress[6]; - - static IPAddress local_IP; - static IPAddress gateway; - static IPAddress subnet; - static IPAddress mqtt_server; - static IPAddress dnsServer; - - static WiFiClient wifi_client; - static PubSubClient* mqtt_client; -}; - -#endif // NETWORK_H diff --git a/include/networkConfig.h b/include/networkConfig.h index d62b70b..13aac37 100644 --- a/include/networkConfig.h +++ b/include/networkConfig.h @@ -2,104 +2,75 @@ * @file networkConfig.h * @author Alexander Klein (alex@kleiax.de) * @brief Contains the settings for the Network class - * + * * With different defines the location can be choosen. - * + * * @version 0.1 * @date 2022-02-15 - * + * * @copyright Copyright (c) 2022 - * + * */ -// General config -#define MQTT_TIME_RECONNECT 2500 +#include -/** - * @brief Wlan connect timeout - * - * This number multiplied by WLAN_CONNECT_LOOP_TIME is the - * timeout in milliseconds. - */ -#define WLAN_CONNECT_TIMEOUT 20 +#define HW1 -/** - * @brief Wlan connect loop time - * - * The time in milliseconds until the next connection attempt. - */ -#define WLAN_CONNECT_LOOP_TIME 500 -#define WLAN_DNS_SERVER "8.8.8.8" -#define ESP_NOW_CONTROLER_MAC {0xC8, 0xC9, 0xA3, 0xC8, 0x57, 0x10} - -#define NTRIP_RTK2GO - -// NTRIP config NRW -#ifdef NTRIP_NRW -#define NTRIP_HOST "www.sapos-nw-ntrip.de" -#define NTRIP_PORT 2101 -#define NTRIP_MOUNT_POINT "VRS_3_3G_NW" -#define NTRIP_USER "nw-916771" -#define NTRIP_PASSWORD "Schalke#246" -#endif // NTRIP_NRW - -// NTRIP config NS -#ifdef NTRIP_NS -#define NTRIP_HOST "openservice-sapos.niedersachsen.de" -#define NTRIP_PORT 2101 -#define NTRIP_MOUNT_POINT "VRS_3_2G_NI" -#define NTRIP_USER "ni_FHDo01" -#define NTRIP_PASSWORD "ALdx-1-3e49" -#endif // NTRIP_NS - -// NTRIP config RTK2Go -#ifdef NTRIP_RTK2GO -#define NTRIP_HOST "rtk2go.com" -#define NTRIP_PORT 2101 -#define NTRIP_MOUNT_POINT "GER-Dortmund" -#define NTRIP_USER "alklein1@gmx.de" -#define NTRIP_PASSWORD "none" -#endif // NTRIP_RTK2GO - - -#define HOTSPOT - -//Network config Hotspot #ifdef HOTSPOT -#define WLAN_SSID "Kleiax Handy" -#define WLAN_PASSWORD "12345677" -#define WLAN_IP "192.168.43.4" -#define WLAN_SUBNETMASK "255.255.128.0" -#define WLAN_GATEWAY "192.168.43.1" -// Only for defines reasons - #define MQTT_SERVER "172.22.64.216" - #define MQTT_PORT 1883 -#endif //HOTSPOT +namespace NetworkConfig +{ + const char ssid[] = "Kleiax Handy"; + const char password[] = "12345677"; + const char ip[] = "192.168.43.4"; + const char subnet[] = "255.255.255.0"; + const char gateway[] = "192.168.43.1"; + const char dns[] = "8.8.8.8"; + const bool mqtt = false; +} +#endif // HOTSPOT -//Network config Rhede +#ifdef HW1 +namespace NetworkConfig +{ + const char ssid[] = "hw1_gast"; + const char password[] = "KeineAhnung"; + const char ip[] = "192.168.0.4"; + const char subnet[] = "255.255.255.0"; + const char gateway[] = "192.168.0.1"; + const char dns[] = "8.8.8.8"; + const bool mqtt = false; +} +#endif // HW1 + +// Network config Rhede #ifdef RHEDE -#define WLAN_SSID "LebennigHuus" -#define WLAN_PASSWORD "Punica-699" -#define WLAN_IP "192.168.11.4" -#define WLAN_SUBNETMASK "255.255.128.0" -#define WLAN_GATEWAY "192.168.0.1" -#define MQTT -#define MQTT_SERVER "192.168.1.7" -#define MQTT_PORT 1883 -#define MQTT_AUTH -#define MQTT_USER "kleiax" -#define MQTT_PASSWORD "p?{$_~5%hBM7wrcFkr55KWr#" -#endif //RHEDE +namespace NetworkConfig +{ + const char ssid[] = "LebennigHuus"; + const char password[] = "Punica-699"; + const char ip[] = "192.168.11.4"; + const char subnet[] = "255.255.128.0"; + const char gateway[] = "192.168.0.1"; + const char dns[] = "8.8.8.8"; + const bool mqtt = true; +} +#endif // RHEDE -//Network config Doro -#ifdef DORO -#define WLAN_SSID "Tuedelkram" -#define WLAN_PASSWORD "3Kaesehoch!" -#define WLAN_IP "192.168.178.4" -#define WLAN_SUBNETMASK "255.255.255.0" -#define WLAN_GATEWAY "192.168.178.1" - // Only for defines reasons - #define MQTT_SERVER "172.22.64.216" - #define MQTT_PORT 1883 -#endif //DORO +namespace MqttConfig +{ + const char server[] = "192.168.1.7"; + const uint16_t port = 1883; + const char user[] = "kleiax"; + const char password[] = "p?{$_~5%hBM7wrcFkr55KWr#"; +} +namespace NtripConfig +{ + const char host[] = "rtk2go.com"; + const uint16_t port = 2101; + // const char mountPoint[] = "GER-Dortmund"; + // const char mountPoint[] = "GER-Papenburg"; + const char mountPoint[] = "Rtkramerntrip"; + const char user[] = "alklein1@gmx.de"; + const char password[] = "none"; +} diff --git a/lib/Battery/battery.cpp b/lib/Battery/battery.cpp index d13e496..73b77de 100644 --- a/lib/Battery/battery.cpp +++ b/lib/Battery/battery.cpp @@ -4,144 +4,235 @@ * @brief Contains the implementation of the class Battery * @version 0.1 * @date 2022-02-05 - * + * * @copyright Copyright (c) 2022 - * + * */ #include "battery.h" -Battery::Battery(uint8_t pin, uint32_t r1, uint32_t r2) { - this->pin = pin; - this->r1 = r1; - this->r2 = r2; +Battery::Battery(uint8_t pin, uint32_t firstResistor, uint32_t secondResistor) + : pin{pin}, firstResistor{firstResistor}, secondResistor{secondResistor}, + batteryVoltageFactor{firstResistor + secondResistor / static_cast(secondResistor)} +{ this->initBuffer(); - this->loopDelay = 100; + Component::loopDelay = Battery::loopDelay; } -Battery::Battery(uint8_t pin) { - this->pin = pin; +Battery::Battery(uint8_t pin) + : pin{pin} +{ this->initBuffer(); - this->loopDelay = 100; + Component::loopDelay = Battery::loopDelay; } -void Battery::run() { - this->readAdcToBuf(); - this->loopCounter++; - - if (this->loopCounter == this->calulationDelayMultiplier) { - this->calculateBatteryVoltage(); - this->calculateBatteryPercent(); - this->loopCounter = 0; - this->calculatetNewValues = true; - } - - return; -} - -double Battery::getBatteryVoltage() const { - double res = this->batteryVoltage; - return (int)(res*100+0.5)/100.0; -} - -bool Battery::isBatteryLow(double voltage) const { - if (this->getBatteryVoltage() <= voltage && this->batteryVoltage > this->absurdLowVoltage) - return true; - return false; -} - -bool Battery::isNewValue() { - if (!this->calculatetNewValues) - return false; - - this->calculatetNewValues = false; - return true; -} - -double Battery::calculateInputVoltage() { - // Reference voltage is 3v3 so maximum reading is 3v3 = 4095 in range 0 to 4095 - double reading = this->getBufAvg(); - if(reading < 1 || reading > 4095) return 0; - return - 0.000000000000016 * pow(reading,4) - + 0.000000000118171 * pow(reading,3) - - 0.000000301211691 * pow(reading,2) - + 0.001109019271794 * reading - + 0.034143524634089; -} - -void Battery::calculateBatteryVoltage() { - if (this->r1 && this->r2) { - this->batteryVoltage = (this->calculateInputVoltage() * (double) (this->r1 + this->r2)) / (double) this->r2; +void Battery::run() +{ + if (this->calibrationState != CalibrationState::None) + { + this->runCalibration(); return; } - uint16_t adcValue = this->getBufAvg(); - if (adcValue < this->rawAdcVoltages[0]) { + this->readAdcToBuf(); + this->loopCounter++; + + if (this->loopCounter >= this->calculateDelayMultiplier) + { + this->calculateBatteryVoltage(); + this->calculateBatteryPercent(); + this->loopCounter = 0; + this->calculatedNewValues = true; + } +} + +void Battery::runCalibration() +{ + if (this->calibrationState != CalibrationState::Reading) + { + return; + } + + this->readAdcToBuf(); + if (this->bufferPos == 0) + { + const uint16_t res = this->getBufAvg(); + this->newRawAdcVoltages[this->currentCalibrationVoltage] = res; + std::cout << "Index: " + << (int)this->currentCalibrationVoltage + << " Value: " + << (int)res + << std::endl; + this->currentCalibrationVoltage++; + this->calibrationState = CalibrationState::Waiting; + + if (this->currentCalibrationVoltage == Battery::rawAdcVoltagesCount) + { + this->calibrationState = CalibrationState::Finished; + } + } +} + +double Battery::getBatteryVoltage() const +{ + return static_cast((this->batteryVoltage * 100 + 0.5)) / 100.0; +} + +bool Battery::isBatteryLow(double voltage) const +{ + if (this->getBatteryVoltage() <= voltage && this->batteryVoltage > this->absurdLowVoltage) + { + return true; + } + return false; +} + +bool Battery::isNewValue() +{ + if (!this->calculatedNewValues) + { + return false; + } + + this->calculatedNewValues = false; + return true; +} + +void Battery::nextVoltageIsReady() +{ + if (this->calibrationState == CalibrationState::Waiting) + { + this->calibrationState = CalibrationState::Reading; + } +} + +void Battery::startCalibration() +{ + this->calibrationState = CalibrationState::Waiting; + this->currentCalibrationVoltage = 0; + this->bufferPos = 0; + Component::loopDelay = Battery::loopDelay / 2; + this->newRawAdcVoltages = new uint16_t[Battery::rawAdcVoltagesCount]; +} + +void Battery::finishCalibration() +{ + if (this->calibrationState != CalibrationState::None) + { + return; + } + + delete[] this->newRawAdcVoltages; + this->calibrationState = CalibrationState::None; + Component::loopDelay = Battery::loopDelay; +} + +double Battery::calculateInputVoltage() +{ + // Reference voltage is 3v3 so maximum reading is 3v3 = 4095 in range 0 to 4095 + double reading = this->getBufAvg(); + if (reading < 1 || reading > Battery::adcMaxValue) + { + return 0; + } + + return -this->adcCurveCoefficient[0] * pow(reading, 4) + this->adcCurveCoefficient[1] * pow(reading, 3) - this->adcCurveCoefficient[2] * pow(reading, 2) + this->adcCurveCoefficient[3] * reading + this->adcCurveCoefficient[4]; +} + +void Battery::calculateBatteryVoltage() +{ + if (this->firstResistor && this->secondResistor) + { + this->batteryVoltage = this->calculateInputVoltage() * this->batteryVoltageFactor; + return; + } + + const uint16_t adcValue = this->getBufAvg(); + if (adcValue < this->rawAdcVoltages[0]) + { this->batteryVoltage = -1; return; } - if (adcValue > this->rawAdcVoltages[this->rawAdcVoltagesCount] + 50) { - this->batteryVoltage = -2; - return; - } - - uint8_t index; - for (index = 1; index < this->rawAdcVoltagesCount; index++) { + uint8_t index = 1; + for (; index < this->rawAdcVoltagesCount; index++) + { if (adcValue < this->rawAdcVoltages[index]) + { break; + } } - double indexDelta = this->rawAdcVoltages[index] - this->rawAdcVoltages[index - 1]; - double valueDelta = this->rawAdcVoltages[index] - adcValue; + const double indexDelta = this->rawAdcVoltages[index] - this->rawAdcVoltages[index - 1]; + const double valueDelta = this->rawAdcVoltages[index] - adcValue; double voltage = this->startVoltage + (index - 1) * this->stepVoltage; voltage += valueDelta / indexDelta * this->stepVoltage; this->batteryVoltage = voltage; - - // std::cout << "Battery::calculateBatteryVoltage() - Voltage: " << voltage << " Index: " <<(int) index << " adcValue: " << (int) adcValue <<" iD: " << indexDelta << " vD: " << valueDelta << std::endl; } -void Battery::calculateBatteryPercent() { +void Battery::calculateBatteryPercent() +{ int8_t size = sizeof(this->capacityVoltages) / sizeof(*this->capacityVoltages); - uint8_t i; - for (i = 0; i < size; i++) { - if (this->batteryVoltage <= this->capacityVoltages[i]) + uint8_t index = 0; + for (; index < size; index++) + { + if (this->batteryVoltage <= this->capacityVoltages[index]) + { break; + } } - - if (i == 0) { - if (this->batteryVoltage > 6) - std::cout << "Critical low battery!" << std::endl; - } else if (i == size - 1) { - } else { - double diffToLowerVal = this->batteryVoltage - this->capacityVoltages[i - 1]; - double diffToHigherVal = this->capacityVoltages[i] - this->batteryVoltage; - if (diffToLowerVal > diffToHigherVal) - i--; + if (index == 0) + { + if (this->batteryVoltage > this->absurdLowVoltage) + { + std::cout << "Critical low battery!" << std::endl; + } } - this->batteryPercent = i * (100 / (size - 1)); + else if (index == size - 1) + { + } + else + { + const double diffToLowerVal = this->batteryVoltage - this->capacityVoltages[index - 1]; + const double diffToHigherVal = this->capacityVoltages[index] - this->batteryVoltage; + if (diffToLowerVal > diffToHigherVal) + { + index--; + } + } + this->batteryPercent = index * (100 / (size - 1)); } -void Battery::readAdcToBuf() { +void Battery::readAdcToBuf() +{ this->adcBuffer[this->bufferPos] = analogRead(this->pin); this->bufferPos++; if (this->bufferPos == Battery::bufferSize) + { this->bufferPos = 0; + } // std::cout << "Battery::readAdcToBuf added Value: " << this->adcBuffer[this->bufferPos] << std::endl; } -void Battery::initBuffer() { +void Battery::initBuffer() +{ for (uint8_t i = 0; i < Battery::bufferSize; i++) + { this->adcBuffer[i] = 0; + } } -uint16_t Battery::getBufAvg() const { +uint16_t Battery::getBufAvg() const +{ uint32_t res = 0; uint8_t emptyPos = 0; - for (uint8_t i = 0; i < Battery::bufferSize; i++) { + for (uint8_t i = 0; i < Battery::bufferSize; i++) + { if (this->adcBuffer[i] == 0) + { emptyPos++; + } res += this->adcBuffer[i]; } return res / (Battery::bufferSize - emptyPos); diff --git a/lib/Battery/battery.h b/lib/Battery/battery.h index f6837be..c2787a3 100644 --- a/lib/Battery/battery.h +++ b/lib/Battery/battery.h @@ -4,9 +4,9 @@ * @brief Contains a class for battery monitoring * @version 0.1 * @date 2022-02-05 - * + * * @copyright Copyright (c) 2022 - * + * */ #ifndef BATTERY_H @@ -21,98 +21,182 @@ /** * @brief A class for battery monitoring - * + * * This class reads the voltage from an analog pin to calculate the * charge level of a 3 Cell Li-Poly battery pack. The battery pack have - * to be after a voltage diveder, so that maximum voltage for the + * to be after a voltage divider, so that maximum voltage for the * microcontroller is 3.3 Volt. */ -class Battery : public Component { - public: - /** - * @brief Construct a new Battery object - * - * The voltage devider have to be calculated, so that the input - * voltage from 3.3 Volt is never exceeded. It is assumed that - * the microcontroller is connected to the second resistor. - * - * @param pin The analog to read from. - * @param r1 First resistor of the voltage devider. - * @param r2 Second resistor of the voltage devider. - */ - Battery(uint8_t pin, uint32_t r1, uint32_t r2); - Battery(uint8_t pin); +class Battery : public Component +{ +public: + /** + * @brief States when the calibration modes is active + * + * - None means that no calibration is running + * - Reading means that the adc takes multiple values to calculate an average + * - Waiting means that the user has to set the new wanted voltage + * - Finished means that all measurements was taken + */ + enum CalibrationState + { + None, + Reading, + Waiting, + Finished + }; - /** - * @brief Get the battery voltage - * - * @return double in Volt - */ - double getBatteryVoltage() const; + /** + * @brief Construct a new Battery object + * + * The voltage divider have to be calculated, so that the input + * voltage from 3.3 Volt is never exceeded. It is assumed that + * the microcontroller is connected to the second resistor. + * + * @param pin The analog to read from. + * @param firstResistor First resistor of the voltage divider. + * @param secondResistor Second resistor of the voltage divider. + */ + Battery(uint8_t pin, uint32_t firstResistor, uint32_t secondResistor); - /** - * @brief Get the charge level of the battery - * - * @return uint8_t charge level in percent - */ - uint8_t getBatteryPercent() const { return this->batteryPercent; } + /** + * @brief Construct a new Battery object + * + * With this constructor the real voltage is not calculated with the + * voltage divider but with a table which contains the raw reading from + * the adc mapped to a specific voltage + * + * @param pin + */ + Battery(uint8_t pin); - /** - * @brief Checks if the battery is low. - * - * The function will also return false if the battery voltage is - * absurd low. This is for the case that the uController is powered - * by usb and no battery is connected. - * - * @param voltage the limit the battery have to - * @return true if the battery is low - * @return false if the battery is high - */ - bool isBatteryLow(double voltage) const; + /** + * @brief Get the battery voltage + * + * @return double in Volt + */ + double getBatteryVoltage() const; - bool isNewValue(); + /** + * @brief Get the charge level of the battery + * + * @return uint8_t charge level in percent + */ + uint8_t getBatteryPercent() const { return this->batteryPercent; } - private: - void run() override; - double calculateInputVoltage(); - void calculateBatteryVoltage(); - void calculateBatteryPercent(); - void readAdcToBuf(); - void initBuffer(); - uint16_t getBufAvg() const; + /** + * @brief Checks if the battery is low. + * + * The function will also return false if the battery voltage is + * absurd low. This is for the case that the uController is powered + * by usb and no battery is connected. + * + * @param voltage the limit the battery have to + * @return true if the battery is low + * @return false if the battery is high + */ + bool isBatteryLow(double voltage) const; - static const uint8_t bufferSize = 30; + /** + * @brief Check if a new voltage was been calculated + * + * @return true + * @return false + */ + bool isNewValue(); - uint8_t absurdLowVoltage = 5; - uint8_t pin; - uint8_t batteryPercent = 0; - uint8_t batteryLowPercent = 10; - uint8_t bufferPos = 0; - uint8_t calulationDelayMultiplier = 10; - uint8_t loopCounter = 0; - uint16_t adcBuffer[bufferSize]; - uint32_t r1 = 0; - uint32_t r2 = 0; - bool calculatetNewValues = false; - double batteryVoltage = 0; + // Calibration + CalibrationState getCalibrationState() const { return this->calibrationState; } - const float capacityVoltages[21] = {9.82, 10.83, 11.06, 11.12, // 0 5 10 15 - 11.18, 11.24, 11.3, 11.36, // 20 25 30 35 - 11.39, 11.45, 11.51, 11.56, // 40 45 50 55 - 11.62, 11.74, 11.86, 11.95, // 60 65 70 75 - 12.07, 12.25, 12.33, 12.45, // 80 85 90 95 - 12.6 }; + /** + * @brief Get the current calibration voltage target + * + * The returned value stand for the index of the table for this reason + * the real value have to be calculated. After the returned voltage has been set + * you have to call nextVoltageIsReady(). + * + * @return uint8_t voltage multiply with 0,1 and add 7 + */ + uint8_t getCurrentCalibrationVoltage() const { return this->currentCalibrationVoltage; } - const uint8_t rawAdcVoltagesCount = 60; - const double startVoltage = 7; - const double stepVoltage = 0.1; - const uint16_t rawAdcVoltages[60] = // from 7.0V to 12.9V in 0.1V steps - {1820, 1851, 1880, 1910, 1937, 1967, 1992, 2020, 2048, 2080, - 2109, 2136, 2163, 2189, 2218, 2244, 2273, 2302, 2334, 2363, - 2391, 2415, 2441, 2471, 2499, 2531, 2557, 2587, 2617, 2646, - 2674, 2699, 2730, 2761, 2791, 2816, 2843, 2872, 2900, 2930, - 2958, 2991, 3017, 3049, 3080, 3115, 3144, 3178, 3208, 3242, - 3276, 3313, 3346, 3389, 3433, 3470, 3509, 3548, 3590, 3636}; + /** + * @brief Read next wanted voltage + * + * If this function is called, the calibration mode reads the new voltage + * and save the value in the table. + */ + void nextVoltageIsReady(); + + /** + * @brief Calibrate the battery readings + * + * This calibration has only an effect if the Component + * uses the table with the raw adc values. The calibration gives + * the user different voltages that have to be set with a + * laboratory power supply. The power supply have to be connected + * instead of the battery. + */ + void startCalibration(); + + /** + * @brief abort the calibration + */ + void finishCalibration(); + +private: + void run() override; + void runCalibration(); + double calculateInputVoltage(); + void calculateBatteryVoltage(); + void calculateBatteryPercent(); + void readAdcToBuf(); + void initBuffer(); + uint16_t getBufAvg() const; + + static constexpr uint8_t bufferSize = 30; + static constexpr uint8_t loopDelay = 100; + static constexpr uint16_t adcMaxValue = 4095; + + CalibrationState calibrationState = CalibrationState::None; + + uint8_t absurdLowVoltage = 5; + uint8_t pin; + uint8_t batteryPercent = 0; + uint8_t batteryLowPercent = 10; + uint8_t bufferPos = 0; + uint8_t calculateDelayMultiplier = 5; + uint8_t loopCounter = 0; + uint8_t currentCalibrationVoltage = 0; // *0.1 + 7 + uint16_t adcBuffer[bufferSize]; + uint16_t *newRawAdcVoltages = nullptr; + uint32_t firstResistor = 0; + uint32_t secondResistor = 0; + bool calculatedNewValues = false; + double batteryVoltage = 0; + double batteryVoltageFactor; + + const float capacityVoltages[21] = {9.82, 10.83, 11.06, 11.12, // 0 5 10 15 + 11.18, 11.24, 11.3, 11.36, // 20 25 30 35 + 11.39, 11.45, 11.51, 11.56, // 40 45 50 55 + 11.62, 11.74, 11.86, 11.95, // 60 65 70 75 + 12.07, 12.25, 12.33, 12.45, // 80 85 90 95 + 12.6}; + + static constexpr uint8_t rawAdcVoltagesCount = 60; + const double startVoltage = 7; + const double stepVoltage = 0.1; + const uint16_t rawAdcVoltages[rawAdcVoltagesCount] = // from 7.0V to 12.9V in 0.1V steps + {1992, 2021, 2056, 2090, 2118, 2145, 2177, 2208, 2241, 2272, + 2298, 2331, 2362, 2387, 2420, 2453, 2482, 2514, 2543, 2577, + 2607, 2640, 2670, 2703, 2736, 2763, 2794, 2826, 2858, 2890, + 2920, 2956, 2983, 3019, 3054, 3088, 3121, 3158, 3189, 3226, + 3264, 3300, 3339, 3379, 3414, 3453, 3500, 3544, 3598, 3636, + 3682, 3730, 3781, 3837, 3887, 3943, 3997, 4054, 4093, 4095}; + const double adcCurveCoefficient[5] = {0.000000000000016, + 0.000000000118171, + 0.000000301211691, + 0.001109019271794, + 0.034143524634089}; }; #endif // BATTERY_H diff --git a/lib/CalcAzimuth/calcAzimuth.cpp b/lib/CalcAzimuth/calcAzimuth.cpp index 8db7a5d..f7f3c4a 100644 --- a/lib/CalcAzimuth/calcAzimuth.cpp +++ b/lib/CalcAzimuth/calcAzimuth.cpp @@ -1,92 +1,121 @@ /** * @file calcAzimuth.cpp * @author Alexander Klein (alex@kleiax.de) - * @brief + * @brief * @version 0.1 * @date 2023-09-03 - * + * * @copyright Copyright (c) 2023 - * + * */ #include "calcAzimuth.h" -CalcAzimuth::CalcAzimuth(Point point) { - this->lastChangePoint = point; - this->currentPosition = point; - this->loopDelay = 50; +CalcAzimuth::CalcAzimuth(Point point) + : lastChangePoint{point}, currentPosition{point} +{ + Component::loopDelay = CalcAzimuth::loopDelay; } -void CalcAzimuth::drivingDirectionChange(Point point) { - if (point.isInit() && point.isValid()) { +void CalcAzimuth::drivingDirectionChange(Point point) +{ + if (point.isInit() && point.isValid()) + { this->directionChangeMode = true; this->lastChangePoint = point; this->state = State::Invalid; } } -void CalcAzimuth::updateCurrentPosition(Point point) { +void CalcAzimuth::updateCurrentPosition(Point point) +{ this->currentPosition = point; this->positionChanged = true; } -void CalcAzimuth::run() { +String CalcAzimuth::stateToString(State state) +{ + switch (state) + { + case State::Invalid: + return "Invalid"; + + case State::Bad: + return "Bad"; + + case State::Ok: + return "Ok"; + + case State::Good: + return "Good"; + + case State::Super: + return "Super"; + + default: + return "UNKOWN"; + } +} + +void CalcAzimuth::run() +{ if (!this->positionChanged) + { return; + } this->positionChanged = false; this->updateAzimuth(); } -void CalcAzimuth::updateAzimuth() { - if (!this->directionChangeMode - || this->lastChangePoint.distanceTo(this->currentPosition) < 1.0) +void CalcAzimuth::updateAzimuth() +{ + if (!this->directionChangeMode || this->lastChangePoint.distanceTo(this->currentPosition) < 1.0) { this->state = State::Invalid; - this->calcAzimuth = 999; + this->calcAzimuth = INT16_MIN; return; } this->calcAzimuth = this->lastChangePoint.courseTo(this->currentPosition); // Map point accuracy to State - if (this->lastChangePoint.getAccuracy() == Point::Accuracy::oneDigOfCM - || this->currentPosition.getAccuracy() == Point::Accuracy::oneDigOfCM) + if (this->lastChangePoint.getAccuracy() == Point::Accuracy::oneDigOfCM || this->currentPosition.getAccuracy() == Point::Accuracy::oneDigOfCM) { this->state = State::Good; - } - else if (this->lastChangePoint.getAccuracy() == Point::Accuracy::twoDigOfCM - || this->currentPosition.getAccuracy() == Point::Accuracy::twoDigOfCM) + } + else if (this->lastChangePoint.getAccuracy() == Point::Accuracy::twoDigOfCM || this->currentPosition.getAccuracy() == Point::Accuracy::twoDigOfCM) { this->state = State::Ok; - } - else if (this->lastChangePoint.getAccuracy() == Point::Accuracy::threeDigOfCM - || this->currentPosition.getAccuracy() == Point::Accuracy::threeDigOfCM) + } + else if (this->lastChangePoint.getAccuracy() == Point::Accuracy::threeDigOfCM || this->currentPosition.getAccuracy() == Point::Accuracy::threeDigOfCM) { this->state = State::Bad; - } - else + } + else { this->state = State::Invalid; } // Upgrade quality if the range grows up - if (this->lastChangePoint.distanceTo(this->currentPosition) > 2.0) { - switch (this->state) { - case State::Bad : - this->state = State::Ok; - break; - - case State::Ok : - this->state = State::Good; - break; + if (this->lastChangePoint.distanceTo(this->currentPosition) > this->minDistanceForBetterQuality) + { + switch (this->state) + { + case State::Bad: + this->state = State::Ok; + break; - case State::Good : - this->state = State::Super; - break; + case State::Ok: + this->state = State::Good; + break; - default: - break; + case State::Good: + this->state = State::Super; + break; + + default: + break; } } } diff --git a/lib/CalcAzimuth/calcAzimuth.h b/lib/CalcAzimuth/calcAzimuth.h index b44cf8f..4b18315 100644 --- a/lib/CalcAzimuth/calcAzimuth.h +++ b/lib/CalcAzimuth/calcAzimuth.h @@ -1,12 +1,12 @@ /** * @file calcAzimuth.h * @author Alexander Klein (alex@kleiax.de) - * @brief + * @brief Contains a class that calculates the azimuth from a last and a current position * @version 0.1 * @date 2023-09-03 - * + * * @copyright Copyright (c) 2023 - * + * */ #ifndef CALC_AZIMUTH_H @@ -15,36 +15,79 @@ #include "component.h" #include "point.h" -class CalcAzimuth : public Component { - public: - enum State { - Invalid, - Bad, - Ok, - Good, - Super - }; +/** + * @brief A class to calculate an azimuth + * + * This class calculates the current Azimuth with the last position + * where the rover has been rotated and the current position + */ +class CalcAzimuth : public Component +{ +public: + /** + * @brief States which represent the quality of the current calculated azimuth + */ + enum State + { + Invalid, + Bad, + Ok, + Good, + Super + }; - CalcAzimuth(Point point); + /** + * @brief Construct a new Calc Azimuth object + * + * @param point current position + */ + CalcAzimuth(Point point); - void drivingDirectionChange(Point point); - void updateCurrentPosition(Point point); - void disableCalcAzimuth() { this->directionChangeMode = false; } + /** + * @brief Have to be called if the rover rotates + * + * @param point current position + */ + void drivingDirectionChange(Point point); - int16_t getAzimuth() const { return this->calcAzimuth; } - State getState() const { return this->state; } + /** + * @brief update the current position + * + * This function should be called if the rover has moved in + * a straight direction, to calculated the current Azimuth. + * More distance to the point given to drivingDirectionChange() + * increase the accuracy of the calculation. + * + * @param point current position + */ + void updateCurrentPosition(Point point); + void disableCalcAzimuth() { this->directionChangeMode = false; } - private: - void run() override; - void updateAzimuth(); + int16_t getAzimuth() const { return this->calcAzimuth; } - State state = State::Invalid; - Point lastChangePoint; - Point currentPosition; + /** + * @brief Get the State struct + * + * @return State current quality of the calculation + */ + State getState() const { return this->state; } - bool positionChanged = false; - bool directionChangeMode = false; - int16_t calcAzimuth = INT16_MAX; + static String stateToString(State state); + +private: + void run() override; + void updateAzimuth(); + + State state = State::Invalid; + Point lastChangePoint; + Point currentPosition; + + bool positionChanged = false; + bool directionChangeMode = false; + int16_t calcAzimuth = INT16_MAX; + double minDistanceForBetterQuality = 2; + + static constexpr uint8_t loopDelay = 50; }; -#endif //CALC_AZIMUTH_H +#endif // CALC_AZIMUTH_H diff --git a/lib/calibrateCompass/calibrateCompass.cpp b/lib/CalibrateCompass/calibrateCompass.cpp similarity index 53% rename from lib/calibrateCompass/calibrateCompass.cpp rename to lib/CalibrateCompass/calibrateCompass.cpp index 90586a2..d618f38 100644 --- a/lib/calibrateCompass/calibrateCompass.cpp +++ b/lib/CalibrateCompass/calibrateCompass.cpp @@ -1,68 +1,81 @@ /** * @file calibrateCompass.cpp * @author Alexander Klein (alex@kleiax.de) - * @brief + * @brief * @version 0.1 * @date 2023-05-23 - * + * * @copyright Copyright (c) 2023 - * + * */ #include "calibrateCompass.h" -CalibrateCompass::CalibrateCompass(QMC5883LCompass* compass) { - this->compass = compass; +CalibrateCompass::CalibrateCompass(QMC5883LCompass *compass) + : compass{compass} +{ this->state = State::Ready; this->clearData(); this->activateOnlyChilds(); } -void CalibrateCompass::runAsChild() { +void CalibrateCompass::runAsChild() +{ if (this->state != State::Calibrating) + { return; + } bool changed = false; this->compass->read(); - int x = this->compass->getX(); - int y = this->compass->getY(); - int z = this->compass->getZ(); + const int xAxis = this->compass->getX(); + const int yAxis = this->compass->getY(); + const int zAxis = this->compass->getZ(); - if(x < this->data.data[0][0]) { - this->data.data[0][0] = x; + if (xAxis < this->data.data[0][0]) + { + this->data.data[0][0] = xAxis; changed = true; } - if(x > this->data.data[0][1]) { - this->data.data[0][1] = x; + if (xAxis > this->data.data[0][1]) + { + this->data.data[0][1] = xAxis; changed = true; } - if(y < this->data.data[1][0]) { - this->data.data[1][0] = y; + if (yAxis < this->data.data[1][0]) + { + this->data.data[1][0] = yAxis; changed = true; } - if(y > this->data.data[1][1]) { - this->data.data[1][1] = y; + if (yAxis > this->data.data[1][1]) + { + this->data.data[1][1] = yAxis; changed = true; } - if(z < this->data.data[2][0]) { - this->data.data[2][0] = z; + if (zAxis < this->data.data[2][0]) + { + this->data.data[2][0] = zAxis; changed = true; } - if(z > this->data.data[2][1]) { - this->data.data[2][1] = z; + if (zAxis > this->data.data[2][1]) + { + this->data.data[2][1] = zAxis; changed = true; } if (changed) + { this->lastChange = millis(); + } - if (millis() - this->lastChange > this->maxTimeWithoutChange) { + if (millis() - this->lastChange > this->maxTimeWithoutChange) + { this->state = State::Finished; this->checkDataValidity(); } @@ -70,9 +83,12 @@ void CalibrateCompass::runAsChild() { void CalibrateCompass::run() {} -void CalibrateCompass::start() { +void CalibrateCompass::start() +{ if (this->state != State::Ready) + { return; + } this->clearData(); this->state = State::Calibrating; @@ -80,39 +96,45 @@ void CalibrateCompass::start() { this->lastChange = millis(); } -void CalibrateCompass::useData() { - if (!this->dataValid) { +void CalibrateCompass::useData() +{ + if (!this->dataValid) + { std::cout << "CalibrateCompass::useData - Data not valid" << std::endl; return; } - this->compass->setCalibration( this->data.data[0][0], - this->data.data[0][1], - this->data.data[1][0], - this->data.data[1][1], - this->data.data[2][0], - this->data.data[2][1] - ); + this->compass->setCalibration(this->data.data[0][0], + this->data.data[0][1], + this->data.data[1][0], + this->data.data[1][1], + this->data.data[2][0], + this->data.data[2][1]); - std::cout << "CalibrateCompass::useData " << *this << std::endl; + std::cout << "CalibrateCompass::useData " << *this << std::endl; } -void CalibrateCompass::removeCalibration() { +void CalibrateCompass::removeCalibration() +{ this->compass->clearCalibration(); } -void CalibrateCompass::reset() { +void CalibrateCompass::reset() +{ this->clearData(); this->state = State::Ready; } -void CalibrateCompass::saveData() { +void CalibrateCompass::saveData() +{ if (!this->dataValid) + { return; + } Preferences preferences; preferences.begin("compass", false); - + preferences.putInt("xLow", this->data.data[0][0]); preferences.putInt("xHigh", this->data.data[0][1]); preferences.putInt("yLow", this->data.data[1][0]); @@ -123,7 +145,8 @@ void CalibrateCompass::saveData() { preferences.end(); } -void CalibrateCompass::loadData() { +void CalibrateCompass::loadData() +{ Preferences preferences; preferences.begin("compass", true); @@ -138,42 +161,46 @@ void CalibrateCompass::loadData() { this->checkDataValidity(); } -void CalibrateCompass::clearData() { - for (uint8_t i = 0; i < 3; i++) { +void CalibrateCompass::clearData() +{ + for (uint8_t i = 0; i < 3; i++) + { this->data.data[i][0] = 0; this->data.data[i][1] = 0; } this->dataValid = false; } -void CalibrateCompass::checkDataValidity() { +void CalibrateCompass::checkDataValidity() +{ int sum = 0; - for (uint8_t i = 0; i < 3; i++) { - if (this->data.data[i][0] > INT16_MAX || this->data.data[i][0] < INT16_MIN - || this->data.data[i][1] > INT16_MAX || this->data.data[i][1] < INT16_MIN) - { + for (uint8_t i = 0; i < 3; i++) + { + if (this->data.data[i][0] > INT16_MAX || this->data.data[i][0] < INT16_MIN || this->data.data[i][1] > INT16_MAX || this->data.data[i][1] < INT16_MIN) + { this->dataValid = false; return; } sum += this->data.data[i][0]; sum += this->data.data[i][1]; } - this->dataValid = sum; + this->dataValid = static_cast(sum); } -std::ostream& operator<<(std::ostream& os, const CalibrateCompass& caliComp) { - os << "("; - os << caliComp.data.data[0][0]; - os << ", "; - os << caliComp.data.data[0][1]; - os << ", "; - os << caliComp.data.data[1][0]; - os << ", "; - os << caliComp.data.data[1][1]; - os << ", "; - os << caliComp.data.data[2][0]; - os << ", "; - os << caliComp.data.data[2][1]; - os << ")"; - return os; +std::ostream &operator<<(std::ostream &stream, const CalibrateCompass &caliComp) +{ + stream << "("; + stream << caliComp.data.data[0][0]; + stream << ", "; + stream << caliComp.data.data[0][1]; + stream << ", "; + stream << caliComp.data.data[1][0]; + stream << ", "; + stream << caliComp.data.data[1][1]; + stream << ", "; + stream << caliComp.data.data[2][0]; + stream << ", "; + stream << caliComp.data.data[2][1]; + stream << ")"; + return stream; } diff --git a/lib/CalibrateCompass/calibrateCompass.h b/lib/CalibrateCompass/calibrateCompass.h new file mode 100644 index 0000000..f5ce2da --- /dev/null +++ b/lib/CalibrateCompass/calibrateCompass.h @@ -0,0 +1,110 @@ +/** + * @file calibrateCompass.h + * @author Alexander Klein (alex@kleiax.de) + * @brief Contains a class to calibrate the compass module + * @version 0.1 + * @date 2023-05-23 + * + * @copyright Copyright (c) 2023 + * + */ + +#pragma once + +#include +#include +#include + +#include "component.h" + +/** + * @brief A Class to calibrate the compass + * + * This class reads the raw values of the compass while + * the rove have to be moved. The lowest and highest values + * are used to calibrate the compass to the current location. + */ +class CalibrateCompass : public Component +{ +public: + /** + * @brief State of the calibration process + */ + enum State + { + Ready, + Calibrating, + Finished + }; + + /** + * @brief Type for calibration data + * + * The data consist of 6 values. For each for the 3 axis + * are to integer needed. + */ + struct CalibrationData + { + int data[3][2]; + }; + + CalibrateCompass(QMC5883LCompass *compass); + + /** + * @brief Starts the calibration + */ + void start(); + + /** + * @brief Use the measured calibration data + */ + void useData(); + + /** + * @brief Remove the measured calibration data + */ + void removeCalibration(); + + /** + * @brief Reset the calibration process to start again + */ + void reset(); + + /** + * @brief Save the measured calibration data to the flash + */ + void saveData(); + + /** + * @brief Load the measured calibration data from the flash + * + */ + void loadData(); + + State getState() const { return this->state; } + CalibrationData getCalibrationData() const { return this->data; } + + /** + * @brief Makes the calibration data printable with std::cout() + * + * @param stream + * @param caliComp + * @return std::ostream& + */ + friend std::ostream &operator<<(std::ostream &stream, const CalibrateCompass &caliComp); + +private: + void runAsChild() override; + void run() override; + void checkDataValidity(); + + QMC5883LCompass *compass; + State state = State::Ready; + CalibrationData data{}; + + void clearData(); + + bool dataValid = false; + const uint16_t maxTimeWithoutChange = 10000; + uint32_t lastChange = 0; +}; diff --git a/lib/Component/component.cpp b/lib/Component/component.cpp index 05b035e..0595c77 100644 --- a/lib/Component/component.cpp +++ b/lib/Component/component.cpp @@ -1,25 +1,35 @@ #include "component.h" -Component::Component(uint16_t loopDelay) { - this->loopDelay = loopDelay; +Component::Component(uint16_t loopDelay) : loopDelay{loopDelay} +{ } -void Component::loop() { +void Component::loop() +{ if (!this->active) + { return; - - if (this->childComponents.size()){ - std::list::iterator it; - for (it = this->childComponents.begin(); it != this->childComponents.end(); it++) - (*it)->loop(); } - this->runAsChild(); + + if (this->childComponents.size()) + { + std::list::iterator it; + for (it = this->childComponents.begin(); it != this->childComponents.end(); it++) + { + (*it)->loop(); + } + } + this->runAsChild();F if (this->onlyChilds) + { return; + } - if (this->loopDelay && millis() - this->lastMillis < this->loopDelay) + if (static_cast(this->loopDelay) && millis() - this->lastMillis < this->loopDelay) + { return; + } this->lastMillis = millis(); @@ -28,13 +38,17 @@ void Component::loop() { this->afterRun(); if (this->timeUpdateAfter) + { this->lastMillis = millis(); + } } -void Component::addChildComponent(Component* child) { +void Component::addChildComponent(Component *child) +{ this->childComponents.push_back(child); } -void Component::removeChildComponent(Component* child) { +void Component::removeChildComponent(Component *child) +{ this->childComponents.remove(child); } diff --git a/lib/Component/component.h b/lib/Component/component.h index d8e5542..bf37591 100644 --- a/lib/Component/component.h +++ b/lib/Component/component.h @@ -1,12 +1,12 @@ /** * @file component.h * @author Alexander Klein (alex@kleiax.de) - * @brief + * @brief Contains an interface to make non blocking components with delay. * @version 0.1 * @date 2023-08-16 - * + * * @copyright Copyright (c) 2023 - * + * */ #pragma once @@ -15,37 +15,130 @@ #include -class Component { - public: - Component() {} - Component(uint16_t loopDelay); +/** + * @brief An Interface to make components + * + * A component is a task that be called in a loop which not + * runs every loop, so every component has a non blocking delay. + * + * A component can manage other components which called children components. + */ +class Component +{ +public: + /** + * @brief Construct a new Component object + * + * With the default constructor the created component is + * by default inactive. This does not affect the execution of + * the children components. + */ + Component() {} - void loop(); - - void deactivate() { this->active = false; } - void activate() { this->active = false; } - - protected: - virtual void runAsChild() {} - virtual void beforeRun() {} - virtual void run() = 0; - virtual void afterRun() {} + /** + * @brief Construct a new Component object + * + * If the given parameter is zero, there are no differences to the + * default constructor. + * + * @param loopDelay the minimum time in milliseconds before the task runs + */ + Component(uint16_t loopDelay); - void addChildComponent(Component* child); - void removeChildComponent(Component* child); + /** + * @brief Runs the children components and the task + * + * The loop() function of the children is called every time this + * loop is called. + * + * The run() function which presents the task of this component is + * only called if the delay is reached. + */ + void loop(); - void activateOnlyChilds() { this->onlyChilds = true; } - void deactivateOnlyChilds() { this->onlyChilds = false; } + /** + * @brief Deactivate this component + * + * If the component is deactivated the call of loop() ha no effect + */ + void deactivate() { this->active = false; } + void activate() { this->active = false; } - void setTimerAfterTask() { this->timeUpdateAfter = true; } +protected: + /** + * @brief Override this function to avoid the delay + */ + virtual void runAsChild() {} - uint16_t loopDelay = 0; + /** + * @brief Runs befor the run() function + * + * This function is only called, if the delay + * is reached. + * The function do nothing, except the function is overwritten + * by the class which inherits this class. + */ + virtual void beforeRun() {} - private: - std::list childComponents; + /** + * @brief The actual task + * + * This function have to be overwritten by the inheriting class. + */ + virtual void run() = 0; - bool active = true; - bool onlyChilds = false; - bool timeUpdateAfter = false; - uint32_t lastMillis = 0; + /** + * @brief Runs after the run() function + * + * This function is only called, if the delay + * is reached. + * The function do nothing, except the function is overwritten + * by the class which inherits this class. + */ + virtual void afterRun() {} + + /** + * @brief Adds a child component + * + * The child component will be called every time the loop() function + * is called. + * + * @param child + */ + void addChildComponent(Component *child); + void removeChildComponent(Component *child); + + void activateOnlyChilds() { this->onlyChilds = true; } + + /** + * @brief Skip the actual task + * + * Same as set the loopDelay to zero. + */ + void deactivateOnlyChilds() { this->onlyChilds = false; } + + /** + * @brief Set the timer after task + * + * If this function is called once the measurement of the delay + * starts after task has finished. The default is, that the + * measurement begins at the start of the task. + */ + void setTimerAfterTask() { this->timeUpdateAfter = true; } + + /** + * @brief the minimum time in milliseconds before the task runs + * + * If this value is zero, the functions beforeRun(), run() and + * afterRun() would not be called + */ + uint16_t loopDelay = 0; + +private: + std::list childComponents; + + bool active = true; + bool onlyChilds = false; + bool timeUpdateAfter = false; + uint32_t lastMillis = 0; }; diff --git a/lib/ControllPad/controlPad.cpp b/lib/ControlPad/controlPad.cpp similarity index 66% rename from lib/ControllPad/controlPad.cpp rename to lib/ControlPad/controlPad.cpp index 187e65c..9fcf9ae 100644 --- a/lib/ControllPad/controlPad.cpp +++ b/lib/ControlPad/controlPad.cpp @@ -1,37 +1,43 @@ /** * @file controlPad.cpp * @author Alexander Klein (alex@kleiax.de) - * @brief + * @brief * @version 0.1 * @date 2023-03-30 - * + * * @copyright Copyright (c) 2023 - * + * */ #include "controlPad.h" -ControlPad::ControlPad() { - this->loopDelay = 5; -} +ControlPad::ControlPad() : Component(10), controlInput{0, 0, 0, 0} {} -void ControlPad::run() { - if(!this->connected) +void ControlPad::run() +{ + if (!this->connected) + { return; + } - if (millis() - this->lastMessageReceive > this->disconnectTime) { + if (millis() - this->lastMessageReceive > this->disconnectTime) + { this->connected = false; this->controlInput.buttons = 0; - this->controlInput.x = 127; - this->controlInput.y = 127; + this->controlInput.x = UINT8_MAX / 2; + this->controlInput.y = UINT8_MAX / 2; return; } if (this->lastButtons != controlInput.buttons) + { this->updated = true; + } - if (this->menuControl && this->controlInput.buttons > 0 && this->updated) { - if (this->firstButtonPress) { + if (static_cast(this->menuControl) && this->controlInput.buttons > 0 && this->updated) + { + if (this->firstButtonPress) + { this->menuControl->printMenu(); this->firstButtonPress = false; std::cout << "ControlPad::loop - First menu print" << std::endl; @@ -39,38 +45,48 @@ void ControlPad::run() { } if (ControlPadButton::isControlPadButtonPressed(&this->controlInput, ControlPadButton::PadButton::Left)) + { this->menuControl->left(); + } else if (ControlPadButton::isControlPadButtonPressed(&this->controlInput, ControlPadButton::PadButton::Right)) + { this->menuControl->right(); + } else if (ControlPadButton::isControlPadButtonPressed(&this->controlInput, ControlPadButton::PadButton::Up)) + { this->menuControl->up(); + } else if (ControlPadButton::isControlPadButtonPressed(&this->controlInput, ControlPadButton::PadButton::Down)) + { this->menuControl->down(); + } else if (ControlPadButton::isControlPadButtonPressed(&this->controlInput, ControlPadButton::PadButton::Yes)) + { this->menuControl->yes(); + } else if (ControlPadButton::isControlPadButtonPressed(&this->controlInput, ControlPadButton::PadButton::No)) + { this->menuControl->no(); + } this->updated = false; this->lastButtons = controlInput.buttons; } - return; } -void ControlPad::insertData(const uint8_t *data) { +void ControlPad::insertData(const uint8_t *data) +{ this->connected = true; this->lastMessageReceive = millis(); - uint8_t lastCount = this->controlInput.counter + 1; + const uint8_t lastCount = this->controlInput.counter + 1; memcpy(&(this->controlInput), data, sizeof(this->controlInput)); if (lastCount != this->controlInput.counter) std::cout << "ControlPad::insertData counter wrong value" << std::endl; - if (this->controlInput.x > 127 - this->deadZoneX - && this->controlInput.x < 127 + this->deadZoneX) - this->controlInput.x = 127; + if (this->controlInput.x > UINT8_MAX / 2 - this->deadZoneX && this->controlInput.x < UINT8_MAX / 2 + this->deadZoneX) + this->controlInput.x = UINT8_MAX / 2; - if (this->controlInput.y > 127 - this->deadZoneY - && this->controlInput.y < 127 + this->deadZoneY) - this->controlInput.y = 127; + if (this->controlInput.y > UINT8_MAX / 2 - this->deadZoneY && this->controlInput.y < UINT8_MAX / 2 + this->deadZoneY) + this->controlInput.y = UINT8_MAX / 2; } diff --git a/lib/ControlPad/controlPad.h b/lib/ControlPad/controlPad.h new file mode 100644 index 0000000..c0f0904 --- /dev/null +++ b/lib/ControlPad/controlPad.h @@ -0,0 +1,80 @@ +/** + * @file controlPad.h + * @author Alexander Klein (alex@kleiax.de) + * @brief Contains a class that gets the input data + * @version 0.1 + * @date 2023-03-30 + * + * @copyright Copyright (c) 2023 + * + */ + +#pragma once + +#include + +#include +#include + +/** + * @brief A class to manage inputs + * + * This class converts the incoming data to the buttons and + * the axis from the joystick. The converted data will be send + * to the menu. + */ +class ControlPad : public Component +{ +public: + ControlPad(); + + /** + * @brief Insert the incoming data to convert + * + * The data is converted to the ControlPadInput struct. + * + * @param data have to be 4 byte long + */ + void insertData(const uint8_t *data); + + /** + * @brief Set the MenuControl object + * + * The MenuControl object is used to control the Menu. + * + * @see Menu + * @see ControlPadInput + * + * @param menuControl + */ + void setMenuControl(MenuControl *menuControl) { this->menuControl = menuControl; } + + /** + * @brief Get the Control Pad Data + * + * The pointer holds the lates data from the ControlPad + * + * @return const ControlPadInput* + */ + const ControlPadInput *getControlPadDataPtr() const { return &this->controlInput; } + + bool isControlPadConnected() const { return this->connected; } + +private: + void run() override; + + MenuControl *menuControl = nullptr; + ControlPadInput controlInput; + + bool connected = false; + bool updated = false; + bool firstButtonPress = true; + + uint8_t deadZoneX = 20; + uint8_t deadZoneY = 20; + uint8_t lastButtons = 0; + + uint16_t disconnectTime = 100; + + uint32_t lastMessageReceive = 0; +}; diff --git a/lib/ControlPad/controlPadInput.h b/lib/ControlPad/controlPadInput.h new file mode 100644 index 0000000..2239744 --- /dev/null +++ b/lib/ControlPad/controlPadInput.h @@ -0,0 +1,79 @@ +/** + * @file controlPadInput.h + * @author Alexander Klein (alex@kleiax.de) + * @brief + * @version 0.1 + * @date 2023-03-30 + * + * @copyright Copyright (c) 2023 + * + */ + +#ifndef CONTROL_PAD_INPUT_H +#define CONTROL_PAD_INPUT_H + +#include + +struct ControlPadInput +{ + uint8_t buttons; + uint8_t x; + uint8_t y; + uint8_t counter; +}; + +class ControlPadButton +{ +public: + enum PadButton + { + Left = 4, + Right = 8, + Up = 2, + Down = 1, + Yes = 32, + No = 16, + Action = 64 + }; + + static bool isControlPadButtonPressed(const ControlPadInput *input, PadButton button) + { + uint8_t buttonNum = input->buttons; + switch (button) + { + case PadButton::Left: + return buttonNum & (uint8_t)PadButton::Left; + break; + + case PadButton::Right: + return buttonNum & (uint8_t)PadButton::Right; + break; + + case PadButton::Up: + return buttonNum & (uint8_t)PadButton::Up; + break; + + case PadButton::Down: + return buttonNum & (uint8_t)PadButton::Down; + break; + + case PadButton::Yes: + return buttonNum & (uint8_t)PadButton::Yes; + break; + + case PadButton::No: + return buttonNum & (uint8_t)PadButton::No; + break; + + case PadButton::Action: + return buttonNum & (uint8_t)PadButton::Action; + break; + + default: + return false; + ; + } + } +}; + +#endif // CONTROL_PAD_INPUT_H diff --git a/lib/ControllPad/controlPad.h b/lib/ControllPad/controlPad.h deleted file mode 100644 index 71412cf..0000000 --- a/lib/ControllPad/controlPad.h +++ /dev/null @@ -1,48 +0,0 @@ -/** - * @file controlPad.h - * @author Alexander Klein (alex@kleiax.de) - * @brief - * @version 0.1 - * @date 2023-03-30 - * - * @copyright Copyright (c) 2023 - * - */ - -#pragma once - -#include - -#include -#include - -class ControlPad : public Component { - public: - ControlPad(); - - void insertData(const uint8_t *data); - - void setMenuControl(MenuControl* menuControl) { this->menuControl = menuControl; } - - const ControlPadInput* getControlPadDataPtr() const { return &this->controlInput; } - - bool isControlPadConnected() const { return this->connected; } - - private: - void run() override; - - MenuControl* menuControl = nullptr; - ControlPadInput controlInput; - - bool connected = false; - bool updated = false; - bool firstButtonPress = true; - - uint8_t deadZoneX = 20; - uint8_t deadZoneY = 20; - uint8_t lastButtons = 0; - - uint16_t disconnectTime = 100; - - uint32_t lastMessageReceive = 0; -}; diff --git a/lib/ControllPad/controlPadInput.h b/lib/ControllPad/controlPadInput.h deleted file mode 100644 index aab44ef..0000000 --- a/lib/ControllPad/controlPadInput.h +++ /dev/null @@ -1,74 +0,0 @@ -/** - * @file controlPadInput.h - * @author Alexander Klein (alex@kleiax.de) - * @brief - * @version 0.1 - * @date 2023-03-30 - * - * @copyright Copyright (c) 2023 - * - */ - -#ifndef CONTROL_PAD_INPUT_H -#define CONTROL_PAD_INPUT_H - -#include - -struct ControlPadInput { - uint8_t buttons; - uint8_t x; - uint8_t y; - uint8_t counter; -}; - - -class ControlPadButton { - public: - enum PadButton { - Left = 4, - Right = 8, - Up = 2, - Down = 1, - Yes = 32, - No = 16, - Action = 64 - }; - - static bool isControlPadButtonPressed(const ControlPadInput *input, PadButton button) { - uint8_t buttonNum = input->buttons; - switch (button) { - case PadButton::Left : - return buttonNum & (uint8_t) PadButton::Left; - break; - - case PadButton::Right : - return buttonNum & (uint8_t) PadButton::Right; - break; - - case PadButton::Up : - return buttonNum & (uint8_t) PadButton::Up; - break; - - case PadButton::Down : - return buttonNum & (uint8_t) PadButton::Down; - break; - - case PadButton::Yes : - return buttonNum & (uint8_t) PadButton::Yes; - break; - - case PadButton::No : - return buttonNum & (uint8_t) PadButton::No; - break; - - case PadButton::Action : - return buttonNum & (uint8_t) PadButton::Action; - break; - - default: - return false;; - } - } -}; - -#endif // CONTROL_PAD_INPUT_H diff --git a/lib/Counter/counter.cpp b/lib/Counter/counter.cpp index ea69173..fa15a30 100644 --- a/lib/Counter/counter.cpp +++ b/lib/Counter/counter.cpp @@ -1,14 +1,14 @@ #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::Counter(uint8_t pin) + : pulsePin{pin}, unit{static_cast(Counter::amountOfCounter)} +{ + if (Counter::amountOfCounter <= Counter::maxCounter) + { Counter::amountOfCounter++; + } pcnt_config_t config; config.unit = this->unit; @@ -24,33 +24,40 @@ Counter::Counter(uint8_t pin) { pcnt_unit_config(&config); } -void Counter::pause() { +void Counter::pause() +{ pcnt_counter_pause(this->unit); } -void Counter::resume() { +void Counter::resume() +{ pcnt_counter_resume(this->unit); } -void Counter::clear() { +void Counter::clear() +{ pcnt_counter_clear(this->unit); } -int16_t Counter::getValue() const { - int16_t res; +int16_t Counter::getValue() const +{ + int16_t res = 0; pcnt_get_counter_value(this->unit, &res); return res; } -void Counter::setFilterValue(uint16_t value) { +void Counter::setFilterValue(uint16_t value) +{ pcnt_set_filter_value(this->unit, value); this->filterEnable(); } -void Counter::filterEnable() { +void Counter::filterEnable() +{ pcnt_filter_enable(this->unit); } -void Counter::filterDisable() { +void Counter::filterDisable() +{ pcnt_filter_disable(this->unit); } diff --git a/lib/Counter/counter.h b/lib/Counter/counter.h index 59c5a56..74c96b6 100644 --- a/lib/Counter/counter.h +++ b/lib/Counter/counter.h @@ -1,30 +1,77 @@ +/** + * @file counter.h + * @author Alexander Klein (alex@kleiax.de) + * @brief Contains a class that abstract the hardware counter from the esp32 + * @version 0.1 + * @date 2023-10-12 + * + * @copyright Copyright (c) 2023 + * + */ #pragma once #include #include +#include -class Counter { - public: - Counter(uint8_t pin); +/** + * @brief A class that abstract the hardware counter from the esp32 + */ +class Counter +{ +public: + /** + * @brief Construct a new Counter object + * + * @param pin with incoming pulses + */ + Counter(uint8_t pin); - void pause(); - void resume(); - void clear(); + /** + * @brief Pause the pulse counting + */ + void pause(); - int16_t getValue() const; + /** + * @brief Resume the pulse counting + */ + void resume(); - void setFilterValue(uint16_t value); - void filterEnable(); - void filterDisable(); + /** + * @brief Start counting by zero again + */ + void clear(); - private: - static constexpr int16_t highLimit = INT16_MAX; - static constexpr uint8_t lowLimit = 0; + /** + * @brief Get the counted pulses + * + * @return int16_t + */ + int16_t getValue() const; - static uint8_t amountOfCounter; + /** + * @brief Set the filter value + * + * The filter skip all pulses after a pulse for the filter time. + * The filter time depends on the frequency of the processor. The time + * for a whole tact multiplied with the filter value results the filter time. + * + * @param value max 1023 + */ + void setFilterValue(uint16_t value); - bool initalised = false; + void filterEnable(); + void filterDisable(); - uint8_t pulsePin; - pcnt_unit_t unit; +private: + static constexpr int16_t highLimit = INT16_MAX; + static constexpr uint8_t lowLimit = 0; + static constexpr uint8_t maxCounter = 6; + + static uint8_t amountOfCounter; + + bool initalized = false; + + uint8_t pulsePin; + pcnt_unit_t unit; }; diff --git a/lib/LcdWrapper/LcdWrapper.cpp b/lib/LcdWrapper/LcdWrapper.cpp index 527202f..80d66db 100644 --- a/lib/LcdWrapper/LcdWrapper.cpp +++ b/lib/LcdWrapper/LcdWrapper.cpp @@ -1,65 +1,83 @@ /** - * @file displayWrapper.cpp + * @file LcdWrapper.cpp * @author Alexander Klein (alex@kleiax.de) * @brief Contains the implementation of the class LcdWrapper * @version 0.1 * @date 2023-01-08 - * + * * @copyright Copyright (c) 2023 - * + * */ #include "LcdWrapper.h" -LcdWrapper::LcdWrapper(LiquidCrystal_I2C* lcd) { - this->lcd = lcd; +LcdWrapper::LcdWrapper(LiquidCrystal_I2C *lcd) + : lcd{lcd}, changed{false} +{ this->clear(); - this->changed = false; } -void LcdWrapper::run() { +void LcdWrapper::run() +{ if (!this->changed) + { return; + } this->lcd->clear(); - for (uint8_t i = 0; i < LcdWrapper::totalLines; i++) { + for (uint8_t i = 0; i < LcdWrapper::totalLines; i++) + { this->lcd->setCursor(0, i); - this->lcd->print(this->data[i]); + this->lcd->print(static_cast(this->data[i])); } - - if (this->callback) + + if (static_cast(this->callback)) + { this->callback(this->data, LcdWrapper::totalLines, LcdWrapper::totalRows); + } this->changed = false; } -void LcdWrapper::clear() { - for (uint8_t i = 0; i < LcdWrapper::totalLines; i++) { - for (uint8_t j = 0; j < LcdWrapper::totalRows; j++) { +void LcdWrapper::clear() +{ + for (uint8_t i = 0; i < LcdWrapper::totalLines; i++) + { + for (uint8_t j = 0; j < LcdWrapper::totalRows; j++) + { data[i][j] = ' '; } } this->changed = true; } -void LcdWrapper::setCursor(uint8_t row, uint8_t line) { +void LcdWrapper::setCursor(uint8_t row, uint8_t line) +{ if (row > LcdWrapper::totalRows - 1) + { row = LcdWrapper::totalRows - 1; + } if (line > LcdWrapper::totalLines - 1) + { line = LcdWrapper::totalLines - 1; + } this->cursorRow = row; this->cursorLine = line; } -void LcdWrapper::print(const char *str) { +void LcdWrapper::print(const char *str) +{ uint8_t inputStringPosition = 0; - for (uint8_t i = this->cursorRow; i < LcdWrapper::totalRows; i++) { + for (uint8_t i = this->cursorRow; i < LcdWrapper::totalRows; i++) + { if (str[inputStringPosition] == '\0') + { break; - else - this->data[this->cursorLine][i] = str[inputStringPosition]; + } + this->data[this->cursorLine][i] = str[inputStringPosition]; + inputStringPosition++; } this->changed = true; diff --git a/lib/LcdWrapper/LcdWrapper.h b/lib/LcdWrapper/LcdWrapper.h index e78e269..e000fef 100644 --- a/lib/LcdWrapper/LcdWrapper.h +++ b/lib/LcdWrapper/LcdWrapper.h @@ -1,12 +1,12 @@ /** - * @file displayWrapper.h + * @file LcdWrapper.h * @author Alexander Klein (alex@kleiax.de) * @brief Contains the LcdWrapper class * @version 0.1 * @date 2023-01-08 - * + * * @copyright Copyright (c) 2023 - * + * */ #ifndef LCD_WRAPPER_H @@ -17,59 +17,59 @@ #include #include -typedef void (*LcdWrapperCallback) (const char data[][16], uint8_t lines, uint8_t rows); +typedef void (*LcdWrapperCallback)(const char data[][16], uint8_t lines, uint8_t rows); /** - * @brief A class for the Menu class to print information - * + * @brief A class for the Menu class to print information + * * This class inherits the DisplayWrapper class as a interface. - * The class takes the information to print from any thread. The + * The class takes the information to print from any thread. The * loop function has to be called to print the data. */ -class LcdWrapper : public DisplayWrapper, public Component { - public: - /** - * @brief Construct a new Lcd Wrapper object - * - * @param lcd - */ - LcdWrapper(LiquidCrystal_I2C* lcd); +class LcdWrapper : public DisplayWrapper, public Component +{ +public: + /** + * @brief Construct a new Lcd Wrapper object + * + * @param lcd + */ + LcdWrapper(LiquidCrystal_I2C *lcd); - /** - * @brief Empty the buffer - * - */ - void clear() override; + /** + * @brief Empty the buffer + */ + void clear() override; - /** - * @brief Set point where data to be saved - * - * @param row - * @param line - */ - void setCursor(uint8_t row, uint8_t line) override; - void setCallback(LcdWrapperCallback callback) { this->callback = callback; } + /** + * @brief Set point where data to be saved + * + * @param row + * @param line + */ + void setCursor(uint8_t row, uint8_t line) override; + void setCallback(LcdWrapperCallback callback) { this->callback = callback; } - /** - * @brief Save the data to be printed - * - * @param str - */ - void print(const char *str) override; + /** + * @brief Save the data to be printed + * + * @param str + */ + void print(const char *str) override; - static constexpr uint8_t totalRows = 16; - static constexpr uint8_t totalLines = 2; - private: - void run() override; + static constexpr uint8_t totalRows = 16; + static constexpr uint8_t totalLines = 2; - - LiquidCrystal_I2C* lcd; - LcdWrapperCallback callback = nullptr; - char data[LcdWrapper::totalLines][LcdWrapper::totalRows]; +private: + void run() override; - uint8_t cursorRow = 0; - uint8_t cursorLine = 0; + LiquidCrystal_I2C *lcd; + LcdWrapperCallback callback = nullptr; + char data[LcdWrapper::totalLines][LcdWrapper::totalRows]; - bool changed = false; + uint8_t cursorRow = 0; + uint8_t cursorLine = 0; + + bool changed = false; }; #endif // DISPLAY_WRAPPER_H diff --git a/lib/MQTT/debugMqtt.cpp b/lib/MQTT/debugMqtt.cpp index 44a2fe9..e042414 100644 --- a/lib/MQTT/debugMqtt.cpp +++ b/lib/MQTT/debugMqtt.cpp @@ -5,99 +5,115 @@ * @see debugMqtt.h * @version 0.1 * @date 2021-12-13 - * + * * @copyright Copyright (c) 2021 - * + * */ #include "debugMqtt.h" -PubSubClient* DebugMqtt::client; +PubSubClient *DebugMqtt::client; Loglevel DebugMqtt::loglevel; bool DebugMqtt::isInit = false; char DebugMqtt::msg[MQTT_BUFFER_SIZE]; char DebugMqtt::topic[MQTT_BUFFER_SIZE]; - -DebugMqtt::DebugMqtt(const char* name, uint8_t bufSize) { - this->name = name; +DebugMqtt::DebugMqtt(const char *name, uint8_t bufSize) + : name{name} +{ if (bufSize != 0) + { this->bufSize = bufSize; + } this->buf = new char[this->bufSize]; } -DebugMqtt::~DebugMqtt() { +DebugMqtt::~DebugMqtt() +{ delete this->buf; } -void DebugMqtt::sendMsg(Loglevel loglevel, String topic, String msg) { - snprintf (DebugMqtt::msg, MQTT_BUFFER_SIZE, "%s: %s",this->name ,msg.c_str()); - this->sendData(loglevel, topic, DebugMqtt::msg); +void DebugMqtt::sendMsg(Loglevel loglevel, String topic, String msg) +{ + snprintf(static_cast(DebugMqtt::msg), MQTT_BUFFER_SIZE, static_cast("%s: %s"), this->name, msg.c_str()); + this->sendData(loglevel, topic, static_cast(DebugMqtt::msg)); } -void DebugMqtt::sendMsg(Loglevel loglevel, String msg) { +void DebugMqtt::sendMsg(Loglevel loglevel, String msg) +{ this->sendMsg(loglevel, "", msg); } -void DebugMqtt::sendData(Loglevel loglevel, String topic, String data) { - if (!DebugMqtt::isInit) { +void DebugMqtt::sendData(Loglevel loglevel, String topic, String data) +{ + if (!DebugMqtt::isInit) + { return; } - if (loglevel <= DebugMqtt::loglevel && loglevel > Loglevel::none) { - snprintf (DebugMqtt::topic, MQTT_BUFFER_SIZE, "%s%s", DebugMqtt::enum_to_string(loglevel).c_str(), topic.c_str()); - snprintf (DebugMqtt::msg, MQTT_BUFFER_SIZE, "%s", data.c_str()); - client->publish(DebugMqtt::topic, DebugMqtt::msg); + if (loglevel <= DebugMqtt::loglevel && loglevel > Loglevel::none) + { + snprintf(static_cast(DebugMqtt::topic), MQTT_BUFFER_SIZE, static_cast("%s%s"), DebugMqtt::enum_to_string(loglevel).c_str(), topic.c_str()); + snprintf(static_cast(DebugMqtt::msg), MQTT_BUFFER_SIZE, static_cast("%s"), data.c_str()); + client->publish(DebugMqtt::topic, static_cast(DebugMqtt::msg)); } } -void DebugMqtt::sendData(Loglevel loglevel, String data){ - this->sendData(loglevel, "", data); +void DebugMqtt::sendData(Loglevel loglevel, String data) +{ + DebugMqtt::sendData(loglevel, "", data); } -void DebugMqtt::writeToInflux(String measurement_name, String field_set, float measurement, uint64_t nanos) { +void DebugMqtt::writeToInflux(String measurement_name, String field_set, float measurement, uint64_t nanos) +{ // Example String: "weather temperature=82 1465839830100400200"; - snprintf(DebugMqtt::msg, MQTT_BUFFER_SIZE, "%s %s=%f %llu", measurement_name.c_str(), field_set.c_str(), measurement, nanos); - this->sendData(Loglevel::influx, DebugMqtt::msg); + snprintf(static_cast(DebugMqtt::msg), MQTT_BUFFER_SIZE, static_cast("%s %s=%f %llu"), measurement_name.c_str(), field_set.c_str(), measurement, nanos); + this->sendData(Loglevel::influx, static_cast(DebugMqtt::msg)); } -void DebugMqtt::addCharacter(char c) { - this->buf[this->bufPos] = c; +void DebugMqtt::addCharacter(char character) +{ + this->buf[this->bufPos] = character; this->bufPos++; - if (c == '\n' || this->bufPos >= this->bufSize - 1) { + if (character == '\n' || this->bufPos >= this->bufSize - 1) + { this->buf[this->bufPos - 1] = '\0'; this->sendMsg(Loglevel::info, buf); this->bufPos = 0; } } -void DebugMqtt::init(PubSubClient *client, Loglevel max_loglevel) { +void DebugMqtt::init(PubSubClient *client, Loglevel max_loglevel) +{ DebugMqtt::client = client; DebugMqtt::loglevel = max_loglevel; DebugMqtt::isInit = true; } -void DebugMqtt::changeLoglevel(Loglevel loglevel) { +void DebugMqtt::changeLoglevel(Loglevel loglevel) +{ DebugMqtt::loglevel = loglevel; } -String DebugMqtt::enum_to_string(Loglevel loglevel) { +String DebugMqtt::enum_to_string(Loglevel loglevel) +{ String topic = ""; topic += MQTT_DEBUG_TOPIC; - switch(loglevel){ - case Loglevel::error : + switch (loglevel) + { + case Loglevel::error: topic += "/Error"; break; - case Loglevel::warn : + case Loglevel::warn: topic += "/Warn"; break; - case Loglevel::info : + case Loglevel::info: topic += "/Info"; break; - case Loglevel::debug : + case Loglevel::debug: topic += "/Debug"; break; - case Loglevel::influx : + case Loglevel::influx: topic += "/Influx"; break; default: @@ -105,5 +121,4 @@ String DebugMqtt::enum_to_string(Loglevel loglevel) { break; } return topic; - } - +} diff --git a/lib/MQTT/debugMqtt.h b/lib/MQTT/debugMqtt.h index a93f783..bb17640 100644 --- a/lib/MQTT/debugMqtt.h +++ b/lib/MQTT/debugMqtt.h @@ -4,9 +4,9 @@ * @brief Inherits a class to send debug messages over MQTT * @version 0.1 * @date 2021-12-13 - * + * * @copyright Copyright (c) 2021 - * + * */ #ifndef DEBUG_MQTT_H #define DEBUG_MQTT_H @@ -14,10 +14,9 @@ #include #include - /** - * @brief - * + * @brief + * * If you want to change the default topic * to an other value, than you have to define this define * in your code befor you include this File. @@ -28,151 +27,155 @@ /** * @brief Defualt for max message size - * + * * If you want to change the default size of 128 Byte * to an other value, than you have to define this define * in your code befor you include this File. */ #ifndef MQTT_BUFFER_SIZE #define MQTT_BUFFER_SIZE 128 -#endif //MQTT_BUFFER_SIZE +#endif // MQTT_BUFFER_SIZE /** * @brief An enum to set the log level - * + * * The log level is the last part of the MQTT topic. * Unless you give sendMsg() or sendData() a additional * topic as String. - * + * * @see sendMsg() * @see sendData() - * + * */ -enum Loglevel { none, - error, - warn, - info, - debug, - influx}; +enum Loglevel +{ + none, + error, + warn, + info, + debug, + influx +}; /** * @brief A class to send debug messages over MQTT - * + * * This class send debug messages over MQTT with different topics * by the enum Loglevel. Besides that this class supports to send * data via Telegraf into Grafana. - * + * * @see Loglevel */ -class DebugMqtt { - public: - /** - * @brief Construct a new Debug Mqtt object. - * - * @param name A String with send with every Message. - * @param bufSize for the addCharacter function. - */ - DebugMqtt(const char* name, uint8_t bufSize = 0); +class DebugMqtt +{ +public: + /** + * @brief Construct a new Debug Mqtt object. + * + * @param name A String with send with every Message. + * @param bufSize for the addCharacter function. + */ + DebugMqtt(const char *name, uint8_t bufSize = 0); - ~DebugMqtt(); + ~DebugMqtt(); - /** - * @brief Send a Message via MQTT - * - * This function uses sendData to send the given string and - * add the name to the message given by the constructer. - * - * @see sendData() - * @see Loglevel - * - * @param loglevel Loglevel given by the enum Loglevel - * @param topic Additional topic behind loglevel - * @param msg The message to send as String - */ - void sendMsg(Loglevel loglevel, String topic, String msg); - void sendMsg(Loglevel loglevel, String msg); + /** + * @brief Send a Message via MQTT + * + * This function uses sendData to send the given string and + * add the name to the message given by the constructer. + * + * @see sendData() + * @see Loglevel + * + * @param loglevel Loglevel given by the enum Loglevel + * @param topic Additional topic behind loglevel + * @param msg The message to send as String + */ + void sendMsg(Loglevel loglevel, String topic, String msg); + void sendMsg(Loglevel loglevel, String msg); - /** - * @brief Send a Message via MQTT - * - * This function sends the Data via MQTT with the given topic - * from Loglevel or followed by given String topic. - * Normally this function is called by sendMsg() or by - * writeToInflux() - * - * @see sendData() - * @see writeToInflux() - * @see Loglevel - * - * @param loglevel Loglevel given by the enum Loglevel - * @param topic Additional topic behind loglevel - * @param data The message to send as String - */ - void sendData(Loglevel loglevel, String topic, String data); - void sendData(Loglevel loglevel, String data); + /** + * @brief Send a Message via MQTT + * + * This function sends the Data via MQTT with the given topic + * from Loglevel or followed by given String topic. + * Normally this function is called by sendMsg() or by + * writeToInflux() + * + * @see sendData() + * @see writeToInflux() + * @see Loglevel + * + * @param loglevel Loglevel given by the enum Loglevel + * @param topic Additional topic behind loglevel + * @param data The message to send as String + */ + static void sendData(Loglevel loglevel, String topic, String data); + static void sendData(Loglevel loglevel, String data); - /** - * @brief Send a Message via MQTT for InfluxDB - * - * This function sends a MQTT message which is intended for - * Telegraf. Telegraf can listen on MQTT messages and put - * them in an Influx Database. - * - * @param measurement_name like a category - * @param field_set the name of the value e.g temperature - * @param measurement the real value - * @param nanos Current time in nanoseconds - */ - void writeToInflux(String measurement_name, String field_set, float measurement, uint64_t nanos); + /** + * @brief Send a Message via MQTT for InfluxDB + * + * This function sends a MQTT message which is intended for + * Telegraf. Telegraf can listen on MQTT messages and put + * them in an Influx Database. + * + * @param measurement_name like a category + * @param field_set the name of the value e.g temperature + * @param measurement the real value + * @param nanos Current time in nanoseconds + */ + void writeToInflux(String measurement_name, String field_set, float measurement, uint64_t nanos); - /** - * @brief Adds a single character to the buf - * - * The buf will be flushed out: - * 1. when the buffer is full - * 2. when the character is '\n' - * - * @param c - */ - void addCharacter(char c); + /** + * @brief Adds a single character to the buf + * + * The buf will be flushed out: + * 1. when the buffer is full + * 2. when the character is '\n' + * + * @param character + */ + void addCharacter(char character); - /** - * @brief Initialize debugMQTT for all instances - * - * You only have to call this function once for your project. - * If you call this function again you overwrite the client and - * the loglevel. If you only want du overwrite the max_loglevel - * use changeLoglevel() - * - * @see changeLoglevel() - * - * @param client PubSubClient - * @param max_loglevel Max loglevel to send. - */ - static void init(PubSubClient* client, Loglevel max_loglevel); + /** + * @brief Initialize debugMQTT for all instances + * + * You only have to call this function once for your project. + * If you call this function again you overwrite the client and + * the loglevel. If you only want du overwrite the max_loglevel + * use changeLoglevel() + * + * @see changeLoglevel() + * + * @param client PubSubClient + * @param max_loglevel Max loglevel to send. + */ + static void init(PubSubClient *client, Loglevel max_loglevel); - /** - * @brief Change loglevel - * - * This function changes the maximum loglevel which be send. - * - * @param loglevel - */ - static void changeLoglevel(Loglevel loglevel); + /** + * @brief Change loglevel + * + * This function changes the maximum loglevel which be send. + * + * @param loglevel + */ + static void changeLoglevel(Loglevel loglevel); - private: - const char* name; - char* buf; - uint8_t bufPos = 0; - uint8_t bufSize = 100; +private: + const char *name; + char *buf; + uint8_t bufPos = 0; + uint8_t bufSize = 100; - static String enum_to_string(Loglevel loglevel); + static String enum_to_string(Loglevel loglevel); - static PubSubClient* client; - static Loglevel loglevel; - static bool isInit; - static char msg[MQTT_BUFFER_SIZE]; - static char topic[MQTT_BUFFER_SIZE]; + static PubSubClient *client; + static Loglevel loglevel; + static bool isInit; + static char msg[MQTT_BUFFER_SIZE]; + static char topic[MQTT_BUFFER_SIZE]; }; #endif // DEBUG_MQTT_H diff --git a/lib/MotorControl/motorControl.cpp b/lib/MotorControl/motorControl.cpp index de366e9..6ff901f 100644 --- a/lib/MotorControl/motorControl.cpp +++ b/lib/MotorControl/motorControl.cpp @@ -5,20 +5,20 @@ * @see motorControl.h * @version 0.1 * @date 2021-12-13 - * + * * @copyright Copyright (c) 2021 - * + * */ #include "motorControl.h" -MotorControl::MotorControl() { - this->setMinPwm(MotorControl::pwmMin); - this->setMaxPwm(MotorControl::pwmMax); +MotorControl::MotorControl() +{ Component::loopDelay = MotorControl::loopDelay; } -void MotorControl::init(uint8_t pwmPin, uint8_t pwmChannel, uint8_t dir_1, uint8_t dir_2) { +void MotorControl::init(uint8_t pwmPin, uint8_t pwmChannel, uint8_t dir_1, uint8_t dir_2) +{ this->pwmPin = pwmPin; this->pwmChannel = pwmChannel; this->dir_1 = dir_1; @@ -35,107 +35,141 @@ void MotorControl::init(uint8_t pwmPin, uint8_t pwmChannel, uint8_t dir_1, uint8 ledcWrite(this->pwmChannel, 0); } -void MotorControl::run() { +void MotorControl::run() +{ // Absolute difference between targetPower and power - uint8_t abs_difference = abs(this->targetPower - this->power); + const uint8_t abs_difference = abs(this->targetPower - this->power); // Difference between targetPower and power - int16_t difference = this->targetPower - this->power; + const int16_t difference = this->targetPower - this->power; // Check that the target speed is close to 0 and that the abs_difference is lower than MotorControl::powerSteps - if (abs(this->targetPower) < MotorControl::powerSteps && abs_difference < MotorControl::powerSteps) { + if (abs(this->targetPower) < MotorControl::powerSteps && abs_difference < MotorControl::powerSteps) + { this->setRealPower(0); return; } // Correct speed - if (abs_difference < MotorControl::powerSteps) { + if (abs_difference < MotorControl::powerSteps) + { return; } // Positive or negative tagret speed - if (this->targetPower >= 0) { + if (this->targetPower >= 0) + { // Positive or negative speed - if (this->power >= 0) { - if (difference > 0) { + if (this->power >= 0) + { + if (difference > 0) + { this->increasePower(MotorControl::powerSteps); - } else { + } + else + { this->increasePower(-MotorControl::powerSteps); } - } else { + } + else + { this->increasePower(MotorControl::powerSteps); } - - } else { + } + else + { // Positive or negative speed - if (this->power >= 0) { + if (this->power >= 0) + { this->increasePower(-MotorControl::powerSteps); - } else { - if (difference > 0) { + } + else + { + if (difference > 0) + { this->increasePower(MotorControl::powerSteps); - } else { + } + else + { this->increasePower(-MotorControl::powerSteps); } } } } -void MotorControl::setMinPwm(uint8_t min) { - if (min > 80) min = 80; - //transform percentage to real pwm value - min = (uint8_t) (((1 << pwmRes) - 1) * (min / 100.0)); +void MotorControl::setMinPwm(uint8_t min) +{ + if (min > MotorControl::maxPwmMin) + { + min = MotorControl::maxPwmMin; + } + // transform percentage to real pwm value + min = static_cast(((static_cast(1) << pwmRes) - 1) * (min / 100.0)); this->dutycycleMin = min; } -void MotorControl::setMaxPwm(uint8_t max) { - if (max > 100) max = 100; - //transform percentage to real pwm value - max = (uint8_t) (((1 << pwmRes) - 1) * (max / 100.0)); +void MotorControl::setMaxPwm(uint8_t max) +{ + if (max > 100) + { + max = 100; + } + // transform percentage to real pwm value + max = static_cast(((static_cast(1) << pwmRes) - 1) * (max / 100.0)); this->dutycycleMax = max; } -void MotorControl::setTargetPower(int8_t power) { +void MotorControl::setTargetPower(int8_t power) +{ if (power <= 100 && power >= -100) + { this->targetPower = power; + } else + { std::cout << " MotorControl::setTargetPower: Invalid Argument - Power: " << power << std::endl; + } } -void MotorControl::stop() { +void MotorControl::stop() +{ this->targetPower = 0; } -void MotorControl::emergencyStop() { +void MotorControl::emergencyStop() +{ setRealPower(0); } -bool MotorControl::isTargetPowerReached() const { - if (this->targetPower == this->power) - return true; - return false; +bool MotorControl::isTargetPowerReached() const +{ + return this->targetPower == this->power; } -bool MotorControl::isAccelerationPositive() const { - if (power < targetPower) - return true; - return false; +bool MotorControl::isAccelerationPositive() const +{ + return power < targetPower; } -bool MotorControl::isAccelerationNegative() const { - if (power > targetPower) - return true; - return false; +bool MotorControl::isAccelerationNegative() const +{ + return power > targetPower; } -void MotorControl::setRealPower(int8_t power) { - //TODO: Exceptionhandling - if (power <= 100 && power >= -100) { +void MotorControl::setRealPower(int8_t power) +{ + // TODO: Exceptionhandling + if (power <= 100 && power >= -100) + { this->power = power; - } else { + } + else + { return; } - if (this->power == 0) { + if (this->power == 0) + { this->direction = 0; digitalWrite(this->dir_1, LOW); digitalWrite(this->dir_2, LOW); @@ -144,13 +178,16 @@ void MotorControl::setRealPower(int8_t power) { return; } - uint8_t pwm_val = map(abs(power), 0, 100, this->dutycycleMin, this->dutycycleMax); + const uint8_t pwm_val = map(abs(power), 0, 100, this->dutycycleMin, this->dutycycleMax); - if ((this->direction == 1 || this->direction == 0) && power < 0){ // new direction backward + 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 + } + 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); @@ -160,10 +197,12 @@ void MotorControl::setRealPower(int8_t power) { this->dutycycle = pwm_val; } -void MotorControl::increasePower(int8_t power) { - //TODO: Exceptionhandling - //TODO: make a stop befor a direction change - if (abs(power) > 2 * MotorControl::powerSteps) { +void MotorControl::increasePower(int8_t power) +{ + // TODO: Exceptionhandling + // TODO: make a stop befor a direction change + if (abs(power) > 2 * MotorControl::powerSteps) + { Serial.println("Invalid Argument in MotorControl::increasePower"); return; } diff --git a/lib/MotorControl/motorControl.h b/lib/MotorControl/motorControl.h index 3294960..fc576cd 100644 --- a/lib/MotorControl/motorControl.h +++ b/lib/MotorControl/motorControl.h @@ -4,9 +4,9 @@ * @brief Inherits a class to control a motor with pwm signal. * @version 0.1 * @date 2021-12-09 - * + * * @copyright Copyright (c) 2021 - * + * */ #ifndef MOTOR_CONTROL_H #define MOTOR_CONTROL_H @@ -23,98 +23,98 @@ * You can control the acceleration of the motor, for example to * prevent a damage on your H-Bridge. */ -class MotorControl : public Component { - public: - MotorControl(); +class MotorControl : public Component +{ +public: + MotorControl(); - /** - * @brief Initialize the motorController - * - * @param pwmPin The output pin for the signal on the esp. - * @param pwmChannel One of the pwm channels from the esp. - * @param dir_1 First direction pin for the H-Bridge. - * @param dir_2 Second direction pin for the H-Bridge. - */ - void init(uint8_t pwmPin, uint8_t pwmChannel, uint8_t dir_1, uint8_t dir_2); + /** + * @brief Initialize the motorController + * + * @param pwmPin The output pin for the signal on the esp. + * @param pwmChannel One of the pwm channels from the esp. + * @param dir_1 First direction pin for the H-Bridge. + * @param dir_2 Second direction pin for the H-Bridge. + */ + void init(uint8_t pwmPin, uint8_t pwmChannel, uint8_t dir_1, uint8_t dir_2); - /** - * @brief Set the minimum duty cycle - * - * @param min duty cycle in percent - */ - void setMinPwm(uint8_t min); + /** + * @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); + /** + * @brief Set the maximum duty cycle + * + * @param max duty cycle in percent + */ + void setMaxPwm(uint8_t max); - /** - * @brief Set the Target Power - * - * If the given power is greater than 100 or smaller than -100, then - * this function only print an error to consol. - * - * @param power power in percent - */ - void setTargetPower(int8_t power); + /** + * @brief Set the Target Power + * + * If the given power is greater than 100 or smaller than -100, then + * this function only print an error to consol. + * + * @param power power in percent + */ + void setTargetPower(int8_t power); - /** - * @brief Stops the motor like setTargetPower() to 0 - * - */ - void stop(); + /** + * @brief Stops the motor like setTargetPower() to 0 + * + */ + void stop(); - /** - * @brief Stops the motor immediately - * - */ - void emergencyStop(); + /** + * @brief Stops the motor immediately + * + */ + void emergencyStop(); - /** - * @brief Get the current power - * - * @return int8_t percent of power (-100 to 100) - */ - int8_t getPower() const { return this->power; }; + /** + * @brief Get the current power + * + * @return int8_t percent of power (-100 to 100) + */ + int8_t getPower() const { return this->power; }; - /** - * @brief Get the target power - * - * @return int8_t percent of power (-100 to 100) - */ - int8_t getTargetPower() const { return this->targetPower; }; + /** + * @brief Get the target power + * + * @return int8_t percent of power (-100 to 100) + */ + int8_t getTargetPower() const { return this->targetPower; }; - uint16_t getDutycycle() const { return this->dutycycle; } - bool isTargetPowerReached() const; - bool isAccelerationPositive() const; - bool isAccelerationNegative() const; + uint16_t getDutycycle() const { return this->dutycycle; } + bool isTargetPowerReached() const; + bool isAccelerationPositive() const; + bool isAccelerationNegative() const; - private: - void run() override; - void setRealPower(int8_t power); - void increasePower(int8_t power); +private: + void run() override; + void setRealPower(int8_t power); + void increasePower(int8_t power); - static constexpr uint8_t loopDelay = 10; - static constexpr uint16_t pwmFreq = 16000; - static constexpr uint8_t pwmRes = 8; - static constexpr uint8_t powerSteps = 2; // A total of 20 levels ( 100 / SPEED_STEPS ) * RUN_MOTOR_CONTROL_DELAY = 500ms - static constexpr uint8_t pwmMin = 55; - static constexpr uint8_t pwmMax = 98; // Max 98% of 2^PWM_RES + static constexpr uint8_t loopDelay = 10; + static constexpr uint16_t pwmFreq = 16000; + static constexpr uint8_t pwmRes = 8; + static constexpr uint8_t powerSteps = 2; // A total of 20 levels ( 100 / SPEED_STEPS ) * RUN_MOTOR_CONTROL_DELAY = 500ms + static constexpr uint8_t maxPwmMin = 80; - int8_t targetPower = 0; - int8_t power = 0; - uint8_t direction = 0; // 0 = stop, 1 = forward, 2 = backward + int8_t targetPower = 0; + int8_t power = 0; + uint8_t direction = 0; // 0 = stop, 1 = forward, 2 = backward - uint8_t pwmPin; - uint8_t pwmChannel; - uint16_t dutycycle = 0; - uint8_t dutycycleMin; - uint8_t dutycycleMax; - uint8_t dir_1; - uint8_t dir_2; + uint8_t pwmPin = 0; + uint8_t pwmChannel = 0; + uint16_t dutycycle = 0; + uint8_t dutycycleMin = 55; + uint8_t dutycycleMax = 98; // Max 98% of 2^PWM_RES + uint8_t dir_1 = 0; + uint8_t dir_2 = 0; }; #endif // MOTOR_CONTROL_H diff --git a/lib/Network/network.cpp b/lib/Network/network.cpp new file mode 100644 index 0000000..93ae142 --- /dev/null +++ b/lib/Network/network.cpp @@ -0,0 +1,201 @@ +/** + * @file network.cpp + * @author Alexander Klein (alex@kleiax.de) + * @brief + * @version 0.1 + * @date 2023-09-18 + * + * @copyright Copyright (c) 2023 + * + */ + +#include "network.h" + +Network::Network(const char *ssid, const char *passphrase) +{ + if (!WiFiGenericClass::mode(WIFI_AP_STA)) + { + std::cout << "Network::connectWiFi failed WiFi.mode" << std::endl; + } + + this->init(ssid, passphrase); +} + +Network::Network(const char *ssid, const char *passphrase, NetworkAddresses adresses) + : addresses{adresses} +{ + if (!WiFiGenericClass::mode(WIFI_AP_STA)) + { + std::cout << "Network::connectWiFi failed WiFi.mode" << std::endl; + } + + if (!WiFi.config(this->addresses.localIP, + this->addresses.gateway, + this->addresses.subnet, + this->addresses.dnsServer)) + { + std::cout << "STA Failed to configure" << std::endl; + } + + this->init(ssid, passphrase); +} + +Network::~Network() +{ + delete mqttClient; +} + +bool Network::activateEspNow(receiveCallbackPtr reci, sendCallbackPtr send) +{ + if (esp_now_init() != ESP_OK) + { + std::cout << "Network::activateEspNow - Error initializing ESP-NOW" << std::endl; + return false; + } + + esp_now_register_send_cb(send); + + esp_now_peer_info_t peerInfo = {}; + memcpy(static_cast(peerInfo.peer_addr), static_cast(this->broadcastAddress), 6); + peerInfo.channel = 0; + peerInfo.encrypt = false; + + if (esp_now_add_peer(&peerInfo) != ESP_OK) + { + std::cout << "Network::connectEspNow - Failed to add peer" << std::endl; + return false; + } + + esp_now_register_recv_cb(reci); + return true; +} + +bool Network::activateMqtt(const char *user, const char *passphrase) +{ + this->mqttUser = user; + this->mqttPassphrase = passphrase; + + this->mqttClient = new PubSubClient(this->wifiClient); + this->mqttClient->setServer(this->addresses.mqttServer, this->addresses.mqttPort); + this->mqttClient->setSocketTimeout(1); + + if (this->wifiConnected) + { + return this->connectMqtt(); + } + return false; +} + +const void Network::printIPs() +{ + std::cout << std::endl; + if (!this->wifiConnected) + { + std::cout << "WiFi is not connected." << std::endl; + return; + } + std::cout << "WiFi is connected to" << std::endl; + std::cout << "IP address: " << std::endl; + std::cout << WiFi.localIP().toString().c_str() << std::endl; + std::cout << "WiFi MAC Address: " << WiFi.macAddress().c_str() << std::endl + << std::endl; +} + +uint8_t Network::getCurrentChannel() +{ + uint8_t channel = 0; + wifi_second_chan_t secondChannel = WIFI_SECOND_CHAN_NONE; + if (esp_wifi_get_channel(&channel, &secondChannel) != ESP_OK) + { + std::cout << "Network::getCurrentChannel - Error!" << std::endl; + return -1; + } + + return channel; +} + +void Network::runAsChild() +{ + if (!this->initSuccessful) + { + return; + } + + this->checkWifi(); + + if (this->wifiConnected && static_cast(this->mqttClient)) + { + this->checkMqtt(); + } +} + +void Network::init(const char *ssid, const char *passphrase) +{ + // Connect to Wi-Fi network with SSID and password + std::cout << "Connecting to " << ssid << std::endl; + WiFi.begin(ssid, passphrase); + uint8_t timeout = Network::wifiConnectTimeout; + while (WiFiSTAClass::status() != WL_CONNECTED) + { + delay(Network::wifiConnectLoopTime); + std::cout << "." << std::flush; + timeout--; + if (timeout == 0) + { + std::cout << std::endl; + std::cout << "WiFi NOT connected." << std::endl; + return; + } + } + + this->wifiConnected = true; + this->printIPs(); +} + +void Network::checkWifi() +{ + if ((WiFiSTAClass::status() != WL_CONNECTED) && (millis() - this->lastWifiReconnectAttempt >= Network::wifiReconnectDelay)) + { + std::cout << "Reconnecting to WiFi..." << std::endl; + WiFi.disconnect(); + this->wifiConnected = WiFi.reconnect(); + this->lastWifiReconnectAttempt = millis(); + } +} + +void Network::checkMqtt() +{ + if (!this->mqttClient->connected() && millis() - this->lastMqttReconnectAttempt > Network::mqttReconnectDelay) + { + this->mqttConnected = this->connectMqtt(); + this->lastMqttReconnectAttempt = millis(); + } + + if (this->mqttConnected) + { + this->mqttClient->loop(); + } +} + +bool Network::connectMqtt() +{ + String clientId = "ESP32Rover-"; + clientId += String(random(), HEX); + + if (static_cast(this->mqttUser)) + { + if (this->mqttClient->connect(clientId.c_str(), this->mqttUser, this->mqttPassphrase)) + { + this->mqttClient->publish("Rover/Info", "Connected to Mqtt-Broker"); + } + } + else + { + if (this->mqttClient->connect(clientId.c_str())) + { + this->mqttClient->publish("Rover/Info", "Connected to Mqtt-Broker"); + } + } + + return this->mqttClient->connected(); +} diff --git a/lib/Network/network.h b/lib/Network/network.h new file mode 100644 index 0000000..b35d255 --- /dev/null +++ b/lib/Network/network.h @@ -0,0 +1,136 @@ +/** + * @file network.h + * @author Alexander Klein (alex@kleiax.de) + * @brief + * @version 0.1 + * @date 2023-09-18 + * + * @copyright Copyright (c) 2023 + * + */ + +#ifndef NETWORK_H +#define NETWORK_H + +#include "component.h" + +#include +#include +#include +#include +#include + +/** + * @brief Typedef to easy handle the receive Callback + */ +typedef void (*receiveCallbackPtr)(const uint8_t *mac, const uint8_t *incomingData, int len); + +/** + * @brief Typedef to easy handle the send Callback + */ +typedef void (*sendCallbackPtr)(const uint8_t *mac_addr, esp_now_send_status_t status); + +/** + * @brief A Struct to hold all network addresses + */ +struct NetworkAddresses +{ + IPAddress localIP; + IPAddress gateway; + IPAddress subnet; + IPAddress dnsServer; + IPAddress mqttServer; + uint16_t mqttPort = 1883; +}; + +/** + * @brief A class to manage the wireless connections + */ +class Network : public Component +{ +public: + /** + * @brief Construct a new Network object + * + * With this constructor the esp gets its ip from a Dhcp server + * + * @param ssid + * @param passphrase + */ + Network(const char *ssid, const char *passphrase); + + /** + * @brief Construct a new Network object + * + * This constructor is used for a static ip setup + * + * @param ssid + * @param passphrase + * @param addresses + */ + Network(const char *ssid, const char *passphrase, NetworkAddresses addresses); + + ~Network(); + + /** + * @brief Activate ESP-NOW to communicate with the remote Control + * + * @param receive + * @param send + * @return true + * @return false + */ + bool activateEspNow(receiveCallbackPtr receive, sendCallbackPtr send); + + /** + * @brief Activate MQTT to send debug messages + * + * @param user + * @param passphrase + * @return true + * @return false + */ + bool activateMqtt(const char *user = nullptr, const char *passphrase = nullptr); + + /** + * @brief Print the current used IPs + */ + const void printIPs(); + + bool isWifiConnected() const { return this->wifiConnected; } + bool isMqttConnected() const { return this->mqttConnected; } + const uint8_t *getBroadcastAddress() const { return this->broadcastAddress; } + PubSubClient *getMqttClient() const { return this->mqttClient; } + + static uint8_t getCurrentChannel(); + +private: + void run() override{}; + void runAsChild() override; + void init(const char *ssid, const char *passphrase); + void checkWifi(); + void checkMqtt(); + bool connectMqtt(); + + NetworkAddresses addresses; + WiFiClient wifiClient; + PubSubClient *mqttClient = nullptr; + + bool initSuccessful = false; + bool wifiConnected = false; + bool mqttConnected = false; + + const char *mqttUser = nullptr; + const char *mqttPassphrase = nullptr; + + uint8_t broadcastAddress[6] = {0xC8, 0xC9, 0xA3, 0xC8, 0x57, 0x10}; + uint32_t lastWifiReconnectAttempt = 0; + uint32_t lastMqttReconnectAttempt = 0; + + static constexpr uint8_t wifiConnectTimeout = 20; + static constexpr uint16_t wifiConnectLoopTime = 500; + static constexpr uint16_t wifiReconnectDelay = 5000; + static constexpr uint16_t mqttReconnectDelay = 2500; +}; + +#endif // NETWORK_H diff --git a/lib/Point/point.cpp b/lib/Point/point.cpp index eee438e..a87be22 100644 --- a/lib/Point/point.cpp +++ b/lib/Point/point.cpp @@ -1,104 +1,118 @@ /** * @file point.cpp * @author Alexander Klein (alex@kleiax.de) - * @brief + * @brief * @version 0.1 * @date 2023-09-03 - * + * * @copyright Copyright (c) 2023 - * + * */ #include "point.h" -Point::Point(double lat, double lon, uint32_t horizontalAccuracy, uint32_t creationTime) { - this->coordinates.lat = lat; - this->coordinates.lon = lon; +Point::Point(double lat, double lon, uint32_t horizontalAccuracy, uint32_t creationTime) + : coordinates{lat, lon} +{ this->init(horizontalAccuracy, creationTime); } -Point::Point(int32_t lat, int32_t lon, uint32_t horizontalAccuracy, uint32_t creationTime) { - this->coordinates.lat = lat / 10000000.0; - this->coordinates.lon = lon / 10000000.0; +Point::Point(int32_t lat, int32_t lon, uint32_t horizontalAccuracy, uint32_t creationTime) + : coordinates{lat / 10000000.0, lon / 10000000.0} +{ this->init(horizontalAccuracy, creationTime); } -Point::Point(Coordinates coords, uint32_t horizontalAccuracy, uint32_t creationTime) { - this->coordinates = coords; +Point::Point(Coordinates coords, uint32_t horizontalAccuracy, uint32_t creationTime) + : coordinates{coords} +{ this->init(horizontalAccuracy, creationTime); } -Point::Point(Coordinates coords, bool imported) { +Point::Point(Coordinates coords, bool imported) +{ this->coordinates = coords; if (imported) + { this->init(UINT32_MAX, 0); + } else + { this->init(0, 0); + } } -Point::Point() { +Point::Point() +{ this->coordinates.lat = 0; - this->coordinates.lon = 0; + this->coordinates.lon = 0; this->init(0, 0); } -bool Point::operator==(const Point& rhs) const { +bool Point::operator==(const Point &rhs) const +{ return this->coordinates == rhs.getCoordinates(); } - // distance = sqrt(dx * dx + dy * dy) - // mit distance: Entfernung in km - // dx = 111.3 * cos(lat) * (lon1 - lon2) - // lat = (lat1 + lat2) / 2 * 0.01745 - // dy = 111.3 * (lat1 - lat2) - // lat1, lat2, lon1, lon2: Breite, Länge in Grad -double Point::distanceTo(const Coordinates& point) const { - Coordinates begin = this->coordinates; - Coordinates end = point; +double Point::distanceTo(const Coordinates &point) const +{ + const Coordinates begin = this->coordinates; + const Coordinates end = point; - double lat = (begin.lat + end.lat) / 2 * ROUTE_DEGREE_TO_RADIANT; - double dy = ROUTE_DISTANCE_BETWEEN_LATITUDE * (begin.lat - end.lat); - double dx = ROUTE_DISTANCE_BETWEEN_LATITUDE * cos(lat) * (begin.lon - end.lon); + const double lat = (begin.lat + end.lat) / 2 * ROUTE_DEGREE_TO_RADIANT; + const double dy = ROUTE_DISTANCE_BETWEEN_LATITUDE * (begin.lat - end.lat); + const double dx = ROUTE_DISTANCE_BETWEEN_LATITUDE * cos(lat) * (begin.lon - end.lon); return sqrt(dx * dx + dy * dy); } -double Point::distanceTo(const Point &point) const { +double Point::distanceTo(const Point &point) const +{ return this->distanceTo(point.getCoordinates()); } -int16_t Point::courseTo(const Coordinates& point) const { - Coordinates begin = this->coordinates; - Coordinates end = point; +int16_t Point::courseTo(const Coordinates &point) const +{ + const Coordinates begin = this->coordinates; + const Coordinates end = point; - double phi = log( tan(end.lat * ROUTE_DEGREE_TO_RADIANT / 2 + M_PI / 4) / tan(begin.lat * ROUTE_DEGREE_TO_RADIANT / 2 + M_PI / 4) ); - double lon = (begin.lon * ROUTE_DEGREE_TO_RADIANT - end.lon * ROUTE_DEGREE_TO_RADIANT); + const double phi = log(tan(end.lat * ROUTE_DEGREE_TO_RADIANT / 2 + M_PI / 4) / tan(begin.lat * ROUTE_DEGREE_TO_RADIANT / 2 + M_PI / 4)); + const double lon = (begin.lon * ROUTE_DEGREE_TO_RADIANT - end.lon * ROUTE_DEGREE_TO_RADIANT); - int16_t res = static_cast(atan2(lon, phi) / ROUTE_DEGREE_TO_RADIANT) * -1; - - // if (res < 0) - // res += 360; - - return res; + return static_cast(atan2(lon, phi) / ROUTE_DEGREE_TO_RADIANT) * -1; } -int16_t Point::courseTo(const Point &point) const { +int16_t Point::courseTo(const Point &point) const +{ return this->courseTo(point.getCoordinates()); } -void Point::init(uint32_t horizontalAccuracy, uint32_t creationTime) { +void Point::init(uint32_t horizontalAccuracy, uint32_t creationTime) +{ this->creationTime = creationTime; if (horizontalAccuracy == UINT32_MAX) + { this->accuracy = Accuracy::imported; + } else if (horizontalAccuracy > 9999) + { this->accuracy = Accuracy::fourDigOfCM; + } else if (horizontalAccuracy > 999) + { this->accuracy = Accuracy::threeDigOfCM; + } else if (horizontalAccuracy > 99) + { this->accuracy = Accuracy::twoDigOfCM; + } else if (horizontalAccuracy > 1) + { this->accuracy = Accuracy::oneDigOfCM; - else - this->accuracy = Accuracy::none; + } + else + { + this->accuracy = Accuracy::none; + } } diff --git a/lib/Point/point.h b/lib/Point/point.h index 9b7072e..b229054 100644 --- a/lib/Point/point.h +++ b/lib/Point/point.h @@ -1,139 +1,141 @@ /** * @file point.h * @author Alexander Klein (alex@kleiax.de) - * @brief + * @brief * @version 0.1 * @date 2023-09-03 - * + * * @copyright Copyright (c) 2023 - * + * */ #ifndef POINT_H #define POINT_H -#include +#include #define ROUTE_DEGREE_TO_RADIANT 0.01745 #define ROUTE_DISTANCE_BETWEEN_LATITUDE 111300 /** - * @brief A to handle points on the earth - * - * The points inherits latidue and longitude as doubles - * + * @brief A class to handle points on the earth + * + * The points inherits latitude and longitude as doubles + * */ -class Point{ - public: - /** - * @brief Hold the data longitude and latitude - * - */ - struct Coordinates { - double lon; - double lat; +class Point +{ +public: + /** + * @brief Hold the data longitude and latitude + * + */ + struct Coordinates + { + double lon; + double lat; - bool operator==(const Coordinates rhs) const { - return ( this->lon == rhs.lon ) && ( this->lon == rhs.lon ); - } - }; + bool operator==(const Coordinates rhs) const + { + return (this->lon == rhs.lon) && (this->lon == rhs.lon); + } + }; - /** - * @brief The Accuracy is set by the constructor - * - */ - enum Accuracy { - none, - fourDigOfCM, - threeDigOfCM, - twoDigOfCM, - oneDigOfCM, - imported - }; + /** + * @brief The Accuracy is set by the constructor + * + */ + enum Accuracy + { + none, + fourDigOfCM, + threeDigOfCM, + twoDigOfCM, + oneDigOfCM, + imported + }; + /** + * @brief Construct a new Point object + * + * @param lat + * @param lon + * @param horizontalAccuracy + * @param creationTime + */ + Point(double lat, double lon, uint32_t horizontalAccuracy = 0, uint32_t creationTime = 0); + Point(int32_t lat, int32_t lon, uint32_t horizontalAccuracy = 0, uint32_t creationTime = 0); + Point(Coordinates coords, uint32_t horizontalAccuracy = 0, uint32_t creationTime = 0); + Point(Coordinates coords, bool imported); + Point(); - /** - * @brief Construct a new Point object - * - * @param lat latitude - * @param lon longitude - * @param horizontalAccuracy mm - * @param coords Coordinates - * @param imported if true than highest accuracy - */ - Point(double lat, double lon, uint32_t horizontalAccuracy = 0, uint32_t creationTime = 0); - Point(int32_t lat, int32_t lon, uint32_t horizontalAccuracy = 0, uint32_t creationTime = 0); - Point(Coordinates coords, uint32_t horizontalAccuracy = 0, uint32_t creationTime = 0); - Point(Coordinates coords, bool imported); - Point(); + /** + * @brief Checks if to points are equal. + * + * @param rhs + * @return true + * @return false + */ + bool operator==(const Point &rhs) const; - /** - * @brief Checks if to points are equal. - * - * @param rhs - * @return true - * @return false - */ - bool operator==(const Point& rhs) const; + /** + * @brief Checks if the point is initalized. + * + * @return true + * @return false + */ + bool isInit() const { return this->coordinates.lat + this->coordinates.lon; } - /** - * @brief Checks if the point is initalized. - * - * @return true - * @return false - */ - bool isInit() const { return this->coordinates.lat + this->coordinates.lon; } + /** + * @brief Checks if the point is valid. + * + * If the accuracy is higher than zero, true will be returned. + * + * @return true + * @return false + */ + bool isValid() const { return (this->accuracy > 0) ? true : false; } - /** - * @brief Checks if the point is valid. - * - * If the accuracy is higher than zero, true will be returned. - * - * @return true - * @return false - */ - bool isValid() const { return (this->accuracy > 0) ? true : false; } + /** + * @brief Calculates the distance between to points. + * + * @param point + * @return double meter + */ + double distanceTo(const Coordinates &point) const; + double distanceTo(const Point &point) const; - /** - * @brief Calculates the distance between to points. - * - * @param point - * @return double meter - */ - double distanceTo(const Coordinates& point) const; - double distanceTo(const Point& point) const; + /** + * @brief Calculates the course to an other point. + * + * @param point + * @return int16_t degree + */ + int16_t courseTo(const Coordinates &point) const; + int16_t courseTo(const Point &point) const; - /** - * @brief Calculates the course to an other point. - * - * @param point - * @return int16_t degree - */ - int16_t courseTo(const Coordinates& point) const; - int16_t courseTo(const Point& point) const; + uint32_t getCreationTime() const { return this->creationTime; } + double getLongitude() const { return this->coordinates.lon; } + double getLatitude() const { return this->coordinates.lat; } + Coordinates getCoordinates() const { return this->coordinates; } - uint32_t getCreationTime() const { return this->creationTime; } - double getLongitude() const { return this->coordinates.lon; } - double getLatitude() const { return this->coordinates.lat; } - Coordinates getCoordinates() const { return this->coordinates; } + /** + * @brief Get the Accuracy object + * + * The higher the value, the greater the accuracy. + * You can check it by Accuracy. + * + * @return Accuracy + */ + Accuracy getAccuracy() const { return this->accuracy; } - /** - * @brief Get the Accuracy object - * - * The higher the value, the greater the accuracy. - * You can check it by Accuracy. - * - * @return Accuracy - */ - Accuracy getAccuracy() const { return this->accuracy; } +private: + void init(uint32_t horizontalAccuracy, uint32_t creationTime); - private: - void init(uint32_t horizontalAccuracy, uint32_t creationTime); + Accuracy accuracy = Accuracy::none; + Coordinates coordinates{0, 0}; - Accuracy accuracy = Accuracy::none; - Coordinates coordinates; - - uint32_t creationTime = 0; + uint32_t creationTime = 0; }; -#endif //POINT_H +#endif // POINT_H diff --git a/lib/Sensors/sensorData.cpp b/lib/Sensors/sensorData.cpp index adde8ca..23db0ef 100644 --- a/lib/Sensors/sensorData.cpp +++ b/lib/Sensors/sensorData.cpp @@ -1,107 +1,195 @@ /** - * @file senors.cpp + * @file sensorData.cpp * @author Alexander Klein (alex@kleiax.de) - * @brief + * @brief * @version 0.1 * @date 2023-09-02 - * + * * @copyright Copyright (c) 2023 - * + * */ #include "sensorData.h" bool SensorData::outputStatusPrintPVTdata = false; -bool SensorData::newData = false; uint32_t SensorData::ubxUpdateTimeStatic = 0; -UBX_NAV_PVT_data_t* SensorData::ubxDataStatic = nullptr; +UBX_NAV_PVT_data_t *SensorData::ubxDataStatic = nullptr; -SensorData::SensorData() { - this->loopDelay = 50; +SensorData::SensorData() +{ + Component::loopDelay = SensorData::loopDelay; } -void SensorData::enableGnss(SPIClass* spiPort, uint8_t csPin) { +void SensorData::enableGnss(SPIClass *spiPort, uint8_t csPin) +{ this->gnss = new SFE_UBLOX_GNSS(); - if (this->gnss->begin(*spiPort, csPin, 4000000) == false) { + if (this->gnss->begin(*spiPort, csPin, 4000000) == false) + { std::cout << "u-blox GNSS not detected on SPI bus. Please check wiring. Freezing." << std::endl; - while (1); + while (true) + { + } } this->initGnss(); } -void SensorData::enableGnss() { +void SensorData::enableGnss() +{ this->gnss = new SFE_UBLOX_GNSS(); - if (this->gnss->begin() == false) { + if (this->gnss->begin() == false) + { std::cout << "u-blox GNSS not detected at default I2C address. Please check wiring. Freezing." << std::endl; - while (1); + while (true) + { + } } this->initGnss(); } -void SensorData::enableRealCompass() { +void SensorData::enableRealCompass() +{ + static constexpr byte address = 0x0d; + this->realCompass = new QMC5883LCompass(); // Init Compass - Wire.beginTransmission(0x0d); + Wire.beginTransmission(address); + // TODO: describe Bytes !!! Wire.write(0x0b); Wire.write(0x01); Wire.endTransmission(); - this->realCompass->setMode(0x01,0x0C,0x10,0X00); + this->realCompass->setMode(0x01, 0x0C, 0x10, 0X00); CalibrateCompass caliCompass(this->realCompass); caliCompass.loadData(); caliCompass.useData(); } -void SensorData::enableCalcCompass() { - +void SensorData::enableCalcCompass() +{ + // TODO: !!! implementieren } -void SensorData::enableGyroskop() { - +void SensorData::enableGyroscope() +{ + this->gyroscope = new MPU6050(); + this->gyroscope->initialize(); + if (!this->gyroscope->testConnection()) + { + std::cout << "SensorData::enableGyroscope: Gyroskop is not conntected. Freeze!" << std::endl; + while (true) + { + } + } + + const uint8_t deviceStatus = this->gyroscope->dmpInitialize(); + + // TODO: !!! MagicNumer 6x + this->gyroscope->setXGyroOffset(220); + this->gyroscope->setYGyroOffset(76); + this->gyroscope->setZGyroOffset(-85); + this->gyroscope->setZAccelOffset(1788); + + if (deviceStatus == 0) + { + this->gyroscope->CalibrateAccel(6); + this->gyroscope->CalibrateGyro(6); + this->gyroscope->PrintActiveOffsets(); + this->gyroscope->setDMPEnabled(true); + } + else + { + // ERROR! + // 1 = initial memory load failed + // 2 = DMP configuration updates failed + // (if it's going to break, usually the code will be 1) + std::cout << "SensorData::enableGyroscope: DMP Initialization failed (code" << static_cast(deviceStatus) << "). Freeze!" << std::endl; + while (true) + { + } + } } -CalcAzimuth::State SensorData::getCalcAzimuthState() const { - if (this->calcCompass) +CalcAzimuth::State SensorData::getCalcAzimuthState() const +{ + if (static_cast(this->calcCompass)) + { return this->calcCompass->getState(); + } return CalcAzimuth::State::Invalid; } -void SensorData::printPVTdata(UBX_NAV_PVT_data_t *ubxDataStruct) { +NTRIPClientStates SensorData::getNtripState() const +{ + if (static_cast(this->ntripClient)) + { + return this->ntripClient->getClientState(); + } + return NTRIPClientStates::notAvailable; +} + +void SensorData::printPVTdata(UBX_NAV_PVT_data_t *ubxDataStruct) +{ + static constexpr uint8_t stringSize = 32; + if (!SensorData::outputStatusPrintPVTdata) + { return; + } - double latitude = (double) ubxDataStruct->lat / 10000000.0; - double longitude = (double) ubxDataStruct->lon / 10000000.0; - double altitude = (double) ubxDataStruct->hMSL / 1000.0; + const double latitude = ubxDataStruct->lat / 10000000.0; + const double longitude = ubxDataStruct->lon / 10000000.0; + const double altitude = ubxDataStruct->hMSL / 1000.0; - uint8_t fixType = ubxDataStruct->fixType; - char fixTypeString[32]; + const uint8_t fixType = ubxDataStruct->fixType; + char fixTypeString[stringSize]; if (fixType == 0) - strcpy(fixTypeString, "None"); + { + strcpy(fixTypeString, static_cast("None")); + } else if (fixType == 1) - strcpy(fixTypeString, "Dead Reckoning"); + { + strcpy(fixTypeString, static_cast("Dead Reckoning")); + } else if (fixType == 2) - strcpy(fixTypeString, "2D"); + { + strcpy(fixTypeString, static_cast("2D")); + } else if (fixType == 3) - strcpy(fixTypeString, "3D"); + { + strcpy(fixTypeString, static_cast("3D")); + } else if (fixType == 3) - strcpy(fixTypeString, "GNSS + Dead Reckoning"); + { + strcpy(fixTypeString, static_cast("GNSS + Dead Reckoning")); + } else if (fixType == 5) - strcpy(fixTypeString, "Time Only"); + { + strcpy(fixTypeString, static_cast("Time Only")); + } else - strcpy(fixTypeString, "UNKNOWN"); + { + strcpy(fixTypeString, static_cast("UNKNOWN")); + } - uint8_t carrSoln = ubxDataStruct->flags.bits.carrSoln; - char carrSolnString[16]; + const uint8_t carrSoln = ubxDataStruct->flags.bits.carrSoln; + char carrSolnString[stringSize]; if (carrSoln == 0) - strcpy(carrSolnString, "None"); + { + strcpy(carrSolnString, static_cast("None")); + } else if (carrSoln == 1) - strcpy(carrSolnString, "Floating"); + { + strcpy(carrSolnString, static_cast("Floating")); + } else if (carrSoln == 2) - strcpy(carrSolnString, "Fixed"); + { + strcpy(carrSolnString, static_cast("Fixed")); + } else - strcpy(carrSolnString, "UNKNOWN"); + { + strcpy(carrSolnString, static_cast("UNKNOWN")); + } - uint32_t hAcc = ubxDataStruct->hAcc; + const uint32_t hAcc = ubxDataStruct->hAcc; std::cout << "Lat: " << latitude << " Lng: " << longitude @@ -112,35 +200,53 @@ void SensorData::printPVTdata(UBX_NAV_PVT_data_t *ubxDataStruct) { << " Horizontal Accuracy Estimate: " << hAcc << " mm" << std::endl; } -void SensorData::savePVTdata(UBX_NAV_PVT_data_t *ubxDataStruct) { +void SensorData::savePVTdata(UBX_NAV_PVT_data_t *ubxDataStruct) +{ SensorData::printPVTdata(ubxDataStruct); - SensorData::newData = true; SensorData::ubxDataStatic = ubxDataStruct; SensorData::ubxUpdateTimeStatic = millis(); } -void SensorData::setOutputStatusPrintPVTdata(bool status) { +void SensorData::setOutputStatusPrintPVTdata(bool status) +{ SensorData::outputStatusPrintPVTdata = status; } -void SensorData::run() { - this->realCompass->read(); - this->realAzimuth = this->realCompass->getAzimuth(); +void SensorData::run() +{ + if (static_cast(this->realCompass)) + { + this->realCompass->read(); + this->realAzimuth = this->realCompass->getAzimuth(); + } + + if (static_cast(this->gyroscope) && this->gyroscope->dmpGetCurrentFIFOPacket(static_cast(this->gyroBuffer))) + { + this->gyroscope->dmpGetQuaternion(&this->quaternion, static_cast(this->gyroBuffer)); + this->gyroscope->dmpGetGravity(&this->gravity, &this->quaternion); + this->gyroscope->dmpGetYawPitchRoll(static_cast(this->yawPitchRoll), &this->quaternion, &this->gravity); + } } -void SensorData::runAsChild() { - this->gnss->checkUblox(); - this->gnss->checkCallbacks(); - - if (SensorData::newData) - SensorData::newData = false; +void SensorData::runAsChild() +{ + if (static_cast(this->gnss)) + { + this->gnss->checkUblox(); + this->gnss->checkCallbacks(); + if (SensorData::ubxUpdateTimeStatic != this->lastUbxUpdate) + { + this->updateUbxData(); + } + } } -void SensorData::initGnss() { - uint8_t versionHigh = this->gnss->getProtocolVersionHigh(); - uint8_t versionLow = this->gnss->getProtocolVersionLow(); - std::cout << "u-blox protocol version: " << unsigned(versionHigh) << "." << unsigned(versionLow) << std::endl; +void SensorData::initGnss() +{ + const uint8_t versionHigh = this->gnss->getProtocolVersionHigh(); + const uint8_t versionLow = this->gnss->getProtocolVersionLow(); + std::cout << "u-blox protocol version: " << static_cast(versionHigh) << "." << static_cast(versionLow) << std::endl; this->gnss->setSPIOutput(COM_TYPE_UBX); this->gnss->enableNMEAMessage(UBX_NMEA_GGA, COM_PORT_SPI, 10); @@ -150,3 +256,15 @@ void SensorData::initGnss() { this->gnss->setNavigationFrequency(1); this->gnss->setAutoPVT(true); } + +void SensorData::updateUbxData() +{ + this->gnssData = SensorData::ubxDataStatic; + this->lastUbxUpdate = SensorData::ubxUpdateTimeStatic; + + Point::Coordinates coords{0, 0}; + coords.lat = this->gnssData->lat / 10000000.0; + coords.lon = this->gnssData->lon / 10000000.0; + + this->currentPosition = Point(coords, this->gnssData->hAcc); +} diff --git a/lib/Sensors/sensorData.h b/lib/Sensors/sensorData.h index 52bac26..f3bcdb6 100644 --- a/lib/Sensors/sensorData.h +++ b/lib/Sensors/sensorData.h @@ -1,18 +1,19 @@ /** * @file sensorData.h * @author Alexander Klein (alex@kleiax.de) - * @brief + * @brief Contains the SensorData class * @version 0.1 * @date 2023-09-02 - * + * * @copyright Copyright (c) 2023 - * + * */ #ifndef SENSOR_DATA_H #define SENSOR_DATA_H #include +#include #include #include "component.h" @@ -20,70 +21,159 @@ #include #include -// Gyroskop +#include #include "calcAzimuth.h" #include "point.h" -class Sensors; -class SensorData : public Component { - public: - SensorData(); - - void enableGnss(SPIClass* spiPort, uint8_t csPin); - void enableGnss(); - void enableRealCompass(); - void enableCalcCompass(); - void enableGyroskop(); +/** + * @brief A class to manage all sensors + * + * Each sensor have separately to be enabled + */ +class SensorData : public Component +{ +public: + SensorData(); + ~SensorData(); - // Interface Const kram - int16_t getRealAzimuth() const { return this->realAzimuth; } - int16_t getCalcAzimuth() const { return this->calcAzimuth; } - CalcAzimuth::State getCalcAzimuthState() const; + void enableNtrip(String host, uint16_t port, String mountPoint, String user, String password); - Point getCurrentPos() const { return this->currentPosition; } - const UBX_NAV_PVT_data_t* getGnssData() const { return this->gnssData; }; - const void* const getGyroData() const; + /** + * @brief Enable the gnss module over spi + * + * @param spiPort + * @param csPin + */ + void enableGnss(SPIClass *spiPort, uint8_t csPin); - CalcAzimuth* getCalcCompass() const { return this->calcCompass; } - QMC5883LCompass* getRealCompass() const { return this->realCompass; } + /** + * @brief Enable the gnss module over i2c + */ + void enableGnss(); - // static - /** - * @brief Set the output status for PVTdata. - * - * If this is true, a lot of information from the gnss module will be printed in - * the interval of navigation frequency. - * - * @param status - */ - static void setOutputStatusPrintPVTdata(bool status); + void enableRealCompass(); + void enableCalcCompass(); + void enableGyroscope(); - private: - void run() override; - void runAsChild() override; - void initGnss(); + // Interface Const + /** + * @brief Get the azimuth measured by the compass module + * + * @return int16_t + */ + int16_t getRealAzimuth() const { return this->realAzimuth; } - QMC5883LCompass* realCompass = nullptr; - CalcAzimuth* calcCompass = nullptr; - SFE_UBLOX_GNSS* gnss = nullptr; + /** + * @brief Get the azimuth calculated by CalcAzimuth + * + * Consider to call getCalcAzimuthState() to check, if the data is valid. + * + * @return int16_t + */ + int16_t getCalcAzimuth() const { return this->calcAzimuth; } - UBX_NAV_PVT_data_t* gnssData; - Point currentPosition; + /** + * @brief Get the CalcAzimuth::State object + * + * Needed to check the quality of calculated azimuth + * + * @return CalcAzimuth::State + */ + CalcAzimuth::State getCalcAzimuthState() const; - int16_t realAzimuth = INT16_MAX; - int16_t calcAzimuth = INT16_MAX; + Point getCurrentPos() const { return this->currentPosition; } + const UBX_NAV_PVT_data_t *getGnssData() const { return this->gnssData; }; + NTRIPClientStates getNtripState() const; - // static - static void printPVTdata(UBX_NAV_PVT_data_t *ubxDataStruct); - static void savePVTdata(UBX_NAV_PVT_data_t *ubxDataStruct); + /** + * @brief Get the data from the gyroscope + * + * The returned float pointer is an array of 3 floats + * - Yaw + * - Pitch + * - Roll + * + * @return const float* + */ + const float *getGyroData() const { return this->yawPitchRoll; } - static UBX_NAV_PVT_data_t* ubxDataStatic; + /** + * @brief Get the CalcCompass object + * @return CalcAzimuth* + */ + CalcAzimuth *getCalcCompass() const { return this->calcCompass; } - static uint32_t ubxUpdateTimeStatic; + /** + * @brief Get the RealCompass object + * @return QMC5883LCompass* + */ + QMC5883LCompass *getRealCompass() const { return this->realCompass; } - static bool outputStatusPrintPVTdata; - static bool newData; + /** + * @brief Get the NTRIPClient object + * @return NTRIPClient* + */ + NTRIPClient *getNtripClient() const { return this->ntripClient; } + + /** + * @brief Get the Gyroscope object + * @return MPU6050* + */ + MPU6050 *getGyroscope() const { return this->gyroscope; } + + // static + /** + * @brief Set the output status for PVTdata. + * + * If this is true, a lot of information from the gnss module will be printed in + * the interval of navigation frequency. + * + * @param status + */ + static void setOutputStatusPrintPVTdata(bool status); + +private: + void run() override; + void runAsChild() override; + void initGnss(); + void updateUbxData(); + + QMC5883LCompass *realCompass = nullptr; + CalcAzimuth *calcCompass = nullptr; + SFE_UBLOX_GNSS *gnss = nullptr; + NTRIPClient *ntripClient = nullptr; + MPU6050 *gyroscope = nullptr; + + UBX_NAV_PVT_data_t *gnssData = nullptr; + Point currentPosition; + Quaternion quaternion; + VectorFloat gravity; + + char *host = nullptr; + char *mountPoint = nullptr; + char *user = nullptr; + char *password = nullptr; + + bool isNtripInit = false; + + uint8_t gyroBuffer[64]; + uint16_t port = 0; + int16_t realAzimuth = INT16_MAX; + int16_t calcAzimuth = INT16_MAX; + uint32_t lastUbxUpdate = 0; + + float yawPitchRoll[3]{0, 0, 0}; + + // static + static void printPVTdata(UBX_NAV_PVT_data_t *ubxDataStruct); + static void savePVTdata(UBX_NAV_PVT_data_t *ubxDataStruct); + + static UBX_NAV_PVT_data_t *ubxDataStatic; + static uint32_t ubxUpdateTimeStatic; + static bool outputStatusPrintPVTdata; + + static constexpr uint8_t loopDelay = 50; }; -#endif //SENSOR_DATA_H +#endif // SENSOR_DATA_H diff --git a/lib/Speedometer/speedometer.cpp b/lib/Speedometer/speedometer.cpp index 8313efa..703aa13 100644 --- a/lib/Speedometer/speedometer.cpp +++ b/lib/Speedometer/speedometer.cpp @@ -5,117 +5,150 @@ * @see speedometer.h * @version 0.1 * @date 2021-12-13 - * + * * @copyright Copyright (c) 2021 - * + * */ #include "speedometer.h" -Speedometer::Speedometer(uint8_t pin, double diameter, uint16_t steps) { - this->diameter = diameter; - this->steps = steps; - - this->pulseCounter = new Counter(pin); - this->pulseCounter->setFilterValue(1000); // ignore pulses less than 1000 x 2.5ns +Speedometer::Speedometer(uint8_t pin, double diameter, uint16_t steps) + : pulseCounter{new Counter(pin)}, diameter{diameter}, steps{steps}, buf{} +{ + this->pulseCounter->setFilterValue(Speedometer::maxFilterValue); // ignore pulses less than 1000 x 2.5ns this->pulseCounter->clear(); this->pulseCounter->resume(); Component::loopDelay = Speedometer::loopDelay; - clearAvgBuf(); + this->clearAvgBuf(); } -Speedometer::~Speedometer() { +Speedometer::~Speedometer() +{ delete this->pulseCounter; } -void Speedometer::run() { +void Speedometer::run() +{ + static constexpr float minimalSpeed = 0.1; + if (this->calibrationRunning) + { return; + } - uint32_t time = millis(); + const uint32_t time = millis(); - uint16_t elapsedTime = time - this->lastMillisCalc; + const uint16_t elapsedTime = time - this->lastMillisCalc; this->lastMillisCalc = time; - int16_t pulse = this->pulseCounter->getValue(); + const double pulse = this->pulseCounter->getValue(); this->pulseCounter->clear(); this->pulseCounter->resume(); - - double n = (double)pulse / 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 + const double wheelRevolutionsAbsolute = pulse / this->steps; + const double wheelRevolutionsRelativ = wheelRevolutionsAbsolute / (elapsedTime / 1000.0); - switch (this->currentDirection) { - case Direction::Forward : - this->speed = ms; - break; - - case Direction::Backward : - this->speed = -ms; - break; + double meterPerSecond = wheelRevolutionsRelativ * (diameter * PI); + double radPerSecond = wheelRevolutionsRelativ * 2 * PI; - case Direction::None : - this->speed = 0; - break; + if (meterPerSecond < minimalSpeed) + { + meterPerSecond = 0; + radPerSecond = 0; + } + + switch (this->currentDirection) + { + case Direction::Forward: + this->speed = meterPerSecond; + this->rad = radPerSecond; + break; + + case Direction::Backward: + this->speed = -meterPerSecond; + this->rad = -radPerSecond; + break; + + case Direction::None: + this->speed = 0; + this->rad = 0; + break; } this->addValToBuf(static_cast(this->speed * Speedometer::conversionFactor)); } -void Speedometer::setDirection(Direction dir) { +void Speedometer::setDirection(Direction dir) +{ if (this->currentDirection == dir) + { return; + } this->currentDirection = dir; this->clearAvgBuf(); } -void Speedometer::setEncFilter(uint16_t val) { - if (val > 1023) - val = 1023; +void Speedometer::setEncFilter(uint16_t val) +{ + if (val > Speedometer::maxFilterValue) + { + val = Speedometer::maxFilterValue; + } this->pulseCounter->setFilterValue(val); } -double Speedometer::getAvgSpeed() const { - int16_t avg = this->calcAverage(); - return (float)avg / Speedometer::conversionFactor; +double Speedometer::getAvgSpeed() const +{ + const double avg = this->calcAverage(); + return avg / Speedometer::conversionFactor; } -void Speedometer::calibrationMeasurementStart() { +void Speedometer::calibrationMeasurementStart() +{ std::cout << "Start" << std::endl; this->calibrationRunning = true; this->pulseCounter->clear(); this->pulseCounter->resume(); } -uint16_t Speedometer::calibrationMeasurementStop() { +uint16_t Speedometer::calibrationMeasurementStop() +{ std::cout << "Ende" << std::endl; this->calibrationRunning = false; - uint16_t res = abs(this->pulseCounter->getValue()); + const uint16_t res = abs(this->pulseCounter->getValue()); this->pulseCounter->clear(); this->pulseCounter->resume(); std::cout << "Result: " << res << std::endl; return res; } -void Speedometer::clearAvgBuf() { - for (uint8_t i = 0; i < bufSize; i++) +void Speedometer::clearAvgBuf() +{ + for (uint8_t i = 0; i < bufSize; i++) + { this->buf[i] = 0; + } } -void Speedometer::addValToBuf(int16_t val) { +void Speedometer::addValToBuf(int16_t val) +{ this->buf[this->bufPos] = val; this->bufPos++; if (bufPos == bufSize) - bufPos = 0; + { + bufPos = 0; + } } -int16_t Speedometer::calcAverage() const { +int16_t Speedometer::calcAverage() const +{ int16_t sum = 0; - for (int i = 0; i < this->bufSize; i++) + for (int i = 0; i < Speedometer::bufSize; i++) + { sum += this->buf[i]; - return sum / this->bufSize; + } + return sum / Speedometer::bufSize; } diff --git a/lib/Speedometer/speedometer.h b/lib/Speedometer/speedometer.h index ae78366..a47f1ff 100644 --- a/lib/Speedometer/speedometer.h +++ b/lib/Speedometer/speedometer.h @@ -4,9 +4,9 @@ * @brief A implementation to measure wheel speeds with an encoder. * @version 0.1 * @date 2021-12-09 - * + * * @copyright Copyright (c) 2021 - * + * */ #ifndef SPEEDOMETER_H @@ -21,120 +21,127 @@ /** * @brief A class which use a encoder to calc the speed - * + * * This class use ESP32 pulse counter hardware peripheral. * The calculated speed is the average of an amount of last measurements. - * + * */ -class Speedometer : public Component { - public: - /** - * @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 Direction is None, no measurement will be taken. - */ - enum Direction { - None, - Forward, - Backward - }; +class Speedometer : public Component +{ +public: + /** + * @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 Direction is None, no measurement will be taken. + */ + enum Direction + { + None, + Forward, + Backward + }; - /** - * @brief Construct a new Speedometer object - * - * @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. - */ - Speedometer(uint8_t pin, double diameter, uint16_t steps); + /** + * @brief Construct a new Speedometer object + * + * @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. + */ + Speedometer(uint8_t pin, double diameter, uint16_t steps); - ~Speedometer(); + ~Speedometer(); - /** - * @brief Set the direction - * - * @param dir Direction - */ - void setDirection(Direction dir); + /** + * @brief Set the direction + * + * @param dir Direction + */ + void setDirection(Direction dir); - /** - * @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 + * + * ignore pulses less than val x 2.5ns + * + * @param val default = 1000, max = 1023 + */ + void setEncFilter(uint16_t val); - /** - * @brief Set the Enc Filter to prevent bouncing - * - * ignore pulses less than val x 2.5ns - * - * @param val default = 1000, max = 1023 - */ - void setEncFilter(uint16_t val); + /** + * @brief Get the Direction + * + * @return Direction + */ + Direction getDirection() const { return this->currentDirection; } - /** - * @brief Get the Direction - * - * @return Direction - */ - Direction getDirection() const { return this->currentDirection; } + /** + * @brief Get the calculated speed of the wheel + * + * @return double speed in m/s + */ + double getSpeed() const { return this->speed; } - /** - * @brief Get the calculated speed of the Wheel - * - * @return double speed in m/s - */ - double getSpeed() const { return this->speed; } - double getAvgSpeed() const; + /** + * @brief Get the calculated speed of the wheel + * + * @return double speed in rad/s + */ + double getSpeedRad() const { return this->rad; }; - /** - * @brief Start calibration - * - * This functions stops the loop. So that steps of one manual wheel turn - * can measured. Call calibrationMeasurementStop to start the loop and get - * the result. - */ - void calibrationMeasurementStart(); + /** + * @brief Get the calculated average speed of the wheel + * + * @return double speed in m/s + */ + double getAvgSpeed() const; - /** - * @brief Stop calibration - * - * Start the loop function and read the past steps. - * - * @return uint16_t steps since calibrationMeasurementStart was called - */ - uint16_t calibrationMeasurementStop(); + /** + * @brief Start calibration + * + * This functions stops the loop. So that steps of one manual wheel turn + * can measured. Call calibrationMeasurementStop to start the loop and get + * the result. + */ + void calibrationMeasurementStart(); + /** + * @brief Stop calibration + * + * Start the loop function and read the past steps. + * + * @return uint16_t steps since calibrationMeasurementStart was called + */ + uint16_t calibrationMeasurementStop(); - private: - void run() override; - void init(uint8_t pin, double diameter, uint16_t steps); - void clearAvgBuf(); - void addValToBuf(int16_t val); - int16_t calcAverage() const; +private: + void run() override; + void clearAvgBuf(); + void addValToBuf(int16_t val); + int16_t calcAverage() const; - static constexpr uint8_t loopDelay = 30; - static constexpr uint8_t bufSize = 5; - static constexpr uint8_t conversionFactor = 100; + static constexpr uint8_t loopDelay = 30; + static constexpr uint8_t bufSize = 5; + static constexpr uint8_t conversionFactor = 100; - Counter* pulseCounter; - Direction currentDirection = Direction::None; + Counter *pulseCounter; + Direction currentDirection = Direction::None; - bool calibrationRunning = false; + bool calibrationRunning = false; - double speed = 0; - double diameter; + double speed = 0; + double rad = 0; + double diameter; - uint8_t printCounter = 0; - uint8_t bufPos = 0; - uint16_t steps; + uint8_t printCounter = 0; + uint8_t bufPos = 0; + uint16_t steps; + int16_t buf[Speedometer::bufSize]; + uint32_t lastMillisCalc = 0; - int16_t buf[Speedometer::bufSize]; - - uint32_t lastMillisCalc = 0; + static constexpr uint16_t maxFilterValue = 1023; }; #endif // SPEEDOMETER_H diff --git a/lib/Times/debugTimes.cpp b/lib/Times/debugTimes.cpp index 7490f5d..82b5d73 100644 --- a/lib/Times/debugTimes.cpp +++ b/lib/Times/debugTimes.cpp @@ -1,45 +1,56 @@ /** * @file debugTimes.cpp * @author Alexander Klein (alex@kleiax.de) - * @brief Implemention of the class debugTimes.h. + * @brief Implementation of the class debugTimes.h. * @version 0.1 * @date 2021-12-13 - * + * * @copyright Copyright (c) 2021 - * + * */ #include "debugTimes.h" bool DebugTimes::print = false; bool DebugTimes::printWarning = true; -DebugTimes::DebugTimes() { - this->startTime = millis(); - - if (DebugTimes::printWarning) { - std::cout << std::endl << "Warning: DebugTimes is muuted, no times are be shown." << std::endl << std::endl; +DebugTimes::DebugTimes() + : startTime{millis()} +{ + if (DebugTimes::printWarning) + { + std::cout << std::endl + << "Warning: DebugTimes is muted, no times are be shown." << std::endl + << std::endl; DebugTimes::printWarning = false; } } -void DebugTimes::restart() { +void DebugTimes::restart() +{ this->startTime = millis(); } -uint16_t DebugTimes::stop() { +const uint16_t DebugTimes::stop() +{ return millis() - this->startTime; } -uint16_t DebugTimes::stopConsol(const char* name, uint16_t minTime) { - uint64_t time = millis() - this->startTime; +const uint16_t DebugTimes::stopConsol(const char *name, uint16_t minTime) +{ + const uint64_t time = millis() - this->startTime; if (time > minTime && DebugTimes::print) + { std::cout << name << " needs " << time << " ms" << std::endl; + } return time; } -void DebugTimes::setConsolOutput(bool enable) { +void DebugTimes::setConsolOutput(bool enable) +{ if (enable == DebugTimes::print) + { return; + } DebugTimes::print = enable; DebugTimes::printWarning = !enable; diff --git a/lib/Times/debugTimes.h b/lib/Times/debugTimes.h index 9155429..da82965 100644 --- a/lib/Times/debugTimes.h +++ b/lib/Times/debugTimes.h @@ -4,9 +4,9 @@ * @brief Inherits a class to measure times of functions. * @version 0.1 * @date 2021-12-13 - * + * * @copyright Copyright (c) 2021 - * + * */ #ifndef DEBUG_TIMES_H @@ -20,57 +20,58 @@ /** * @brief A class to measure times of functions. - * + * * This simple class only save the value of the millis() * function when you call the constructor or restart(). * To get the elapsed time call stop() or stopConsol(). - * + * * @warning This class is not very accurate * It only give you the time in milliseconds. */ -class DebugTimes { - public: - /** - * @brief Construct a new Debug Times object - * Starts to count milliseconds - */ +class DebugTimes +{ +public: + /** + * @brief Construct a new Debug Times object + * Starts to count milliseconds + */ - DebugTimes(); - /** - * @brief Set the counter to 0 - */ + DebugTimes(); + /** + * @brief Set the counter to 0 + */ - void restart(); + void restart(); - /** - * @brief Give the elapsed time - * - * @return uint16_t elapsed milliseconds - */ - uint16_t stop(); + /** + * @brief Give the elapsed time + * + * @return uint16_t elapsed milliseconds + */ + const uint16_t stop(); - /** - * @brief Print the elapsed time to consol - * - * @param name Functionname to print - * @param minTime A minimum time before printing - * - * @return uint16_t elapsed milliseconds - */ - uint16_t stopConsol(const char* name, uint16_t minTime = 0); + /** + * @brief Print the elapsed time to consol + * + * @param name FunctionName to print + * @param minTime A minimum time before printing + * + * @return uint16_t elapsed milliseconds + */ + const uint16_t stopConsol(const char *name, uint16_t minTime = 0); - /** - * @brief Sets if the result should be printed. - * - * @param enable - */ - static void setConsolOutput(bool enable); + /** + * @brief Sets if the result should be printed. + * + * @param enable + */ + static void setConsolOutput(bool enable); - private: - uint64_t startTime; +private: + uint64_t startTime; - static bool print; - static bool printWarning; + static bool print; + static bool printWarning; }; -#endif //DEBUG_TIMES_H +#endif // DEBUG_TIMES_H diff --git a/lib/calibrateCompass/calibrateCompass.h b/lib/calibrateCompass/calibrateCompass.h deleted file mode 100644 index 72bffb0..0000000 --- a/lib/calibrateCompass/calibrateCompass.h +++ /dev/null @@ -1,60 +0,0 @@ -/** - * @file calibrateCompass.h - * @author Alexander Klein (alex@kleiax.de) - * @brief - * @version 0.1 - * @date 2023-05-23 - * - * @copyright Copyright (c) 2023 - * - */ - -#pragma once - -#include -#include -#include - -#include "component.h" - -class CalibrateCompass : public Component { - public: - enum State { - Ready, - Calibrating, - Finished - }; - - struct CallibrationData { - int data[3][2]; - }; - - CalibrateCompass(QMC5883LCompass* compass); - - void start(); - void useData(); - void removeCalibration(); - void reset(); - void saveData(); - void loadData(); - - State getState() const { return this->state; } - CallibrationData getCallibrationData() const { return this->data; } - - friend std::ostream& operator<<(std::ostream& os, const CalibrateCompass& caliComp); - - private: - void runAsChild() override; - void run() override; - void checkDataValidity(); - - QMC5883LCompass* compass; - State state; - CallibrationData data; - - void clearData(); - - bool dataValid = false; - const uint16_t maxTimeWithoutChange = 10000; - uint32_t lastChange = 0; -}; diff --git a/platformio.ini b/platformio.ini index da001b9..9d9aec8 100644 --- a/platformio.ini +++ b/platformio.ini @@ -15,7 +15,7 @@ board_build.partitions = no_ota.csv framework = arduino monitor_speed = 115200 upload_speed = 921600 -monitor_port = COM6 +; monitor_port = COM6 lib_deps = https://git.kleiax.de/PlatformIO-Libs/SparkFunGNSS.git knolleary/PubSubClient@^2.8 @@ -23,16 +23,18 @@ lib_deps = marcoschwartz/LiquidCrystal_I2C@^1.1.4 bblanchon/ArduinoJson@^6.20.0 mprograms/QMC5883LCompass@^1.2.0 - https://git.kleiax.de/PlatformIO-Libs/Menu.git - nrf24/RF24@^1.4.5 -upload_port = COM6 -; extra_scripts = -; pre:autoVersionIncrement/version_increment_pre.py -; post:autoVersionIncrement/version_increment_post.py + jrowberg/I2Cdevlib-MPU6050@^1.0.0 +; upload_port = COM6 test_ignore = test_desktop build_type = debug monitor_filters = esp32_exception_decoder check_tool = clangtidy +check_flags = + clangtidy: --checks=-*,cert-*,cppcoreguidelines-*,clang-analyzer-*,google-*,hicpp-*,modernize-*,performance-*,portability-*,readability-*,bugprone-*,misc-*,openmp-*,objc-* --fix +check_src_filters = + + + + + + [env:native] platform = native @@ -40,3 +42,21 @@ test_ignore = test_embedded [platformio] description = A Rover who should be drive a route by gps. + + +; Bei Möglichkeit folgendes aus clang tidy entfernen +; cppcoreguidelines-owning-memory +; +; +; +; +; +; +; +; +; +; +; +; +; +# diff --git a/src/OutputBuf/outputBuf.cpp b/src/OutputBuf/outputBuf.cpp index 1e761d0..7d4cd83 100644 --- a/src/OutputBuf/outputBuf.cpp +++ b/src/OutputBuf/outputBuf.cpp @@ -1,59 +1,76 @@ /** - * @file outputBufMqtt.cpp + * @file outputBuf.cpp * @author Alexander Klein (alex@kleiax.de) * @brief Contains the implementation of the class OutputBufMqtt * @version 0.1 * @date 2022-02-14 - * + * * @copyright Copyright (c) 2022 - * + * */ #include "OutputBuf/outputBuf.h" -OutputBuf::OutputBuf(DebugMqtt* debugMqtt, BluetoothSerial* serialBT) - : std::streambuf() { - this->debugMqtt = debugMqtt; - this->serialBT = serialBT; - - if (debugMqtt) +OutputBuf::OutputBuf(DebugMqtt *debugMqtt, BluetoothSerial *serialBT) + : debugMqtt{debugMqtt}, serialBT{serialBT} +{ + if (static_cast(debugMqtt)) + { this->isMqttActive = true; + } - if (serialBT) + if (static_cast(serialBT)) + { this->isMqttActive = true; + } } -void OutputBuf::activateMqtt(bool status) { - if (debugMqtt) +void OutputBuf::activateMqtt(bool status) +{ + if (static_cast(debugMqtt)) + { this->isMqttActive = status; + } } -void OutputBuf::activateSerialBT(bool status) { - if (serialBT) +void OutputBuf::activateSerialBT(bool status) +{ + if (static_cast(serialBT)) + { this->isSerialBTActive = status; + } } -void OutputBuf::setDebugMqtt(DebugMqtt* debugMqtt) { +void OutputBuf::setDebugMqtt(DebugMqtt *debugMqtt) +{ this->debugMqtt = debugMqtt; this->isMqttActive = true; } -void OutputBuf::setSerialBT(BluetoothSerial* serialBT) { +void OutputBuf::setSerialBT(BluetoothSerial *serialBT) +{ this->serialBT = serialBT; - this->isSerialBTActive = true;; + this->isSerialBTActive = true; + ; } -std::streambuf::int_type OutputBuf::overflow(std::streambuf::int_type c) { - if (c != EOF) { +std::streambuf::int_type OutputBuf::overflow(std::streambuf::int_type character) +{ + if (character != EOF) + { // c = std::toupper(static_cast(c), getloc()); if (this->debugMqtt && this->isMqttActive) - this->debugMqtt->addCharacter((char) c); + { + this->debugMqtt->addCharacter(static_cast(character)); + } if (this->serialBT && this->isSerialBTActive && this->serialBT->connected()) - this->serialBT->print((char) c); + { + this->serialBT->print(static_cast(character)); + } - Serial.print((char) c); + Serial.print(static_cast(character)); } - return c; + return character; } diff --git a/src/OutputBuf/outputBuf.h b/src/OutputBuf/outputBuf.h index 6b785b4..f75cfe2 100644 --- a/src/OutputBuf/outputBuf.h +++ b/src/OutputBuf/outputBuf.h @@ -1,16 +1,16 @@ /** - * @file outputBufMqtt.h + * @file outputBuf.h * @author Alexander Klein (alex@kleiax.de) * @brief Contains a small class that provide a streambuf - * + * * The streambuf is used to double the std::cout to consol * and MQTT. - * + * * @version 0.1 * @date 2022-02-14 - * + * * @copyright Copyright (c) 2022 - * + * */ #ifndef OUTPUT_STREAM_MQTT_H @@ -26,60 +26,61 @@ #include /** - * @brief A alternativ streambuf for std::cout - * + * @brief A alternative streambuf for std::cout + * * The streambuf is used to double the std::cout to consol * and MQTT. - * + * */ -class OutputBuf : public std::streambuf { - public: - /** - * @brief Construct a new Output Buf Mqtt object - * - * @param debugMqtt - */ - OutputBuf(DebugMqtt* debugMqtt = nullptr, BluetoothSerial* serialBT = nullptr); +class OutputBuf : public std::streambuf +{ +public: + /** + * @brief Construct a new Output Buf Mqtt object + * + * @param debugMqtt + * @param serialBT + */ + OutputBuf(DebugMqtt *debugMqtt = nullptr, BluetoothSerial *serialBT = nullptr); - /** - * @brief Activate the mqtt output - * - * This only works if the DebugMqtt is set by - * setDebugMqtt. - * - * @param status - */ - void activateMqtt(bool status); + /** + * @brief Activate the mqtt output + * + * This only works if the DebugMqtt is set by + * setDebugMqtt(). + * + * @param status + */ + void activateMqtt(bool status); - /** - * @brief Active the output over the bluetooth serial consol - * - * This only works if BluetoothSerial is set by - * setSerialBT. - * - * @param status - */ - void activateSerialBT(bool status); + /** + * @brief Active the output over the bluetooth serial consol + * + * This only works if BluetoothSerial is set by + * setSerialBT(). + * + * @param status + */ + void activateSerialBT(bool status); - void setDebugMqtt(DebugMqtt* debugMqtt); - void setSerialBT(BluetoothSerial* serialBT); + void setDebugMqtt(DebugMqtt *debugMqtt); + void setSerialBT(BluetoothSerial *serialBT); +protected: + /** + * @brief + * + * @param c + * @return std::streambuf::int_type + */ + virtual std::streambuf::int_type overflow(std::streambuf::int_type c); - protected: - /** - * @brief - * - * @param c - * @return std::streambuf::int_type - */ - virtual std::streambuf::int_type overflow(std::streambuf::int_type c); +private: + bool isMqttActive = false; + bool isSerialBTActive = false; - private: - bool isMqttActive = false; - bool isSerialBTActive = false; - - DebugMqtt* debugMqtt; - BluetoothSerial* serialBT; + DebugMqtt *debugMqtt; + BluetoothSerial *serialBT; }; #endif // OUTPUT_STREAM_MQTT_H diff --git a/src/SpecialMenus/CalibrateBattery/menuCalibrateBattery.cpp b/src/SpecialMenus/CalibrateBattery/menuCalibrateBattery.cpp new file mode 100644 index 0000000..ca8b0ff --- /dev/null +++ b/src/SpecialMenus/CalibrateBattery/menuCalibrateBattery.cpp @@ -0,0 +1,70 @@ +/** + * @file menuCalibrateBattery.cpp + * @author Alexander Klein (alex@kleiax.de) + * @brief + * @version 0.1 + * @date 2023-09-26 + * + * @copyright Copyright (c) 2023 + * + */ + +#include "menuCalibrateBattery.h" + +MenuCalibrateBattery::MenuCalibrateBattery(Battery *battery) + : battery{battery} +{ + this->setUpdateDelay(MenuCalibrateBattery::updateDelay); +} + +void MenuCalibrateBattery::printMenu() +{ + if (this->battery->getCalibrationState() == Battery::CalibrationState::None) + { + this->battery->startCalibration(); + } + + String lineOne = ""; + String lineTwo = ""; + + switch (this->battery->getCalibrationState()) + { + case Battery::CalibrationState::Waiting: + lineOne = "Target Voltage:"; + lineTwo = "7 + "; + lineTwo.concat(this->battery->getCurrentCalibrationVoltage()); + lineTwo.concat(" / 10"); + break; + + case Battery::CalibrationState::Reading: + lineOne = "Reading..."; + break; + + case Battery::CalibrationState::Finished: + lineOne = "Finished!"; + break; + + default: + break; + } + + this->print(lineOne, lineTwo); +} + +void MenuCalibrateBattery::left() +{ + this->battery->finishCalibration(); + + if (static_cast(parentMenu)) + { + this->parentMenu->printMenu(); + } +} + +void MenuCalibrateBattery::right() +{ + if (this->battery->getCalibrationState() == Battery::CalibrationState::Waiting) + { + this->battery->nextVoltageIsReady(); + } +} diff --git a/src/SpecialMenus/CalibrateBattery/menuCalibrateBattery.h b/src/SpecialMenus/CalibrateBattery/menuCalibrateBattery.h new file mode 100644 index 0000000..50b45eb --- /dev/null +++ b/src/SpecialMenus/CalibrateBattery/menuCalibrateBattery.h @@ -0,0 +1,50 @@ +/** + * @file menuCalibrateBattery.h + * @author Alexander Klein (alex@kleiax.de) + * @brief Contains a Menu class to calibrate the Battery class + * @version 0.1 + * @date 2023-09-26 + * + * @copyright Copyright (c) 2023 + * + */ + +#ifndef MENU_CALIBRATE_BATTERY_H +#define MENU_CALIBRATE_BATTERY_H + +#include "menuControl.h" +#include "battery.h" + +/** + * @brief A Menu class to calibrate the Battery class + */ +class MenuCalibrateBattery : public MenuControl +{ +public: + /** + * @brief Construct a new MenuCalibrateBattery object + * + * Prepare the calibration. + * + * @param battery + */ + MenuCalibrateBattery(Battery *battery); + + /** + * @brief Prints the instructions for the calibration + */ + void printMenu() override; + + // User Inputs + void left() override; + void no() override { this->left(); } + void right() override; + void yes() override { this->right(); } + +private: + Battery *battery; + + static constexpr uint16_t updateDelay = 400; +}; + +#endif // MENU_CALIBRATE_BATTERY_H diff --git a/src/SpecialMenus/GPS/menuGPS.cpp b/src/SpecialMenus/GPS/menuGPS.cpp deleted file mode 100644 index f255155..0000000 --- a/src/SpecialMenus/GPS/menuGPS.cpp +++ /dev/null @@ -1,132 +0,0 @@ -/** - * @file menuGPS.cpp - * @author Alexander Klein (alex@kleiax.de) - * @brief Contains an implementation of the class MenuGPS - * @version 0.1 - * @date 2022-01-29 - * - * @copyright Copyright (c) 2022 - * - */ - -#include "menuGPS.h" - -MenuGPS::MenuGPS(SensorData* sensorData) : - MenuInformationSites(10) { - this->sensorData = sensorData; -} - -void MenuGPS::printPage() const { - const UBX_NAV_PVT_data_t* gpsData = this->sensorData->getGnssData(); - uint8_t fixType = 0; - if (gpsData) - fixType = gpsData->fixType; - - String lineOne = "Data isn't valid"; - String lineTwo = "or no GPS signal"; - - switch (this->getCurrentPage()) { - case 0: - if (fixType) { - lineOne = "Sats: "; - lineOne.concat(gpsData->numSV); - lineTwo = "PDOP: "; - lineTwo.concat(gpsData->pDOP); - } - break; - - case 1: - if (fixType) { - lineOne = "Lat: "; - lineOne.concat(gpsData->lat); - lineTwo = "Lon: "; - lineTwo.concat(gpsData->lon); - } - break; - - case 2: - lineOne = "Time: "; - lineTwo = ""; - if (fixType) { - if (gpsData->hour < 10) - lineTwo.concat("0"); - lineTwo.concat(gpsData->hour); - lineTwo.concat(":"); - if (gpsData->min < 10) - lineTwo.concat("0"); - lineTwo.concat(gpsData->min); - lineTwo.concat(":"); - if (gpsData->sec < 10) - lineTwo.concat("0"); - lineTwo.concat(gpsData->sec); - } else - lineTwo = "00:00:00"; - - break; - - case 3: - lineOne = "Fix type:"; - if (fixType == 0) - lineTwo = "None"; - else if (fixType == 1) - lineTwo = "Dead Reckoning"; - else if (fixType == 2) - lineTwo = "2D"; - else if (fixType == 3) - lineTwo = "3D"; - else if (fixType == 3) - lineTwo = "GNSS + Dead Reck"; - else if (fixType == 5) - lineTwo = "Time Only"; - else - lineTwo = "UNKNOWN"; - break; - - case 4: { - lineOne = "Carrier Solution"; - uint8_t carrSoln = gpsData->flags.bits.carrSoln; - if (carrSoln == 0) - lineTwo = "None"; - else if (carrSoln == 1) - lineTwo = "Floating"; - else if (carrSoln == 2) - lineTwo = "Fixed"; - else - lineTwo = "UNKNOWN"; - } - break; - - case 5: - lineOne = "Hrizntl Accuracy"; - if (fixType) { - lineTwo = ""; - lineTwo.concat(gpsData->hAcc); - } else - lineTwo = "0"; - break; - - case 6: - lineOne = "magDec: "; - lineTwo = "magAcc: "; - if (fixType) { - lineOne.concat(gpsData->magDec); - lineTwo.concat(gpsData->magAcc); - } else { - lineOne.concat("---"); - lineTwo.concat("---"); - } - break; - - case 7: - lineOne = "Azimuth: "; - lineTwo = ""; - lineTwo.concat(this->sensorData->getRealAzimuth()); - break; - - default: - this->printDefault(); - return; - } - - this->print(lineOne, lineTwo); -} diff --git a/src/SpecialMenus/GPS/menuGPS.h b/src/SpecialMenus/GPS/menuGPS.h deleted file mode 100644 index 902c673..0000000 --- a/src/SpecialMenus/GPS/menuGPS.h +++ /dev/null @@ -1,39 +0,0 @@ -/** - * @file menuGPS.h - * @author Alexander Klein (alex@kleiax.de) - * @brief Contains a class to print informations about the GPS object - * @version 0.1 - * @date 2022-01-29 - * - * @copyright Copyright (c) 2022 - * - */ - -#ifndef MENU_GPS_H -#define MENU_GPS_H - -#include - -#include "menuInformationSites.h" -#include "sensorData.h" - -/** - * @brief A class to print informations about the GPS object - * - */ -class MenuGPS : public MenuInformationSites { - public: - /** - * @brief Construct a new Menu GPS object - * - * @param gps - */ - MenuGPS(SensorData* sensorData); - - private: - void printPage() const override; - - SensorData* sensorData; -}; - -#endif // MENU_GPS_H diff --git a/src/SpecialMenus/PID/menuPidSettings.cpp b/src/SpecialMenus/PID/menuPidSettings.cpp index f5f5c00..d85c9ac 100644 --- a/src/SpecialMenus/PID/menuPidSettings.cpp +++ b/src/SpecialMenus/PID/menuPidSettings.cpp @@ -4,19 +4,20 @@ * @brief Contains an implementation of the class MenuPidSettings * @version 0.1 * @date 2022-01-19 - * + * * @copyright Copyright (c) 2022 - * + * */ #include "menuPidSettings.h" -MenuPidSettings::MenuPidSettings(PID* pid) { - this->pid = pid; -} +MenuPidSettings::MenuPidSettings(PID *pid) : pid{pid} {} -void MenuPidSettings::action(int16_t* values, uint8_t length) { - if (length != 3) +void MenuPidSettings::action(int16_t *values, uint8_t length) +{ + if (length != 3) + { return; + } this->pid->SetTunings(values[0], values[1], values[2]); } diff --git a/src/SpecialMenus/PID/menuPidSettings.h b/src/SpecialMenus/PID/menuPidSettings.h index 61d5c08..cc9f877 100644 --- a/src/SpecialMenus/PID/menuPidSettings.h +++ b/src/SpecialMenus/PID/menuPidSettings.h @@ -4,9 +4,9 @@ * @brief Contains a class to tune the PID settings * @version 0.1 * @date 2022-01-19 - * + * * @copyright Copyright (c) 2022 - * + * */ #ifndef PID_SETTINGS_H @@ -18,22 +18,22 @@ /** * @brief A class to tune the PID settings - * + * */ -class MenuPidSettings : public MenuIntInputWrapper { - public: - /** - * @brief Construct a new Menu Pid Settings object - * - * @param pid - */ - MenuPidSettings(PID* pid); +class MenuPidSettings : public MenuIntInputWrapper +{ +public: + /** + * @brief Construct a new Menu Pid Settings object + * + * @param pid + */ + MenuPidSettings(PID *pid); - void action(int16_t* values, uint8_t length); - - private: - PID* pid; + void action(int16_t *values, uint8_t length) override; +private: + PID *pid; }; #endif // PID_SETTINGS_H diff --git a/src/SpecialMenus/SensorData/menuSensorData.cpp b/src/SpecialMenus/SensorData/menuSensorData.cpp new file mode 100644 index 0000000..19ec032 --- /dev/null +++ b/src/SpecialMenus/SensorData/menuSensorData.cpp @@ -0,0 +1,152 @@ +/** + * @file menuSensorData.cpp + * @author Alexander Klein (alex@kleiax.de) + * @brief + * @version 0.1 + * @date 2023-09-04 + * + * @copyright Copyright (c) 2023 + * + */ + +#include "menuSensorData.h" + +MenuSensorData::MenuSensorData(SensorData *sensorData) : MenuInformationSites(7), sensorData{sensorData} +{ + this->noEqualLeft = true; +} + +void MenuSensorData::printPage() const +{ + const UBX_NAV_PVT_data_t *gpsData = this->sensorData->getGnssData(); + uint8_t fixType = 0; + if (static_cast(gpsData)) + { + fixType = gpsData->fixType; + } + + String lineOne = ""; + String lineTwo = ""; + + switch (this->getCurrentPage()) + { + case 0: + lineOne = " Y P R :"; + lineTwo.concat(this->sensorData->getGyroData()[0]); + lineTwo.concat(" "); + lineTwo.concat(this->sensorData->getGyroData()[1]); + lineTwo.concat(" "); + lineTwo.concat(this->sensorData->getGyroData()[2]); + break; + + case 1: + lineOne = "Real Azimuth:"; + lineTwo.concat(this->sensorData->getRealAzimuth()); + break; + + case 2: + lineOne = "Calc Azimuth:"; + lineTwo.concat(this->sensorData->getCalcAzimuth()); + lineTwo.concat(" "); + lineTwo.concat(CalcAzimuth::stateToString(this->sensorData->getCalcAzimuthState())); + break; + + case 3: + { + lineOne = "Lat:"; + lineTwo = "Lon:"; + const Point pos = this->sensorData->getCurrentPos(); + lineOne.concat(pos.getLatitude()); + lineTwo.concat(pos.getLongitude()); + } + break; + + case 4: + lineOne = "Time: "; + lineTwo = ""; + if (static_cast(fixType)) + { + if (gpsData->hour < 10) + { + lineTwo.concat("0"); + } + lineTwo.concat(gpsData->hour); + lineTwo.concat(":"); + if (gpsData->min < 10) + { + lineTwo.concat("0"); + } + lineTwo.concat(gpsData->min); + lineTwo.concat(":"); + if (gpsData->sec < 10) + { + lineTwo.concat("0"); + } + lineTwo.concat(gpsData->sec); + } + else + { + lineTwo = "00:00:00"; + } + + break; + + case 5: + { + lineOne = "CarSol: "; + lineTwo = "Ntrip: "; + const uint8_t carrSoln = gpsData->flags.bits.carrSoln; + if (carrSoln == 0) + { + lineOne.concat("None"); + } + else if (carrSoln == 1) + { + lineOne.concat("Floating"); + } + else if (carrSoln == 2) + { + lineOne.concat("Fixed"); + } + else + { + lineOne = "UNKNOWN"; + } + const NTRIPClientStates status = this->sensorData->getNtripState(); + if (status == NTRIPClientStates::pushData) + { + lineTwo.concat("enabled"); + } + else if (status == NTRIPClientStates::notAvailable) + { + lineTwo.concat(" N/A"); + } + else + { + lineTwo.concat("disabled"); + } + } + break; + + case 6: + lineOne = "HAcc: "; + lineTwo = "Sats: "; + if (static_cast(fixType)) + { + lineOne.concat(gpsData->hAcc); + lineTwo.concat(gpsData->numSV); + } + else + { + lineOne.concat("-/-"); + lineTwo.concat("-/-"); + } + break; + + default: + this->printDefault(); + return; + } + + this->print(lineOne, lineTwo); +} diff --git a/src/SpecialMenus/SensorData/menuSensorData.h b/src/SpecialMenus/SensorData/menuSensorData.h new file mode 100644 index 0000000..83d820b --- /dev/null +++ b/src/SpecialMenus/SensorData/menuSensorData.h @@ -0,0 +1,37 @@ +/** + * @file menuSensorData.h + * @author Alexander Klein (alex@kleiax.de) + * @brief Contains a MenuInformationClass to print the SensorData + * @version 0.1 + * @date 2023-09-04 + * + * @copyright Copyright (c) 2023 + * + */ + +#ifndef MENU_SENSOR_DATA_H +#define MENU_SENSOR_DATA_H + +#include "menuInformationSites.h" +#include "sensorData.h" + +/** + * @brief A MenuInformationClass to print the SensorData + */ +class MenuSensorData : public MenuInformationSites +{ +public: + /** + * @brief Construct a new MenuSensorData object + * + * @param SensorData + */ + MenuSensorData(SensorData *SensorData); + +private: + void printPage() const override; + + SensorData *sensorData; +}; + +#endif // MENU_SENSOR_DATA_H diff --git a/src/SpecialMenus/Speed/menuSpeed.cpp b/src/SpecialMenus/Speed/menuSpeed.cpp new file mode 100644 index 0000000..6f4b170 --- /dev/null +++ b/src/SpecialMenus/Speed/menuSpeed.cpp @@ -0,0 +1,22 @@ +/** + * @file menuSpeed.cpp + * @author Alexander Klein (alex@kleiax.de) + * @brief + * @version 0.1 + * @date 2023-09-26 + * + * @copyright Copyright (c) 2023 + * + */ + +#include "menuSpeed.h" + +void MenuSpeed::action(int16_t *values, uint8_t length) +{ + if (length != 2) + { + return; + } + this->speeds.x = values[0] / 10.0; + this->speeds.rot = values[1] / 10.0; +} diff --git a/src/SpecialMenus/Speed/menuSpeed.h b/src/SpecialMenus/Speed/menuSpeed.h new file mode 100644 index 0000000..419cf91 --- /dev/null +++ b/src/SpecialMenus/Speed/menuSpeed.h @@ -0,0 +1,38 @@ +/** + * @file menuSpeed.h + * @author Alexander Klein (alex@kleiax.de) + * @brief Contains a MenuIntInputWrapper to change speeds + * @version 0.1 + * @date 2023-09-26 + * + * @copyright Copyright (c) 2023 + * + */ + +#ifndef MENU_SPEED_H +#define MENU_SPEED_H + +#include +#include "moveControl.h" + +/** + * @brief A MenuIntInputWrapper class to change speeds + */ +class MenuSpeed : public MenuIntInputWrapper +{ +public: + MenuSpeed(DrivingSpeeds &speeds) : speeds(speeds){}; + + /** + * @brief Changes the speeds + * + * @param values + * @param length + */ + void action(int16_t *values, uint8_t length) override; + +private: + DrivingSpeeds &speeds; +}; + +#endif // MENU_SPEED_H diff --git a/src/SpecialMenus/Systeminformation/menuSysteminformatio.cpp b/src/SpecialMenus/Systeminformation/menuSysteminformatio.cpp index fd2e5ab..a145123 100644 --- a/src/SpecialMenus/Systeminformation/menuSysteminformatio.cpp +++ b/src/SpecialMenus/Systeminformation/menuSysteminformatio.cpp @@ -4,71 +4,73 @@ * @brief Contains an implementation of the class MenuSysteminformation * @version 0.1 * @date 2022-12-27 - * + * * @copyright Copyright (c) 2022 - * + * */ #include "menuSysteminformation.h" -MenuSysteminformation::MenuSysteminformation(Battery* mainBattery) - : MenuInformationSites(8) { - this->mainBattery = mainBattery; +MenuSysteminformation::MenuSysteminformation(Battery *mainBattery) + : MenuInformationSites(8), mainBattery{mainBattery} +{ + this->noEqualLeft = true; } -void MenuSysteminformation::printPage() const { +void MenuSysteminformation::printPage() const +{ String lineOne = ""; String lineTwo = ""; - switch (this->getCurrentPage()) { - case 0: - lineOne = "Free Heap:"; - lineTwo.concat(ESP.getFreeHeap()); - break; + switch (this->getCurrentPage()) + { + case 0: + lineOne = "Free Heap:"; + lineTwo.concat(ESP.getFreeHeap()); + break; - case 1: - lineOne = "Max alloc heap:"; - lineTwo.concat(ESP.getMaxAllocHeap()); - break; + case 1: + lineOne = "Max alloc Heap:"; + lineTwo.concat(ESP.getMaxAllocHeap()); + break; - case 2: - lineOne = "Uptime in secs:"; - lineTwo.concat((millis() / 1000)); - break; + case 2: + lineOne = "Uptime in secs:"; + lineTwo.concat((millis() / 1000)); + break; - case 3: - lineOne = "Main battery:"; - lineTwo = ""; - lineTwo.concat(this->mainBattery->getBatteryPercent()); - lineTwo.concat("% - V="); - lineTwo.concat(this->mainBattery->getBatteryVoltage()); - break; + case 3: + lineOne = "Main battery:"; + lineTwo = ""; + lineTwo.concat(this->mainBattery->getBatteryPercent()); + lineTwo.concat("% - V="); + lineTwo.concat(this->mainBattery->getBatteryVoltage()); + break; - case 4: - lineOne = "Current WiFi"; - lineTwo = "channel: "; - lineTwo.concat(Network::getCurrentChannel()); - break; + case 4: + lineOne = "Current WiFi"; + lineTwo = "channel: "; + lineTwo.concat(Network::getCurrentChannel()); + break; - case 5: - lineOne = "Software verion:"; - lineTwo = VERSION; - break; + case 5: + lineOne = "Software verion:"; + lineTwo = VERSION; + break; - case 6: - lineOne = "Build timestamp:"; - lineTwo = String(BUILD_TIMESTAMP).substring(0, 16); - break; + case 6: + lineOne = "Build timestamp:"; + lineTwo = String(BUILD_TIMESTAMP).substring(0, 16); + break; - case 7: - lineOne = "Kleiax Rover by"; - lineTwo = "Alexander Klein"; - break; + case 7: + lineOne = "Kleiax Rover by"; + lineTwo = "Alexander Klein"; + break; - default: - this->printDefault(); - return; + default: + this->printDefault(); + return; } this->print(lineOne, lineTwo); } - diff --git a/src/SpecialMenus/Systeminformation/menuSysteminformation.h b/src/SpecialMenus/Systeminformation/menuSysteminformation.h index d0d885a..754596f 100644 --- a/src/SpecialMenus/Systeminformation/menuSysteminformation.h +++ b/src/SpecialMenus/Systeminformation/menuSysteminformation.h @@ -4,9 +4,9 @@ * @brief Contains a class to print informations about the system * @version 0.1 * @date 2022-12-27 - * + * * @copyright Copyright (c) 2022 - * + * */ #ifndef MENU_SYSTEMINFORMATION_H @@ -23,23 +23,23 @@ /** * @brief Prints information about the current system status. - * + * * The informations are about the batteries, memory and uptime. */ -class MenuSysteminformation : public MenuInformationSites { - public: - /** - * @brief Construct a new Menu Systeminformation object - * - * @param mainBattery - * @param gamepad - */ - MenuSysteminformation(Battery* mainBattery); +class MenuSysteminformation : public MenuInformationSites +{ +public: + /** + * @brief Construct a new Menu Systeminformation object + * + * @param mainBattery + */ + MenuSysteminformation(Battery *mainBattery); - private: - void printPage() const override; +private: + void printPage() const override; - Battery* mainBattery; + Battery *mainBattery; }; #endif // MENU_SYSTEMINFORMATION_H diff --git a/src/SpecialMenus/driveModi/CalibrateCompass/menuCalibrateCompass.cpp b/src/SpecialMenus/driveModi/CalibrateCompass/menuCalibrateCompass.cpp index 9069641..83950a6 100644 --- a/src/SpecialMenus/driveModi/CalibrateCompass/menuCalibrateCompass.cpp +++ b/src/SpecialMenus/driveModi/CalibrateCompass/menuCalibrateCompass.cpp @@ -4,159 +4,167 @@ * @brief Contains an implementation of the class MenuManualDrive * @version 0.1 * @date 2022-01-20 - * + * * @copyright Copyright (c) 2022 - * + * */ #include "menuCalibrateCompass.h" -MenuCalibrateCompass::MenuCalibrateCompass(DriveManager* driveManager) : MenuDriveMode(driveManager) {} +MenuCalibrateCompass::MenuCalibrateCompass(DriveManager *driveManager) : MenuDriveMode(driveManager) {} -MenuCalibrateCompass::~MenuCalibrateCompass() { +MenuCalibrateCompass::~MenuCalibrateCompass() +{ delete this->caliCompass; this->caliCompassMode->setCalibrateCompass(); } -void MenuCalibrateCompass::printPage() const { +void MenuCalibrateCompass::printPage() const +{ String lineOne = ""; String lineTwo = ""; - switch (this->getCurrentPage()) { - case 0: - lineOne = "-Ready to drive-"; - lineTwo = "Compass Mode"; + switch (this->getCurrentPage()) + { + case 0: + lineOne = "-Ready to drive-"; + lineTwo = "Compass Mode"; + break; + + case 1: + lineOne = "Azimuth:"; + lineTwo.concat(this->caliCompassMode->getSensorData()->getRealAzimuth()); + break; + + case 2: + switch (this->caliCompass->getState()) + { + case CalibrateCompass::State::Ready: + lineOne = "Start compass"; + lineTwo = "calibration"; break; - case 1: - lineOne = "Azimuth:"; - lineTwo.concat(this->caliCompassMode->getSensorData()->getRealAzimuth()); - break; - - case 2: - switch (this->caliCompass->getState()) { - case CalibrateCompass::State::Ready : - lineOne = "Start compass"; - lineTwo = "calibration"; - break; - - case CalibrateCompass::State::Calibrating : - lineOne = "Calibrating..."; - lineTwo = "Move around"; - break; - - case CalibrateCompass::State::Finished: - lineOne = "Calibration"; - lineTwo = "finished"; - break; - - default: - break; - } + case CalibrateCompass::State::Calibrating: + lineOne = "Calibrating..."; + lineTwo = "Move around"; break; - case 3: - lineOne = "Save compass"; - lineTwo = "data in flash"; - break; - - case 4: - lineOne = "Load data"; - lineTwo = "from flash"; - break; - - case 5: - lineOne = "Remove"; - lineTwo = "calibration data"; - break; - - case 6: - lineOne = "Use current"; - lineTwo = "calibration data"; - break; - - case 7: - lineOne = "Reset for new"; - lineTwo = "calibration run"; - break; - - case 8: - lineOne = "X min: "; - lineOne.concat(this->caliCompass->getCallibrationData().data[0][0]); - lineTwo = "X max: "; - lineTwo.concat(this->caliCompass->getCallibrationData().data[0][1]); - break; - - case 9: - lineOne = "Y min: "; - lineOne.concat(this->caliCompass->getCallibrationData().data[1][0]); - lineTwo = "Y max: "; - lineTwo.concat(this->caliCompass->getCallibrationData().data[1][1]); - break; - - case 10: - lineOne = "Z min: "; - lineOne.concat(this->caliCompass->getCallibrationData().data[2][0]); - lineTwo = "Z max: "; - lineTwo.concat(this->caliCompass->getCallibrationData().data[2][1]); + case CalibrateCompass::State::Finished: + lineOne = "Calibration"; + lineTwo = "finished"; break; default: - this->printDefault(); + break; + } + break; + + case 3: + lineOne = "Save compass"; + lineTwo = "data in flash"; + break; + + case 4: + lineOne = "Load data"; + lineTwo = "from flash"; + break; + + case 5: + lineOne = "Remove"; + lineTwo = "calibration data"; + break; + + case 6: + lineOne = "Use current"; + lineTwo = "calibration data"; + break; + + case 7: + lineOne = "Reset for new"; + lineTwo = "calibration run"; + break; + + case 8: + lineOne = "X min: "; + lineOne.concat(this->caliCompass->getCalibrationData().data[0][0]); + lineTwo = "X max: "; + lineTwo.concat(this->caliCompass->getCalibrationData().data[0][1]); + break; + + case 9: + lineOne = "Y min: "; + lineOne.concat(this->caliCompass->getCalibrationData().data[1][0]); + lineTwo = "Y max: "; + lineTwo.concat(this->caliCompass->getCalibrationData().data[1][1]); + break; + + case 10: + lineOne = "Z min: "; + lineOne.concat(this->caliCompass->getCalibrationData().data[2][0]); + lineTwo = "Z max: "; + lineTwo.concat(this->caliCompass->getCalibrationData().data[2][1]); + break; + + default: + this->printDefault(); return; } this->print(lineOne, lineTwo); } -void MenuCalibrateCompass::init() { +void MenuCalibrateCompass::init() +{ this->firstPrint = false; - this->driveManager->changeModus(Modi::CalibrateCompass); - this->caliCompassMode = (CalibrateCompassM*) this->driveManager->getDriveModiPtr(); + this->caliCompassMode = new CalibrateCompassM(); + this->driveManager->changeModus(this->caliCompassMode); this->caliCompass = new CalibrateCompass(this->caliCompassMode->getSensorData()->getRealCompass()); this->setCountPages(11); - this->updateDelay = 500; + MenuDriveMode::updateDelay = MenuCalibrateCompass::updateDelay; } -void MenuCalibrateCompass::runCommand() const { - switch (this->getCurrentPage()) { - case 2: - switch (this->caliCompass->getState()) { - case CalibrateCompass::State::Ready : - this->caliCompassMode->setCalibrateCompass(this->caliCompass); - this->caliCompass->start(); - break; - - case CalibrateCompass::State::Finished: - this->caliCompassMode->setCalibrateCompass(); - this->caliCompass->useData(); - break; - - default: - break; - } +void MenuCalibrateCompass::runCommand() +{ + switch (this->getCurrentPage()) + { + case 2: + switch (this->caliCompass->getState()) + { + case CalibrateCompass::State::Ready: + this->caliCompassMode->setCalibrateCompass(this->caliCompass); + this->caliCompass->start(); break; - case 3: - this->caliCompass->saveData(); - break; - - case 4: - this->caliCompass->loadData(); - break; - - case 5: - this->caliCompass->removeCalibration(); - break; - - case 6: + case CalibrateCompass::State::Finished: + this->caliCompassMode->setCalibrateCompass(); this->caliCompass->useData(); - break; + break; - case 7: - this->caliCompass->reset(); - break; - default: break; + } + break; + + case 3: + this->caliCompass->saveData(); + break; + + case 4: + this->caliCompass->loadData(); + break; + + case 5: + this->caliCompass->removeCalibration(); + break; + + case 6: + this->caliCompass->useData(); + break; + + case 7: + this->caliCompass->reset(); + break; + + default: + break; } } diff --git a/src/SpecialMenus/driveModi/CalibrateCompass/menuCalibrateCompass.h b/src/SpecialMenus/driveModi/CalibrateCompass/menuCalibrateCompass.h index 284e200..fd57f64 100644 --- a/src/SpecialMenus/driveModi/CalibrateCompass/menuCalibrateCompass.h +++ b/src/SpecialMenus/driveModi/CalibrateCompass/menuCalibrateCompass.h @@ -4,9 +4,9 @@ * @brief Contains a class to print information about the class ManualControl * @version 0.1 * @date 2022-01-20 - * + * * @copyright Copyright (c) 2022 - * + * */ #ifndef MENU_CALIBRATE_COMPASS_H @@ -18,35 +18,37 @@ /** * @brief A class to print informations about the class ManualControl - * + * */ -class MenuCalibrateCompass : public MenuDriveMode { - public: - /** - * @brief Construct a new Menu Manual Control object - * - * @param driveManager - */ - MenuCalibrateCompass(DriveManager* driveManager); - ~MenuCalibrateCompass(); +class MenuCalibrateCompass : public MenuDriveMode +{ +public: + /** + * @brief Construct a new Menu Manual Control object + * + * @param driveManager + */ + MenuCalibrateCompass(DriveManager *driveManager); + ~MenuCalibrateCompass(); - /** - * @brief Prints the Information to display and console - * - * The informations are only printed to the display if it - * set. - * - * Changes the DriveModi to Autopilot if it is the first time called. - */ - void printPage() const override; + /** + * @brief Prints the Information to display and console + * + * The informations are only printed to the display if it + * set. + * + * Changes the DriveModi to Autopilot if it is the first time called. + */ + void printPage() const override; - protected: - void init() override; - void runCommand() const override; +private: + void init() override; + void runCommand() override; - private: - CalibrateCompassM* caliCompassMode; - CalibrateCompass* caliCompass; + CalibrateCompassM *caliCompassMode = nullptr; + CalibrateCompass *caliCompass = nullptr; + + static constexpr uint16_t updateDelay = 500; }; #endif // MENU_CALIBRATE_COMPASS_H diff --git a/src/SpecialMenus/driveModi/ManualDrive/menuManualDrive.cpp b/src/SpecialMenus/driveModi/ManualDrive/menuManualDrive.cpp index f77d6c3..9ef590e 100644 --- a/src/SpecialMenus/driveModi/ManualDrive/menuManualDrive.cpp +++ b/src/SpecialMenus/driveModi/ManualDrive/menuManualDrive.cpp @@ -4,94 +4,83 @@ * @brief Contains an implementation of the class MenuManualDrive * @version 0.1 * @date 2022-01-20 - * + * * @copyright Copyright (c) 2022 - * + * */ #include "menuManualDrive.h" -MenuManualControl::MenuManualControl(DriveManager* driveManager) : MenuDriveMode(driveManager) { - +MenuManualControl::MenuManualControl(DriveManager *driveManager) : MenuDriveMode(driveManager) +{ + this->noEqualLeft = true; } -MenuManualControl::~MenuManualControl() { -} - -void MenuManualControl::printPage() const { +void MenuManualControl::printPage() const +{ String lineOne = ""; String lineTwo = ""; - switch (this->getCurrentPage()) { - case 0: - lineOne = "-Ready to drive-"; - break; + switch (this->getCurrentPage()) + { + case 0: + lineOne = "-Ready to drive-"; + break; - case 1: - lineOne = "Input mode:"; - if (this->manualControl->getInputMode() == ManualControl::InputMode::Analog) - lineTwo = "Analog"; - else - lineTwo = "Digital"; - break; + case 1: + lineOne = "Input mode:"; + if (this->manualControl->getInputMode() == ManualControl::InputMode::Analog) + { + lineTwo = "Analog"; + } + else + { + lineTwo = "Digital"; + } + break; - case 2: - lineOne = "Speed: "; - lineOne.concat(this->manualControl->getMaxSpeed()); - lineTwo = "- dec 0.1 inc +"; - break; + case 2: + lineOne = "Speed Menu"; + break; - case 3: - lineOne = "RotSpeed: "; - lineOne.concat(this->manualControl->getMaxRotation()); - lineTwo = "- dec 0.1 inc +"; - break; + case 3: + lineOne = "Sensor Menu"; + break; - default: - this->printDefault(); + default: + this->printDefault(); return; } this->print(lineOne, lineTwo); } -void MenuManualControl::init() { +void MenuManualControl::init() +{ this->firstPrint = false; - this->driveManager->changeModus(Modi::ManualControl); - this->manualControl = (ManualControl*) this->driveManager->getDriveModiPtr(); + this->manualControl = new ManualControl(); + this->driveManager->changeModus(this->manualControl); this->setCountPages(4); - this->updateDelay = 500; + MenuDriveMode::updateDelay = MenuManualControl::updateDelay; + this->activateSpeedMenu(); + MenuDriveMode::init(); } -void MenuManualControl::runCommand() const { - switch (this->getCurrentPage()) { - case 1: - this->manualControl->switchInputMode(); - break; - - case 2: - this->manualControl->increaseMaxSpeed(); - break; - - case 3: - this->manualControl->increaseMaxRotation(); - break; - - default: - break; - } -} - -void MenuManualControl::runCommandNo() const { +void MenuManualControl::runCommand() +{ switch (this->getCurrentPage()) { + case 1: + this->manualControl->switchInputMode(); + break; + case 2: - this->manualControl->decreaseMaxSpeed(); + this->enterSpeedMenu(); break; case 3: - this->manualControl->decreaseMaxRotation(); + this->enterSensorMenu(); break; - + default: break; } diff --git a/src/SpecialMenus/driveModi/ManualDrive/menuManualDrive.h b/src/SpecialMenus/driveModi/ManualDrive/menuManualDrive.h index 2b1c3d8..f405a00 100644 --- a/src/SpecialMenus/driveModi/ManualDrive/menuManualDrive.h +++ b/src/SpecialMenus/driveModi/ManualDrive/menuManualDrive.h @@ -4,49 +4,49 @@ * @brief Contains a class to print information about the class ManualControl * @version 0.1 * @date 2022-01-20 - * + * * @copyright Copyright (c) 2022 - * + * */ #ifndef MENU_MANUAL_DRIVE_H #define MENU_MANUAL_DRIVE_H #include "SpecialMenus/driveModi/menuDriveMode.h" -#include "calibrateCompass.h" +#include "driveModi/Modi/ManualControl/manualControl.h" /** * @brief A class to print informations about the class ManualControl - * + * */ -class MenuManualControl : public MenuDriveMode { - public: - /** - * @brief Construct a new Menu Manual Control object - * - * @param driveManager - */ - MenuManualControl(DriveManager* driveManager); - ~MenuManualControl(); +class MenuManualControl : public MenuDriveMode +{ +public: + /** + * @brief Construct a new Menu Manual Control object + * + * @param driveManager + */ + MenuManualControl(DriveManager *driveManager); - /** - * @brief Prints the Information to display and console - * - * The informations are only printed to the display if it - * set. - * - * Changes the DriveModi to Autopilot if it is the first time called. - */ - void printPage() const override; + /** + * @brief Prints the Information to display and console + * + * The informations are only printed to the display if it + * set. + * + * Changes the DriveModi to Autopilot if it is the first time called. + */ + void printPage() const override; - protected: - void init() override; - void runCommand() const override; - void runCommandNo() const override; +private: + void init() override; + void runCommand() override; - private: - ManualControl* manualControl; - CalibrateCompass* caliCompass; + ManualControl *manualControl = nullptr; + CalibrateCompass *caliCompass = nullptr; + + static constexpr uint16_t updateDelay = 500; }; #endif // MENU_MANUAL_DRIVE_H diff --git a/src/SpecialMenus/driveModi/TestMode/menuTestMode.cpp b/src/SpecialMenus/driveModi/TestMode/menuTestMode.cpp index 18a40d1..aafcca5 100644 --- a/src/SpecialMenus/driveModi/TestMode/menuTestMode.cpp +++ b/src/SpecialMenus/driveModi/TestMode/menuTestMode.cpp @@ -4,24 +4,29 @@ * @brief Contains the implementation of the class MenuTestMode. * @version 0.1 * @date 2022-09-08 - * + * * @copyright Copyright (c) 2022 - * + * */ #include "menuTestMode.h" -MenuTestMode::MenuTestMode(DriveManager* driveManager) { - this->driveManager = driveManager; +MenuTestMode::MenuTestMode(DriveManager *driveManager) + : driveManager{driveManager} +{ } -MenuTestMode::~MenuTestMode() { - if (isInit) { +MenuTestMode::~MenuTestMode() +{ + if (isInit) + { delete this->mainMenu; } } -void MenuTestMode::printMenu() { - if (!this->isInit) { +void MenuTestMode::printMenu() +{ + if (!this->isInit) + { this->isInit = true; this->init(); } @@ -29,76 +34,98 @@ void MenuTestMode::printMenu() { this->mainMenu->printMenu(); } -void MenuTestMode::down() { +void MenuTestMode::down() +{ if (this->checkInput()) + { this->mainMenu->down(); + } } -void MenuTestMode::up() { +void MenuTestMode::up() +{ if (this->checkInput()) + { this->mainMenu->up(); + } } -void MenuTestMode::right() { +void MenuTestMode::right() +{ if (this->checkInput()) + { this->mainMenu->right(); + } } -void MenuTestMode::left() { +void MenuTestMode::left() +{ if (this->mainMenu->isInSubmenu()) if (this->checkInput()) + { this->mainMenu->left(); - else { + } + else + { this->testMode->abortManeuver(); this->mainMenu->left(); } else + { this->parentMenu->printMenu(); + } } -void MenuTestMode::yes() { +void MenuTestMode::yes() +{ if (this->checkInput()) + { this->mainMenu->yes(); + } } -void MenuTestMode::no() { +void MenuTestMode::no() +{ if (this->mainMenu->isInSubmenu()) + { if (this->checkInput()) this->mainMenu->no(); - else { + else + { this->testMode->abortManeuver(); this->mainMenu->no(); } + } else + { this->left(); + } } -void MenuTestMode::init() { - this->driveManager->changeModus(Modi::TestMode); - this->testMode = (TestMode*) this->driveManager->getDriveModiPtr(); +void MenuTestMode::init() +{ + this->testMode = new TestMode(); + this->driveManager->changeModus(this->testMode); - testMode->setMaxSpeed(1); - testMode->setMaxRotation(8); - - auto dummy = []() { - std::cout << "Dummy in Action" <mainMenu = new Menu; - Menu* engineMenu = new Menu; - Menu* lightMenu = new Menu; - Menu* encoderMenu = new Menu; + auto *engineMenu = new Menu; + auto *lightMenu = new Menu; + auto *encoderMenu = new Menu; // Sound // Ping google - MenuIntInput* drivingMenu = new MenuIntInput(2, new MenuTestModeWrapper(this, &TestMode::drive)); - MenuIntInput* engineLeftMenu = new MenuIntInput(2, new MenuTestModeWrapper(this, &TestMode::leftEngine)); - MenuIntInput* engineRightMenu = new MenuIntInput(2, new MenuTestModeWrapper(this, &TestMode::rightEngine)); - MenuIntInput* engineBothMenu = new MenuIntInput(2, new MenuTestModeWrapper(this, &TestMode::bothEngine)); - SpeedometerTest* encoderLeftMenu = new SpeedometerTest(this->testMode->getSpeedometerLeft()); - SpeedometerTest* encoderRightMenu = new SpeedometerTest(this->testMode->getSpeedometerRight()); - + auto *drivingMenu = new MenuIntInput(2, new MenuTestModeWrapper(this, &TestMode::drive)); + auto *engineLeftMenu = new MenuIntInput(2, new MenuTestModeWrapper(this, &TestMode::leftEngine)); + auto *engineRightMenu = new MenuIntInput(2, new MenuTestModeWrapper(this, &TestMode::rightEngine)); + auto *engineBothMenu = new MenuIntInput(2, new MenuTestModeWrapper(this, &TestMode::bothEngine)); + auto *encoderLeftMenu = new SpeedometerTest(this->testMode->getSpeedometerLeft()); + auto *encoderRightMenu = new SpeedometerTest(this->testMode->getSpeedometerRight()); // Set menus on LCD this->mainMenu->setLcd(this->lcd); @@ -113,49 +140,53 @@ void MenuTestMode::init() { encoderRightMenu->setLcd(this->lcd); // Other menu config + static constexpr uint8_t percentMax = 100; + static constexpr uint16_t degreeMax = 360; + static constexpr uint8_t secondsMax = 120; + static constexpr uint8_t steps = 5; + drivingMenu->setEntry(0, "Centimeter", 100); - drivingMenu->setMinMaxSteps(0, -1000, 1000, 25); + drivingMenu->setMinMaxSteps(0, -1000, 1000, steps * 5); drivingMenu->setEntry(1, "Degree"); - drivingMenu->setMinMaxSteps(1, -360, 360, 15); + drivingMenu->setMinMaxSteps(1, -degreeMax, degreeMax, steps * 3); drivingMenu->setPrintParentMenu(false); engineLeftMenu->setEntry(0, "Percentage"); - engineLeftMenu->setMinMaxSteps(0, -100, 100, 5); - engineLeftMenu->setEntry(1, "Seconds", 5); - engineLeftMenu->setMinMaxSteps(1, 0, 120, 1); + engineLeftMenu->setMinMaxSteps(0, -percentMax, percentMax, steps); + engineLeftMenu->setEntry(1, "Seconds", steps); + engineLeftMenu->setMinMaxSteps(1, 0, secondsMax, 1); engineLeftMenu->setPrintParentMenu(false); engineRightMenu->setEntry(0, "Percentage"); - engineRightMenu->setMinMaxSteps(0, -100, 100, 5); - engineRightMenu->setEntry(1, "Seconds", 5); - engineRightMenu->setMinMaxSteps(1, 0, 120, 1); + engineRightMenu->setMinMaxSteps(0, -percentMax, percentMax, steps); + engineRightMenu->setEntry(1, "Seconds", steps); + engineRightMenu->setMinMaxSteps(1, 0, secondsMax, 1); engineRightMenu->setPrintParentMenu(false); engineBothMenu->setEntry(0, "Percentage"); - engineBothMenu->setMinMaxSteps(0, -100, 100, 5); - engineBothMenu->setEntry(1, "Seconds", 5); - engineBothMenu->setMinMaxSteps(1, 0, 120, 1); + engineBothMenu->setMinMaxSteps(0, -percentMax, percentMax, steps); + engineBothMenu->setEntry(1, "Seconds", steps); + engineBothMenu->setMinMaxSteps(1, 0, secondsMax, 1); engineBothMenu->setPrintParentMenu(false); - // Entrys for the menus - MenuAction* engineAction = new MenuAction("Engine", engineMenu); - MenuAction* engineLeftAction = new MenuAction("Left", engineLeftMenu); - MenuAction* engineRightAction = new MenuAction("Right", engineRightMenu); - MenuAction* engineBothAction = new MenuAction("Both", engineBothMenu); + auto *engineAction = new MenuAction("Engine", engineMenu); + auto *engineLeftAction = new MenuAction("Left", engineLeftMenu); + auto *engineRightAction = new MenuAction("Right", engineRightMenu); + auto *engineBothAction = new MenuAction("Both", engineBothMenu); - MenuAction* drivingAction = new MenuAction("Driving", drivingMenu); + auto *drivingAction = new MenuAction("Driving", drivingMenu); - MenuAction* encoderAction = new MenuAction("Encoder", encoderMenu); - MenuAction* encoderLeftAction = new MenuAction("Left", encoderLeftMenu); - MenuAction* encoderRightAction = new MenuAction("Right", encoderRightMenu); + auto *encoderAction = new MenuAction("Encoder", encoderMenu); + auto *encoderLeftAction = new MenuAction("Left", encoderLeftMenu); + auto *encoderRightAction = new MenuAction("Right", encoderRightMenu); - MenuAction* lightAction = new MenuAction("Light", lightMenu); - MenuAction* lightFlashAction = new MenuAction("Flash", dummy); - MenuAction* lightFadeAction = new MenuAction("Fade", dummy); - MenuAction* lightRedAction = new MenuAction("Red", dummy); - MenuAction* lightGreenAction = new MenuAction("Green", dummy); - MenuAction* lightBlueAction = new MenuAction("Blue", dummy); - MenuAction* lightOffAction = new MenuAction("Off", dummy); + auto *lightAction = new MenuAction("Light", lightMenu); + auto *lightFlashAction = new MenuAction("Flash", dummy); + auto *lightFadeAction = new MenuAction("Fade", dummy); + auto *lightRedAction = new MenuAction("Red", dummy); + auto *lightGreenAction = new MenuAction("Green", dummy); + auto *lightBlueAction = new MenuAction("Blue", dummy); + auto *lightOffAction = new MenuAction("Off", dummy); - // Add entrys to the menus + // Add entries to the menus this->mainMenu->addEntry(engineAction); this->mainMenu->addEntry(drivingAction); this->mainMenu->addEntry(encoderAction); @@ -173,69 +204,89 @@ void MenuTestMode::init() { lightMenu->addEntry(lightRedAction); lightMenu->addEntry(lightGreenAction); lightMenu->addEntry(lightBlueAction); - lightMenu->addEntry(lightOffAction); + lightMenu->addEntry(lightOffAction); } -void MenuTestMode::update() { +void MenuTestMode::update() +{ if (this->maneuverInAction) - if (millis() - this->lastUpdateTime > updateDelay) { + { + if (millis() - this->lastUpdateTime > updateDelay) + { this->printManeuverTime(); this->lastUpdateTime = millis(); } + } } -bool MenuTestMode::checkInput() { - if (this->testMode->getBusy()) { +bool MenuTestMode::checkInput() +{ + if (this->testMode->getBusy()) + { this->printManeuverTime(); this->maneuverInAction = true; return false; } if (this->maneuverInAction) + { this->maneuverInAction = false; - - return true; + } + + return true; } -void MenuTestMode::printManeuverTime() { +void MenuTestMode::printManeuverTime() +{ uint8_t maneuverTime = this->testMode->getRemainingManeuverTime(); - if (maneuverTime) { + if (maneuverTime) + { String lineTwo = ""; lineTwo.concat(maneuverTime); lineTwo.concat(" secs"); this->print("Maneuver time:", lineTwo); - } else { + } + else + { this->maneuverInAction = false; this->printMenu(); } } -MenuTestModeWrapper::MenuTestModeWrapper(MenuTestMode* menu, TestModeFunctionSingle testModeFunction) { - this->menu = menu; - this->testMode = this->menu->getTestMode(); - this->testModeFunctionSingle = testModeFunction; +MenuTestModeWrapper::MenuTestModeWrapper(MenuTestMode *menu, TestModeFunctionSingle testModeFunction) + : menu{menu}, + testModeFunctionSingle{testModeFunction}, + testMode{menu->getTestMode()} +{ } -MenuTestModeWrapper::MenuTestModeWrapper(MenuTestMode* menu, TestModeFunctionDouble testModeFunction) { - this->menu = menu; - this->testMode = this->menu->getTestMode(); - this->testModeFunctionDouble = testModeFunction; +MenuTestModeWrapper::MenuTestModeWrapper(MenuTestMode *menu, TestModeFunctionDouble testModeFunction) + : menu{menu}, + testModeFunctionDouble{testModeFunction}, + testMode{menu->getTestMode()} +{ } -void MenuTestModeWrapper::action(int16_t* values, uint8_t length) { +void MenuTestModeWrapper::action(int16_t *values, uint8_t length) +{ // This function calls a member function with a pointer // Very helpful site: // https://isocpp.org/wiki/faq/pointers-to-members#fnptr-vs-memfnptr-types bool res = false; - if (length == 2 && this->testModeFunctionDouble) + if (length == 2 && static_cast(this->testModeFunctionDouble)) + { res = (this->testMode->*this->testModeFunctionDouble)(values[0], values[1]); - else if (length == 1 && this->testModeFunctionSingle) + } + else if (length == 1 && static_cast(this->testModeFunctionSingle)) + { res = (this->testMode->*this->testModeFunctionSingle)(values[0]); + } - if (res) { + if (res) + { this->menu->printManeuverTime(); this->menu->maneuverStarted(); } -} +} diff --git a/src/SpecialMenus/driveModi/TestMode/menuTestMode.h b/src/SpecialMenus/driveModi/TestMode/menuTestMode.h index cb3c3b5..6d22523 100644 --- a/src/SpecialMenus/driveModi/TestMode/menuTestMode.h +++ b/src/SpecialMenus/driveModi/TestMode/menuTestMode.h @@ -4,9 +4,9 @@ * @brief Contains a class to use and print information about the TestMode. * @version 0.1 * @date 2022-09-08 - * + * * @copyright Copyright (c) 2022 - * + * */ #ifndef MENU_TEST_MODE_H #define MENU_TEST_MODE_H @@ -20,80 +20,82 @@ /** * @brief Interact with the TestMode - * + * */ -class MenuTestMode : public MenuControl { - public: - /** - * @brief Construct a new Menu Test Mode object - * - * @param driveManager - */ - MenuTestMode(DriveManager *driveManager); - ~MenuTestMode(); +class MenuTestMode : public MenuControl +{ +public: + /** + * @brief Construct a new Menu Test Mode object + * + * @param driveManager + */ + MenuTestMode(DriveManager *driveManager); + ~MenuTestMode(); - /** - * @brief Prints the last informations - * - * On first call this function calls the init function. - * On every call this functions call the printMenu function from - * the mainMenu of this class. - */ - void printMenu() override; + /** + * @brief Prints the last informations + * + * On first call this function calls the init function. + * On every call this functions call the printMenu function from + * the mainMenu of this class. + */ + void printMenu() override; - /** - * @brief Prints maneuver Timer - * - * Aside from that the function looks if the maneuver is done and - * give the user input free. - */ - void printManeuverTime(); + /** + * @brief Prints maneuver Timer + * + * Aside from that the function looks if the maneuver is done and + * give the user input free. + */ + void printManeuverTime(); - void down() override; - void up() override; - void right() override; - void left() override; - void yes() override; - void no() override; + void down() override; + void up() override; + void right() override; + void left() override; + void yes() override; + void no() override; - /** - * @brief Calls the printManeuverTime function if a maneuver is in process. - * - */ - void update() override; + /** + * @brief Calls the printManeuverTime function if a maneuver is in process. + * + */ + void update() override; - TestMode* getTestMode() const { return this->testMode; } - void maneuverStarted() { this->maneuverInAction = true; } + TestMode *getTestMode() const { return this->testMode; } + void maneuverStarted() { this->maneuverInAction = true; } - private: - void init(); - bool checkInput(); +private: + void init(); + bool checkInput(); - DriveManager* driveManager; - TestMode* testMode; - Menu* mainMenu; + DriveManager *driveManager; + TestMode *testMode = nullptr; + Menu *mainMenu = nullptr; - bool isInit = false; - bool maneuverInAction = false; + bool isInit = false; + bool maneuverInAction = false; - uint32_t lastUpdateTime = 0; - uint16_t updateDelay = 1000; + uint32_t lastUpdateTime = 0; + uint16_t updateDelay = 1000; }; -typedef bool (TestMode::*TestModeFunctionSingle)(int16_t); -typedef bool (TestMode::*TestModeFunctionDouble)(int16_t, int16_t); -class MenuTestModeWrapper : public MenuIntInputWrapper { - public: - MenuTestModeWrapper(MenuTestMode* menu, TestModeFunctionSingle testModeFunction); - MenuTestModeWrapper(MenuTestMode* menu, TestModeFunctionDouble testModeFunction); +typedef bool (TestMode::*TestModeFunctionSingle)(int16_t); +typedef bool (TestMode::*TestModeFunctionDouble)(int16_t, int16_t); +class MenuTestModeWrapper : public MenuIntInputWrapper +{ +public: + MenuTestModeWrapper(MenuTestMode *menu, TestModeFunctionSingle testModeFunction); + MenuTestModeWrapper(MenuTestMode *menu, TestModeFunctionDouble testModeFunction); - void action(int16_t* values, uint8_t length) override; + void action(int16_t *values, uint8_t length) override; - private: - MenuTestMode* menu; - TestMode* testMode; - TestModeFunctionSingle testModeFunctionSingle = nullptr; - TestModeFunctionDouble testModeFunctionDouble = nullptr; +private: + MenuTestMode *menu; + TestMode *testMode; + TestModeFunctionSingle testModeFunctionSingle = nullptr; + TestModeFunctionDouble testModeFunctionDouble = nullptr; }; #endif // MENU_TEST_MODE_H diff --git a/src/SpecialMenus/driveModi/TestMode/speedometerTest.cpp b/src/SpecialMenus/driveModi/TestMode/speedometerTest.cpp index cf06a1b..4f3735c 100644 --- a/src/SpecialMenus/driveModi/TestMode/speedometerTest.cpp +++ b/src/SpecialMenus/driveModi/TestMode/speedometerTest.cpp @@ -1,79 +1,87 @@ /** - * @file menuTest.cpp + * @file speedometerTest.cpp * @author Alexander Klein (alex@kleiax.de) * @brief Contains the implementation of the class SpeedometerTest. * @version 0.1 * @date 2023-01-24 - * + * * @copyright Copyright (c) 2023 - * + * */ #include "speedometerTest.h" -SpeedometerTest::SpeedometerTest(Speedometer* speedometer) { - this->speedometer = speedometer; +SpeedometerTest::SpeedometerTest(Speedometer *speedometer) + : speedometer{speedometer} +{ this->speedometer->setDirection(Speedometer::Forward); } +void SpeedometerTest::printMenu() +{ + switch (this->state) + { + case State::Off: + this->print("Press Yes (O)", "to start"); + break; -void SpeedometerTest::printMenu() { - switch (this->state) { - case State::Off : - this->print("Press Yes (O)", "to start"); - break; + case State::Running: + this->print("Running, Yes (O)", "to stop"); + break; - case State::Running : - this->print("Running, Yes (O)", "to stop"); - break; + case State::Finished: + { + String res = ""; + res.concat(this->result); + this->print("Result: ", res); + } + break; - case State::Finished : { - String res = ""; - res.concat(this->result); - this->print("Result: ", res); - } - break; - - default: - this->print("Error in", "MenuTest.cpp"); - break; + default: + this->print("Error in", "MenuTest.cpp"); + break; } } - -void SpeedometerTest::left() { +void SpeedometerTest::left() +{ this->no(); } -void SpeedometerTest::no() { +void SpeedometerTest::no() +{ if (this->state == State::Running) + { speedometer->calibrationMeasurementStop(); + } this->state = State::Off; this->parentMenu->printMenu(); } -void SpeedometerTest::yes() { - switch (this->state) { - case State::Off : - this->state = State::Running; - this->speedometer->calibrationMeasurementStart(); - this->printMenu(); - break; +void SpeedometerTest::yes() +{ + switch (this->state) + { + case State::Off: + this->state = State::Running; + this->speedometer->calibrationMeasurementStart(); + this->printMenu(); + break; - case State::Running : - this->state = State::Finished; - this->result = this->speedometer->calibrationMeasurementStop(); - this->printMenu(); - break; - - case State::Finished : - this->state = State::Running; - this->speedometer->calibrationMeasurementStart(); - this->printMenu(); - break; + case State::Running: + this->state = State::Finished; + this->result = this->speedometer->calibrationMeasurementStop(); + this->printMenu(); + break; - default: - break; + case State::Finished: + this->state = State::Running; + this->speedometer->calibrationMeasurementStart(); + this->printMenu(); + break; + + default: + break; } } diff --git a/src/SpecialMenus/driveModi/TestMode/speedometerTest.h b/src/SpecialMenus/driveModi/TestMode/speedometerTest.h index 609e474..ad8173a 100644 --- a/src/SpecialMenus/driveModi/TestMode/speedometerTest.h +++ b/src/SpecialMenus/driveModi/TestMode/speedometerTest.h @@ -1,12 +1,12 @@ /** - * @file menuTest.h + * @file speedometerTest.h * @author Alexander Klein (alex@kleiax.de) * @brief Contains a class to test the wheel encoder. * @version 0.1 * @date 2023-01-24 - * + * * @copyright Copyright (c) 2023 - * + * */ #pragma once @@ -16,49 +16,51 @@ /** * @brief Tests the wheel encoder. - * + * */ -class SpeedometerTest : public MenuControl { - public: - /** - * @brief The states of the test. - */ - enum State { - Off, - Running, - Finished - }; +class SpeedometerTest : public MenuControl +{ +public: + /** + * @brief The states of the test. + */ + enum State + { + Off, + Running, + Finished + }; - /** - * @brief Construct a new Speedometer Test object - * - * @param speedometer - */ - SpeedometerTest(Speedometer* speedometer); + /** + * @brief Construct a new Speedometer Test object + * + * @param speedometer + */ + SpeedometerTest(Speedometer *speedometer); - /** - * @brief Prints the actual state of the test. - */ - void printMenu() override; + /** + * @brief Prints the actual state of the test. + */ + void printMenu() override; - /** - * @brief Calls no. - */ - void left() override; + /** + * @brief Calls no. + */ + void left() override; - /** - * @brief Exit the menu or end the test. - */ - void no() override; + /** + * @brief Exit the menu or end the test. + */ + void no() override; - /** - * @brief Starts or restarts the test. - */ - void yes() override; + /** + * @brief Starts or restarts the test. + */ + void yes() override; - private: - Speedometer* speedometer; - State state = State::Off; +private: + Speedometer *speedometer; + State state = State::Off; - uint16_t result = 0; + uint16_t result = 0; }; diff --git a/src/SpecialMenus/driveModi/menuDriveMode.cpp b/src/SpecialMenus/driveModi/menuDriveMode.cpp index 76b0bf3..527aeca 100644 --- a/src/SpecialMenus/driveModi/menuDriveMode.cpp +++ b/src/SpecialMenus/driveModi/menuDriveMode.cpp @@ -4,27 +4,168 @@ * @brief Contains an implementation of the class MenuDriveMode * @version 0.1 * @date 2022-01-31 - * + * * @copyright Copyright (c) 2022 - * + * */ #include "menuDriveMode.h" -MenuDriveMode::MenuDriveMode(DriveManager* driveManager) { - this->driveManager = driveManager; +MenuDriveMode::MenuDriveMode(DriveManager *driveManager) + : driveManager{driveManager} +{ } -void MenuDriveMode::left() { - this->firstPrint = true; - this->configureOnLeave(); - this->driveManager->changeModus(Modi::Off); - if (parentMenu) { - this->parentMenu->printMenu(); - this->leaved = true; +MenuDriveMode::~MenuDriveMode() +{ + if (this->selfCreatedMenuSpeed) + { + delete this->menuSpeed; } } -void MenuDriveMode::no() { - this->left(); +void MenuDriveMode::addSpeedMenu(MenuIntInput *menuSpeed) +{ + if (this->selfCreatedMenuSpeed) + { + delete this->menuSpeed; + this->selfCreatedMenuSpeed = false; + } + + this->menuSpeed = menuSpeed; +} + +void MenuDriveMode::left() +{ + if (static_cast(this->activeMenu)) + { + if (static_cast(this->activeMenu)) + { + this->activeMenu->left(); + } + return; + } + + this->firstPrint = true; + this->configureOnLeave(); + delete this->menuSpeed; + this->driveManager->changeModus(); + MenuInformationSites::left(); +} + +void MenuDriveMode::right() +{ + if (static_cast(this->activeMenu)) + { + this->activeMenu->right(); + return; + } + MenuInformationSites::right(); +} + +void MenuDriveMode::up() +{ + if (static_cast(this->activeMenu)) + { + this->activeMenu->up(); + return; + } + MenuInformationSites::up(); +} + +void MenuDriveMode::down() +{ + if (static_cast(this->activeMenu)) + { + this->activeMenu->down(); + return; + } + MenuInformationSites::down(); +} + +void MenuDriveMode::yes() +{ + if (static_cast(this->activeMenu)) + { + this->activeMenu->yes(); + return; + } + MenuInformationSites::yes(); +} + +void MenuDriveMode::no() +{ + if (static_cast(this->activeMenu)) + { + this->activeMenu->no(); + return; + } + MenuInformationSites::no(); +} + +void MenuDriveMode::prepareReenterMenu() +{ + this->activeMenu = nullptr; +} + +void MenuDriveMode::printMenu() +{ + if (static_cast(this->activeMenu)) + { + this->activeMenu->printMenu(); + return; + } + + MenuInformationSites::printMenu(); +} + +void MenuDriveMode::init() +{ + this->activeMenu = nullptr; +} + +void MenuDriveMode::activateSpeedMenu() +{ + if (!this->driveManager->isActive()) + { + return; + } + + if (this->selfCreatedMenuSpeed) + { + delete this->menuSpeed; + } + + DrivingSpeeds &speeds = this->driveManager->getDriveModiPtr()->getSpeedsRef(); + auto *menu = new MenuIntInput(2, new MenuSpeed(speeds)); + + menu->setMinMax(5, UINT8_MAX); + menu->setEntry(0, "x m/s e-1", driveManager->getDrivingSpeedsRef().x * 10); + menu->setEntry(1, "z rad/s e-1", driveManager->getDrivingSpeedsRef().rot * 10); + + this->selfCreatedMenuSpeed = true; + this->addSpeedMenu(menu); +} + +void MenuDriveMode::enterSpeedMenu() +{ + this->activeMenu = this->menuSpeed; + this->enterMenu(); +} + +void MenuDriveMode::enterSensorMenu() +{ + this->activeMenu = this->menuSensor; + this->enterMenu(); +} + +void MenuDriveMode::enterMenu() +{ + if (!static_cast(this->activeMenu)) + { + return; + } + + this->activeMenu->printMenu(); + this->activeMenu->setParentMenu(this); } diff --git a/src/SpecialMenus/driveModi/menuDriveMode.h b/src/SpecialMenus/driveModi/menuDriveMode.h index 7e4ad87..ce9bcbf 100644 --- a/src/SpecialMenus/driveModi/menuDriveMode.h +++ b/src/SpecialMenus/driveModi/menuDriveMode.h @@ -4,44 +4,94 @@ * @brief Contains a base class for Menus about DriveModi * @version 0.1 * @date 2022-01-31 - * + * * @copyright Copyright (c) 2022 - * + * */ #ifndef MENU_DRIVE_MODI_H #define MENU_DRIVE_MODI_H #include "menuInformationSites.h" #include "driveModi/driveManager.h" +#include "SpecialMenus/SensorData/menuSensorData.h" +#include "SpecialMenus/Speed/menuSpeed.h" /** * @brief A base class for Menus about DriveModi - * + * */ -class MenuDriveMode : public MenuInformationSites { - public: - /** - * @brief Construct a new Menu Drive Mode object - * - * @param driveManager - */ - MenuDriveMode(DriveManager* driveManager); +class MenuDriveMode : public MenuInformationSites +{ +public: + /** + * @brief Construct a new Menu Drive Mode object + * + * @param driveManager + */ + MenuDriveMode(DriveManager *driveManager); - /** - * @brief Goes back to the parentMenu. - */ - void left() override; + ~MenuDriveMode(); - /** - * @brief Calls left. - */ - void no() override; + void addSensorMenu(MenuSensorData *menuSensor) { this->menuSensor = menuSensor; } + void addSpeedMenu(MenuIntInput *menuSpeed); - protected: - void configureOnLeave() {} + /** + * @brief Goes back to the parentMenu. + */ + void left() override; + void right() override; + void up() override; + void down() override; + void yes() override; + void no() override; - DriveManager* driveManager; - bool firstPrint = true; + void prepareReenterMenu() override; + void printMenu() override; + +protected: + void init() override; + + /** + * @brief Prepare leaving the menu + * + * Can be overwritten to add extra functionality to safely + * exit the menu. This function is called before the parentMenu + * is printed. + */ + virtual void configureOnLeave() {} + + /** + * @brief Creates an own SpeedMenu + * + * This function uses addSpeedMenu() at the end to that the new menu + */ + void activateSpeedMenu(); + + /** + * @brief Enter a Menu to edit the speeds + * + * To enter this menu it has to be set by addSpeedMenu() or activateSpeedMenu() + */ + void enterSpeedMenu(); + + /** + * @brief Enter a Menu with the SensorData + * + * To enter this menu it has to be set by addSensorMenu() + */ + void enterSensorMenu(); + + DriveManager *driveManager; + + bool firstPrint = true; + +private: + void enterMenu(); + + MenuControl *activeMenu = nullptr; + MenuSensorData *menuSensor = nullptr; + MenuIntInput *menuSpeed = nullptr; + + bool selfCreatedMenuSpeed = false; }; - #endif // MENU_DRIVE_MODI_H diff --git a/src/driveModi/Modi/CalibrateCompass/calibrateCompassM.cpp b/src/driveModi/Modi/CalibrateCompass/calibrateCompassM.cpp index d2e195b..f4a2bd4 100644 --- a/src/driveModi/Modi/CalibrateCompass/calibrateCompassM.cpp +++ b/src/driveModi/Modi/CalibrateCompass/calibrateCompassM.cpp @@ -1,23 +1,25 @@ /** * @file calibrateCompassM.cpp * @author Alexander Klein (alex@kleiax.de) - * @brief + * @brief * @version 0.1 * @date 2023-09-03 - * + * * @copyright Copyright (c) 2023 - * + * */ #include "calibrateCompassM.h" -CalibrateCompassM::CalibrateCompassM(DriveModiParams params) : ManualControl(params) {} - -void CalibrateCompassM::setCalibrateCompass(CalibrateCompass *caliCompass) { - if (caliCompass) { +void CalibrateCompassM::setCalibrateCompass(CalibrateCompass *caliCompass) +{ + if (static_cast(caliCompass)) + { this->caliCompass = caliCompass; this->addChildComponent(this->caliCompass); - } else if (this->caliCompass) { + } + else if (static_cast(this->caliCompass)) + { this->removeChildComponent(this->caliCompass); this->caliCompass = caliCompass; } diff --git a/src/driveModi/Modi/CalibrateCompass/calibrateCompassM.h b/src/driveModi/Modi/CalibrateCompass/calibrateCompassM.h index b101721..50ef4f7 100644 --- a/src/driveModi/Modi/CalibrateCompass/calibrateCompassM.h +++ b/src/driveModi/Modi/CalibrateCompass/calibrateCompassM.h @@ -1,12 +1,12 @@ /** * @file calibrateCompassM.h * @author Alexander Klein (alex@kleiax.de) - * @brief + * @brief Contains a Menu class to calibrate the compass * @version 0.1 * @date 2023-09-03 - * + * * @copyright Copyright (c) 2023 - * + * */ #ifndef CALIBRATE_COMPASS_M_H @@ -16,14 +16,21 @@ #include "driveModi/Modi/ManualControl/manualControl.h" -class CalibrateCompassM : public ManualControl { - public: - CalibrateCompassM(DriveModiParams params); +/** + * @brief A Menu class to interact with CalibrateCompass class + */ +class CalibrateCompassM : public ManualControl +{ +public: + /** + * @brief Set the CalibrateCompass object + * + * @param caliCompass + */ + void setCalibrateCompass(CalibrateCompass *caliCompass = nullptr); - void setCalibrateCompass(CalibrateCompass* caliCompass = nullptr); - - private: - CalibrateCompass* caliCompass = nullptr; +private: + CalibrateCompass *caliCompass = nullptr; }; -#endif //CALIBRATE_COMPASS_M_H +#endif // CALIBRATE_COMPASS_M_H diff --git a/src/driveModi/Modi/ManualControl/manualControl.cpp b/src/driveModi/Modi/ManualControl/manualControl.cpp index a6891ab..de54097 100644 --- a/src/driveModi/Modi/ManualControl/manualControl.cpp +++ b/src/driveModi/Modi/ManualControl/manualControl.cpp @@ -4,73 +4,89 @@ * @brief Implementation of the class manualControl.h * @version 0.1 * @date 2021-12-13 - * + * * @copyright Copyright (c) 2021 - * + * */ #include "manualControl.h" -void ManualControl::run() { - switch (this->inputMode) { - case InputMode::Analog : - this->analogControl(); - break; +void ManualControl::run() +{ + switch (this->inputMode) + { + case InputMode::Analog: + this->analogControl(); + break; - case InputMode::Digital : - this->digitalControl(); - break; - - default: - break; + case InputMode::Digital: + this->digitalControl(); + break; + + default: + break; } } -void ManualControl::switchInputMode() { +void ManualControl::switchInputMode() +{ this->inputMode = (this->inputMode == InputMode::Analog) ? InputMode::Digital : InputMode::Analog; } -void ManualControl::analogControl() { +void ManualControl::analogControl() +{ // Have to be int16_t to avoid overflow (int8_t = 255 - 127 = -128) - int16_t x = this->input->x - 127; - int16_t y = this-> input->y - 127; + const int16_t xAxis = this->input->x - 127; + const int16_t yAxis = this->input->y - 127; - // static int counter = 0; - // if (counter % 60 == 0) { - // std::cout << "Input: x: " << (int) this->input->x << " y: " << (int) this->input->y << std::endl; - // std::cout << "Output: x: " << (int) x << " y: " << (int) y << std::endl; - // } - // counter++; + double value_per_step = this->maxSpeeds.x * 2 / UINT8_MAX; + this->moveControl->setSpeed(-xAxis * value_per_step); - double value_per_step = this->maxForwardSpeed * 2 / UINT8_MAX; - this->moveControl->setSpeed(-x * value_per_step); - - value_per_step = this->maxRotationSpeed * 2 / UINT8_MAX; - this->moveControl->setRotationSpeed(y * value_per_step); + value_per_step = this->maxSpeeds.rot * 2 / UINT8_MAX; + this->moveControl->setRotationSpeed(yAxis * value_per_step); } -void ManualControl::digitalControl() { - int16_t y = this->input->x - 127; - int16_t x = this->input->y - 127; +void ManualControl::digitalControl() +{ + static constexpr uint8_t deadzone = 120; + const int16_t yAxis = this->input->x - 127; + const int16_t xAxis = this->input->y - 127; - if (y > 120) - this->moveControl->setSpeed(-this->maxForwardSpeed); - else if (y < -120) - this->moveControl->setSpeed(this->maxForwardSpeed); + if (yAxis > deadzone) + { + this->moveControl->setSpeed(-this->maxSpeeds.x); + } + else if (yAxis < -deadzone) + { + this->moveControl->setSpeed(this->maxSpeeds.rot); + } else + { this->moveControl->setSpeed(0); + } - if (x > 120) - this->moveControl->setRotationSpeed(this->maxRotationSpeed); - else if (x < -120) - this->moveControl->setRotationSpeed(-this->maxRotationSpeed); + if (xAxis > deadzone) + { + this->moveControl->setRotationSpeed(this->maxSpeeds.rot); + } + else if (xAxis < -deadzone) + { + this->moveControl->setRotationSpeed(-this->maxSpeeds.rot); + } else + { this->moveControl->setRotationSpeed(0); + } - if (this->directionChangeWrapper && (x > 120 || x < -120)) + if (static_cast(this->directionChangeWrapper) && (xAxis > deadzone || xAxis < -deadzone)) + { this->lastLoopTurned = true; - else if (this->lastLoopTurned) { - if (this->directionChangeWrapper) + } + else if (this->lastLoopTurned) + { + if (static_cast(this->directionChangeWrapper)) + { this->directionChangeWrapper->action(); + } this->lastLoopTurned = false; - } + } } diff --git a/src/driveModi/Modi/ManualControl/manualControl.h b/src/driveModi/Modi/ManualControl/manualControl.h index 138c573..319f1d3 100644 --- a/src/driveModi/Modi/ManualControl/manualControl.h +++ b/src/driveModi/Modi/ManualControl/manualControl.h @@ -4,55 +4,70 @@ * @brief A small class to drive the Rover by the controller joystick. * @version 0.1 * @date 2021-12-13 - * + * * @copyright Copyright (c) 2021 - * + * */ #ifndef MANUAL_CONTROL_H #define MANUAL_CONTROL_H #include "driveModi/driveModi.h" -class DirectionChangeWrapper { - public: - virtual void action() = 0; +class DirectionChangeWrapper +{ +public: + virtual void action() = 0; }; /** * @brief Drive the Rover with a Joystick - * - * This class gets the x and y value from the PS3 controller + * + * This class gets the x and y value from the ControlPad * and map the values to moveControl - * + * * @see MoveControl */ -class ManualControl : public DriveModi { - public: - enum class InputMode : uint8_t { - Analog, - Digital - }; +class ManualControl : public DriveModi +{ +public: + /** + * @brief The enum is used to change the interpretation of the joystick data + */ + enum class InputMode : uint8_t + { + Analog, + Digital + }; - ManualControl(DriveModiParams params) : DriveModi(params){}; + /** + * @brief Change the InputMode to the opposite + */ + void switchInputMode(); + void setInputMode(InputMode mode) { this->inputMode = mode; } + InputMode getInputMode() const { return this->inputMode; } - void switchInputMode(); - void setInputMode(InputMode mode) { this->inputMode = mode; } - InputMode getInputMode() const { return this->inputMode; } - void setDirectionChangeCallback(DirectionChangeWrapper* callback) { this->directionChangeWrapper = callback; } + /** + * @brief Set the DirectionChangeWrapper object + * + * This callback is used to inform the CalcAzimuth Component about a direction change + * + * @param callback + */ + void setDirectionChangeCallback(DirectionChangeWrapper *callback) { this->directionChangeWrapper = callback; } - uint16_t getDutycycleLeft() const { return this->moveControl->getDutycycleLeft(); } - uint16_t getDutycycleRight() const { return this->moveControl->getDutycycleRight(); } + uint16_t getDutycycleLeft() const { return this->moveControl->getDutycycleLeft(); } + uint16_t getDutycycleRight() const { return this->moveControl->getDutycycleRight(); } - protected: - void run() override; +protected: + void run() override; - private: - void analogControl(); - void digitalControl(); +private: + void analogControl(); + void digitalControl(); - bool lastLoopTurned = false; - DirectionChangeWrapper* directionChangeWrapper = nullptr; - InputMode inputMode = InputMode::Analog; + bool lastLoopTurned = false; + DirectionChangeWrapper *directionChangeWrapper = nullptr; + InputMode inputMode = InputMode::Analog; }; #endif // MANUAL_CONTROL_H diff --git a/src/driveModi/Modi/TestMode/testMode.cpp b/src/driveModi/Modi/TestMode/testMode.cpp index 7bd13a0..afda3dd 100644 --- a/src/driveModi/Modi/TestMode/testMode.cpp +++ b/src/driveModi/Modi/TestMode/testMode.cpp @@ -4,27 +4,25 @@ * @brief Contains the implementation of the class TestMode. * @version 0.1 * @date 2022-09-08 - * + * * @copyright Copyright (c) 2022 - * + * */ #include "testMode.h" -TestMode::TestMode(DriveModiParams params) - : DriveModi(params) {} - -TestMode::~TestMode() { - this->moveControl->setDrivingStatus(MoveControl::Status::Stop); -} - -void TestMode::run() { - if (this->maneuver == Maneuver::Turn) { +void TestMode::run() +{ + if (this->maneuver == Maneuver::Turn) + { uint16_t delta = abs(this->azimuth - this->getSensorData()->getRealAzimuth()); if (delta > this->degree) + { this->abort = true; + } } - if (millis() - this->actionStart > this->maneuverTime || this->abort) { + if (this->busy && millis() - this->actionStart > this->maneuverTime || this->abort) + { this->busy = false; this->abort = false; this->moveControl->setDrivingStatus(MoveControl::Status::Stop); @@ -32,71 +30,95 @@ void TestMode::run() { } } -bool TestMode::drive(int16_t cm, int16_t degree) { +bool TestMode::drive(int16_t cmDistance, int16_t degree) +{ if (this->busy) + { return false; + } this->actionStart = millis(); this->moveControl->setDrivingStatus(MoveControl::Status::Drive); - if (cm == 0) { - //Only left or right + if (cmDistance == 0) + { + // Only left or right this->moveControl->setSpeed(0); if (degree < 0) - this->moveControl->setRotationSpeed(-this->maxRotationSpeed); + { + this->moveControl->setRotationSpeed(-this->maxSpeeds.rot); + } else if (degree > 0) - this->moveControl->setRotationSpeed(this->maxRotationSpeed); - + { + this->moveControl->setRotationSpeed(this->maxSpeeds.rot); + } + this->azimuth = this->getSensorData()->getRealAzimuth(); this->degree = degree; this->maneuverTime = 5 * 1000; this->busy = true; return true; - - } else if (degree == 0) { - //Only forward or backward + } + if (degree == 0) + { + // Only forward or backward this->moveControl->setRotationSpeed(0); - if (cm < 0) - this->moveControl->setSpeed(-this->maxForwardSpeed); - else if (cm > 0) - this->moveControl->setSpeed(this->maxForwardSpeed); - - this->maneuverTime = (uint32_t) (((double) abs(cm) / 100.0) / this->maxForwardSpeed) * 1000; + if (cmDistance < 0) + { + this->moveControl->setSpeed(-this->maxSpeeds.x); + } + else if (cmDistance > 0) + { + this->moveControl->setSpeed(this->maxSpeeds.x); + } + + this->maneuverTime = static_cast(((abs(cmDistance) / 100.0) / this->maxSpeeds.x) * 1000); this->busy = true; this->maneuver = Maneuver::Drive; return true; - } else { - //forward or backward and left or right - // TODO: Calculate roationspeed - if (cm < 0) - this->moveControl->setSpeed(-this->maxForwardSpeed); - else if (cm > 0) - this->moveControl->setSpeed(this->maxForwardSpeed); - - this->maneuverTime = (uint32_t) (((double) cm / 100.0) / this->maxForwardSpeed) * 1000; + } + else + { + // forward or backward and left or right + // TODO: Calculate roationspeed + if (cmDistance < 0) + { + this->moveControl->setSpeed(-this->maxSpeeds.x); + } + else if (cmDistance > 0) + { + this->moveControl->setSpeed(this->maxSpeeds.x); + } + + this->maneuverTime = static_cast(((cmDistance / 100.0) / this->maxSpeeds.x) * 1000); this->busy = true; this->maneuver = Maneuver::Drive; return true; } - this->moveControl->setDrivingStatus(MoveControl::Status::Stop); + this->moveControl->setDrivingStatus(MoveControl::Status::Stop); return false; } -bool TestMode::leftEngine(int16_t powerPercentage, int16_t seconds) { - if (this->engineInit(powerPercentage, seconds)) { +bool TestMode::leftEngine(int16_t powerPercentage, int16_t seconds) +{ + if (this->engineInit(powerPercentage, seconds)) + { this->moveControl->setRawPowerLeft(powerPercentage); this->maneuver = Maneuver::LeftEngine; + std::cout << "TestMode::leftEngine" << std::endl; return true; } return false; } -bool TestMode::rightEngine(int16_t powerPercentage, int16_t seconds) { - if (this->engineInit(powerPercentage, seconds)) { +bool TestMode::rightEngine(int16_t powerPercentage, int16_t seconds) +{ + if (this->engineInit(powerPercentage, seconds)) + { this->moveControl->setRawPowerRight(powerPercentage); this->maneuver = Maneuver::RightEngine; return true; @@ -104,8 +126,10 @@ bool TestMode::rightEngine(int16_t powerPercentage, int16_t seconds) { return false; } -bool TestMode::bothEngine(int16_t powerPercentage, int16_t seconds) { - if (this->engineInit(powerPercentage, seconds)) { +bool TestMode::bothEngine(int16_t powerPercentage, int16_t seconds) +{ + if (this->engineInit(powerPercentage, seconds)) + { this->moveControl->setRawPowerLeft(powerPercentage); this->moveControl->setRawPowerRight(powerPercentage); this->maneuver = Maneuver::BothEngine; @@ -114,23 +138,34 @@ bool TestMode::bothEngine(int16_t powerPercentage, int16_t seconds) { return false; } -void TestMode::abortManeuver() { +void TestMode::abortManeuver() +{ this->abort = true; } -uint8_t TestMode::getRemainingManeuverTime() const { - if (busy) - return (uint8_t) ((this->maneuverTime - (millis() - this->actionStart)) / 1000); +uint8_t TestMode::getRemainingManeuverTime() const +{ + if (this->busy) + { + return static_cast((this->maneuverTime - (millis() - this->actionStart)) / 1000); + } return 0; } -bool TestMode::engineInit(int16_t powerPercentage, int16_t seconds) { +bool TestMode::engineInit(int16_t powerPercentage, int16_t seconds) +{ if (this->busy) + { return false; - if (powerPercentage >= 100 || powerPercentage <= -100) + } + if (powerPercentage > TestMode::maxPercentage || powerPercentage < -TestMode::maxPercentage) + { return false; + } if (seconds < 0) + { return false; + } this->actionStart = millis(); this->moveControl->setDrivingStatus(MoveControl::Status::Raw); diff --git a/src/driveModi/Modi/TestMode/testMode.h b/src/driveModi/Modi/TestMode/testMode.h index 844d9d4..f82a493 100644 --- a/src/driveModi/Modi/TestMode/testMode.h +++ b/src/driveModi/Modi/TestMode/testMode.h @@ -4,11 +4,11 @@ * @brief Contains a class to test different functions from the rover. * @version 0.1 * @date 2022-09-08 - * + * * @copyright Copyright (c) 2022 - * + * */ -#ifndef TEST_MODE_H +#ifndef TEST_MODE_H #define TEST_MODE_H #include @@ -16,105 +16,108 @@ #include "moveControl.h" #include "driveModi/driveModi.h" - /** * @brief Test different functions from the rover. - * + * * You can test engine, engine controller, light and speed meter. */ -class TestMode : public DriveModi { - public: - /** - * @brief Different states to test the engine - * - */ - enum Maneuver { - None, - LeftEngine, - RightEngine, - BothEngine, - Turn, - Drive - }; +class TestMode : public DriveModi +{ +public: + /** + * @brief Different states to test the engine + */ + enum Maneuver + { + None, + LeftEngine, + RightEngine, + BothEngine, + Turn, + Drive + }; - /** - * @brief Construct a new Test Mode object - * - * @param moveControl - */ - TestMode(DriveModiParams params); - ~TestMode(); + /** + * @brief Let the rover drive + * + * @param cmDistance to drive + * @param degree degree to rotate over the hole distance + * @return true + * @return false + */ + bool drive(int16_t cmDistance = 0, int16_t degree = 0); - bool drive(int16_t cm = 0, int16_t degree = 0); + /** + * @brief Sets the left Engine to a specific power value. + * + * This start a maneuver with the given time. + * + * @param powerPercentage from 0% to 100% + * @param seconds time to run the engine + * @return true success + * @return false failure + */ + bool leftEngine(int16_t powerPercentage, int16_t seconds); - /** - * @brief Sets the left Engine to a specific power value. - * - * This start a maneuver with the given time. - * - * @param powerPercentage from 0% to 100% - * @param seconds time to run the engine - * @return true success - * @return false failure - */ - bool leftEngine(int16_t powerPercentage, int16_t seconds); + /** + * @brief See leftEngine + * + * @param powerPercentage + * @param seconds + * @return true + * @return false + */ + bool rightEngine(int16_t powerPercentage, int16_t seconds); - /** - * @brief See leftEngine - * - * @param powerPercentage - * @param seconds - * @return true - * @return false - */ - bool rightEngine(int16_t powerPercentage, int16_t seconds); + /** + * @brief See leftEngine + * + * @param powerPercentage + * @param seconds + * @return true + * @return false + */ + bool bothEngine(int16_t powerPercentage, int16_t seconds); - /** - * @brief See leftEngine - * - * @param powerPercentage - * @param seconds - * @return true - * @return false - */ - bool bothEngine(int16_t powerPercentage, int16_t seconds); + /** + * @brief Abort the running maneuver. + * + * Stops all movement. This is the only possibility to cancel a maneuver + * before the time is up. + */ + void abortManeuver(); - /** - * @brief Abort the running maneuver. - * - * Stops all movement. This is the only possibility to cancel a maneuver - * before the time is up. - */ - void abortManeuver(); + uint8_t getRemainingManeuverTime() const; - uint8_t getRemainingManeuverTime() const; + /** + * @brief Returns true if a maneuver is running + * + * @return true + * @return false + */ + bool getBusy() const { return this->busy; } - /** - * @brief Returns true if a maneuver is running - * - * @return true - * @return false - */ - bool getBusy() const { return this->busy; } - Maneuver getManeuver() const { return this->maneuver; } - Speedometer* getSpeedometerLeft() const { return this->moveControl->getSpeedometerLeft(); } - Speedometer* getSpeedometerRight() const { return this->moveControl->getSpeedometerRight(); } + Maneuver getManeuver() const { return this->maneuver; } + Speedometer *getSpeedometerLeft() const { return this->moveControl->getSpeedometerLeft(); } + Speedometer *getSpeedometerRight() const { return this->moveControl->getSpeedometerRight(); } - private: - void run() override; - bool engineInit(int16_t powerPercentage, int16_t seconds); +private: + void run() override; + bool engineInit(int16_t powerPercentage, int16_t seconds); - Maneuver maneuver = Maneuver::None; - int16_t maneuverValueOne = 0; - int16_t maneuverValueTwo = 0; + Maneuver maneuver = Maneuver::None; + int16_t maneuverValueOne = 0; + int16_t maneuverValueTwo = 0; - bool busy = false; - bool abort = false; - - uint16_t azimuth; - int16_t degree; - uint32_t maneuverTime = 0; - uint32_t actionStart = 0; + bool busy = false; + bool abort = false; + + uint16_t azimuth; + int16_t degree; + uint32_t maneuverTime = 0; + uint32_t actionStart = 0; + + static constexpr int8_t maxPercentage = 100; }; #endif // TEST_MODE_H diff --git a/src/driveModi/driveManager.cpp b/src/driveModi/driveManager.cpp index bce31f0..7b62f3c 100644 --- a/src/driveModi/driveManager.cpp +++ b/src/driveModi/driveManager.cpp @@ -4,61 +4,46 @@ * @brief Implementation of the class DriveManager * @version 0.1 * @date 2021-12-14 - * + * * @copyright Copyright (c) 2021 - * + * */ #include "driveModi/driveManager.h" -DriveManager::DriveManager(MoveControl *moveControl, const SensorData* sensorData, const ControlPadInput *input) { - this->driveModiParams.input = input; - this->driveModiParams.moveControl = moveControl; - this->driveModiParams.sensorData = sensorData; - +DriveManager::DriveManager(MoveControl *moveControl, const SensorData *sensorData, const ControlPadInput *input) + : driveModiParams{moveControl, input, sensorData} +{ this->init(); } -DriveManager::DriveManager(DriveModiParams params){ - this->driveModiParams = params; - +DriveManager::DriveManager(DriveModiParams params) + : driveModiParams{params} +{ this->init(); } -void DriveManager::changeModus(Modi modus) { - this->currentModus = modus; - - if (this->currentModusPtr) { +void DriveManager::changeModus(DriveModi *modus) +{ + if (static_cast(this->currentModusPtr)) + { this->removeChildComponent(this->currentModusPtr); delete this->currentModusPtr; - } - - switch (modus) { - case Modi::Off: - this->currentModusPtr = nullptr; - break; - - case Modi::ManualControl: { - this->currentModusPtr = new ManualControl(this->driveModiParams); - } - break; - - case Modi::TestMode: { - this->currentModusPtr = new TestMode(driveModiParams); - } - break; - - default: - this->currentModusPtr = nullptr; - break; + this->currentModusPtr = nullptr; } - if (this->currentModusPtr) + this->currentModusPtr = modus; + + if (static_cast(this->currentModusPtr)) + { + this->currentModusPtr->activate(this->driveModiParams); + this->currentModusPtr->setSpeeds(this->drivingSpeeds); this->addChildComponent(this->currentModusPtr); + } } -void DriveManager::init() { +void DriveManager::init() +{ this->addChildComponent(this->driveModiParams.moveControl); this->activateOnlyChilds(); } - diff --git a/src/driveModi/driveManager.h b/src/driveModi/driveManager.h index 9a6e2d4..ed8e16b 100644 --- a/src/driveModi/driveManager.h +++ b/src/driveModi/driveManager.h @@ -4,91 +4,76 @@ * @brief Contains a class to switch the DriveModi * @version 0.1 * @date 2021-12-14 - * + * * @copyright Copyright (c) 2021 - * + * */ #ifndef DRIVE_MANAGER_H #define DRIVE_MANAGER_H -#include -#include - #include "moveControl.h" #include "driveModi/driveModi.h" #include "config.h" -#include "networkConfig.h" -#include "debugTimes.h" #include "controlPadInput.h" #include "component.h" #include "sensorData.h" -// All Drive Modi -#include "driveModi/Modi/ManualControl/manualControl.h" -#include "driveModi/Modi/TestMode/testMode.h" - /** - * @brief An enum to choose the DriveMode - * + * @brief A class to manage and hold the different DriveModi + * */ -enum class Modi { - Off, - ManualControl, - CalibrateCompass, - CaptureRoute, - Autopilot, - TestMode +class DriveManager : public Component +{ +public: + /** + * @brief Construct a new Drive Manager object + * + * @param moveControl + * @param sensorData + * @param input + */ + DriveManager(MoveControl *moveControl, const SensorData *sensorData, const ControlPadInput *input); + + /** + * @brief Construct a new Drive Manager object + * + * @param params + */ + DriveManager(DriveModiParams params); + + /** + * @brief Change the DriveModi to a specific value + * + * Sets MoveControl to a safe state, delete the last + * DriveModi and than set the new DriveModi + * + * @param modus + */ + void changeModus(DriveModi *modus = nullptr); + + /** + * @brief Get the DriveModi Ptr object + * + * This is a pointer to the Object of the current DriveMode. + * If you know which DriveMode is active, you can cast this + * pointer to it. + * + * @see getDriveModi + * @return DriveModi* + */ + DriveModi *getDriveModiPtr() const { return this->currentModusPtr; } + DrivingSpeeds &getDrivingSpeedsRef() { return this->drivingSpeeds; } + + bool isActive() const { return this->currentModusPtr; } + +private: + void run() override{}; + void init(); + + DriveModi *currentModusPtr = nullptr; + DriveModiParams driveModiParams; + DrivingSpeeds drivingSpeeds = {1, 7}; }; -class DriveManager : public Component { - public: - DriveManager(MoveControl *moveControl, const SensorData* sensorData, const ControlPadInput *input); - DriveManager(DriveModiParams params); - - /** - * @brief Destroy the Drive Manager object - * - */ - ~DriveManager(); - - /** - * @brief Change the DriveModi to a specific value - * - * Sets MoveControl to a safe state, delete the last - * DriveModi and than set the new DriveModi - * - * @param modus - */ - void changeModus(Modi modus); - - /** - * @brief Get the DriveModi Ptr object - * - * This is a pointer to the Object of the current DriveMode. - * If you know which DriveMode is active, you can cast this - * pointer to it. - * - * @see getDriveModi - * @return DriveModi* - */ - DriveModi* getDriveModiPtr() const { return this->currentModusPtr; } - - /** - * @brief Get the DriveModi enum - * - * @return Modi - */ - Modi getDriveModi() const { return this->currentModus; } - - private: - void run() override {}; - void init(); - - Modi currentModus = Modi::Off; - DriveModi *currentModusPtr = nullptr; - DriveModiParams driveModiParams; -}; - - #endif // DRIVE_MANAGER_H diff --git a/src/driveModi/driveModi.cpp b/src/driveModi/driveModi.cpp index f07e6fe..37a75c5 100644 --- a/src/driveModi/driveModi.cpp +++ b/src/driveModi/driveModi.cpp @@ -1,26 +1,31 @@ #include "driveModi.h" -DriveModi::DriveModi(MoveControl* moveControl, ControlPadInput* input, SensorData* sensorData) { +DriveModi::~DriveModi() +{ + this->moveControl->setSpeed(0); + this->moveControl->setRotationSpeed(0); + this->moveControl->setDrivingStatus(MoveControl::Status::Stop); +} + +void DriveModi::activate(MoveControl *moveControl, ControlPadInput *input, SensorData *sensorData) +{ this->moveControl = moveControl; this->input = input; this->sensorData = sensorData; this->init(); } -DriveModi::DriveModi(DriveModiParams params){ +void DriveModi::activate(DriveModiParams params) +{ this->moveControl = params.moveControl; this->input = params.input; this->sensorData = params.sensorData; this->init(); } -DriveModi::~DriveModi() { - this->moveControl->setSpeed(0); - this->moveControl->setRotationSpeed(0); - this->moveControl->setDrivingStatus(MoveControl::Status::Stop); -} - -void DriveModi::init() { +void DriveModi::init() +{ this->moveControl->setDrivingStatus(MoveControl::Status::Drive); - this->loopDelay = 40; + this->loopDelay = DriveModi::defaultDelay; + this->afterActivate(); } diff --git a/src/driveModi/driveModi.h b/src/driveModi/driveModi.h index b2267c3..ba40324 100644 --- a/src/driveModi/driveModi.h +++ b/src/driveModi/driveModi.h @@ -4,9 +4,9 @@ * @brief Contains a virtual class for all Modi * @version 0.1 * @date 2021-12-14 - * + * * @copyright Copyright (c) 2021 - * + * */ #ifndef DRIVEMODI_H @@ -17,54 +17,64 @@ #include "controlPadInput.h" #include "sensorData.h" -struct DriveModiParams { - MoveControl* moveControl; - const ControlPadInput* input; - const SensorData* sensorData; +/** + * @brief The struct contains all objects needed by a DriveMode + */ +struct DriveModiParams +{ + MoveControl *moveControl; + const ControlPadInput *input; + const SensorData *sensorData; }; /** * @brief Baseclass to build DriveModi - * + * * This class must be inherited by other classes which want to be * act as a DriveModi, because the DriveModi structure uses polymorphism. */ -class DriveModi : public Component { - public: - DriveModi(MoveControl* moveControl, ControlPadInput* input, SensorData* sensorData); - DriveModi(DriveModiParams); - virtual ~DriveModi(); +class DriveModi : public Component +{ +public: + virtual ~DriveModi(); - const SensorData* getSensorData() const { return this->sensorData; } + const SensorData *getSensorData() const { return this->sensorData; } - /** - * @brief Set the max speed - * - * @param maxSpeed in m/s - */ - void setMaxSpeed(double maxForwardSpeed) { maxForwardSpeed = maxForwardSpeed; } - void increaseMaxSpeed(double increase = 0.1) { maxForwardSpeed += increase; } - void decreaseMaxSpeed(double increase = 0.1) { maxForwardSpeed -= increase; } - double getMaxSpeed() const { return this->maxForwardSpeed; } + /** + * @brief This activates a DriveMode + * + * This function have to be called to provide the DriveMode + * with this objects + * + * @param moveControl + * @param input + * @param sensorData + */ + void activate(MoveControl *moveControl, ControlPadInput *input, SensorData *sensorData); + void activate(DriveModiParams params); - /** - * @brief Set the max rotation - * - * @param maxRotation in rad/s (maybe) - */ - void setMaxRotation(double maxRotation) { maxRotationSpeed = maxRotation; } - void increaseMaxRotation(double increase = 0.1) { maxRotationSpeed += increase; } - void decreaseMaxRotation(double increase = 0.1) { maxRotationSpeed -= increase; } - double getMaxRotation() const { return this->maxRotationSpeed; } + void setSpeeds(DrivingSpeeds speeds) { this->maxSpeeds = speeds; } + DrivingSpeeds getSpeeds() const { return this->maxSpeeds; } + DrivingSpeeds &getSpeedsRef() { return this->maxSpeeds; } - protected: - MoveControl *moveControl; - const ControlPadInput* input; - const SensorData* sensorData; - double maxForwardSpeed = 1; - double maxRotationSpeed = 7; +protected: + /** + * @brief Prepare other things + * + * This function can be overwritten to prepare things + * for the specific DriveModi. This function will be + * called after activate(). + */ + virtual void afterActivate() {} - private: - void init(); + MoveControl *moveControl = nullptr; + DrivingSpeeds maxSpeeds = {1, 7}; + const ControlPadInput *input = nullptr; + const SensorData *sensorData = nullptr; + +private: + void init(); + + static constexpr uint8_t defaultDelay = 40; }; #endif // DRIVEMODI_H diff --git a/src/main.cpp b/src/main.cpp index b006764..0224e73 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -2,15 +2,15 @@ * @file main.cpp * @author Alexander Klein (alex@kleiax.de) * @brief The main file. - * + * * Sets up Network stuff, PS3-Controller, Menu and Lcd * Handles Controller Input - * + * * @version 0.1 * @date 2022-02-15 - * + * * @copyright Copyright (c) 2022 - * + * */ #include @@ -29,6 +29,7 @@ #include "LcdWrapper.h" #include "moveControl.h" #include "network.h" +#include "networkConfig.h" #include "debugMqtt.h" #include "battery.h" #include "sensorData.h" @@ -42,73 +43,78 @@ #include "SpecialMenus/driveModi/CalibrateCompass/menuCalibrateCompass.h" #include "SpecialMenus/Systeminformation/menuSysteminformation.h" #include "SpecialMenus/PID/menuPidSettings.h" -#include "SpecialMenus/GPS/menuGPS.h" +#include "SpecialMenus/Route/menuRoute.h" +#include "SpecialMenus/SensorData/menuSensorData.h" +#include "SpecialMenus/CalibrateBattery/menuCalibrateBattery.h" MoveControl moveController; -DriveManager* driveManager; -Menu* main_m; -LiquidCrystal_I2C* lcd; -LcdWrapper* lcdWrapper; -OutputBuf* outputBuf; -DebugMqtt* debugMqtt = nullptr; -Battery* mainBattery; -SPIClass* spiPort; -SensorData* sensorData; -ControlPad* controlPad; +Network *network; +DriveManager *driveManager; +Menu *main_m; +LiquidCrystal_I2C *lcd; +LcdWrapper *lcdWrapper; +OutputBuf *outputBuf; +DebugMqtt *debugMqtt = nullptr; +Battery *mainBattery; +SPIClass *spiPort; +SensorData *sensorData; +ControlPad *controlPad; -bool wifiIsActive; +constexpr uint16_t displayUpdateDelay = 500; -void i2cScanner(void); -void makeMenu(void); -void restart(void); -void receiveCallback (const uint8_t * mac, const uint8_t *incomingData, int len); -void sendCallback (const uint8_t *mac_addr, esp_now_send_status_t status); -void lcdWrapperCallback (const char data[][LcdWrapper::totalRows], uint8_t lines, uint8_t rows); +void i2cScanner(); +void makeMenu(); +void restart(); +void receiveCallback(const uint8_t *mac, const uint8_t *incomingData, int len); +void sendCallback(const uint8_t *mac_addr, esp_now_send_status_t status); +void lcdWrapperCallback(const char data[][LcdWrapper::totalRows], uint8_t lines, uint8_t rows); +NetworkAddresses setIPs(); - -void setup() { - Serial.begin(115200); +void setup() +{ + Serial.begin(Settings::baudRate); // DebugTimes::setConsolOutput(true); DebugTimes setupTime; - char wifiIndicator = 'X'; - - WiFi.mode(WIFI_MODE_STA); - Serial.print("MAC Address: "); - Serial.println(WiFi.macAddress()); spiPort = new SPIClass(HSPI); spiPort->begin(PinNumbers::spiSck, PinNumbers::spiCipo, PinNumbers::spiCopi, PinNumbers::gnssSpiCs); - mainBattery = new Battery(35); + mainBattery = new Battery(PinNumbers::battery); controlPad = new ControlPad(); - Wire.begin(21, 19); - Wire.setClock(400000); + Wire.begin(PinNumbers::sda, PinNumbers::scl); + Wire.setClock(Settings::i2cSpeed); i2cScanner(); - lcd = new LiquidCrystal_I2C(0x3F,16,2); + lcd = new LiquidCrystal_I2C(0x3F, 16, 2); lcd->init(); lcd->clear(); lcd->noBacklight(); - Network::setIps(); - Network::setupMQTT(); - wifiIsActive = Network::connectWifi(); - if (wifiIsActive) { - Network::initMQTT(); - if (Network::checkMQTT()) { - DebugMqtt::init(Network::getMqttClient(), Loglevel::debug); - std::cout << "Activate additional output via MQTT..." << std::endl; + network = new Network(static_cast(NetworkConfig::ssid), + static_cast(NetworkConfig::password), + setIPs()); + network->activateEspNow(receiveCallback, sendCallback); + if (NetworkConfig::mqtt) + { + if (network->activateMqtt(static_cast(MqttConfig::user), + static_cast(MqttConfig::password))) + { + DebugMqtt::init(network->getMqttClient(), Loglevel::debug); debugMqtt = new DebugMqtt("Console"); outputBuf = new OutputBuf(debugMqtt); - } else + outputBuf->activateMqtt(true); + } + else + { outputBuf = new OutputBuf(); - wifiIndicator = '-'; - } else + } + } + else + { outputBuf = new OutputBuf(); + } std::cout.rdbuf(outputBuf); - Network::connectEspNow(receiveCallback, sendCallback); - std::cout << "Welcome to Kleiax-Rover" << std::endl; std::cout << "Project verion: " << VERSION << std::endl; std::cout << "Build timestamp: " << BUILD_TIMESTAMP << std::endl; @@ -117,10 +123,19 @@ void setup() { sensorData = new SensorData(); sensorData->enableGnss(spiPort, PinNumbers::gnssSpiCs); sensorData->enableRealCompass(); + sensorData->enableNtrip(static_cast(NtripConfig::host), + NtripConfig::port, + static_cast(NtripConfig::mountPoint), + static_cast(NtripConfig::user), + static_cast(NtripConfig::password)); + // sensorData->enableGyroscope(); driveManager = new DriveManager(&moveController, sensorData, controlPad->getControlPadDataPtr()); - outputBuf->activateMqtt(true); - + char wifiIndicator = 'X'; + if (network->isWifiConnected()) + { + wifiIndicator = '-'; + } lcd->backlight(); lcd->printf("%c Kleiax-Rover %c", wifiIndicator, wifiIndicator); lcd->setCursor(0, 1); @@ -133,17 +148,9 @@ void setup() { setupTime.stopConsol("Setup"); } -void loop() { - if (wifiIsActive) { - DebugTimes wifiTime; - Network::checkWiFi(); - #ifdef MQTT - bool err = Network::checkMQTT(); - if (!err) - std::cout << "loop(): checkMQTT returns false" << std::endl; - #endif //MQTT - wifiTime.stopConsol("WiFi-Time", 10); - } +void loop() +{ + network->loop(); sensorData->loop(); driveManager->loop(); controlPad->loop(); @@ -151,82 +158,119 @@ void loop() { lcdWrapper->loop(); mainBattery->loop(); - if (mainBattery->isNewValue()) { + // new Value ervery 0.5s + if (mainBattery->isNewValue()) + { static uint8_t batteryLowCounter = 0; - if (mainBattery->isBatteryLow(10)) + static constexpr uint8_t minVoltage = 10; + if (mainBattery->isBatteryLow(minVoltage)) + { batteryLowCounter++; + } else + { batteryLowCounter = 0; + } - if (batteryLowCounter >= 10) { + if (batteryLowCounter >= 10) + { moveController.emergencyStop(); - uint16_t voltage = (uint16_t) (mainBattery->getBatteryVoltage() * 100); + const auto voltage = static_cast(mainBattery->getBatteryVoltage() * 100); lcd->setCursor(0, 0); lcd->printf("Low Battery: %u", voltage); lcd->setCursor(0, 1); lcd->print("Please turn off."); - - while (true); + + while (true) + { + } } } } -void i2cScanner() { +void i2cScanner() +{ + constexpr uint8_t checkForLength = 16; + constexpr uint8_t maxAdresses = UINT8_MAX / 2; std::cout << "\nI2C Scanner" << std::endl; - byte error, address; - int nDevices; + byte error = 0; + byte address = 0; + int nDevices = 0; std::cout << "Scanning..." << std::endl; - nDevices = 0; - for(address = 1; address < 127; address++ ) { + for (address = 1; address < maxAdresses; address++) + { Wire.beginTransmission(address); error = Wire.endTransmission(); - if (error == 0) { + if (error == 0) + { std::cout << "I2C device found at address 0x"; - if (address<16) + if (address < checkForLength) + { std::cout << "0"; - std::cout << std::hex << (int) address << std::dec << std::endl; + } + std::cout << std::hex << static_cast(address) << std::dec << std::endl; // std::cout << (int) address << std::endl; nDevices++; } - else if (error==4) { + else if (error == 4) + { std::cout << "Unknow error at address 0x"; - if (address<16) + if (address < checkForLength) + { std::cout << "0"; - std::cout << std::hex << (int) address << std::dec << std::endl; - } + } + std::cout << std::hex << static_cast(address) << std::dec << std::endl; + } } if (nDevices == 0) - std::cout << "No I2C devices found\n" << std::endl; + { + std::cout << "No I2C devices found\n" + << std::endl; + } else - std::cout << "done\n" << std::endl; + { + std::cout << "done\n" + << std::endl; + } } -void makeMenu() { - auto dummy = []() { - std::cout << "Dummy in Action" <setLcd(lcdWrapper); - Menu* mode_m = new Menu(); - Menu* pid_m = new Menu(); - MenuIntInput* pidl_m = new MenuIntInput(3, new MenuPidSettings(moveController.getPID(0))); - MenuIntInput* pidr_m = new MenuIntInput(3, new MenuPidSettings(moveController.getPID(1))); - MenuManualControl* man_m = new MenuManualControl(driveManager); - MenuTestMode* testM_m = new MenuTestMode(driveManager); - MenuCalibrateCompass* comp_m = new MenuCalibrateCompass(driveManager); - MenuGPS* gps_m = new MenuGPS(sensorData); - MenuSysteminformation* sys_m = new MenuSysteminformation(mainBattery); + auto *mode_m = new Menu(); + auto *set_m = new Menu(); + auto *pid_m = new Menu(); + auto *pidl_m = new MenuIntInput(3, new MenuPidSettings(moveController.getPID(0))); + auto *pidr_m = new MenuIntInput(3, new MenuPidSettings(moveController.getPID(1))); + auto *speed_m = new MenuIntInput(2, new MenuSpeed(driveManager->getDrivingSpeedsRef())); + auto *man_m = new MenuManualControl(driveManager); + auto *cap_m = new MenuCaptureRoute(driveManager); + auto *auto_m = new MenuAutopilot(driveManager); + auto *testM_m = new MenuTestMode(driveManager); + auto *comp_m = new MenuCalibrateCompass(driveManager); + auto *sys_m = new MenuSysteminformation(mainBattery); + auto *sen_m = new MenuSensorData(sensorData); + // TODO: Wie bekommt jeder die dumme Route? + auto *rout_m = new MenuRoute(new Route()); + auto *bat_m = new MenuCalibrateBattery(mainBattery); - gps_m->setUpdateDelay(1000); - sys_m->setUpdateDelay(1500); + auto_m->setUpdateDelay(displayUpdateDelay); + sys_m->setUpdateDelay(displayUpdateDelay); + sen_m->setUpdateDelay(displayUpdateDelay); // Entry for the main menu main_m->addEntry(new MenuAction("Mode", mode_m)); - main_m->addEntry(new MenuAction("GPS", gps_m)); - main_m->addEntry(new MenuAction("PID", pid_m)); + main_m->addEntry(new MenuAction("Sensor", sen_m)); + main_m->addEntry(new MenuAction("Route", rout_m)); + main_m->addEntry(new MenuAction("Settings", set_m)); main_m->addEntry(new MenuAction("Systeminfo", sys_m)); main_m->addEntry(new MenuAction("Restart", restart)); @@ -234,6 +278,14 @@ void makeMenu() { mode_m->addEntry(new MenuAction("Manual Control", man_m)); mode_m->addEntry(new MenuAction("Gauge Compass", comp_m)); mode_m->addEntry(new MenuAction("Test Mode", testM_m)); + mode_m->addEntry(new MenuAction("Consol Control", dummy)); + + // Entry for the setting menu + set_m->addEntry(new MenuAction("PID", pid_m)); + set_m->addEntry(new MenuAction("Speed", speed_m)); + set_m->addEntry(new MenuAction("Distance", dummy)); + set_m->addEntry(new MenuAction("WiFi", dummy)); + set_m->addEntry(new MenuAction("Battery", bat_m)); // Entry for the PID Menu pid_m->addEntry(new MenuAction("Left", pidl_m)); @@ -241,42 +293,76 @@ void makeMenu() { // Other config pidl_m->setMinMax(0, UINT8_MAX); - pidl_m->setEntry(0, "P-Part", (uint8_t) moveController.getPID(0)->GetKp()); - pidl_m->setEntry(1, "I-Part", (uint8_t) moveController.getPID(0)->GetKi()); - pidl_m->setEntry(2, "D-Part", (uint8_t) moveController.getPID(0)->GetKd()); + pidl_m->setEntry(0, "P-Part", static_cast(moveController.getPID(0)->GetKp())); + pidl_m->setEntry(1, "I-Part", static_cast(moveController.getPID(0)->GetKi())); + pidl_m->setEntry(2, "D-Part", static_cast(moveController.getPID(0)->GetKd())); pidr_m->setMinMax(0, UINT8_MAX); - pidr_m->setEntry(0, "P-Part", (uint8_t) moveController.getPID(1)->GetKp()); - pidr_m->setEntry(1, "I-Part", (uint8_t) moveController.getPID(1)->GetKi()); - pidr_m->setEntry(2, "D-Part", (uint8_t) moveController.getPID(1)->GetKd()); + pidr_m->setEntry(0, "P-Part", static_cast(moveController.getPID(1)->GetKp())); + pidr_m->setEntry(1, "I-Part", static_cast(moveController.getPID(1)->GetKi())); + pidr_m->setEntry(2, "D-Part", static_cast(moveController.getPID(1)->GetKd())); + + speed_m->setMinMax(5, UINT8_MAX); + speed_m->setEntry(0, "x m/s e-1", driveManager->getDrivingSpeedsRef().x * 10); + speed_m->setEntry(1, "z rad/s e-1", driveManager->getDrivingSpeedsRef().rot * 10); + + man_m->addSensorMenu(sen_m); controlPad->setMenuControl(main_m); } -void restart(void) { +void restart() +{ lcdWrapper->clear(); lcdWrapper->setCursor(0, 0); lcdWrapper->print("Rebooting ..."); ESP.restart(); } -void receiveCallback (const uint8_t * mac, const uint8_t *incomingData, int len) { +void receiveCallback(const uint8_t *mac, const uint8_t *incomingData, int len) +{ if (len != sizeof(ControlPadInput)) + { return; + } controlPad->insertData(incomingData); } -void sendCallback (const uint8_t *mac_addr, esp_now_send_status_t status) { +void sendCallback(const uint8_t *mac_addr, esp_now_send_status_t status) +{ if (status != ESP_NOW_SEND_SUCCESS) + { std::cout << "sendCallback - Delivery Fail" << std::endl; + } } -void lcdWrapperCallback (const char data[][LcdWrapper::totalRows], uint8_t lines, uint8_t rows) { +void lcdWrapperCallback(const char data[][LcdWrapper::totalRows], uint8_t lines, uint8_t rows) +{ if (!controlPad->isControlPadConnected()) + { return; + } + + const esp_err_t result = esp_now_send(network->getBroadcastAddress(), reinterpret_cast(data), lines * rows); - esp_err_t result = esp_now_send(Network::getBroadcastAddress(), (uint8_t *) data, lines * rows); - if (result != ESP_OK) + { Serial.println("Error sending the data"); + } +} + +NetworkAddresses setIPs() +{ + NetworkAddresses adresses; + adresses.localIP.fromString(static_cast(NetworkConfig::ip)); + adresses.subnet.fromString(static_cast(NetworkConfig::subnet)); + adresses.gateway.fromString(static_cast(NetworkConfig::gateway)); + adresses.dnsServer.fromString(static_cast(NetworkConfig::dns)); + if (NetworkConfig::mqtt) + { + adresses.mqttServer.fromString(static_cast(MqttConfig::server)); + adresses.mqttPort = MqttConfig::port; + } + + return adresses; } diff --git a/src/moveControl.cpp b/src/moveControl.cpp index eb87aa2..6ad879d 100644 --- a/src/moveControl.cpp +++ b/src/moveControl.cpp @@ -4,75 +4,76 @@ * @brief Contains an implementation of the class MoveControl * @version 0.1 * @date 2022-02-15 - * + * * @copyright Copyright (c) 2022 - * + * */ #include "moveControl.h" -MoveControl::MoveControl() { - this->loopDelay = 20; - this->left_motor = new MotorControl(); - this->right_motor = new MotorControl(); - this->left_speedometer = new Speedometer(PinNumbers::LeftMotor::encoder, Settings::wheelDiameter, Settings::encoderSteps); - this->right_speedometer = new Speedometer(PinNumbers::RightMotor::encoder, Settings::wheelDiameter, Settings::encoderSteps); +MoveControl::MoveControl() + : Component(MoveControl::loopDelay), + leftMotor{new MotorControl()}, + rightMotor{new MotorControl()}, + leftSpeedometer{new Speedometer(PinNumbers::LeftMotor::encoder, Settings::wheelDiameter, Settings::encoderSteps)}, + rightSpeedometer{new Speedometer(PinNumbers::RightMotor::encoder, Settings::wheelDiameter, Settings::encoderSteps)}, + leftPid{new PID(&this->wheelspeedLeft, + &this->leftPidOut, + &this->wheelspeedLeftTarget, + Settings::Pid::Left::P, + Settings::Pid::Left::I, + Settings::Pid::Left::D, + DIRECT)}, + rightPid{new PID(&this->wheelspeedRight, + &this->rightPidOut, + &this->wheelspeedRightTarget, + Settings::Pid::Right::P, + Settings::Pid::Right::I, + Settings::Pid::Right::D, + DIRECT)} +{ + this->leftPid->SetOutputLimits(Settings::Pid::outMin, Settings::Pid::outMax); + this->leftPid->SetSampleTime(Settings::Pid::sampleTime); + this->leftPid->SetMode(AUTOMATIC); - this->left_pid = new PID( &this->wheelspeed_left, - &this->left_pid_out, - &this->wheelspeed_left_target, - Settings::Pid::Left::P, - Settings::Pid::Left::I, - Settings::Pid::Left::D, - DIRECT); - this->right_pid = new PID( &this->wheelspeed_right, - &this->right_pid_out, - &this->wheelspeed_right_target, - Settings::Pid::Right::P, - Settings::Pid::Right::I, - Settings::Pid::Right::D, - DIRECT); + this->rightPid->SetOutputLimits(Settings::Pid::outMin, Settings::Pid::outMax); + this->rightPid->SetSampleTime(Settings::Pid::sampleTime); + this->rightPid->SetMode(AUTOMATIC); - this->left_pid->SetOutputLimits(Settings::Pid::outMin, Settings::Pid::outMax); - this->left_pid->SetSampleTime(Settings::Pid::sampleTime); - this->left_pid->SetMode(AUTOMATIC); + this->leftMotor->init(PinNumbers::LeftMotor::pwm, PinNumbers::LeftMotor::pmwChannel, PinNumbers::LeftMotor::dir1, PinNumbers::LeftMotor::dir2); + this->rightMotor->init(PinNumbers::RightMotor::pwm, PinNumbers::RightMotor::pmwChannel, PinNumbers::RightMotor::dir1, PinNumbers::RightMotor::dir2); - this->right_pid->SetOutputLimits(Settings::Pid::outMin, Settings::Pid::outMax); - this->right_pid->SetSampleTime(Settings::Pid::sampleTime); - this->right_pid->SetMode(AUTOMATIC); - - this->left_motor->init(PinNumbers::LeftMotor::pwm, PinNumbers::LeftMotor::pmwChannel, PinNumbers::LeftMotor::dir1, PinNumbers::LeftMotor::dir2); - this->right_motor->init(PinNumbers::RightMotor::pwm, PinNumbers::RightMotor::pmwChannel, PinNumbers::RightMotor::dir1, PinNumbers::RightMotor::dir2); - - this->addChildComponent(this->left_motor); - this->addChildComponent(this->right_motor); - this->addChildComponent(this->left_speedometer); - this->addChildComponent(this->right_speedometer); + this->addChildComponent(this->leftMotor); + this->addChildComponent(this->rightMotor); + this->addChildComponent(this->leftSpeedometer); + this->addChildComponent(this->rightSpeedometer); } -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; - delete this->left_pid; - delete this->right_pid; +MoveControl::~MoveControl() +{ + this->leftMotor->emergencyStop(); + this->rightMotor->emergencyStop(); + delete this->leftMotor; + delete this->rightMotor; + delete this->leftSpeedometer; + delete this->rightSpeedometer; + delete this->leftPid; + delete this->rightPid; } -void MoveControl::run() { +void MoveControl::run() +{ this->updateCurrentWheelSpeed(); this->calcTargetWheelSpeed(); - this->right_pid->Compute(); - this->left_pid->Compute(); + this->rightPid->Compute(); + this->leftPid->Compute(); this->regulateMotors(); } -void MoveControl::setDrivingStatus(Status status) { +void MoveControl::setDrivingStatus(Status status) +{ this->setSpeed(0); this->setRotationSpeed(0); this->setRawPowerLeft(0); @@ -80,11 +81,15 @@ void MoveControl::setDrivingStatus(Status status) { this->driving_status = status; // switch (this->driving_status) { // case Status::Stop : - // Serial.println("New drivingState = Stop in MoveControl::setDrivingStatus"); + // std::cout << "New drivingState = Stop in MoveControl::setDrivingStatus" << std::endl; // break; // case Status::Drive : - // Serial.println("New drivingState = Drive in MoveControl::setDrivingStatus"); + // std::cout << "New drivingState = Drive in MoveControl::setDrivingStatus" << std::endl; + // break; + + // case Status::Raw : + // std::cout << "New drivingState = Raw in MoveControl::setDrivingStatus" << std::endl; // break; // default: @@ -93,9 +98,10 @@ void MoveControl::setDrivingStatus(Status status) { // } } -void MoveControl::emergencyStop() { - this->left_motor->emergencyStop(); - this->right_motor->emergencyStop(); +void MoveControl::emergencyStop() +{ + this->leftMotor->emergencyStop(); + this->rightMotor->emergencyStop(); this->setSpeed(0); this->setRotationSpeed(0); this->setRawPowerLeft(0); @@ -103,106 +109,142 @@ void MoveControl::emergencyStop() { this->driving_status = Status::Stop; } -void MoveControl::setSpeed(double speed) { - if (speed < 0.2 && speed > -0.2) { - this->x_speed = 0; +void MoveControl::setSpeed(double speed) +{ + if (speed < MoveControl::minSpeed && speed > -MoveControl::minSpeed) + { + this->drivingSpeeds.x = 0; return; } - this->x_speed = speed; + this->drivingSpeeds.x = speed; } -void MoveControl::setRotationSpeed(double speed) { - if (speed < 0.1 && speed > -0.1){ - this->rotation_speed = 0; +void MoveControl::setRotationSpeed(double speed) +{ + if (speed < MoveControl::minSpeed && speed > -MoveControl::minSpeed) + { + this->drivingSpeeds.rot = 0; return; } - this->rotation_speed = speed; + this->drivingSpeeds.rot = speed; } -void MoveControl::setRawPowerLeft(int16_t power) { - if (power <= 100 && power >= -100) - this->rawPowerLeft = power; +void MoveControl::setSpeeds(DrivingSpeeds drivingSpeeds) +{ + this->setSpeed(drivingSpeeds.x); + this->setRotationSpeed(drivingSpeeds.rot); } -void MoveControl::setRawPowerRight(int16_t power) { - if (power <= 100 && power >= -100) - this->rawPowerRight = power; +void MoveControl::setRawPowerLeft(int16_t power) +{ + if (power <= MoveControl::maxPercentage && power >= -MoveControl::maxPercentage) + { + this->rawPowerLeft = static_cast(power); + } } -void MoveControl::setPidTunings(uint8_t side, double p, double i, double d) { - PID* selectedPID = nullptr; +void MoveControl::setRawPowerRight(int16_t power) +{ + if (power <= MoveControl::maxPercentage && power >= -MoveControl::maxPercentage) + { + this->rawPowerRight = static_cast(power); + } +} + +void MoveControl::setPidTunings(uint8_t side, double pPart, double iPart, double dPart) +{ + PID *selectedPID = nullptr; if (side == 0) - selectedPID = this->left_pid; + { + selectedPID = this->leftPid; + } else if (side == 1) - selectedPID = this->right_pid; + { + selectedPID = this->rightPid; + } - selectedPID->SetTunings(p, i, d); + selectedPID->SetTunings(pPart, iPart, dPart); } -PID* MoveControl::getPID(uint8_t side) const { +PID *MoveControl::getPID(uint8_t side) const +{ if (side == 0) - return this->left_pid; - else if (side == 1) - return this->right_pid; - else - return nullptr; + { + return this->leftPid; + } + if (side == 1) + { + return this->rightPid; + } + return nullptr; } -void MoveControl::setSpeedometerDirection(Speedometer *speedometer, double value) { +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() { +void MoveControl::calcTargetWheelSpeed() +{ /* original formula: (1 / r) / 1 b \ / x \ = / Xl \ \ 1 -b / \ T / \ Xr / */ // (1 / r) * 1 - constexpr double A = 15.82278481; + constexpr double A1r1 = 1.0 / (Settings::wheelDiameter / 2); // (1 / r) * b - constexpr double B = 2.096518987; + constexpr double B1rb = (1.0 / (Settings::wheelDiameter / 2)) * (Settings::wheelDistance / 2); - this->wheelspeed_right_target = (A * this->x_speed + B * this->rotation_speed) * (Settings::wheelDiameter / 2); - this->wheelspeed_left_target = (A * this->x_speed + (-B) * this->rotation_speed) * (Settings::wheelDiameter / 2); + this->wheelspeedRightTarget = (A1r1 * this->drivingSpeeds.x + B1rb * this->drivingSpeeds.rot); + this->wheelspeedLeftTarget = (A1r1 * this->drivingSpeeds.x + (-B1rb) * this->drivingSpeeds.rot); } -void MoveControl::regulateMotors() { - switch (this->driving_status) { - case Status::Stop : - this->left_motor->setTargetPower(0); - this->right_motor->setTargetPower(0); - this->setSpeedometerDirection(this->left_speedometer, 0); - this->setSpeedometerDirection(this->right_speedometer, 0); - break; +void MoveControl::regulateMotors() +{ + switch (this->driving_status) + { + case Status::Stop: + this->leftMotor->setTargetPower(0); + this->rightMotor->setTargetPower(0); + this->setSpeedometerDirection(this->leftSpeedometer, 0); + this->setSpeedometerDirection(this->rightSpeedometer, 0); + break; - case Status::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_motor->getPower()); - this->setSpeedometerDirection(this->right_speedometer, this->right_motor->getPower()); - break; + case Status::Drive: + this->leftMotor->setTargetPower(static_cast(this->leftPidOut)); + this->rightMotor->setTargetPower(static_cast(this->rightPidOut)); + this->setSpeedometerDirection(this->leftSpeedometer, this->leftMotor->getPower()); + this->setSpeedometerDirection(this->rightSpeedometer, this->rightMotor->getPower()); + break; - case Status::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; + case Status::Raw: + this->leftMotor->setTargetPower(this->rawPowerLeft); + this->rightMotor->setTargetPower(this->rawPowerRight); + this->setSpeedometerDirection(this->leftSpeedometer, this->rawPowerLeft); + this->setSpeedometerDirection(this->rightSpeedometer, this->rawPowerRight); + break; - default: - Serial.println("Wrong drivingState in MoveControl::regulateMotors"); - break; + default: + Serial.println("Wrong drivingState in MoveControl::regulateMotors"); + break; } } -void MoveControl::updateCurrentWheelSpeed() { - this->wheelspeed_left = this->left_speedometer->getSpeed(); - this->wheelspeed_right = this->right_speedometer->getSpeed(); +void MoveControl::updateCurrentWheelSpeed() +{ + this->wheelspeedLeft = this->leftSpeedometer->getSpeedRad(); + this->wheelspeedRight = this->rightSpeedometer->getSpeedRad(); } diff --git a/src/network.cpp b/src/network.cpp deleted file mode 100644 index baaa932..0000000 --- a/src/network.cpp +++ /dev/null @@ -1,174 +0,0 @@ -/** - * @file network.cpp - * @author Alexander Klein (alex@kleiax.de) - * @brief Contains the implementation of the ststic class Network - * @version 0.1 - * @date 2021-12-14 - * - * @copyright Copyright (c) 2021 - * - */ - -#include "network.h" - -IPAddress Network::local_IP; -IPAddress Network::gateway; -IPAddress Network::subnet; -IPAddress Network::mqtt_server; -IPAddress Network::dnsServer; - -WiFiClient Network::wifi_client; -PubSubClient* Network::mqtt_client; - -esp_now_peer_info_t Network::peerInfo; -uint8_t Network::broadcastAddress[6] = ESP_NOW_CONTROLER_MAC; - -void Network::setIps() { - local_IP.fromString(WLAN_IP); - gateway.fromString(WLAN_GATEWAY); - subnet.fromString(WLAN_SUBNETMASK); - mqtt_server.fromString(MQTT_SERVER); - dnsServer.fromString(WLAN_DNS_SERVER); -} - -void Network::setupMQTT() { - Network::mqtt_client = new PubSubClient(wifi_client); - Network::mqtt_client->setServer(MQTT_SERVER, MQTT_PORT); - Network::mqtt_client->setSocketTimeout(1); -} - -bool Network::connectMQTT() { - #ifndef MQTT - return false; - #endif - - // Create a random client ID - String clientId = "ESP32Rover-"; - clientId += String(random(0xffff), HEX); - - #ifdef MQTT_AUTH - if (mqtt_client->connect(clientId.c_str(), MQTT_USER, MQTT_PASSWORD)) { - #endif //MQTT_AUTH - - #ifndef MQTT_AUTH - if (mqtt_client->connect(clientId.c_str())) { - #endif //MQTT_AUTH - - // Once connected, publish an announcement... - mqtt_client->publish("Rover/Info", "Connected to Mqtt-Broker"); - } - return mqtt_client->connected(); -} -bool Network::connectWifi() { - // Configures static IP address - - if (!WiFi.mode(WIFI_AP_STA)) - std::cout << "Network::connectWiFi failed WiFi.mode" << std::endl; - - if (!WiFi.config(local_IP, gateway, subnet, dnsServer)) - std::cout << "STA Failed to configure" << std::endl; - - // Connect to Wi-Fi network with SSID and password - std::cout << "Connecting to " << WLAN_SSID << std::endl; - WiFi.begin(WLAN_SSID, WLAN_PASSWORD); - uint8_t timeout = WLAN_CONNECT_TIMEOUT; - while (WiFi.status() != WL_CONNECTED) { - delay(WLAN_CONNECT_LOOP_TIME); - std::cout << "." << std::flush; - timeout--; - if (timeout == 0) { - std::cout << std::endl; - std::cout << "WiFi NOT connected." << std::endl; - return false; - } - } - - // Print local IP address and start web server - std::cout << std::endl; - std::cout << "WiFi connected." << std::endl; - std::cout << "IP address: " << std::endl; - std::cout << WLAN_IP << std::endl; - // std::cout << "WiFi MAC Address: " << WiFi.macAddress() << std::endl << std::endl; - return true; -} - -bool Network::connectEspNow(recieveCallbackPtr reci, sendCallbackPtr send) { - if (esp_now_init() != ESP_OK) { - std::cout << "Network::connectEspNow - Error initializing ESP-NOW" << std::endl; - return false; - } - - esp_now_register_send_cb(send); - - memcpy(peerInfo.peer_addr, broadcastAddress, 6); - peerInfo.channel = 0; - peerInfo.encrypt = false; - - if (esp_now_add_peer(&peerInfo) != ESP_OK) { - std::cout << "Network::connectEspNow - Failed to add peer" << std::endl; - return false; - } - - esp_now_register_recv_cb(reci); - return true; -} - -uint8_t Network::getCurrentChannel() { - uint8_t channel; - wifi_second_chan_t secondChannel; - if (esp_wifi_get_channel(&channel, &secondChannel) != ESP_OK) { - std::cout << "Network::getCurrentChannel - Error!" << std::endl; - return -1; - } - // std::cout << "Network::getCurrentChannel - Current WiFi channel: " - // << (int) channel << " second channel: " << (int) secondChannel - // << std::endl; - return channel; -} - -bool Network::checkMQTT() { - static uint64_t lastReconnectAttempt = 0; - if (!mqtt_client->connected()) { - long now = millis(); - if (now - lastReconnectAttempt > MQTT_TIME_RECONNECT) { - lastReconnectAttempt = now; - // Attempt to reconnect - if (connectMQTT()) - return true; - } - return false; - } - mqtt_client->loop(); - return true; -} - -bool Network::initMQTT() { - if (connectMQTT()) - return true; - return false; -} - -int32_t Network::getWiFiChannel(const char *ssid) { - if (int32_t n = WiFi.scanNetworks()) - for (uint8_t i=0; i= delay)) { - std::cout << "Reconnecting to WiFi..." << std::endl; - WiFi.disconnect(); - WiFi.reconnect(); - previousMillis = currentMillis; - } - -} - -PubSubClient* Network::getMqttClient() { - return mqtt_client; -}