kleiax/Bachelorarbeit-Rover
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases 1 Wiki Activity

Merge branch 'main' into projektarbeit

Browse Source
This commit is contained in:
kleiax
2023-10-12 20:35:31 +02:00
parent f78804d836 451517e7cd
commit e511afb2a5
80 changed files with 5031 additions and 3385 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.gitignore
+1 -2
View File
@@ -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
.vscode/settings.json
Vendored
+8
View File
@@ -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
doc/Doxygen/Doxyfile
+5 -5
View File
@@ -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
doc/TODO allgemein.txt
+23 -20
View File
@@ -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
3D:
Oberrahmen soll nur aussehen wie ein A
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
doc/latexSnippet.cpp
-9
View File
@@ -1,9 +0,0 @@
void KomponenteA::loop() {
this->komponenteB->loop();
this->komponenteC->loop();
if (millis() - this->lastMillis < this->delayMillis)
return;
this->doSomething();
}
include/Version.h
+7 -8
View File
@@ -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
// 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
include/config.h
+38 -16
View File
@@ -1,7 +1,7 @@
/**
* @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
*
@@ -11,42 +11,64 @@
#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;
}
include/moveControl.h
+149 -124
View File
@@ -22,6 +22,15 @@
#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
@@ -31,148 +40,164 @@
* 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
include/network.h
-107
View File
@@ -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 <iostream>
#include <WiFi.h>
#include <PubSubClient.h>
#include <esp_now.h>
#include <esp_wifi.h>
#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
include/networkConfig.h
+55 -84
View File
@@ -12,94 +12,65 @@
*
*/
// General config
#define MQTT_TIME_RECONNECT 2500
#include <stdint.h>
/**
* @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";
}
lib/Battery/battery.cpp
+177 -86
View File
@@ -11,137 +11,228 @@
#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<double>(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<int>((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)
if (index == 0)
{
if (this->batteryVoltage > this->absurdLowVoltage)
{
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--;
}
}
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);
lib/Battery/battery.h
+162 -78
View File
@@ -24,95 +24,179 @@
*
* 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
lib/CalcAzimuth/calcAzimuth.cpp
+60 -31
View File
@@ -11,57 +11,84 @@
#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;
}
@@ -71,22 +98,24 @@ void CalcAzimuth::updateAzimuth() {
}
// 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;
if (this->lastChangePoint.distanceTo(this->currentPosition) > this->minDistanceForBetterQuality)
{
switch (this->state)
{
case State::Bad:
this->state = State::Ok;
break;
case State::Ok :
this->state = State::Good;
break;
case State::Ok:
this->state = State::Good;
break;
case State::Good :
this->state = State::Super;
break;
case State::Good:
this->state = State::Super;
break;
default:
break;
default:
break;
}
}
}
lib/CalcAzimuth/calcAzimuth.h
+69 -26
View File
@@ -1,7 +1,7 @@
/**
* @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
*
@@ -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
lib/calibrateCompass/calibrateCompass.cpp → lib/CalibrateCompass/calibrateCompass.cpp
+83 -56
View File
@@ -11,58 +11,71 @@
#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,35 +96,41 @@ 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;
}
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);
@@ -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<bool>(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;
}
lib/CalibrateCompass/calibrateCompass.h
+110
View File
@@ -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 <QMC5883LCompass.h>
#include <Preferences.h>
#include <iostream>
#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;
};
lib/Component/component.cpp
+26 -12
View File
@@ -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<Component*>::iterator it;
for (it = this->childComponents.begin(); it != this->childComponents.end(); it++)
(*it)->loop();
}
this->runAsChild();
if (this->childComponents.size())
{
std::list<Component *>::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<bool>(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);
}
lib/Component/component.h
+118 -25
View File
@@ -1,7 +1,7 @@
/**
* @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
*
@@ -15,37 +15,130 @@
#include <list>
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();
/**
* @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 deactivate() { this->active = false; }
void activate() { this->active = false; }
/**
* @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();
protected:
virtual void runAsChild() {}
virtual void beforeRun() {}
virtual void run() = 0;
virtual void afterRun() {}
/**
* @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 addChildComponent(Component* child);
void removeChildComponent(Component* child);
protected:
/**
* @brief Override this function to avoid the delay
*/
virtual void runAsChild() {}
void activateOnlyChilds() { this->onlyChilds = true; }
void deactivateOnlyChilds() { this->onlyChilds = false; }
/**
* @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() {}
void setTimerAfterTask() { this->timeUpdateAfter = true; }
/**
* @brief The actual task
*
* This function have to be overwritten by the inheriting class.
*/
virtual void run() = 0;
uint16_t loopDelay = 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() {}
private:
std::list<Component*> childComponents;
/**
* @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);
bool active = true;
bool onlyChilds = false;
bool timeUpdateAfter = false;
uint32_t lastMillis = 0;
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<Component *> childComponents;
bool active = true;
bool onlyChilds = false;
bool timeUpdateAfter = false;
uint32_t lastMillis = 0;
};
lib/ControllPad/controlPad.cpp → lib/ControlPad/controlPad.cpp
+35 -19
View File
@@ -11,27 +11,33 @@
#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<bool>(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;
}
lib/ControlPad/controlPad.h
+80
View File
@@ -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 <menuControl.h>
#include <controlPadInput.h>
#include <component.h>
/**
* @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;
};
lib/ControlPad/controlPadInput.h
+79
View File
@@ -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 <stdint.h>
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
lib/ControllPad/controlPad.h
-48
View File
@@ -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 <menuControl.h>
#include <controlPadInput.h>
#include <component.h>
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;
};
lib/ControllPad/controlPadInput.h
-74
View File
@@ -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 <stdint.h>
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
lib/Counter/counter.cpp
+21 -14
View File
@@ -1,14 +1,14 @@
#include "counter.h"
#include <inttypes.h>
uint8_t Counter::amountOfCounter = 0;
Counter::Counter(uint8_t pin) {
this->pulsePin = pin;
this->unit = static_cast<pcnt_unit_t>(Counter::amountOfCounter);
if (Counter::amountOfCounter < 7)
Counter::Counter(uint8_t pin)
: pulsePin{pin}, unit{static_cast<pcnt_unit_t>(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);
}
lib/Counter/counter.h
+64 -17
View File
@@ -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 <Arduino.h>
#include <driver/pcnt.h>
#include <cinttypes>
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;
};
lib/LcdWrapper/LcdWrapper.cpp
+37 -19
View File
@@ -1,5 +1,5 @@
/**
* @file displayWrapper.cpp
* @file LcdWrapper.cpp
* @author Alexander Klein (alex@kleiax.de)
* @brief Contains the implementation of the class LcdWrapper
* @version 0.1
@@ -11,55 +11,73 @@
#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++) {
this->lcd->setCursor(0, i);
this->lcd->print(this->data[i]);
}
if (this->callback)
this->lcd->clear();
for (uint8_t i = 0; i < LcdWrapper::totalLines; i++)
{
this->lcd->setCursor(0, i);
this->lcd->print(static_cast<const char *>(this->data[i]));
}
if (static_cast<bool>(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;
lib/LcdWrapper/LcdWrapper.h
+39 -39
View File
@@ -1,5 +1,5 @@
/**
* @file displayWrapper.h
* @file LcdWrapper.h
* @author Alexander Klein (alex@kleiax.de)
* @brief Contains the LcdWrapper class
* @version 0.1
@@ -17,7 +17,7 @@
#include <displayWrapper.h>
#include <component.h>
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
*
@@ -25,51 +25,51 @@ typedef void (*LcdWrapperCallback) (const char data[][16], uint8_t lines, uint8_
* 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;
private:
void run() override;
LiquidCrystal_I2C* lcd;
LcdWrapperCallback callback = nullptr;
char data[LcdWrapper::totalLines][LcdWrapper::totalRows];
LiquidCrystal_I2C *lcd;
LcdWrapperCallback callback = nullptr;
char data[LcdWrapper::totalLines][LcdWrapper::totalRows];
uint8_t cursorRow = 0;
uint8_t cursorLine = 0;
uint8_t cursorRow = 0;
uint8_t cursorLine = 0;
bool changed = false;
bool changed = false;
};
#endif // DISPLAY_WRAPPER_H
lib/MQTT/debugMqtt.cpp
+49 -34
View File
@@ -11,93 +11,109 @@
*/
#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<char *>(DebugMqtt::msg), MQTT_BUFFER_SIZE, static_cast<const char *>("%s: %s"), this->name, msg.c_str());
this->sendData(loglevel, topic, static_cast<const char *>(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<char *>(DebugMqtt::topic), MQTT_BUFFER_SIZE, static_cast<const char *>("%s%s"), DebugMqtt::enum_to_string(loglevel).c_str(), topic.c_str());
snprintf(static_cast<char *>(DebugMqtt::msg), MQTT_BUFFER_SIZE, static_cast<const char *>("%s"), data.c_str());
client->publish(DebugMqtt::topic, static_cast<const char *>(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<char *>(DebugMqtt::msg), MQTT_BUFFER_SIZE, static_cast<const char *>("%s %s=%f %llu"), measurement_name.c_str(), field_set.c_str(), measurement, nanos);
this->sendData(Loglevel::influx, static_cast<const char *>(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;
}
}
lib/MQTT/debugMqtt.h
+110 -107
View File
@@ -14,7 +14,6 @@
#include <iostream>
#include <PubSubClient.h>
/**
* @brief
*
@@ -35,7 +34,7 @@
*/
#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
@@ -48,12 +47,15 @@
* @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
@@ -64,115 +66,116 @@ enum Loglevel { none,
*
* @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
lib/MotorControl/motorControl.cpp
+94 -55
View File
@@ -12,13 +12,13 @@
#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<uint8_t>(((static_cast<uint8_t>(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<uint8_t>(((static_cast<uint8_t>(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;
}
lib/MotorControl/motorControl.h
+79 -79
View File
@@ -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
lib/Network/network.cpp
+201
View File
@@ -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<void *>(peerInfo.peer_addr), static_cast<const void *>(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<bool>(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<bool>(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();
}
lib/Network/network.h
+136
View File
@@ -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 <iostream>
#include <WiFi.h>
#include <PubSubClient.h>
#include <esp_now.h>
#include <esp_wifi.h>
/**
* @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
lib/Point/point.cpp
+51 -37
View File
@@ -11,94 +11,108 @@
#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->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<int16_t>(atan2(lon, phi) / ROUTE_DEGREE_TO_RADIANT) * -1;
// if (res < 0)
// res += 360;
return res;
return static_cast<int16_t>(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;
}
}
lib/Point/point.h
+103 -101
View File
@@ -18,122 +18,124 @@
#define ROUTE_DISTANCE_BETWEEN_LATITUDE 111300
/**
* @brief A to handle points on the earth
* @brief A class to handle points on the earth
*
* The points inherits latidue and longitude as doubles
* 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
lib/Sensors/sensorData.cpp
+173 -55
View File
@@ -1,5 +1,5 @@
/**
* @file senors.cpp
* @file sensorData.cpp
* @author Alexander Klein (alex@kleiax.de)
* @brief
* @version 0.1
@@ -12,96 +12,184 @@
#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<int>(deviceStatus) << "). Freeze!" << std::endl;
while (true)
{
}
}
}
CalcAzimuth::State SensorData::getCalcAzimuthState() const {
if (this->calcCompass)
CalcAzimuth::State SensorData::getCalcAzimuthState() const
{
if (static_cast<bool>(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<bool>(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<const char *>("None"));
}
else if (fixType == 1)
strcpy(fixTypeString, "Dead Reckoning");
{
strcpy(fixTypeString, static_cast<const char *>("Dead Reckoning"));
}
else if (fixType == 2)
strcpy(fixTypeString, "2D");
{
strcpy(fixTypeString, static_cast<const char *>("2D"));
}
else if (fixType == 3)
strcpy(fixTypeString, "3D");
{
strcpy(fixTypeString, static_cast<const char *>("3D"));
}
else if (fixType == 3)
strcpy(fixTypeString, "GNSS + Dead Reckoning");
{
strcpy(fixTypeString, static_cast<const char *>("GNSS + Dead Reckoning"));
}
else if (fixType == 5)
strcpy(fixTypeString, "Time Only");
{
strcpy(fixTypeString, static_cast<const char *>("Time Only"));
}
else
strcpy(fixTypeString, "UNKNOWN");
{
strcpy(fixTypeString, static_cast<const char *>("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<const char *>("None"));
}
else if (carrSoln == 1)
strcpy(carrSolnString, "Floating");
{
strcpy(carrSolnString, static_cast<const char *>("Floating"));
}
else if (carrSoln == 2)
strcpy(carrSolnString, "Fixed");
{
strcpy(carrSolnString, static_cast<const char *>("Fixed"));
}
else
strcpy(carrSolnString, "UNKNOWN");
{
strcpy(carrSolnString, static_cast<const char *>("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<bool>(this->realCompass))
{
this->realCompass->read();
this->realAzimuth = this->realCompass->getAzimuth();
}
if (static_cast<bool>(this->gyroscope) && this->gyroscope->dmpGetCurrentFIFOPacket(static_cast<uint8_t *>(this->gyroBuffer)))
{
this->gyroscope->dmpGetQuaternion(&this->quaternion, static_cast<uint8_t *>(this->gyroBuffer));
this->gyroscope->dmpGetGravity(&this->gravity, &this->quaternion);
this->gyroscope->dmpGetYawPitchRoll(static_cast<float *>(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<bool>(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<int>(versionHigh) << "." << static_cast<int>(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);
}
lib/Sensors/sensorData.h
+139 -49
View File
@@ -1,7 +1,7 @@
/**
* @file sensorData.h
* @author Alexander Klein (alex@kleiax.de)
* @brief
* @brief Contains the SensorData class
* @version 0.1
* @date 2023-09-02
*
@@ -13,6 +13,7 @@
#define SENSOR_DATA_H
#include <SPI.h>
#include <I2Cdev.h>
#include <iostream>
#include "component.h"
@@ -20,70 +21,159 @@
#include <SparkFun_u-blox_GNSS_Arduino_Library.h>
#include <QMC5883LCompass.h>
// Gyroskop
#include <MPU6050_6Axis_MotionApps20.h>
#include "calcAzimuth.h"
#include "point.h"
class Sensors;
class SensorData : public Component {
public:
SensorData();
/**
* @brief A class to manage all sensors
*
* Each sensor have separately to be enabled
*/
class SensorData : public Component
{
public:
SensorData();
~SensorData();
void enableGnss(SPIClass* spiPort, uint8_t csPin);
void enableGnss();
void enableRealCompass();
void enableCalcCompass();
void enableGyroskop();
void enableNtrip(String host, uint16_t port, String mountPoint, String user, String password);
// Interface Const kram
int16_t getRealAzimuth() const { return this->realAzimuth; }
int16_t getCalcAzimuth() const { return this->calcAzimuth; }
CalcAzimuth::State getCalcAzimuthState() const;
/**
* @brief Enable the gnss module over spi
*
* @param spiPort
* @param csPin
*/
void enableGnss(SPIClass *spiPort, uint8_t csPin);
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 i2c
*/
void enableGnss();
CalcAzimuth* getCalcCompass() const { return this->calcCompass; }
QMC5883LCompass* getRealCompass() const { return this->realCompass; }
void enableRealCompass();
void enableCalcCompass();
void enableGyroscope();
// 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);
// Interface Const
/**
* @brief Get the azimuth measured by the compass module
*
* @return int16_t
*/
int16_t getRealAzimuth() const { return this->realAzimuth; }
private:
void run() override;
void runAsChild() override;
void initGnss();
/**
* @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; }
QMC5883LCompass* realCompass = nullptr;
CalcAzimuth* calcCompass = nullptr;
SFE_UBLOX_GNSS* gnss = nullptr;
/**
* @brief Get the CalcAzimuth::State object
*
* Needed to check the quality of calculated azimuth
*
* @return CalcAzimuth::State
*/
CalcAzimuth::State getCalcAzimuthState() const;
UBX_NAV_PVT_data_t* gnssData;
Point currentPosition;
Point getCurrentPos() const { return this->currentPosition; }
const UBX_NAV_PVT_data_t *getGnssData() const { return this->gnssData; };
NTRIPClientStates getNtripState() const;
int16_t realAzimuth = INT16_MAX;
int16_t calcAzimuth = INT16_MAX;
/**
* @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
static void printPVTdata(UBX_NAV_PVT_data_t *ubxDataStruct);
static void savePVTdata(UBX_NAV_PVT_data_t *ubxDataStruct);
/**
* @brief Get the CalcCompass object
* @return CalcAzimuth*
*/
CalcAzimuth *getCalcCompass() const { return this->calcCompass; }
static UBX_NAV_PVT_data_t* ubxDataStatic;
/**
* @brief Get the RealCompass object
* @return QMC5883LCompass*
*/
QMC5883LCompass *getRealCompass() const { return this->realCompass; }
static uint32_t ubxUpdateTimeStatic;
/**
* @brief Get the NTRIPClient object
* @return NTRIPClient*
*/
NTRIPClient *getNtripClient() const { return this->ntripClient; }
static bool outputStatusPrintPVTdata;
static bool newData;
/**
* @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
lib/Speedometer/speedometer.cpp
+74 -41
View File
@@ -11,111 +11,144 @@
*/
#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();
const double wheelRevolutionsAbsolute = pulse / this->steps;
const double wheelRevolutionsRelativ = wheelRevolutionsAbsolute / (elapsedTime / 1000.0);
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
double meterPerSecond = wheelRevolutionsRelativ * (diameter * PI);
double radPerSecond = wheelRevolutionsRelativ * 2 * PI;
switch (this->currentDirection) {
case Direction::Forward :
this->speed = ms;
break;
if (meterPerSecond < minimalSpeed)
{
meterPerSecond = 0;
radPerSecond = 0;
}
case Direction::Backward :
this->speed = -ms;
break;
switch (this->currentDirection)
{
case Direction::Forward:
this->speed = meterPerSecond;
this->rad = radPerSecond;
break;
case Direction::None :
this->speed = 0;
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<int16_t>(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() {
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;
}
lib/Speedometer/speedometer.h
+99 -92
View File
@@ -26,115 +26,122 @@
* 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
lib/Times/debugTimes.cpp
+22 -11
View File
@@ -1,7 +1,7 @@
/**
* @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
*
@@ -13,33 +13,44 @@
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;
lib/Times/debugTimes.h
+38 -37
View File
@@ -28,49 +28,50 @@
* @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
lib/calibrateCompass/calibrateCompass.h
-60
View File
@@ -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 <QMC5883LCompass.h>
#include <Preferences.h>
#include <iostream>
#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;
};
platformio.ini
+27 -7
View File
@@ -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 =
+<src/*>
+<lib/*>
+<include/*>
[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
;
;
;
;
;
;
;
;
;
;
;
;
;
#
src/OutputBuf/outputBuf.cpp
+38 -21
View File
@@ -1,5 +1,5 @@
/**
* @file outputBufMqtt.cpp
* @file outputBuf.cpp
* @author Alexander Klein (alex@kleiax.de)
* @brief Contains the implementation of the class OutputBufMqtt
* @version 0.1
@@ -11,49 +11,66 @@
#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<bool>(debugMqtt))
{
this->isMqttActive = true;
}
if (serialBT)
if (static_cast<bool>(serialBT))
{
this->isMqttActive = true;
}
}
void OutputBuf::activateMqtt(bool status) {
if (debugMqtt)
void OutputBuf::activateMqtt(bool status)
{
if (static_cast<bool>(debugMqtt))
{
this->isMqttActive = status;
}
}
void OutputBuf::activateSerialBT(bool status) {
if (serialBT)
void OutputBuf::activateSerialBT(bool status)
{
if (static_cast<bool>(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<char>(c), getloc());
if (this->debugMqtt && this->isMqttActive)
this->debugMqtt->addCharacter((char) c);
{
this->debugMqtt->addCharacter(static_cast<char>(character));
}
if (this->serialBT && this->isSerialBTActive && this->serialBT->connected())
this->serialBT->print((char) c);
{
this->serialBT->print(static_cast<char>(character));
}
Serial.print((char) c);
Serial.print(static_cast<char>(character));
}
return c;
return character;
}
src/OutputBuf/outputBuf.h
+45 -44
View File
@@ -1,5 +1,5 @@
/**
* @file outputBufMqtt.h
* @file outputBuf.h
* @author Alexander Klein (alex@kleiax.de)
* @brief Contains a small class that provide a streambuf
*
@@ -26,60 +26,61 @@
#include <BluetoothSerial.h>
/**
* @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
src/SpecialMenus/CalibrateBattery/menuCalibrateBattery.cpp
+70
View File
@@ -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<bool>(parentMenu))
{
this->parentMenu->printMenu();
}
}
void MenuCalibrateBattery::right()
{
if (this->battery->getCalibrationState() == Battery::CalibrationState::Waiting)
{
this->battery->nextVoltageIsReady();
}
}
src/SpecialMenus/CalibrateBattery/menuCalibrateBattery.h
+50
View File
@@ -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
src/SpecialMenus/GPS/menuGPS.cpp
-132
View File
@@ -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);
}
src/SpecialMenus/GPS/menuGPS.h
-39
View File
@@ -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 <SparkFun_u-blox_GNSS_Arduino_Library.h>
#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
src/SpecialMenus/PID/menuPidSettings.cpp
+5 -4
View File
@@ -11,12 +11,13 @@
#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) {
void MenuPidSettings::action(int16_t *values, uint8_t length)
{
if (length != 3)
{
return;
}
this->pid->SetTunings(values[0], values[1], values[2]);
}
src/SpecialMenus/PID/menuPidSettings.h
+12 -12
View File
@@ -20,20 +20,20 @@
* @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
src/SpecialMenus/SensorData/menuSensorData.cpp
+152
View File
@@ -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<bool>(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<bool>(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<bool>(fixType))
{
lineOne.concat(gpsData->hAcc);
lineTwo.concat(gpsData->numSV);
}
else
{
lineOne.concat("-/-");
lineTwo.concat("-/-");
}
break;
default:
this->printDefault();
return;
}
this->print(lineOne, lineTwo);
}
src/SpecialMenus/SensorData/menuSensorData.h
+37
View File
@@ -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
src/SpecialMenus/Speed/menuSpeed.cpp
+22
View File
@@ -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;
}
src/SpecialMenus/Speed/menuSpeed.h
+38
View File
@@ -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 <menuIntInput.h>
#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
src/SpecialMenus/Systeminformation/menuSysteminformatio.cpp
+47 -45
View File
@@ -11,64 +11,66 @@
#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);
}
src/SpecialMenus/Systeminformation/menuSysteminformation.h
+12 -12
View File
@@ -26,20 +26,20 @@
*
* 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
src/SpecialMenus/driveModi/CalibrateCompass/menuCalibrateCompass.cpp
+123 -115
View File
@@ -10,153 +10,161 @@
*/
#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());
case CalibrateCompass::State::Calibrating:
lineOne = "Calibrating...";
lineTwo = "Move around";
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;
}
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;
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;
}
}
src/SpecialMenus/driveModi/CalibrateCompass/menuCalibrateCompass.h
+26 -24
View File
@@ -20,33 +20,35 @@
* @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
src/SpecialMenus/driveModi/ManualDrive/menuManualDrive.cpp
+44 -55
View File
@@ -10,86 +10,75 @@
*/
#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:
src/SpecialMenus/driveModi/ManualDrive/menuManualDrive.h
+26 -26
View File
@@ -13,40 +13,40 @@
#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
src/SpecialMenus/driveModi/TestMode/menuTestMode.cpp
+130 -79
View File
@@ -10,18 +10,23 @@
*/
#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" <<std::endl;
auto dummy = []()
{
std::cout << "Dummy in Action" << std::endl;
};
// Create menu
this->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);
@@ -176,65 +207,85 @@ void MenuTestMode::init() {
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;
}
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<bool>(this->testModeFunctionDouble))
{
res = (this->testMode->*this->testModeFunctionDouble)(values[0], values[1]);
else if (length == 1 && this->testModeFunctionSingle)
}
else if (length == 1 && static_cast<bool>(this->testModeFunctionSingle))
{
res = (this->testMode->*this->testModeFunctionSingle)(values[0]);
}
if (res) {
if (res)
{
this->menu->printManeuverTime();
this->menu->maneuverStarted();
}
src/SpecialMenus/driveModi/TestMode/menuTestMode.h
+61 -59
View File
@@ -22,78 +22,80 @@
* @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
src/SpecialMenus/driveModi/TestMode/speedometerTest.cpp
+51 -43
View File
@@ -1,5 +1,5 @@
/**
* @file menuTest.cpp
* @file speedometerTest.cpp
* @author Alexander Klein (alex@kleiax.de)
* @brief Contains the implementation of the class SpeedometerTest.
* @version 0.1
@@ -11,69 +11,77 @@
#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::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::Finished:
this->state = State::Running;
this->speedometer->calibrationMeasurementStart();
this->printMenu();
break;
default:
break;
default:
break;
}
}
src/SpecialMenus/driveModi/TestMode/speedometerTest.h
+39 -37
View File
@@ -1,5 +1,5 @@
/**
* @file menuTest.h
* @file speedometerTest.h
* @author Alexander Klein (alex@kleiax.de)
* @brief Contains a class to test the wheel encoder.
* @version 0.1
@@ -18,47 +18,49 @@
* @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;
};
src/SpecialMenus/driveModi/menuDriveMode.cpp
+152 -11
View File
@@ -11,20 +11,161 @@
#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<bool>(this->activeMenu))
{
if (static_cast<bool>(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<bool>(this->activeMenu))
{
this->activeMenu->right();
return;
}
MenuInformationSites::right();
}
void MenuDriveMode::up()
{
if (static_cast<bool>(this->activeMenu))
{
this->activeMenu->up();
return;
}
MenuInformationSites::up();
}
void MenuDriveMode::down()
{
if (static_cast<bool>(this->activeMenu))
{
this->activeMenu->down();
return;
}
MenuInformationSites::down();
}
void MenuDriveMode::yes()
{
if (static_cast<bool>(this->activeMenu))
{
this->activeMenu->yes();
return;
}
MenuInformationSites::yes();
}
void MenuDriveMode::no()
{
if (static_cast<bool>(this->activeMenu))
{
this->activeMenu->no();
return;
}
MenuInformationSites::no();
}
void MenuDriveMode::prepareReenterMenu()
{
this->activeMenu = nullptr;
}
void MenuDriveMode::printMenu()
{
if (static_cast<bool>(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<bool>(this->activeMenu))
{
return;
}
this->activeMenu->printMenu();
this->activeMenu->setParentMenu(this);
}
src/SpecialMenus/driveModi/menuDriveMode.h
+71 -21
View File
@@ -13,35 +13,85 @@
#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
src/driveModi/Modi/CalibrateCompass/calibrateCompassM.cpp
+7 -5
View File
@@ -11,13 +11,15 @@
#include "calibrateCompassM.h"
CalibrateCompassM::CalibrateCompassM(DriveModiParams params) : ManualControl(params) {}
void CalibrateCompassM::setCalibrateCompass(CalibrateCompass *caliCompass) {
if (caliCompass) {
void CalibrateCompassM::setCalibrateCompass(CalibrateCompass *caliCompass)
{
if (static_cast<bool>(caliCompass))
{
this->caliCompass = caliCompass;
this->addChildComponent(this->caliCompass);
} else if (this->caliCompass) {
}
else if (static_cast<bool>(this->caliCompass))
{
this->removeChildComponent(this->caliCompass);
this->caliCompass = caliCompass;
}
src/driveModi/Modi/CalibrateCompass/calibrateCompassM.h
+16 -9
View File
@@ -1,7 +1,7 @@
/**
* @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
*
@@ -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
src/driveModi/Modi/ManualControl/manualControl.cpp
+55 -39
View File
@@ -10,67 +10,83 @@
*/
#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;
case InputMode::Digital:
this->digitalControl();
break;
default:
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<bool>(this->directionChangeWrapper) && (xAxis > deadzone || xAxis < -deadzone))
{
this->lastLoopTurned = true;
else if (this->lastLoopTurned) {
if (this->directionChangeWrapper)
}
else if (this->lastLoopTurned)
{
if (static_cast<bool>(this->directionChangeWrapper))
{
this->directionChangeWrapper->action();
}
this->lastLoopTurned = false;
}
}
src/driveModi/Modi/ManualControl/manualControl.h
+40 -25
View File
@@ -13,46 +13,61 @@
#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
src/driveModi/Modi/TestMode/testMode.cpp
+78 -43
View File
@@ -10,21 +10,19 @@
*/
#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,21 +30,29 @@ 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;
@@ -54,29 +60,40 @@ bool TestMode::drive(int16_t cm, int16_t 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);
if (cmDistance < 0)
{
this->moveControl->setSpeed(-this->maxSpeeds.x);
}
else if (cmDistance > 0)
{
this->moveControl->setSpeed(this->maxSpeeds.x);
}
this->maneuverTime = (uint32_t) (((double) abs(cm) / 100.0) / this->maxForwardSpeed) * 1000;
this->maneuverTime = static_cast<uint32_t>(((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);
}
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 = (uint32_t) (((double) cm / 100.0) / this->maxForwardSpeed) * 1000;
this->maneuverTime = static_cast<uint32_t>(((cmDistance / 100.0) / this->maxSpeeds.x) * 1000);
this->busy = true;
this->maneuver = Maneuver::Drive;
return true;
@@ -86,17 +103,22 @@ bool TestMode::drive(int16_t cm, int16_t degree) {
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<uint8_t>((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);
src/driveModi/Modi/TestMode/testMode.h
+85 -82
View File
@@ -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;
bool busy = false;
bool abort = false;
uint16_t azimuth;
int16_t degree;
uint32_t maneuverTime = 0;
uint32_t actionStart = 0;
uint16_t azimuth;
int16_t degree;
uint32_t maneuverTime = 0;
uint32_t actionStart = 0;
static constexpr int8_t maxPercentage = 100;
};
#endif // TEST_MODE_H
src/driveModi/driveManager.cpp
+19 -34
View File
@@ -11,54 +11,39 @@
#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<bool>(this->currentModusPtr))
{
this->removeChildComponent(this->currentModusPtr);
delete this->currentModusPtr;
this->currentModusPtr = nullptr;
}
switch (modus) {
case Modi::Off:
this->currentModusPtr = nullptr;
break;
this->currentModusPtr = modus;
case Modi::ManualControl: {
this->currentModusPtr = new ManualControl(this->driveModiParams);
}
break;
case Modi::TestMode: {
this->currentModusPtr = new TestMode(driveModiParams);
}
break;
default:
this->currentModusPtr = nullptr;
break;
}
if (this->currentModusPtr)
if (static_cast<bool>(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();
}
src/driveModi/driveManager.h
+52 -67
View File
@@ -12,83 +12,68 @@
#ifndef DRIVE_MANAGER_H
#define DRIVE_MANAGER_H
#include <Arduino.h>
#include <SPI.h>
#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
src/driveModi/driveModi.cpp
+15 -10
View File
@@ -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();
}
src/driveModi/driveModi.h
+46 -36
View File
@@ -17,10 +17,14 @@
#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;
};
/**
@@ -29,42 +33,48 @@ struct DriveModiParams {
* 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
src/main.cpp
+187 -101
View File
@@ -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<const char *>(NetworkConfig::ssid),
static_cast<const char *>(NetworkConfig::password),
setIPs());
network->activateEspNow(receiveCallback, sendCallback);
if (NetworkConfig::mqtt)
{
if (network->activateMqtt(static_cast<const char *>(MqttConfig::user),
static_cast<const char *>(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<const char *>(NtripConfig::host),
NtripConfig::port,
static_cast<const char *>(NtripConfig::mountPoint),
static_cast<const char *>(NtripConfig::user),
static_cast<const char *>(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<uint16_t>(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<int>(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<int>(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" <<std::endl;
void makeMenu()
{
auto dummy = []()
{
std::cout << "Dummy in Action" << std::endl;
};
// Create Menu
main_m = new Menu();
main_m->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<uint8_t>(moveController.getPID(0)->GetKp()));
pidl_m->setEntry(1, "I-Part", static_cast<uint8_t>(moveController.getPID(0)->GetKi()));
pidl_m->setEntry(2, "D-Part", static_cast<uint8_t>(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<uint8_t>(moveController.getPID(1)->GetKp()));
pidr_m->setEntry(1, "I-Part", static_cast<uint8_t>(moveController.getPID(1)->GetKi()));
pidr_m->setEntry(2, "D-Part", static_cast<uint8_t>(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;
}
esp_err_t result = esp_now_send(Network::getBroadcastAddress(), (uint8_t *) data, lines * rows);
const esp_err_t result = esp_now_send(network->getBroadcastAddress(), reinterpret_cast<const uint8_t *>(data), lines * rows);
if (result != ESP_OK)
{
Serial.println("Error sending the data");
}
}
NetworkAddresses setIPs()
{
NetworkAddresses adresses;
adresses.localIP.fromString(static_cast<const char *>(NetworkConfig::ip));
adresses.subnet.fromString(static_cast<const char *>(NetworkConfig::subnet));
adresses.gateway.fromString(static_cast<const char *>(NetworkConfig::gateway));
adresses.dnsServer.fromString(static_cast<const char *>(NetworkConfig::dns));
if (NetworkConfig::mqtt)
{
adresses.mqttServer.fromString(static_cast<const char *>(MqttConfig::server));
adresses.mqttPort = MqttConfig::port;
}
return adresses;
}
src/moveControl.cpp
+152 -110
View File
@@ -10,69 +10,70 @@
*/
#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<int8_t>(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<int8_t>(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<int8_t>(this->leftPidOut));
this->rightMotor->setTargetPower(static_cast<int8_t>(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();
}
src/network.cpp
-174
View File
@@ -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<n; i++)
if (!strcmp(ssid, WiFi.SSID(i).c_str()))
return WiFi.channel(i);
return 0;
}
void Network::checkWiFi() {
static uint64_t previousMillis = 0;
static uint16_t delay = 5000;
uint64_t currentMillis = millis();
if ((WiFi.status() != WL_CONNECTED) && (currentMillis - previousMillis >= delay)) {
std::cout << "Reconnecting to WiFi..." << std::endl;
WiFi.disconnect();
WiFi.reconnect();
previousMillis = currentMillis;
}
}
PubSubClient* Network::getMqttClient() {
return mqtt_client;
}