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

- remove NtripReconnect from Autopilot and CaptureRoute

Browse Source 
- shiftet moveControl States in the class namespace
- added auto reconeect to NtripClient
- a lot of refactor
This commit is contained in:
kleiax
2023-05-30 23:40:05 +02:00
parent 98b65c4aec
commit 22eb6788c3
18 changed files with 369 additions and 280 deletions
Download Patch File Download Diff File Expand all files Collapse all files
include/moveControl.h
+18 -16
View File
@@ -21,15 +21,6 @@
#include "moveControlConfig.h"
#include "debugTimes.h"
/**
* @brief used to set driving status
*
* When set to stop all motors are set to halt
*
*/
enum DrivingStatus {stop,
drive,
raw};
/**
* @brief This class manages the motors and the encoders
@@ -41,6 +32,17 @@ enum DrivingStatus {stop,
*/
class MoveControl {
public:
/**
* @brief used to set driving status
*
* When set to stop all motors are set to halt
*
*/
enum Status {Stop,
Drive,
Raw};
/**
* @brief Construct a new Move Control object
*
@@ -73,12 +75,12 @@ class MoveControl {
void runMoveControl();
/**
* @brief Set the DrivingStatus
* @brief Set the Status
*
* @see DrivingStatus
* @see Status
* @param status
*/
void setDrivingStatus(DrivingStatus status);
void setDrivingStatus(Status status);
/**
* @brief Stops the engine immediately
@@ -107,7 +109,7 @@ class MoveControl {
/**
* @brief Set Raw Power Left
*
* This value has only an effect if DrivingStatus is raw.
* This value has only an effect if Status is raw.
*
* @param power between -100 and 100
*/
@@ -116,7 +118,7 @@ class MoveControl {
/**
* @brief Set the Raw Power Right
*
* This value has only an effect if DrivingStatus is raw.
* This value has only an effect if Status is raw.
*
* @param power between -100 and 100
*/
@@ -178,7 +180,7 @@ class MoveControl {
/**
* @brief Set the target power to motors
*
* Checks if the driving_status is set to drive.
* Checks if the driving_status is set to Drive.
* If yes, then the motors get the pid_out values as targetpower.
* If no, then the motors target power is set to zero.
*/
@@ -196,7 +198,7 @@ class MoveControl {
PID *left_pid;
PID *right_pid;
DrivingStatus driving_status = DrivingStatus::stop;
Status driving_status = Status::Stop;
double x_speed = 0;
double rotation_speed = 0;
lib/Navigation/navigation.cpp
+40 -38
View File
@@ -124,69 +124,54 @@ bool Navigation::startNavigation() {
return this->navigationStarted;
}
bool Navigation::getCourseCorrection(CourseCorrection& correction) {
//TODO: Check accuracy befor make something
correction.correction = 0;
correction.distance = 0;
Navigation::Status Navigation::getCourseCorrection(CourseCorrection& correction) {
if (this->navigationFinished)
return Status::Complete;
if (this->currentPosition.getAccuracy() <= this->minAccuracy)
return Status::InsufficientAccuracy;
if (this->currentPosition.distanceTo(this->lastPointCalcCorrection) < (this->minDistanceToReachPoint / 2.0))
return Status::Unchanged;
int16_t targetCourse = this->currentPosition.courseTo(this->targetPoint);
double distance = this->currentPosition.distanceTo(this->targetPoint);
if (distance < 0 || this->navigationFinished)
return false;
// Check if I need a new Point
if (distance < this->minDistanceToReachPoint && this->preventNextPoint == false) {
if (!this->nextPoint()) {
this->navigationFinished = true;
this->navigationStarted = false;
return false; // End of navigation
return Status::Complete; // End of navigation
}
distance = this->currentPosition.distanceTo(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;
//Rotate result by 180° to corrigate Azimuth
correctionCourse += 180;
correctionCourse = correctionCourse > 180 ? correctionCourse -360 : correctionCourse;
correction.correction = correctionCourse;
correction.correction = this->calculateCourseCorrection();
correction.distance = distance;
return true;
this->lastPointCalcCorrection = this->currentPosition;
return Status::Updated;
}
bool Navigation::addCurrentPosToRoute() {
if (!this->currentPosition.isValid()) { return false; }
Navigation::Status Navigation::addCurrentPosToRoute() {
if (this->currentPosition.getAccuracy() <= this->minAccuracy)
return Status::InsufficientAccuracy;
// First Point
if (this->route->getRouteInfo().totalPoints == 0) {
this->route->addPointToRoute(this->currentPosition);
this->lastPoint = this->currentPosition;
return true;
this->lastPointRouteInsert = this->currentPosition;
return Status::Updated;
}
// Every Point after the first
if (MIN_DISTANCE_BETWEEN_POINTS <= this->currentPosition.distanceTo(this->lastPoint)) {
if (MIN_DISTANCE_BETWEEN_POINTS <= this->currentPosition.distanceTo(this->lastPointRouteInsert)) {
this->route->addPointToRoute(this->currentPosition);
this->lastPoint = this->currentPosition;
return true;
this->lastPointRouteInsert = this->currentPosition;
return Status::Updated;
}
return false;
return Status::Unchanged;
}
// bool Navigation::setPointBeforeTurn() {
// if (millis() - this->currentPosition.getCreationTime() > 1000)
// return false;
// this->beforeTurnPoint = this->currentPosition;
// return true;
// }
void Navigation::updateCurrentLocation() {
if (Navigation::ubxUpdateTimeStatic == this->ubxUpdateTime)
return;
@@ -201,6 +186,23 @@ void Navigation::updateCurrentLocation() {
this->currentPosition = Point(coords, this->ubxData->hAcc);
}
int16_t Navigation::calculateCourseCorrection() {
int16_t targetCourse = this->currentPosition.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;
//Rotate result by 180° to corrigate Azimuth
correctionCourse += 180;
correctionCourse = correctionCourse > 180 ? correctionCourse -360 : correctionCourse;
return correctionCourse;
}
bool Navigation::nextPoint() {
if (!this->navigationStarted)
return false;
lib/Navigation/navigation.h
+15 -5
View File
@@ -29,7 +29,7 @@
* around the half accuracy of the positioning system
*
*/
#define MIN_DISTANCE_BETWEEN_POINTS 0.1
#define MIN_DISTANCE_BETWEEN_POINTS 0.3
#define MIN_DISTANCE_TO_REACH_POINT 0.5
#define AZIMUTH_UPDATE_DELAY 20
@@ -55,6 +55,14 @@ struct CourseCorrection {
*/
class Navigation {
public:
enum Status {
InsufficientAccuracy,
Unchanged,
Updated,
Complete
};
/**
* @brief Construct a new Navigation object and using I2C
*
@@ -123,7 +131,7 @@ class Navigation {
* @return true if new correction data provided
* @return false if route is finished
*/
bool getCourseCorrection(CourseCorrection& correction);
Status getCourseCorrection(CourseCorrection& correction);
/**
* @brief Tries to add the current Position to the route
@@ -133,7 +141,7 @@ class Navigation {
* @return true successful added point
* @return false no point added to route
*/
bool addCurrentPosToRoute();
Status addCurrentPosToRoute();
// TODO: can be deleted?
// bool setPointBeforeTurn();
@@ -189,6 +197,7 @@ class Navigation {
private:
void updateCurrentLocation();
int16_t calculateCourseCorrection();
/**
* @brief Set the next point as target
@@ -215,10 +224,11 @@ class Navigation {
NTRIPClient* ntripClient = nullptr;
QMC5883LCompass* compass;
Point lastPoint;
Point lastPointRouteInsert;
Point lastPointCalcCorrection;
Point targetPoint;
Point currentPosition;
Point beforeTurnPoint;
Point::Accuracy minAccuracy = Point::Accuracy::twoDigOfCM;
bool navigationStarted = false;
lib/Navigation/route.cpp
+6 -6
View File
@@ -90,17 +90,17 @@ void Point::init(uint32_t horizontalAccuracy) {
this->creationTime = millis();
if (horizontalAccuracy == UINT32_MAX)
this->accuracy = PointAccuracy::imported;
this->accuracy = Accuracy::imported;
else if (horizontalAccuracy > 9999)
this->accuracy = PointAccuracy::fourDigOfCM;
this->accuracy = Accuracy::fourDigOfCM;
else if (horizontalAccuracy > 999)
this->accuracy = PointAccuracy::threeDigOfCM;
this->accuracy = Accuracy::threeDigOfCM;
else if (horizontalAccuracy > 99)
this->accuracy = PointAccuracy::twoDigOfCM;
this->accuracy = Accuracy::twoDigOfCM;
else if (horizontalAccuracy > 1)
this->accuracy = PointAccuracy::oneDigOfCM;
this->accuracy = Accuracy::oneDigOfCM;
else
this->accuracy = PointAccuracy::none;
this->accuracy = Accuracy::none;
}
lib/Navigation/route.h
+5 -5
View File
@@ -47,7 +47,7 @@ class Point{
* @brief The Accuracy is set by the constructor
*
*/
enum PointAccuracy {
enum Accuracy {
none,
fourDigOfCM,
threeDigOfCM,
@@ -126,16 +126,16 @@ class Point{
* @brief Get the Accuracy object
*
* The higher the value, the greater the accuracy.
* You can check it by PointAccuracy.
* You can check it by Accuracy.
*
* @return PointAccuracy
* @return Accuracy
*/
PointAccuracy getAccuracy() { return this->accuracy; }
Accuracy getAccuracy() { return this->accuracy; }
private:
void init(uint32_t horizontalAccuracy);
PointAccuracy accuracy = PointAccuracy::none;
Accuracy accuracy = Accuracy::none;
Coordinates coordinates;
uint32_t creationTime = 0;
lib/NtripClient/ntripClient.cpp
+55 -31
View File
@@ -29,27 +29,14 @@ NTRIPClient::~NTRIPClient() {
}
void NTRIPClient::loop() {
this->pushGPGGA();
if (millis() - this->lastLoopTime < this->delayTime)
return;
this->lastLoopTime = millis();
if (millis() - this->lastGPGGAPushTime > this->pushGPGGATime && this->activated) {
this->lastGPGGAPushTime = millis();
// std::cout << "Try to push GPGGA data." << std::endl;
NMEA_GGA_data_t *data = new NMEA_GGA_data_t;
DebugTimes requestGpsData;
uint8_t res = this->gps->getLatestNMEAGPGGA(data);
requestGpsData.stopConsol("RequestGpsData", 5);
if (res == 2)
this->pushGPGGA(data);
delete data;
}
switch (this->state) {
case NTRIPClientStates::openConnection:
if (millis() - this->lastNtripConnectTime < this->tryReconnectTime)
break;
if (!this->activated) {
this->state = NTRIPClientStates::closeConnection;
break;
@@ -60,10 +47,9 @@ void NTRIPClient::loop() {
std::cout << "Connected to the NTRIP caster!" << std::endl;
this->state = NTRIPClientStates::pushData;
} else {
uint8_t seconds = this->tryReconnectTime / 1000;
std::cout << "Could not connect to the caster. Trying again in "
<< (int) seconds << " seconds." << std::endl;
this->lastNtripConnectTime = millis();
std::cout << "Failed!" << std::endl;
this->state = NTRIPClientStates::wait;
this->activated = false;
}
break;
@@ -81,9 +67,8 @@ void NTRIPClient::loop() {
case NTRIPClientStates::wait:
if (this->activated)
this->state = NTRIPClientStates::openConnection;
if (this->activated) {
// std::cout << "state is openConnection" << std::endl;
}
else
this->checkAutoReconnect();
break;
case NTRIPClientStates::notAvailable:
@@ -105,16 +90,27 @@ void NTRIPClient::gpsConfiguration() {
this->gps->enableNMEAMessage(UBX_NMEA_GGA, COM_PORT_SPI, 10);
}
bool NTRIPClient::setActivated(bool b) {
bool NTRIPClient::setActivated(bool state) {
// std::cout << "NTRIPClient::setActivated: b - " << b << std::endl;
if (b && this->state != NTRIPClientStates::notAvailable)
if (state && this->state != NTRIPClientStates::notAvailable)
this->activated = true;
else if (b)
else if (state)
return false;
else
else {
this->activated = false;
this->autoReconnect = false;
}
return true;
}
void NTRIPClient::setAutoReconnect(bool state) {
if (state) {
this->reconnectAttemps = 0;
this->autoReconnect = true;
return;
}
this->autoReconnect = false;
}
bool NTRIPClient::beginClient() {
@@ -252,11 +248,39 @@ bool NTRIPClient::processConnection() {
return true;
}
void NTRIPClient::pushGPGGA(NMEA_GGA_data_t *nmeaData) {
if (this->ntripClient->connected() && this->transmitLocation)
this->ntripClient->print((const char *)nmeaData->nmea);
else
std::cout << "Failed to pushing GGA to server: " << (const char *)nmeaData->nmea << std::endl;
void NTRIPClient::checkAutoReconnect() {
if (!this->autoReconnect)
return;
if (millis() - this->lastReconnectTime < this->reconnectDelayTime)
return;
this->lastReconnectTime = millis();
if (this->reconnectAttemps >= this->maxReconnectAttemps) {
this->autoReconnect = false;
return;
}
this->activated = true;
this->reconnectAttemps++;
}
void NTRIPClient::pushGPGGA() {
if (!this->transmitLocation && !this->activated)
return;
if (millis() - this->lastGPGGAPushTime < this->pushGPGGATime)
return;
this->lastGPGGAPushTime = millis();
if (!this->ntripClient->connected())
std::cout << "Failed to pushing GGA to server: " << std::endl;
NMEA_GGA_data_t *data = new NMEA_GGA_data_t;
uint8_t res = this->gps->getLatestNMEAGPGGA(data);
if (res == 2)
this->ntripClient->print((const char *)data);
delete data;
}
bool NTRIPClient::isConnected() {
lib/NtripClient/ntripClient.h
+11 -5
View File
@@ -81,11 +81,12 @@ class NTRIPClient {
/**
* @brief Activate or deactivate the connection to the server.
*
* @param b
* @param state
* @return true success
* @return false failure
*/
bool setActivated(bool b);
bool setActivated(bool state);
void setAutoReconnect(bool state);
bool isConnected();
@@ -99,10 +100,11 @@ class NTRIPClient {
NTRIPClientStates getClientState() { return this->state; }
private:
void pushGPGGA(NMEA_GGA_data_t *nmeaData);
void pushGPGGA();
bool beginClient();
void closeConnection();
bool processConnection();
void checkAutoReconnect();
SFE_UBLOX_GNSS* gps;
WiFiClient* ntripClient;
@@ -110,20 +112,24 @@ class NTRIPClient {
bool transmitLocation = true;
bool activated = true;
bool autoReconnect = false;
uint8_t reconnectAttemps = 0;
uint16_t port;
uint32_t lastReceivedRtcmTime = 0;
uint32_t lastNtripConnectTime = 0;
// uint32_t lastNtripConnectTime = 0; // can deleted?
uint32_t lastGPGGAPushTime = 0;
uint32_t lastLoopTime = 0;
uint32_t lastReconnectTime = 0;
char host[128];
char mountPoint[128];
char user[128];
char password[128];
const uint8_t delayTime = 20;
const uint8_t maxReconnectAttemps = 10;
const uint16_t reconnectDelayTime = 1000;
const uint16_t timeOut = 5000;
const uint16_t tryReconnectTime = 5000;
const uint16_t bufferSize = 512;
const uint16_t pushGPGGATime = 10000;
};
src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp
+1 -1
View File
@@ -42,7 +42,7 @@ void MenuAutopilot::printPage() const {
lineOne = "Status: ";
lineTwo = "";
switch (this->autopilot->getState()) {
case Autopilot::State::NoNtrip :
case Autopilot::State::InsufficientAccuarcy :
lineTwo = "Err: No NTRIP";
break;
src/SpecialMenus/driveModi/CaptureRoute/menuCaptureRoute.cpp
+50 -4
View File
@@ -12,12 +12,12 @@
void MenuCaptureRoute::printPage() const {
RouteInfo routeInfo = this->captureRoute->getRouteInfo();
UBX_NAV_PVT_data_t* gpsData = this->driveManager->getNavigation()->getUbxData();
String lineOne = "";
String lineTwo = "";
switch (this->getCurrentPage())
{
switch (this->getCurrentPage()) {
case 0:
lineOne = "Capture Route";
lineTwo = "You can drive";
@@ -29,12 +29,33 @@ void MenuCaptureRoute::printPage() const {
break;
case 2:
lineOne = "Last status:";
switch (this->captureRoute->getLastStatus()) {
case Navigation::Status::InsufficientAccuracy:
lineTwo = "Poor Accuracy";
break;
case Navigation::Status::Updated:
lineTwo = "Point added";
break;
case Navigation::Status::Unchanged:
lineTwo = "Point too close";
break;
default:
lineTwo = "---";
break;
}
break;
case 3:
lineOne = "Distance to last";
lineTwo = "point: ";
lineTwo.concat(this->captureRoute->getDistanceToLastPoint());
break;
case 3: {
case 4: {
NTRIPClientStates status = this->driveManager->getNavigation()->getNTRIPClient()->getClientState();
lineOne = "NTRIP Client is";
@@ -47,6 +68,30 @@ void MenuCaptureRoute::printPage() const {
break;
}
case 5: {
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 6:
lineOne = "hAccuracy: ";
lineTwo = "Azimuth: ";
lineTwo.concat(this->driveManager->getNavigation()->getAzimuth());
if (gpsData->fixType)
lineOne.concat(gpsData->hAcc);
else
lineOne.concat("0");
break;
default:
this->printDefault();
return;
@@ -62,7 +107,8 @@ void MenuCaptureRoute::update() {
}
void MenuCaptureRoute::init() {
this->setCountPages(4);
this->setCountPages(7);
this->updateDelay = 500;
this->driveManager->changeModus(Modi::CaptureRoute);
this->captureRoute = (CaptureRoute*) this->driveManager->getDriveModiPtr();
src/driveModi/Modi/Autopilot/autopilot.cpp
+102 -67
View File
@@ -10,6 +10,7 @@
*/
#include "driveModi/Modi/Autopilot/autopilot.h"
#include "autopilot.h"
Autopilot::Autopilot(MoveControl* moveControl, const ControlPadInput *input, Navigation* navigation)
: ManualControl(moveControl, input) {
@@ -18,64 +19,64 @@ Autopilot::Autopilot(MoveControl* moveControl, const ControlPadInput *input, Nav
}
Autopilot::~Autopilot() {
this->navigation->getNTRIPClient()->setActivated(false);
// this->navigation->getNTRIPClient()->setActivated(false);
}
void Autopilot::loop() {
if (this->state < State::SelfDriving)
ManualControl::loop();
if (millis() - this->loopLastMillis < loopDelayMillis)
return;
this->runAutopilot();
this->loopLastMillis = millis();
}
void Autopilot::runAutopilot() {
if (this->state < State::NavigationStarted)
return;
if (!this->navigation->getCourseCorrection(this->courseCorrection)) {
this->state = State::TargetReached;
this->moveControl->setSpeed(0);
this->moveControl->setRotationSpeed(0);
this->updateDisplay = true;
return;
}
this->routeInfo = this->navigation->getRouteInfo();
if (millis() - this->displayUpdateLastMillis > this->displayUpdateDelayMillis) {
this->updateDisplay = true;
this->displayUpdateLastMillis = millis();
}
this->checkNtrip();
if (millis() - this->loopLastMillis < loopDelayMillis)
return;
// Check if start Point is near to current Location
if (this->routeInfo.currentPoint >= 2 && this->state == State::GetToStartPoint)
this->routeInfo = this->navigation->getRouteInfo();
this->runAutopilot();
this->loopLastMillis = millis();
}
void Autopilot::runAutopilot() {
switch (this->state) {
case State::InsufficientAccuarcy:
this->askNavigationForOrder();
return;
case State::NoRoute:
return;
case State::None:
return;
case State::NavigationStarted:
this->askNavigationForOrder();
break;
case State::GetToStartPoint:
this->askNavigationForOrder();
if (this->routeInfo.currentPoint >= 2)
this->state = State::SelfDrivingAvailable;
break;
if (ControlPadButton::isControlPadButtonPressed(this->input, ControlPadButton::PadButton::Action)
&& millis() - this->lastAutopilotChangeMillis > this->autopilotChangeDelayMillis) {
case State::SelfDrivingAvailable:
this->askNavigationForOrder();
this->checkButtonInput();
break;
if (this->state == State::SelfDrivingAvailable) {
this->state = State::SelfDriving;
this->updateDisplay = true;
this->lastAutopilotChangeMillis = millis();
} else if (this->state == State::SelfDriving) {
this->state = State::SelfDrivingAvailable;
this->updateDisplay = true;
this->lastAutopilotChangeMillis = millis();
}
}
case State::SelfDriving:
this->askNavigationForOrder();
this->checkButtonInput();
this->selfDriving();
break;
if (this->state == State::SelfDriving) {
if (abs(this->courseCorrection.correction) >= this->maxCourseDeviationBerforeAct)
this->rotate();
else
this->drive();
case State::TargetReached:
break;
default:
break;
}
}
@@ -97,38 +98,15 @@ void Autopilot::init() {
else
this->state = State::NoRoute;
this->routeInfo = this->navigation->getRouteInfo();
this->navigation->getNTRIPClient()->setAutoReconnect(true);
this->lastState = this->navigation->getNTRIPClient()->getClientState();
this->lastOrderStatus = this->navigation->getCourseCorrection(this->courseCorrection);
this->courseCorrection.correction = 0;
this->courseCorrection.distance = 0;
this->updateDisplay = true;
}
void Autopilot::checkNtrip() {
NTRIPClient* client = this->navigation->getNTRIPClient();
NTRIPClientStates state = client->getClientState();
if (state == NTRIPClientStates::notAvailable) {
this->state = State::NoNtrip;
} else if (state == NTRIPClientStates::pushData) {
this->ntripReconnectAttemps = 0;
if (this->state < State::GetToStartPoint)
this->state = State::GetToStartPoint;
} else if (state ==NTRIPClientStates::wait) {
if (millis() - this->reconnectNtripLastMillis < this->reconnectNtripDelayMillis)
return;
if (this->ntripReconnectAttemps > this->ntripReconnectMaxAttemps) {
this->state = State::NoNtrip;
return;
}
this->reconnectNtripLastMillis = millis();
client->setActivated(true);
this->ntripReconnectAttemps++;
}
}
void Autopilot::drive() {
this->moveControl->setRotationSpeed(0);
if (this->courseCorrection.distance >= this->minRemainingDistance)
@@ -142,5 +120,62 @@ void Autopilot::rotate() {
if (this->courseCorrection.correction > 0)
this->moveControl->setRotationSpeed(this->rotationSpeed);
else
this->moveControl->setRotationSpeed(-this->rotationSpeed);
}
void Autopilot::checkButtonInput() {
if (ControlPadButton::isControlPadButtonPressed(this->input, ControlPadButton::PadButton::Action)
&& millis() - this->lastAutopilotChangeMillis > this->autopilotChangeDelayMillis) {
if (this->state == State::SelfDrivingAvailable) {
this->state = State::SelfDriving;
this->updateDisplay = true;
this->lastAutopilotChangeMillis = millis();
} else if (this->state == State::SelfDriving) {
this->state = State::SelfDrivingAvailable;
this->updateDisplay = true;
this->lastAutopilotChangeMillis = millis();
}
}
}
void Autopilot::askNavigationForOrder() {
this->lastOrderStatus = this->navigation->getCourseCorrection(this->courseCorrection);
switch (this->lastOrderStatus) {
case Navigation::Status::Complete:
this->state = State::TargetReached;
this->moveControl->setSpeed(0);
this->moveControl->setRotationSpeed(0);
break;
case Navigation::Status::InsufficientAccuracy:
this->lastState = this->state;
this->state = State::InsufficientAccuarcy;
this->moveControl->setSpeed(0);
this->moveControl->setRotationSpeed(0);
break;
case Navigation::Status::Unchanged:
if (this->state == State::InsufficientAccuarcy)
this->state = this->lastState;
break;
case Navigation::Status::Updated:
if (this->state == State::InsufficientAccuarcy)
this->state = this->lastState;
break;
default:
break;
}
}
void Autopilot::selfDriving() {
if (this->lastOrderStatus == Navigation::Status::Updated) {
if (abs(this->courseCorrection.correction) >= this->maxCourseDeviationBerforeAct)
this->rotate();
else
this->drive();
}
}
src/driveModi/Modi/Autopilot/autopilot.h
+7 -8
View File
@@ -29,7 +29,7 @@
class Autopilot : public ManualControl {
public:
enum State {
NoNtrip = -2,
InsufficientAccuarcy = -2,
NoRoute = -1,
None = 0,
NavigationStarted,
@@ -108,32 +108,31 @@ class Autopilot : public ManualControl {
private:
void init();
void checkNtrip();
void drive();
void rotate();
void checkButtonInput();
void askNavigationForOrder();
void selfDriving();
Navigation* navigation;
CourseCorrection courseCorrection;
RouteInfo routeInfo;
State state = State::None;
NTRIPClientStates lastState;
State lastState = State::None;
Navigation::Status lastOrderStatus;
bool updateDisplay = false;
uint8_t loopDelayMillis = 40;
uint8_t ntripReconnectAttemps = 0;
uint8_t ntripReconnectMaxAttemps = 10;
uint8_t maxCourseDeviationBerforeAct = 5;
uint16_t displayUpdateDelayMillis = 1000;
uint16_t reconnectNtripDelayMillis = 1000;
uint16_t autopilotChangeDelayMillis = 500;
uint32_t displayUpdateLastMillis = 0;
uint32_t loopLastMillis = 0;
uint32_t reconnectNtripLastMillis = 0;
uint32_t lastAutopilotChangeMillis = 0;
double drivingSpeed = 1;
double rotationSpeed = 7;
double rotationSpeed = 3;
double minRemainingDistance = 0.25;
};
src/driveModi/Modi/CaptureRoute/captureRoute.cpp
+6 -33
View File
@@ -15,31 +15,28 @@ CaptureRoute::CaptureRoute(MoveControl* moveControl, const ControlPadInput *inpu
: ManualControl(moveControl, input) {
this->navigation = navigation;
this->navigation->getRoute()->clear();
this->navigation->getNTRIPClient()->setAutoReconnect(true);
this->routeInfo = navigation->getRouteInfo();
}
CaptureRoute::~CaptureRoute() {
this->navigation->getNTRIPClient()->setActivated(false);
// this->navigation->getNTRIPClient()->setActivated(false);
}
void CaptureRoute::loop() {
ManualControl::loop();
if (millis() - this->lastMillis < this->delay) {
if (millis() - this->lastMillis < this->delay)
return;
}
this->runCaptureRoute();
this->lastMillis = millis();
}
void CaptureRoute::runCaptureRoute() {
this->checkNtrip();
if (this->state != NtripState::Enabled)
return;
if (ControlPadButton::isControlPadButtonPressed(this->input, ControlPadButton::PadButton::Action)) {
if (this->navigation->addCurrentPosToRoute()) {
this->status = this->navigation->addCurrentPosToRoute();
if (this->status == Navigation::Status::Updated) {
this->lastSavedPoint = this->navigation->getCurrentPosition();
this->routeInfo = navigation->getRouteInfo();
this->updateDisplay = true;
@@ -60,27 +57,3 @@ bool CaptureRoute::shouldUpdate() {
}
return false;
}
void CaptureRoute::checkNtrip() {
NTRIPClient* client = this->navigation->getNTRIPClient();
NTRIPClientStates state = client->getClientState();
if (state == NTRIPClientStates::notAvailable) {
this->state = NtripState::NoNtrip;
} else if (state == NTRIPClientStates::pushData) {
this->ntripReconnectAttemps = 0;
this->state = NtripState::Enabled;
} else if (state ==NTRIPClientStates::wait) {
this->state = NtripState::Waiting;
if (millis() - this->reconnectNtripLastMillis < this->reconnectNtripDelayMillis)
return;
if (this->ntripReconnectAttemps > this->ntripReconnectMaxAttemps) {
this->state = NtripState::NoNtrip;
return;
}
this->reconnectNtripLastMillis = millis();
client->setActivated(true);
this->ntripReconnectAttemps++;
}
}
src/driveModi/Modi/CaptureRoute/captureRoute.h
+2 -13
View File
@@ -27,12 +27,6 @@
*/
class CaptureRoute : public ManualControl {
public:
enum NtripState {
NoNtrip,
Waiting,
Enabled
};
/**
* @brief Construct a new Capture Route object
*
@@ -76,6 +70,7 @@ class CaptureRoute : public ManualControl {
* @return RouteInfo
*/
RouteInfo getRouteInfo() const { return this->routeInfo; }
Navigation::Status getLastStatus() const { return this->status; }
/**
* @brief Get the distance to the last saved oint
@@ -92,19 +87,13 @@ class CaptureRoute : public ManualControl {
*/
bool shouldUpdate();
private:
void checkNtrip();
Navigation* navigation;
RouteInfo routeInfo;
Point lastSavedPoint;
NtripState state = NtripState::Waiting;
Navigation::Status status = Navigation::Status::Complete;
uint8_t ntripReconnectAttemps = 0;
uint8_t ntripReconnectMaxAttemps = 10;
uint16_t reconnectNtripDelayMillis = 1000;
uint16_t delay = 200;
uint32_t lastMillis = 0;
uint32_t reconnectNtripLastMillis = 0;
bool updateDisplay = false;
};
src/driveModi/Modi/ManualControl/manualControl.cpp
+2 -2
View File
@@ -13,13 +13,13 @@
ManualControl::ManualControl(MoveControl *moveControl, const ControlPadInput *input) {
this->moveControl = moveControl;
this->input = input;
this->moveControl->setDrivingStatus(DrivingStatus::drive);
this->moveControl->setDrivingStatus(MoveControl::Status::Drive);
}
ManualControl::~ManualControl() {
this->moveControl->setSpeed(0);
this->moveControl->setRotationSpeed(0);
this->moveControl->setDrivingStatus(DrivingStatus::stop);
this->moveControl->setDrivingStatus(MoveControl::Status::Stop);
}
void ManualControl::loop() {
src/driveModi/Modi/ManualControl/manualControl.h
+1 -1
View File
@@ -30,7 +30,7 @@ class ManualControl : public DriveModi {
/**
* @brief Destroy the Manual Control object
* Set in moveControl speed and rotation to 0 and set DrivingStatus::stop
* Set in moveControl speed and rotation to 0 and set DrivingStatus::Stop
*/
~ManualControl();
src/driveModi/Modi/TestMode/testMode.cpp
+5 -5
View File
@@ -16,7 +16,7 @@ TestMode::TestMode(MoveControl *moveControl, Navigation* navigation) {
}
TestMode::~TestMode() {
this->moveControl->setDrivingStatus(DrivingStatus::stop);
this->moveControl->setDrivingStatus(MoveControl::Status::Stop);
}
void TestMode::loop() {
@@ -32,7 +32,7 @@ void TestMode::loop() {
if (millis() - this->actionStart > this->maneuverTime || this->abort) {
this->busy = false;
this->abort = false;
this->moveControl->setDrivingStatus(DrivingStatus::stop);
this->moveControl->setDrivingStatus(MoveControl::Status::Stop);
this->maneuver = Maneuver::None;
}
@@ -52,7 +52,7 @@ bool TestMode::drive(int16_t cm, int16_t degree) {
return false;
this->actionStart = millis();
this->moveControl->setDrivingStatus(DrivingStatus::drive);
this->moveControl->setDrivingStatus(MoveControl::Status::Drive);
if (cm == 0) {
//Only left or right
@@ -97,7 +97,7 @@ bool TestMode::drive(int16_t cm, int16_t degree) {
return true;
}
this->moveControl->setDrivingStatus(DrivingStatus::stop);
this->moveControl->setDrivingStatus(MoveControl::Status::Stop);
return false;
}
@@ -148,7 +148,7 @@ bool TestMode::engineInit(int16_t powerPercentage, int16_t seconds) {
return false;
this->actionStart = millis();
this->moveControl->setDrivingStatus(DrivingStatus::raw);
this->moveControl->setDrivingStatus(MoveControl::Status::Raw);
this->maneuverTime = seconds * 1000;
this->busy = true;
return true;
src/driveModi/driveManager.cpp
+1 -1
View File
@@ -59,7 +59,7 @@ void DriveManager::changeModus(Modi modus) {
//Set moveControl to a safe state
this->moveControl->setSpeed(0);
this->moveControl->setRotationSpeed(0);
this->moveControl->setDrivingStatus(DrivingStatus::stop);
this->moveControl->setDrivingStatus(MoveControl::Status::Stop);
switch (modus) {
case Modi::Off:
src/moveControl.cpp
+12 -9
View File
@@ -47,6 +47,9 @@ MoveControl::~MoveControl() {
delete this->right_motor;
delete this->left_speedometer;
delete this->right_speedometer;
delete this->left_pid;
delete this->right_pid;
}
void MoveControl::loop() {
@@ -91,19 +94,19 @@ void MoveControl::runMoveControl() {
this->regulateMotors();
}
void MoveControl::setDrivingStatus(DrivingStatus status) {
void MoveControl::setDrivingStatus(Status status) {
this->setSpeed(0);
this->setRotationSpeed(0);
this->setRawPowerLeft(0);
this->setRawPowerRight(0);
this->driving_status = status;
// switch (this->driving_status) {
// case DrivingStatus::stop :
// Serial.println("New drivingState = stop in MoveControl::setDrivingStatus");
// case Status::Stop :
// Serial.println("New drivingState = Stop in MoveControl::setDrivingStatus");
// break;
// case DrivingStatus::drive :
// Serial.println("New drivingState = drive in MoveControl::setDrivingStatus");
// case Status::Drive :
// Serial.println("New drivingState = Drive in MoveControl::setDrivingStatus");
// break;
// default:
@@ -119,7 +122,7 @@ void MoveControl::emergencyStop() {
this->setRotationSpeed(0);
this->setRawPowerLeft(0);
this->setRawPowerRight(0);
this->driving_status = DrivingStatus::stop;
this->driving_status = Status::Stop;
}
void MoveControl::setSpeed(double speed) {
@@ -194,21 +197,21 @@ void MoveControl::calcTargetWheelSpeed() {
void MoveControl::regulateMotors() {
switch (this->driving_status) {
case DrivingStatus::stop :
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;
case DrivingStatus::drive :
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_pid_out);
this->setSpeedometerDirection(this->right_speedometer, this->right_pid_out);
break;
case DrivingStatus::raw :
case Status::Raw :
this->left_motor->setTargetPower(this->rawPowerLeft);
this->right_motor->setTargetPower(this->rawPowerRight);
this->setSpeedometerDirection(this->left_speedometer, this->rawPowerLeft);