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
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
+90 -19
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;
return 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 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;
};