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

Merge branch 'main' of git.kleiax.de:kleiax/Projektarbeit-Rover into main

Browse Source
This commit is contained in:
kleiax
2023-08-14 07:15:26 +02:00
parent 147eea1f92 bad84b1ef3
commit bf4c1dd49f
39 changed files with 763 additions and 315 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.vscode/settings.json
Vendored
+41 -1
View File
@@ -77,7 +77,47 @@
"istream": "cpp",
"streambuf": "cpp",
"functional": "cpp",
"cmath": "cpp"
"cmath": "cpp",
"atomic": "cpp",
"cctype": "cpp",
"chrono": "cpp",
"clocale": "cpp",
"cstdarg": "cpp",
"cstddef": "cpp",
"cstdint": "cpp",
"cstdio": "cpp",
"cstdlib": "cpp",
"cstring": "cpp",
"ctime": "cpp",
"cwchar": "cpp",
"cwctype": "cpp",
"unordered_map": "cpp",
"unordered_set": "cpp",
"exception": "cpp",
"algorithm": "cpp",
"iterator": "cpp",
"map": "cpp",
"memory": "cpp",
"memory_resource": "cpp",
"numeric": "cpp",
"optional": "cpp",
"random": "cpp",
"ratio": "cpp",
"string_view": "cpp",
"system_error": "cpp",
"tuple": "cpp",
"type_traits": "cpp",
"utility": "cpp",
"initializer_list": "cpp",
"iomanip": "cpp",
"iosfwd": "cpp",
"iostream": "cpp",
"limits": "cpp",
"ostream": "cpp",
"sstream": "cpp",
"stdexcept": "cpp",
"cinttypes": "cpp",
"typeinfo": "cpp"
},
"cSpell.words": [
"Ahnung",
autoVersionIncrement
Submodule
+1
Submodule autoVersionIncrement added at fd51b62f00
doc/Notes and TODOs/SPIpushRawData.txt
-9
View File
@@ -1,9 +0,0 @@
Added this code in SparkFun_u-blox_GNSS_Arduino_Libary.cpp in the function pushRawData.
_spiPort->beginTransaction(SPISettings(_spiSpeed, MSBFIRST, SPI_MODE0));
digitalWrite(_csPin, LOW);
for (uint16_t i = 0; i < numDataBytes; i++) {
spiTransfer(dataBytes[i]);
}
digitalWrite(_csPin, HIGH);
_spiPort->endTransaction();
doc/Notes and TODOs/ps3 reconnect.txt
-26
View File
@@ -1,26 +0,0 @@
ps3.h
void ps3ResetGlobals();
ps3.c
void ps3ResetGlobals() {
// Own Code
is_active = false;
ps3_connection_cb = NULL;
ps3_connection_object_cb = NULL;
ps3_connection_object = NULL;
ps3_event_cb = NULL;
ps3_event_object_cb = NULL;
ps3_event_object = NULL;
}
Ps3Controller.h
private
void resetGlobals();
Ps3Controller.cpp
void Ps3Controller::resetGlobals() {
ps3ResetGlobals();
}
in -> Ps3Controller::begin
this->resetGlobals();
doc/PinBelegungen.txt
-30
View File
@@ -1,30 +0,0 @@
ESP32 Rover Pinbelegung
3V3 GND
x 23 DirR1
x 22 PWMR
x TX PC
x RX PC Connector I2C
BATTERY 35 21 SDA SDA
INTL1 32 GND GND
INTL2 33 19 SCL SCL
INTR1 25 18 VCC
INTR2 26 5 SPI_CSK
DirL1 27 17 SPI_CS
DirR2 14 16 SPI_COPI
DirL2 12 4 SPI_CIPO
GND x
PWML 13 2 Probleme beim flashen
x 15 WS2812 LED ?
x x
CMD x
5V USB x
Connector Encoder
- 2 1 +
OB 4 3 -
3V3 6 [5] OA
8 7
10 9
doc/TODO allgemein.txt
+12
View File
@@ -0,0 +1,12 @@
Für irgendwann:
-> Program underfloorLighting
-> Add an beeper
-> Program the beeper
-> Update GNSS Lib to v3
Besser zügig:
Speedometer buffer ergibt kaum Sinn
Abweichung max 25 cm bevor zu ungenau damit nie mehr als 50 cm gesamt.
Doxygen Kommentare aktualisieren
Motortreiber vielleicht bei wenigster Leistung ohne Kurve
doc/controller.txt
-17
View File
@@ -1,17 +0,0 @@
Controller
PS3-Button: Change Mode
Mode: ManualControl
L3: Joystick Mode
Left Joystick: Drive
R3: Shoulder Trigger Mode
L2: Spin left side
R2: Spin right side
Mode: Autopilot
Mode: CaptureRoute
Mode: ConsolControl
include/Version.h
+9
View File
@@ -0,0 +1,9 @@
// 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
include/network.h
+2
View File
@@ -16,6 +16,7 @@
#include <WiFi.h>
#include <PubSubClient.h>
#include <esp_now.h>
#include <esp_wifi.h>
#include "networkConfig.h"
@@ -44,6 +45,7 @@ class Network {
*/
static bool connectWifi();
static bool connectEspNow(recieveCallbackPtr reci, sendCallbackPtr send);
static uint8_t getCurrentChannel();
/**
* @brief Set all general settings to connect to a broker.
lib/MotorControl/motorControl.cpp
+31 -31
View File
@@ -17,9 +17,9 @@ MotorControl::MotorControl() {
this->setMaxPwm(PWMMAX);
}
void MotorControl::init(uint8_t pwm_pin, uint8_t pwm_channel, uint8_t dir_1, uint8_t dir_2) {
this->pwm_pin = pwm_pin;
this->pwm_channel = pwm_channel;
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;
this->dir_2 = dir_2;
@@ -29,17 +29,17 @@ void MotorControl::init(uint8_t pwm_pin, uint8_t pwm_channel, uint8_t dir_1, uin
digitalWrite(this->dir_1, LOW);
digitalWrite(this->dir_2, LOW);
ledcSetup(this->pwm_channel, PWMFREQ, this->pwm_res);
ledcAttachPin(this->pwm_pin, this->pwm_channel);
ledcWrite(this->pwm_channel, 0);
ledcSetup(this->pwmChannel, PWMFREQ, this->pwmRes);
ledcAttachPin(this->pwmPin, this->pwmChannel);
ledcWrite(this->pwmChannel, 0);
}
uint16_t MotorControl::loop() {
uint32_t time = millis();
uint16_t elapsed_time = time - this->lastMillis;
//Cancel if delay is not reached
if (elapsed_time < delay)
//Cancel if delayLoop is not reached
if (elapsed_time < delayLoop)
return elapsed_time;
runMotorControl();
@@ -48,14 +48,14 @@ uint16_t MotorControl::loop() {
}
void MotorControl::runMotorControl() {
// Absolute difference between target_power and power
uint8_t abs_difference = abs(this->target_power - this->power);
// Absolute difference between targetPower and power
uint8_t abs_difference = abs(this->targetPower - this->power);
// Difference between target_power and power
int16_t difference = this->target_power - this->power;
// Difference between targetPower and power
int16_t difference = this->targetPower - this->power;
// Check that the target speed is close to 0 and that the abs_difference is lower than powersteps
if (abs(this->target_power) < powersteps && abs_difference < powersteps) {
if (abs(this->targetPower) < powersteps && abs_difference < powersteps) {
this->setRealPower(0);
return;
}
@@ -66,7 +66,7 @@ void MotorControl::runMotorControl() {
}
// Positive or negative tagret speed
if (this->target_power >= 0) {
if (this->targetPower >= 0) {
// Positive or negative speed
if (this->power >= 0) {
if (difference > 0) {
@@ -95,36 +95,36 @@ void MotorControl::runMotorControl() {
void MotorControl::setMinPwm(uint8_t min) {
if (min > 80) min = 80;
//transform percentage to real pwm value
min = (uint8_t) (((1 << pwm_res) - 1) * (min / 100.0));
this->dutycycle_min = min;
min = (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 << pwm_res) - 1) * (max / 100.0));
this->dutycycle_max = max;
max = (uint8_t) (((1 << pwmRes) - 1) * (max / 100.0));
this->dutycycleMax = max;
}
uint16_t MotorControl::setPowerSteps(uint8_t increment) {
this->powersteps = increment;
return (uint16_t) (delay * ( 100 / powersteps ));
return (uint16_t) (delayLoop * ( 100 / powersteps ));
}
void MotorControl::setTargetPower(int8_t power) {
if (power <= 100 && power >= -100)
this->target_power = power;
this->targetPower = power;
else
std::cout << " MotorControl::setTargetPower: Invalid Argument - Power: " << power << std::endl;
}
uint16_t MotorControl::setDelay(uint8_t delay) {
this->delay = delay;
return (uint16_t) (delay * ( 100 / powersteps ));
uint16_t MotorControl::setDelay(uint8_t delayLoop) {
this->delayLoop = delayLoop;
return (uint16_t) (delayLoop * ( 100 / powersteps ));
}
void MotorControl::stop() {
this->target_power = 0;
this->targetPower = 0;
}
void MotorControl::emergencyStop() {
@@ -136,23 +136,23 @@ int8_t MotorControl::getPower() {
}
int8_t MotorControl::getTargetPower() {
return this->target_power;
return this->targetPower;
}
bool MotorControl::isTargetPowerReached() {
if (this->target_power == this->power)
if (this->targetPower == this->power)
return true;
return false;
}
bool MotorControl::isAccelerationPositive() {
if (power < target_power)
if (power < targetPower)
return true;
return false;
}
bool MotorControl::isAccelerationNegative() {
if (power > target_power)
if (power > targetPower)
return true;
return false;
}
@@ -169,12 +169,12 @@ void MotorControl::setRealPower(int8_t power) {
this->direction = 0;
digitalWrite(this->dir_1, LOW);
digitalWrite(this->dir_2, LOW);
ledcWrite(this->pwm_channel, 0);
ledcWrite(this->pwmChannel, 0);
this->dutycycle = 0;
return;
}
uint8_t pwm_val = map(abs(power), 0, 100, this->dutycycle_min, this->dutycycle_max);
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
this->direction = 2;
@@ -186,7 +186,7 @@ void MotorControl::setRealPower(int8_t power) {
digitalWrite(this->dir_2, LOW);
}
ledcWrite(this->pwm_channel, pwm_val);
ledcWrite(this->pwmChannel, pwm_val);
this->dutycycle = pwm_val;
}
lib/MotorControl/motorControl.h
+18 -18
View File
@@ -21,7 +21,7 @@
#define PWMRES 8
#define POWERSTEPS 2 // A total of 20 levels ( 100 / SPEED_STEPS ) * RUN_MOTOR_CONTROL_DELAY = 500ms
#define PWMMIN 55
#define PWMMAX 80 // Max 98% of 2^PWM_RES
#define PWMMAX 94 // Max 98% of 2^PWM_RES
/**
* @brief A class which use PWM to control the power of DC Motor
@@ -35,17 +35,17 @@ class MotorControl {
/**
* @brief Initialize the motorController
*
* @param pwm_pin The output pin for the signal on the esp.
* @param pwm_channel One of the pwm channels from the esp.
* @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 pwm_pin, uint8_t pwm_channel, uint8_t dir_1, uint8_t dir_2);
void init(uint8_t pwmPin, uint8_t pwmChannel, uint8_t dir_1, uint8_t dir_2);
/**
* @brief Calls runMotorControl() to update the pwm signal
*
* This function should be called every mainloop. If the delay is not reached, than the
* This function should be called every mainloop. If the delayLoop is not reached, than the
* functions returns immediately.
* @see runMotorControl()
* @see DELAY
@@ -81,10 +81,10 @@ class MotorControl {
*
* Set the increment of the steps with which the dutycycle is
* increased or decreased. Note the dependency between the increment
* and delay().
* and delayLoop().
*
* The formula for the time between 0% and 100% power is:
* time[ms] = delay * ( 100 / increment )
* time[ms] = delayLoop * ( 100 / increment )
* 500 ms are recommended
*
* @see setDelay()
@@ -105,17 +105,17 @@ class MotorControl {
void setTargetPower(int8_t power);
/**
* @brief Set the min delay between each loop
* @brief Set the min delayLoop between each loop
*
* Note the dependency between delay and
* Note the dependency between delayLoop and
* setPowerSteps().
*
* @see setPowerSteps()
*
* @param delay time in Milliseconds
* @param delayLoop time in Milliseconds
* @return time from 0% power to 100% power in Milliseconds
*/
uint16_t setDelay(uint8_t delay);
uint16_t setDelay(uint8_t delayLoop);
/**
* @brief Stops the motor like setTargetPower() to 0
@@ -152,19 +152,19 @@ class MotorControl {
void setRealPower(int8_t power);
void increasePower(int8_t power);
int8_t target_power = 0;
int8_t targetPower = 0;
int8_t power = 0;
uint8_t direction = 0; // 0 = stop, 1 = forward, 2 = backward
uint8_t pwm_pin;
uint8_t pwm_channel;
uint8_t pwm_res = PWMRES;
uint8_t pwmPin;
uint8_t pwmChannel;
uint8_t pwmRes = PWMRES;
uint16_t dutycycle = 0;
uint8_t dutycycle_min;
uint8_t dutycycle_max;
uint8_t dutycycleMin;
uint8_t dutycycleMax;
uint8_t dir_1;
uint8_t dir_2;
uint8_t delay = DELAY;
uint8_t delayLoop = DELAY;
uint8_t powersteps = POWERSTEPS;
uint32_t lastMillis = 0;
lib/Navigation/navigation.cpp
+89 -18
View File
@@ -98,14 +98,11 @@ void Navigation::loop() {
this->ntripClient->loop();
if (millis() - this->lastMillis > AZIMUTH_UPDATE_DELAY) {
// if (millis() - this->lastMillis > 500) {
this->compass->read();
this->azimuth = this->compass->getAzimuth();
// std::cout << "Compass x: " << compass->getX()
// << " y: " << compass->getY()
// << " z: " << compass->getZ()
// << " Azi: " << compass->getAzimuth()
// << std::endl;
this->realAzimuth = this->compass->getAzimuth();
this->updateMagneticDeclination();
this->lastMillis = millis();
}
}
@@ -124,6 +121,15 @@ bool Navigation::startNavigation() {
return this->navigationStarted;
}
void Navigation::drivingDirectionChange() {
Point tmp = this->currentPosition;
if (tmp.isInit() && tmp.isValid()) {
this->directionChangeMode = true;
this->lastPointDrivingDirectionChange = tmp;
this->calcAzimuthState = CalcAzimuthState::Invalid;
}
}
Navigation::Status Navigation::getCourseCorrection(CourseCorrection& correction, bool forceUpdate) {
if (this->navigationFinished)
return Status::Complete;
@@ -134,6 +140,7 @@ Navigation::Status Navigation::getCourseCorrection(CourseCorrection& correction,
if (this->currentPosition.distanceTo(this->lastPointCalcCorrection) < (this->minDistanceToReachPoint / 2.0)
&& !forceUpdate) {
correction.correction = this->calculateCourseCorrection(this->lastPointCalcCorrection);
correction.distance = this->lastPointCalcCorrection.distanceTo(this->targetPoint);
return Status::Unchanged;
}
@@ -189,21 +196,75 @@ void Navigation::updateCurrentLocation() {
this->currentPosition = Point(coords, this->ubxData->hAcc);
}
void Navigation::updateMagneticDeclination() {
if (!this->directionChangeMode
|| this->lastPointDrivingDirectionChange.distanceTo(this->currentPosition) < 1.0)
{
this->calcAzimuthState = CalcAzimuthState::Invalid;
this->calcAzimuth = 999;
return;
}
this->calcAzimuth = this->lastPointDrivingDirectionChange.courseTo(this->currentPosition);
// Map point accuracy to CalcAzimuthState
if (this->lastPointDrivingDirectionChange.getAccuracy() == Point::Accuracy::oneDigOfCM
|| this->currentPosition.getAccuracy() == Point::Accuracy::oneDigOfCM)
{
this->calcAzimuthState = CalcAzimuthState::Good;
}
else if (this->lastPointDrivingDirectionChange.getAccuracy() == Point::Accuracy::twoDigOfCM
|| this->currentPosition.getAccuracy() == Point::Accuracy::twoDigOfCM)
{
this->calcAzimuthState = CalcAzimuthState::Ok;
}
else if (this->lastPointDrivingDirectionChange.getAccuracy() == Point::Accuracy::threeDigOfCM
|| this->currentPosition.getAccuracy() == Point::Accuracy::threeDigOfCM)
{
this->calcAzimuthState = CalcAzimuthState::Bad;
}
else
{
this->calcAzimuthState = CalcAzimuthState::Invalid;
}
// Upgrade quality if the range grows up
if (this->lastPointDrivingDirectionChange.distanceTo(this->currentPosition) > 2.0) {
switch (this->calcAzimuthState) {
case CalcAzimuthState::Bad :
this->calcAzimuthState = CalcAzimuthState::Ok;
break;
case CalcAzimuthState::Ok :
this->calcAzimuthState = CalcAzimuthState::Good;
break;
case CalcAzimuthState::Good :
this->calcAzimuthState = CalcAzimuthState::Super;
break;
default:
break;
}
}
}
int16_t Navigation::calculateCourseCorrection(Point& point) {
int16_t targetCourse = point.courseTo(this->targetPoint);
// correction = targetCourse - currentCourse
int16_t signedAzimuth = this->azimuth > 180 ? this->azimuth - 360 : this->azimuth;
int16_t correctionCourse = targetCourse - signedAzimuth;
if (correctionCourse > 180)
correctionCourse -= 360;
else if (correctionCourse < -180)
correctionCourse += 360;
int16_t correctionCourse;
//Rotate result by 180° to corrigate Azimuth
correctionCourse += 180;
correctionCourse = correctionCourse > 180 ? correctionCourse -360 : correctionCourse;
if (this->calcAzimuthState == CalcAzimuthState::Good
|| this->calcAzimuthState == CalcAzimuthState::Super)
{
correctionCourse = targetCourse - this->calcAzimuth;
this->lastUsedCalcAzimuth = true;
} else {
correctionCourse = targetCourse - this->realAzimuth;
this->lastUsedCalcAzimuth = false;
}
return correctionCourse;
return Navigation::fixDegree(correctionCourse);
}
bool Navigation::nextPoint() {
@@ -224,6 +285,16 @@ void Navigation::setOutputStatusPrintPVTdata(bool status) {
Navigation::outputStatusPrintPVTdata = status;
}
int16_t Navigation::fixDegree(int16_t degree) {
while (degree < -180 || degree > 180) {
if (degree > 180)
degree -= 360;
else if (degree < -180)
degree += 360;
}
return degree;
}
void Navigation::printPVTdata(UBX_NAV_PVT_data_t *ubxDataStruct) {
if (!Navigation::outputStatusPrintPVTdata)
return;
lib/Navigation/navigation.h
+32 -22
View File
@@ -62,6 +62,14 @@ class Navigation {
Complete
};
enum CalcAzimuthState {
Invalid,
Bad,
Ok,
Good,
Super
};
/**
* @brief Construct a new Navigation object and using I2C
@@ -118,6 +126,8 @@ class Navigation {
*/
bool startNavigation();
void freezeTargetPoint(bool val = true) { this->preventNextPoint = val; };
void drivingDirectionChange();
void dissableCalcAzimuth() { this->directionChangeMode = false; }
double increaseMinDistanceToReachPoint() { return this->minDistanceToReachPoint += 0.1; }
double decreaseMinDistanceToReachPoint() { return this->minDistanceToReachPoint -= 0.1; }
@@ -178,15 +188,21 @@ class Navigation {
Point getCurrentPosition() const { return this->currentPosition; }
/**
* @brief Get azimuth
* @brief Get realAzimuth
*
* This value represents the angle between north and
* the line of sight. Clockwise.
*
* @return uint16_t degree
*/
uint16_t getAzimuth() const { return this->azimuth; }
int16_t getAzimuth() const { return this->realAzimuth; }
QMC5883LCompass* getCompass() const { return this->compass; }
int16_t getCalcAzimuth() const { return this->calcAzimuth; }
CalcAzimuthState getCalcAzimuthState() const { return this->calcAzimuthState; }
bool getLastUsedCalcAzimuth() const { return this->lastUsedCalcAzimuth; }
Point::Accuracy getMinAccuracy() const { return this->minAccuracy; }
void setMinAccuracy(Point::Accuracy accuracy) { this->minAccuracy = accuracy; }
/**
* @brief Set the output status for PVTdata.
@@ -197,29 +213,16 @@ class Navigation {
* @param status
*/
static void setOutputStatusPrintPVTdata(bool status);
// map input in range from -180 to 180 degree
static int16_t fixDegree(int16_t degree);
private:
void updateCurrentLocation();
int16_t calculateCourseCorrection(Point& point);
/**
* @brief Set the next point as target
*
* @return true
* @return false
*/
bool nextPoint();
/**
* @brief Set the target point
*
* @param target
* @return true
* @return false
*/
bool setTargetPoint(Point target);
void updateMagneticDeclination();
void init(Route* route);
bool nextPoint();
bool setTargetPoint(Point target);
int16_t calculateCourseCorrection(Point& point);
SFE_UBLOX_GNSS* gps;
UBX_NAV_PVT_data_t* ubxData = nullptr;
@@ -229,24 +232,31 @@ class Navigation {
Point lastPointRouteInsert;
Point lastPointCalcCorrection;
Point lastPointDrivingDirectionChange;
Point targetPoint;
Point currentPosition;
Point::Accuracy minAccuracy = Point::Accuracy::twoDigOfCM;
CalcAzimuthState calcAzimuthState = CalcAzimuthState::Invalid;
bool navigationStarted = false;
bool navigationFinished = false;
bool isNtripInit = false;
bool preventNextPoint = false;
bool directionChangeMode = false;
bool lastUsedCalcAzimuth = false;
char* host;
char* mountPoint;
char* user;
char* password;
int16_t realAzimuth = INT16_MAX;
int16_t calcAzimuth = INT16_MAX;
uint8_t timeToWait = 200;
uint16_t port;
uint16_t azimuth = UINT16_MAX;
uint32_t lastMillis = 0;
uint32_t ubxUpdateTime = 0;
lib/NtripClient/ntripClient.cpp
+1 -1
View File
@@ -221,7 +221,7 @@ void NTRIPClient::closeConnection() {
bool NTRIPClient::processConnection() {
if (this->ntripClient->connected()) {
uint8_t rtcmData[this->bufferSize * 4];
uint8_t rtcmData[this->bufferSize * 8];
uint16_t rtcmCount = 0;
while (this->ntripClient->available()) {
lib/NtripClient/ntripClient.h
+1 -1
View File
@@ -129,7 +129,7 @@ class NTRIPClient {
const uint8_t delayTime = 20;
const uint8_t maxReconnectAttemps = 10;
const uint16_t reconnectDelayTime = 1000;
const uint16_t timeOut = 5000;
const uint16_t timeOut = 10000;
const uint16_t bufferSize = 512;
const uint16_t pushGPGGATime = 10000;
};
lib/Navigation/route.cpp → lib/Route/route.cpp
+12 -12
View File
@@ -11,35 +11,35 @@
#include "route.h"
Point::Point(double lat, double lon, uint32_t horizontalAccuracy) {
Point::Point(double lat, double lon, uint32_t horizontalAccuracy, uint32_t creationTime) {
this->coordinates.lat = lat;
this->coordinates.lon = lon;
this->init(horizontalAccuracy);
this->init(horizontalAccuracy, creationTime);
}
Point::Point(int32_t lat, int32_t lon, uint32_t horizontalAccuracy) {
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;
this->init(horizontalAccuracy);
this->init(horizontalAccuracy, creationTime);
}
Point::Point(Coordinates coords, uint32_t horizontalAccuracy) {
Point::Point(Coordinates coords, uint32_t horizontalAccuracy, uint32_t creationTime) {
this->coordinates = coords;
this->init(horizontalAccuracy);
this->init(horizontalAccuracy, creationTime);
}
Point::Point(Coordinates coords, bool imported) {
this->coordinates = coords;
if (imported)
this->init(UINT32_MAX);
this->init(UINT32_MAX, 0);
else
this->init(0);
this->init(0, 0);
}
Point::Point() {
this->coordinates.lat = 0;
this->coordinates.lon = 0;
this->init(0);
this->init(0, 0);
}
bool Point::operator==(const Point& rhs) const {
@@ -74,7 +74,7 @@ int16_t Point::courseTo(const Coordinates& point) const {
double phi = log( tan(end.lat * ROUTE_DEGREE_TO_RADIANT / 2 + ROUTE_PI / 4) / tan(begin.lat * ROUTE_DEGREE_TO_RADIANT / 2 + ROUTE_PI / 4) );
double lon = (begin.lon * ROUTE_DEGREE_TO_RADIANT - end.lon * ROUTE_DEGREE_TO_RADIANT);
int16_t res = (int16_t) atan2(lon, phi) / ROUTE_DEGREE_TO_RADIANT;
int16_t res = static_cast<int16_t>(atan2(lon, phi) / ROUTE_DEGREE_TO_RADIANT) * -1;
// if (res < 0)
// res += 360;
@@ -86,8 +86,8 @@ int16_t Point::courseTo(const Point &point) const {
return this->courseTo(point.getCoordinates());
}
void Point::init(uint32_t horizontalAccuracy) {
this->creationTime = millis();
void Point::init(uint32_t horizontalAccuracy, uint32_t creationTime) {
this->creationTime = creationTime;
if (horizontalAccuracy == UINT32_MAX)
this->accuracy = Accuracy::imported;
lib/Navigation/route.h → lib/Route/route.h
+4 -6
View File
@@ -12,8 +12,6 @@
#ifndef ROUTE_H
#define ROUTE_H
#include <Arduino.h>
#include <cstdint>
#include <list>
#include <cmath>
@@ -66,9 +64,9 @@ class Point{
* @param coords Coordinates
* @param imported if true than highest accuracy
*/
Point(double lat, double lon, uint32_t horizontalAccuracy = 0);
Point(int32_t lat, int32_t lon, uint32_t horizontalAccuracy = 0);
Point(Coordinates coords, uint32_t horizontalAccuracy = 0);
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();
@@ -133,7 +131,7 @@ class Point{
Accuracy getAccuracy() { return this->accuracy; }
private:
void init(uint32_t horizontalAccuracy);
void init(uint32_t horizontalAccuracy, uint32_t creationTime);
Accuracy accuracy = Accuracy::none;
Coordinates coordinates;
lib/calibrateCompass/calibrateCompass.cpp
+2 -2
View File
@@ -73,7 +73,7 @@ void CalibrateCompass::start() {
this->clearData();
this->state = State::Calibrating;
this->compass->removeCalibration();
this->compass->clearCalibration();
this->lastChange = millis();
}
@@ -95,7 +95,7 @@ void CalibrateCompass::useData() {
}
void CalibrateCompass::removeCalibration() {
this->compass->removeCalibration();
this->compass->clearCalibration();
}
void CalibrateCompass::reset() {
lib/calibrateCompass/calibrateCompass.h
+1 -1
View File
@@ -52,6 +52,6 @@ class CalibrateCompass {
void clearData();
bool dataValid = false;
const uint16_t maxTimeWithoutChange = 5000;
const uint16_t maxTimeWithoutChange = 10000;
uint32_t lastChange = 0;
};
platformio.ini
+10 -7
View File
@@ -8,7 +8,7 @@
; Please visit documentation for the other options and examples
; https://docs.platformio.org/page/projectconf.html
[env]
[env:embedded]
platform = espressif32
board = esp32doit-devkit-v1
board_build.partitions = no_ota.csv
@@ -22,18 +22,21 @@ lib_deps =
br3ttb/PID@^1.2.1
marcoschwartz/LiquidCrystal_I2C@^1.1.4
bblanchon/ArduinoJson@^6.20.0
mprograms/QMC5883LCompass@^1.1.1
mprograms/QMC5883LCompass@^1.2.0
https://git.kleiax.de/PlatformIO-Libs/Menu.git
nrf24/RF24@^1.4.5
upload_port = COM3
[env:release]
build_type = release
[env:debug]
extra_scripts =
pre:autoVersionIncrement/version_increment_pre.py
post:autoVersionIncrement/version_increment_post.py
test_ignore = test_desktop
build_type = debug
monitor_filters = esp32_exception_decoder
check_tool = clangtidy
[env:native]
platform = native
test_ignore = test_embedded
[platformio]
description = A Rover who should be drive a route by gps.
src/SpecialMenus/Route/menuRoute.cpp
+56 -14
View File
@@ -83,18 +83,13 @@ void MenuRoute::init() {
this->mainMenu = new Menu;
MenuRoutePoints* pointsMenu = new MenuRoutePoints(this->route);
Menu* importMenu = new Menu;
Menu* exportMenu = new Menu;
Menu* clearMenu = new Menu;
this->mainMenu->setLcd(this->lcd);
pointsMenu->setLcd(this->lcd);
importMenu->setLcd(this->lcd);
exportMenu->setLcd(this->lcd);
clearMenu->setLcd(this->lcd);
MenuIntInput* importWrapper = new MenuIntInput(1, new MenuRouteWrapper(this, &MenuRoute::importRoute));
MenuIntInput* exportWrapper = new MenuIntInput(1, new MenuRouteWrapper(this, &MenuRoute::exportRoute));
MenuIntInput* deleteWrapper = new MenuIntInput(1, new MenuRouteWrapper(this, &MenuRoute::deleteRoute));
MenuActionRoute* clearWrapper = new MenuActionRoute(this, &MenuRoute::clearRoute);
importWrapper->setLcd(this->lcd);
@@ -107,15 +102,16 @@ void MenuRoute::init() {
exportWrapper->setPrintParentMenu(false);
exportWrapper->setEntry(0, "Export Route");
MenuAction* pointsAction = new MenuAction("Points", pointsMenu);
MenuAction* importAction = new MenuAction("Import", importWrapper);
MenuAction* exportAction = new MenuAction("Export", exportWrapper);
MenuAction* clearAction = new MenuAction("Clear", clearWrapper);
deleteWrapper->setLcd(this->lcd);
deleteWrapper->setMinMax(0, 100);
deleteWrapper->setPrintParentMenu(false);
deleteWrapper->setEntry(0, "Delete Route");
this->mainMenu->addEntry(pointsAction);
this->mainMenu->addEntry(importAction);
this->mainMenu->addEntry(exportAction);
this->mainMenu->addEntry(clearAction);
this->mainMenu->addEntry(new MenuAction("Points", pointsMenu));
this->mainMenu->addEntry(new MenuAction("Clear", clearWrapper));
this->mainMenu->addEntry(new MenuAction("Import", importWrapper));
this->mainMenu->addEntry(new MenuAction("Export", exportWrapper));
this->mainMenu->addEntry(new MenuAction("Delete", deleteWrapper));
}
void MenuRoute::importRoute(uint8_t routeNumber) {
@@ -236,6 +232,52 @@ void MenuRoute::exportRoute(uint8_t routeNumber) {
http.end();
}
void MenuRoute::deleteRoute(uint8_t routeNumber) {
this->blockInput = true;
String lineOne = "";
String lineTwo = "";
if (WiFi.status() != WL_CONNECTED) {
lineOne = "Not connected to";
lineTwo = "the WiFi.";
this->print(lineOne, lineTwo);
this->blockInput = false;
return;
}
if (ESP.getMaxAllocHeap() < JSON_DOCUMENT_SIZE_ROUTE) {
lineOne = "Not enough mem";
lineTwo = "for Json obj";
this->print(lineOne, lineTwo);
this->blockInput = false;
return;
}
WiFiClient client;
HTTPClient http;
DynamicJsonDocument doc(JSON_DOCUMENT_SIZE_ROUTE);
String host = "http://rover.kleiax.de/api/";
host.concat(routeNumber);
http.begin(client, host);
int httpResponseCode = http.sendRequest("DELETE");
lineOne = "Delete complete";
lineTwo = "Code: ";
if (httpResponseCode == 202)
deserializeJson(doc, http.getStream());
else
lineOne = "HTTP Error";
lineTwo.concat(httpResponseCode);
this->print(lineOne, lineTwo);
this->blockInput = false;
http.end();
}
void MenuRoute::clearRoute(uint8_t none) {
this->route->clear();
this->print("Currente route", "deleted...");
src/SpecialMenus/Route/menuRoute.h
+2
View File
@@ -109,6 +109,8 @@ class MenuRoute : public MenuControl {
*/
void exportRoute(uint8_t routeNumber);
void deleteRoute(uint8_t routeNumber);
/**
* @brief Delete the current Route.
*
src/SpecialMenus/Systeminformation/menuSysteminformatio.cpp
+17 -1
View File
@@ -12,7 +12,7 @@
#include "menuSysteminformation.h"
MenuSysteminformation::MenuSysteminformation(Battery* mainBattery)
: MenuInformationSites(5) {
: MenuInformationSites(8) {
this->mainBattery = mainBattery;
}
@@ -44,6 +44,22 @@ void MenuSysteminformation::printPage() const {
break;
case 4:
lineOne = "Current WiFi";
lineTwo = "channel: ";
lineTwo.concat(Network::getCurrentChannel());
break;
case 5:
lineOne = "Software verion:";
lineTwo = VERSION;
break;
case 6:
lineOne = "Build timestamp:";
lineTwo = String(BUILD_TIMESTAMP).substring(0, 16);
break;
case 7:
lineOne = "Kleiax Rover by";
lineTwo = "Alexander Klein";
break;
src/SpecialMenus/Systeminformation/menuSysteminformation.h
+3
View File
@@ -14,9 +14,12 @@
#include <Arduino.h>
#include "Version.h"
#include "controlPadInput.h"
#include "menuInformationSites.h"
#include "battery.h"
#include "network.h"
/**
* @brief Prints information about the current system status.
src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp
+88 -2
View File
@@ -43,7 +43,7 @@ void MenuAutopilot::printPage() const {
lineTwo = "";
switch (this->autopilot->getState()) {
case Autopilot::State::InsufficientAccuarcy :
lineTwo = "Err: No NTRIP";
lineTwo = "Err: LowAccuracy";
break;
case Autopilot::State::NoRoute :
@@ -66,6 +66,10 @@ void MenuAutopilot::printPage() const {
lineTwo = "Autopilot active";
break;
case Autopilot::State::SelfDrivingRotate :
lineTwo = "Rotating";
break;
case Autopilot::State::TargetReached :
lineTwo = "Target reached";
break;
@@ -158,6 +162,65 @@ void MenuAutopilot::printPage() const {
lineTwo.concat(" - Decrease");
break;
case 10:
lineOne = "Current minimal";
if (this->driveManager->getNavigation()->getMinAccuracy() >= Point::Accuracy::twoDigOfCM)
lineTwo = "accuracy is high";
else
lineTwo = "accuracy is low";
break;
case 11:
lineOne = "CalcAzi: ";
lineTwo = "State: ";
lineOne.concat(this->driveManager->getNavigation()->getCalcAzimuth());
switch (this->driveManager->getNavigation()->getCalcAzimuthState()) {
case Navigation::CalcAzimuthState::Bad :
lineTwo.concat("Bad");
break;
case Navigation::CalcAzimuthState::Good :
lineTwo.concat("Good");
break;
case Navigation::CalcAzimuthState::Invalid :
lineTwo.concat("Invalid");
break;
case Navigation::CalcAzimuthState::Ok :
lineTwo.concat("Ok");
break;
case Navigation::CalcAzimuthState::Super :
lineTwo.concat("Super");
break;
default:
lineTwo.concat("Unkown");
break;
}
break;
case 12:
lineOne = "Test rotate";
lineTwo = "180 degree";
break;
case 13:
lineOne = "Test rotate";
lineTwo = "270 degree";
break;
case 14:
lineOne = "Test rotate";
lineTwo = "45 degree";
break;
case 15:
lineOne = "Test rotate";
lineTwo = "20 degree";
break;
default:
this->printDefault();
return;
@@ -190,6 +253,29 @@ void MenuAutopilot::runCommand() const {
this->minDistance = this->driveManager->getNavigation()->decreaseMinDistanceToReachPoint();
break;
case 10:
if (this->driveManager->getNavigation()->getMinAccuracy() >= Point::Accuracy::twoDigOfCM)
this->driveManager->getNavigation()->setMinAccuracy(Point::Accuracy::none);
else
this->driveManager->getNavigation()->setMinAccuracy(Point::Accuracy::twoDigOfCM);
break;
case 12:
this->autopilot->testRotate(180);
break;
case 13:
this->autopilot->testRotate(270);
break;
case 14:
this->autopilot->testRotate(45);
break;
case 15:
this->autopilot->testRotate(20);
break;
default:
break;
}
@@ -204,7 +290,7 @@ void MenuAutopilot::update() {
}
void MenuAutopilot::init() {
this->setCountPages(10);
this->setCountPages(16);
this->driveManager->changeModus(Modi::Autopilot);
this->autopilot = (Autopilot*) this->driveManager->getDriveModiPtr();
this->routeInfo = this->autopilot->getRouteInfo();
src/SpecialMenus/driveModi/CalibrateCompass/menuCalibrateCompass.cpp
+12 -4
View File
@@ -77,20 +77,25 @@ void MenuCalibrateCompass::printPage() const {
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 8:
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 9:
case 10:
lineOne = "Z min: ";
lineOne.concat(this->caliCompass->getCallibrationData().data[2][0]);
lineTwo = "Z max: ";
@@ -109,7 +114,7 @@ void MenuCalibrateCompass::init() {
this->firstPrint = false;
this->driveManager->changeModus(Modi::ManualControl);
this->manualControl = (ManualControl*) this->driveManager->getDriveModiPtr();
this->setCountPages(10);
this->setCountPages(11);
this->updateDelay = 500;
}
@@ -125,7 +130,6 @@ void MenuCalibrateCompass::runCommand() const {
case CalibrateCompass::State::Finished:
this->manualControl->setCalibrateCompass();
this->caliCompass->useData();
this->caliCompass->reset();
break;
default:
@@ -149,6 +153,10 @@ void MenuCalibrateCompass::runCommand() const {
this->caliCompass->useData();
break;
case 7:
this->caliCompass->reset();
break;
default:
break;
}
src/SpecialMenus/driveModi/CaptureRoute/menuCaptureRoute.cpp
+24 -1
View File
@@ -92,6 +92,14 @@ void MenuCaptureRoute::printPage() const {
lineOne.concat("0");
break;
case 7:
lineOne = "Current minimal";
if (this->driveManager->getNavigation()->getMinAccuracy() >= Point::Accuracy::twoDigOfCM)
lineTwo = "accuracy is high";
else
lineTwo = "accuracy is low";
break;
default:
this->printDefault();
return;
@@ -106,8 +114,23 @@ void MenuCaptureRoute::update() {
this->printMenu();
}
void MenuCaptureRoute::runCommand() const {
switch (this->getCurrentPage())
{
case 7:
if (this->driveManager->getNavigation()->getMinAccuracy() >= Point::Accuracy::twoDigOfCM)
this->driveManager->getNavigation()->setMinAccuracy(Point::Accuracy::none);
else
this->driveManager->getNavigation()->setMinAccuracy(Point::Accuracy::twoDigOfCM);
break;
default:
break;
}
}
void MenuCaptureRoute::init() {
this->setCountPages(7);
this->setCountPages(8);
this->updateDelay = 500;
this->driveManager->changeModus(Modi::CaptureRoute);
this->captureRoute = (CaptureRoute*) this->driveManager->getDriveModiPtr();
src/SpecialMenus/driveModi/CaptureRoute/menuCaptureRoute.h
+2
View File
@@ -42,6 +42,8 @@ class MenuCaptureRoute : public MenuDriveMode {
*/
void update() override;
void runCommand() const override;
private:
void init() override;
src/SpecialMenus/driveModi/ManualDrive/menuManualDrive.cpp
+40 -15
View File
@@ -27,44 +27,66 @@ void MenuManualControl::printPage() const {
break;
case 1:
lineOne = "Speed: ";
lineOne.concat(this->manualControl->getMaxSpeed());
lineTwo = "Increase by 0.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 = "Decrease by 0.1";
lineTwo = "Increase by 0.1";
break;
case 3:
lineOne = "RotSpeed: ";
lineOne.concat(this->manualControl->getMaxRotation());
lineTwo = "Increase by 0.1";
lineOne = "Speed: ";
lineOne.concat(this->manualControl->getMaxSpeed());
lineTwo = "Decrease by 0.1";
break;
case 4:
lineOne = "RotSpeed: ";
lineOne.concat(this->manualControl->getMaxRotation());
lineTwo = "Decrease by 0.1";
lineTwo = "Increase by 0.1";
break;
case 5:
lineOne = "RotSpeed: ";
lineOne.concat(this->manualControl->getMaxRotation());
lineTwo = "Decrease by 0.1";
break;
case 6:
lineOne = "Dutycycle Left:";
lineTwo.concat(this->manualControl->getDutycycleLeft());
break;
case 6:
case 7:
lineOne = "Dutycycle Right:";
lineTwo.concat(this->manualControl->getDutycycleRight());
break;
case 7:
case 8:
lineOne = "Azimuth:";
lineTwo.concat(this->driveManager->getNavigation()->getAzimuth());
break;
case 9: {
UBX_NAV_PVT_data_t* gpsData = this->driveManager->getNavigation()->getUbxData();
lineOne = "magDec: ";
lineTwo = "magAcc: ";
if (gpsData->valid.bits.validMag) {
lineOne.concat(gpsData->magDec);
lineTwo.concat(gpsData->magAcc);
} else {
lineOne.concat("invalid");
lineTwo.concat("invalid");
}
break;
}
default:
this->printDefault();
return;
@@ -77,29 +99,32 @@ void MenuManualControl::init() {
this->firstPrint = false;
this->driveManager->changeModus(Modi::ManualControl);
this->manualControl = (ManualControl*) this->driveManager->getDriveModiPtr();
this->setCountPages(8);
this->setCountPages(10);
this->updateDelay = 500;
}
void MenuManualControl::runCommand() const {
switch (this->getCurrentPage()) {
case 1:
this->manualControl->increaseMaxSpeed();
this->manualControl->switchInputMode();
break;
case 2:
this->manualControl->decreaseMaxSpeed();
this->manualControl->increaseMaxSpeed();
break;
case 3:
this->manualControl->increaseMaxRotation();
this->manualControl->decreaseMaxSpeed();
break;
case 4:
this->manualControl->increaseMaxRotation();
break;
case 5:
this->manualControl->increaseMaxRotation();
break;
default:
break;
}
src/driveModi/Modi/Autopilot/autopilot.cpp
+79 -12
View File
@@ -12,13 +12,31 @@
#include "driveModi/Modi/Autopilot/autopilot.h"
#include "autopilot.h"
DirectionChangeSignal::DirectionChangeSignal(Navigation* navigation) {
this->navigation = navigation;
this->action();
}
DirectionChangeSignal::~DirectionChangeSignal() {
navigation->dissableCalcAzimuth();
}
void DirectionChangeSignal::action() {
this->navigation->drivingDirectionChange();
}
Autopilot::Autopilot(MoveControl* moveControl, const ControlPadInput *input, Navigation* navigation)
: ManualControl(moveControl, input) {
this->setInputMode(ManualControl::InputMode::Digital);
this->directionChangeSignal = new DirectionChangeSignal(navigation);
this->setDirectionChangeCallback(this->directionChangeSignal);
this->navigation = navigation;
this->init();
}
Autopilot::~Autopilot() {
delete this->directionChangeSignal;
// this->navigation->getNTRIPClient()->setActivated(false);
}
@@ -53,6 +71,7 @@ void Autopilot::runAutopilot() {
case State::NavigationStarted:
this->askNavigationForOrder();
this->checkButtonInput();
break;
case State::GetToStartPoint:
@@ -72,6 +91,11 @@ void Autopilot::runAutopilot() {
this->selfDriving();
break;
case SelfDrivingRotate:
this->checkButtonInput();
this->rotate();
break;
case State::TargetReached:
break;
@@ -92,6 +116,14 @@ bool Autopilot::shouldUpdate() {
return false;
}
void Autopilot::testRotate(int16_t degree) {
if (!degree)
return;
this->courseCorrection.correction = degree;
this->beginRotate();
}
void Autopilot::init() {
if (this->navigation->startNavigation())
this->state = State::NavigationStarted;
@@ -115,29 +147,63 @@ void Autopilot::drive() {
this,moveControl->setSpeed(0);
}
void Autopilot::rotate() {
void Autopilot::beginRotate() {
if (this->state != State::SelfDrivingRotate) {
this->lastState = this->state;
this->state = State::SelfDrivingRotate;
this->rotationAimAzimuth = this->navigation->getAzimuth() + this->courseCorrection.correction;
this->rotationAimAzimuth = Navigation::fixDegree(this->rotationAimAzimuth);
this->moveControl->setSpeed(0);
if (this->courseCorrection.correction > 0)
this->moveControl->setRotationSpeed(this->rotationSpeed);
else
this->moveControl->setRotationSpeed(-this->rotationSpeed);
else
this->moveControl->setRotationSpeed(this->rotationSpeed);
}
}
void Autopilot::rotate() {
if (abs(this->navigation->getAzimuth() - this->rotationAimAzimuth) < 5) {
this->endRotate();
return;
}
if ((abs(this->navigation->getAzimuth() - this->rotationAimAzimuth) < 15)
&&
((this->courseCorrection.correction > 0
&& this->rotationAimAzimuth < this->navigation->getAzimuth())
|| (this->courseCorrection.correction < 0
&& this->rotationAimAzimuth > this->navigation->getAzimuth())))
{
this->endRotate();
std::cout << "Autopilot::rotate: Rover rotated too far" << std::endl;
}
}
void Autopilot::endRotate() {
if (this->state != State::SelfDrivingRotate)
return;
this->state = this->lastState;
this->navigation->drivingDirectionChange();
this->moveControl->setRotationSpeed(0);
this->moveControl->emergencyStop();
this->moveControl->setDrivingStatus(MoveControl::Status::Drive);
}
void Autopilot::checkButtonInput() {
if (ControlPadButton::isControlPadButtonPressed(this->input, ControlPadButton::PadButton::Action)
&& millis() - this->lastAutopilotChangeMillis > this->autopilotChangeDelayMillis) {
if (this->state == State::SelfDrivingAvailable) {
if (this->state == State::SelfDrivingAvailable)
this->state = State::SelfDriving;
this->updateDisplay = true;
this->lastAutopilotChangeMillis = millis();
} else if (this->state == State::SelfDriving) {
else if (this->state == State::SelfDriving || this->state == State::SelfDrivingRotate)
this->state = State::SelfDrivingAvailable;
else if (this->state == State::NavigationStarted)
this->state = State::GetToStartPoint;
this->updateDisplay = true;
this->lastAutopilotChangeMillis = millis();
}
}
}
void Autopilot::askNavigationForOrder() {
@@ -172,10 +238,11 @@ void Autopilot::askNavigationForOrder() {
}
void Autopilot::selfDriving() {
if (this->lastOrderStatus == Navigation::Status::Updated) {
if (this->state != State::SelfDriving)
return;
if (abs(this->courseCorrection.correction) >= this->maxCourseDeviationBerforeAct)
this->rotate();
this->beginRotate();
else
this->drive();
}
}
src/driveModi/Modi/Autopilot/autopilot.h
+19 -10
View File
@@ -17,6 +17,16 @@
#include "moveControl.h"
#include "driveModi/Modi/ManualControl/manualControl.h"
class DirectionChangeSignal : public DirectionChangeWrapper {
public:
DirectionChangeSignal(Navigation* navigation);
~DirectionChangeSignal();
void action() override;
private:
Navigation* navigation;
};
/**
* @brief This class use the navigate class to drive automaticaly
*
@@ -36,6 +46,7 @@ class Autopilot : public ManualControl {
GetToStartPoint,
SelfDrivingAvailable,
SelfDriving,
SelfDrivingRotate,
TargetReached
};
@@ -66,15 +77,6 @@ class Autopilot : public ManualControl {
*/
void loop();
/**
* @brief Manges the autopilot
*
* If the first Point is near to current location you can turn
* the autopilot on.
* Gets the course correction and decide what to do.
*/
void runAutopilot();
void restart();
/**
@@ -105,11 +107,15 @@ class Autopilot : public ManualControl {
* @return false
*/
bool shouldUpdate();
void testRotate(int16_t degree);
void endRotate();
private:
void init();
void drive();
void beginRotate();
void rotate();
void runAutopilot();
void checkButtonInput();
void askNavigationForOrder();
void selfDriving();
@@ -120,6 +126,7 @@ class Autopilot : public ManualControl {
State state = State::None;
State lastState = State::None;
Navigation::Status lastOrderStatus;
DirectionChangeSignal* directionChangeSignal;
bool updateDisplay = false;
@@ -131,8 +138,10 @@ class Autopilot : public ManualControl {
uint32_t loopLastMillis = 0;
uint32_t lastAutopilotChangeMillis = 0;
int16_t rotationAimAzimuth;
double drivingSpeed = 1;
double rotationSpeed = 3;
double rotationSpeed = 4.5;
double minRemainingDistance = 0.25;
};
src/driveModi/Modi/ManualControl/manualControl.cpp
+46 -3
View File
@@ -26,14 +26,30 @@ void ManualControl::loop() {
if (this->caliCompass)
this->caliCompass->loop();
if (millis() - this->lastMillis < delay) {
if (millis() - this->lastMillis < delay)
return;
switch (this->inputMode) {
case InputMode::Analog :
this->analogControl();
break;
case InputMode::Digital :
this->digitalControl();
break;
default:
break;
}
this->runManualControl();
this->lastMillis = millis();
}
void ManualControl::runManualControl() {
void ManualControl::switchInputMode() {
this->inputMode = (this->inputMode == InputMode::Analog) ? InputMode::Digital : InputMode::Analog;
}
void ManualControl::analogControl() {
// Have to be int16_t to avoid overflow (int8_t = 255 - 127 = -128)
int16_t y = this->input->x - 127;
int16_t x = this->input->y - 127;
@@ -49,3 +65,30 @@ void ManualControl::runManualControl() {
value_per_step = this->max_rotation * 2 / UINT8_MAX;
this->moveControl->setRotationSpeed(x * value_per_step);
}
void ManualControl::digitalControl() {
int16_t y = this->input->x - 127;
int16_t x = this->input->y - 127;
if (y > 120)
this->moveControl->setSpeed(-this->max_speed);
else if (y < -120)
this->moveControl->setSpeed(this->max_speed);
else
this->moveControl->setSpeed(0);
if (x > 120)
this->moveControl->setRotationSpeed(this->max_rotation);
else if (x < -120)
this->moveControl->setRotationSpeed(-this->max_rotation);
else
this->moveControl->setRotationSpeed(0);
if (this->directionChangeWrapper && (x > 120 || x < -120))
this->lastLoopTurned = true;
else if (this->lastLoopTurned) {
if (this->directionChangeWrapper)
this->directionChangeWrapper->action();
this->lastLoopTurned = false;
}
}
src/driveModi/Modi/ManualControl/manualControl.h
+27 -8
View File
@@ -16,6 +16,11 @@
#include "controlPadInput.h"
#include "calibrateCompass.h"
class DirectionChangeWrapper {
public:
virtual void action() = 0;
};
/**
* @brief Drive the Rover with a Joystick
*
@@ -26,6 +31,11 @@
*/
class ManualControl : public DriveModi {
public:
enum class InputMode : uint8_t {
Analog,
Digital
};
ManualControl(MoveControl *moveControl, const ControlPadInput *input);
/**
@@ -35,7 +45,7 @@ class ManualControl : public DriveModi {
~ManualControl();
/**
* @brief Calls runManualControl() to update all values.
* @brief Calls analogControl() to update all values.
*
* This function should be called every mainloop. If the delay is not reached, than the
* functions returns immediately.
@@ -44,12 +54,6 @@ class ManualControl : public DriveModi {
*/
void loop() override;
/**
* @brief Noramly called repeatedly by loop() to calcluate new values.
* Set new values for speed and rotation in moveControl
*/
void runManualControl();
/**
* @brief Set the min delay between each loop
*
@@ -79,6 +83,11 @@ class ManualControl : public DriveModi {
void setCalibrateCompass(CalibrateCompass* val = nullptr) { this->caliCompass = val; }
void switchInputMode();
void setInputMode(InputMode mode) { this->inputMode = mode; }
InputMode getInputMode() const { return this->inputMode; }
void setDirectionChangeCallback(DirectionChangeWrapper* callback) { this->directionChangeWrapper = callback; }
uint16_t getDutycycleLeft() const { return this->moveControl->getDutycycleLeft(); }
uint16_t getDutycycleRight() const { return this->moveControl->getDutycycleRight(); }
@@ -87,11 +96,21 @@ class ManualControl : public DriveModi {
const ControlPadInput* input;
private:
uint32_t lastMillis = 0;
/**
* @brief Noramly called repeatedly by loop() to calcluate new values.
* Set new values for speed and rotation in moveControl
*/
void analogControl();
void digitalControl();
bool lastLoopTurned = false;
uint8_t delay = 10;
uint32_t lastMillis = 0;
double max_speed = 1;
double max_rotation = 7;
CalibrateCompass* caliCompass = nullptr;
DirectionChangeWrapper* directionChangeWrapper = nullptr;
InputMode inputMode = InputMode::Analog;
};
#endif // MANUAL_CONTROL_H
src/main.cpp
+19 -31
View File
@@ -22,6 +22,8 @@
#include <LiquidCrystal_I2C.h>
#include <SPI.h>
#include "Version.h"
#include "driveModi/driveManager.h"
#include "OutputBuf/outputBuf.h"
#include "LcdWrapper.h"
@@ -109,6 +111,8 @@ void setup() {
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;
std::cout << "All actions from the main program run on Core -> " << xPortGetCoreID() << std::endl;
driveManager = new DriveManager(&moveController, spiPort, controlPad->getControlPadDataPtr(), wifiIsActive);
@@ -118,7 +122,7 @@ void setup() {
lcd->backlight();
lcd->printf("%c Kleiax-Rover %c", wifiIndicator, wifiIndicator);
lcd->setCursor(0, 1);
lcd->print("Press PS-Button");
lcd->printf("WiFi channel %u", Network::getCurrentChannel());
lcdWrapper = new LcdWrapper(lcd);
lcdWrapper->setCallback(lcdWrapperCallback);
@@ -231,41 +235,25 @@ void makeMenu() {
sys_m->setUpdateDelay(1500);
rout_m->setLcd(lcdWrapper);
// Entry for the main menu
MenuAction* mode_e = new MenuAction("Mode", mode_m);
MenuAction* gps_e = new MenuAction("GPS", gps_m);
MenuAction* pid_e = new MenuAction("PID", pid_m);
MenuAction* restart_e = new MenuAction("Restart", restart);
MenuAction* sys_e = new MenuAction("Systeminfo", sys_m);
MenuAction* rout_e = new MenuAction("Route", rout_m);
main_m->addEntry(mode_e);
main_m->addEntry(gps_e);
main_m->addEntry(rout_e);
main_m->addEntry(pid_e);
main_m->addEntry(sys_e);
main_m->addEntry(restart_e);
main_m->addEntry(new MenuAction("Mode", mode_m));
main_m->addEntry(new MenuAction("GPS", gps_m));
main_m->addEntry(new MenuAction("Route", rout_m));
main_m->addEntry(new MenuAction("PID", pid_m));
main_m->addEntry(new MenuAction("Systeminfo", sys_m));
main_m->addEntry(new MenuAction("Restart", restart));
// Entry for the mode Menu
MenuAction* autopilot_e = new MenuAction("Autopilot", auto_m);
MenuAction* captureRoute_e = new MenuAction("Capture Route", cap_m);
MenuAction* consolControl_e = new MenuAction("Consol Control", dummy);
MenuAction* manualControl_e = new MenuAction("Manual Control", man_m);
MenuAction* testMode_e = new MenuAction("Test Mode", testM_m);
MenuAction* caliComp_e = new MenuAction("Gauge Compass", comp_m);
mode_m->addEntry(manualControl_e);
mode_m->addEntry(captureRoute_e);
mode_m->addEntry(autopilot_e);
mode_m->addEntry(caliComp_e);
mode_m->addEntry(testMode_e);
mode_m->addEntry(consolControl_e);
mode_m->addEntry(new MenuAction("Manual Control", man_m));
mode_m->addEntry(new MenuAction("Capture Route", cap_m));
mode_m->addEntry(new MenuAction("Autopilot", auto_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 PID Menu
MenuAction* pidl_e = new MenuAction("Left", pidl_m);
MenuAction* pidr_e = new MenuAction("Right", pidr_m);
pid_m->addEntry(pidl_e);
pid_m->addEntry(pidr_e);
pid_m->addEntry(new MenuAction("Left", pidl_m));
pid_m->addEntry(new MenuAction("Right", pidr_m));
// Other config
pidl_m->setMinMax(0, UINT8_MAX);
src/moveControl.cpp
+2 -2
View File
@@ -187,9 +187,9 @@ void MoveControl::calcTargetWheelSpeed() {
\ 1 -b / \ T / \ Xr / */
// (1 / r) * 1
const static double A = 15.82278481;
constexpr double A = 15.82278481;
// (1 / r) * b
const static double B = 2.096518987;
constexpr double B = 2.096518987;
this->wheelspeed_right_target = (A * this->x_speed + B * this->rotation_speed) * (WHEEL_DIAMETER / 2);
this->wheelspeed_left_target = (A * this->x_speed + (-B) * this->rotation_speed) * (WHEEL_DIAMETER / 2);
src/network.cpp
+13
View File
@@ -113,6 +113,19 @@ bool Network::connectEspNow(recieveCallbackPtr reci, sendCallbackPtr send) {
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()) {
test/test_desktop/bootstrap.cpp
+22
View File
@@ -0,0 +1,22 @@
#include "courseCalculation.hpp"
#include <unity.h>
void setUp() {
}
void tearDown() {
}
void uselessTest() {
TEST_ASSERT_EQUAL(12, 12);
}
int main(int argc, char **argv) {
UNITY_BEGIN();
RUN_TEST(testCourseCalculation);
UNITY_END();
return 0;
}
test/test_desktop/courseCalculation.hpp
+15
View File
@@ -0,0 +1,15 @@
#include "route.h"
#include <unity.h>
#include <iostream>
void testCourseCalculation(void) {
// Point pointA(53.106725, 7.248069); // 45
// Point pointA(53.112188, 7.264404); // 90
Point pointA(53.112072, 7.266619); // -90
Point pointB(53.112123, 7.265354); //Home
int res = pointA.courseTo(pointB);
std::cout << "" << std::endl;
std::cout << "" << std::endl;
std::cout << "Return Value: " << res << std::endl;
TEST_ASSERT_LESS_OR_EQUAL(10, res);
}
version
+1
View File
@@ -0,0 +1 @@
0.8.36