delete everthing for bachelore
This commit is contained in:
@@ -1,254 +0,0 @@
|
||||
/**
|
||||
* @file menuAutopilot.cpp
|
||||
* @author Alexander Klein (alex@kleiax.de)
|
||||
* @brief Contains an implementation of the class MenuAutopilot
|
||||
* @version 0.1
|
||||
* @date 2022-02-03
|
||||
*
|
||||
* @copyright Copyright (c) 2022
|
||||
*
|
||||
*/
|
||||
|
||||
#include "menuAutopilot.h"
|
||||
|
||||
void MenuAutopilot::printPage() const {
|
||||
CourseCorrection correction = this->autopilot->getCourseCorrection();
|
||||
UBX_NAV_PVT_data_t* gpsData = this->driveManager->getNavigation()->getUbxData();
|
||||
bool navigationStarted = this->autopilot->getState() >= Autopilot::State::NavigationStarted;
|
||||
|
||||
String distanceString = "";
|
||||
if (navigationStarted) {
|
||||
uint16_t distance = 0;
|
||||
if (correction.distance < 1) {
|
||||
distance = (uint16_t) (correction.distance * 100);
|
||||
distanceString.concat(distance);
|
||||
distanceString.concat("cm");
|
||||
} else if (correction.distance < 1000) {
|
||||
distance = (uint16_t) correction.distance;
|
||||
distanceString.concat(distance);
|
||||
distanceString.concat("m");
|
||||
} else {
|
||||
distance = (uint16_t) (correction.distance / 1000);
|
||||
distanceString.concat(distance);
|
||||
distanceString.concat("km");
|
||||
}
|
||||
}
|
||||
|
||||
String lineOne = "No information";
|
||||
String lineTwo = "available";
|
||||
|
||||
switch (this->getCurrentPage()) {
|
||||
case 0: {
|
||||
lineOne = "Status: ";
|
||||
lineTwo = "";
|
||||
switch (this->autopilot->getState()) {
|
||||
case Autopilot::State::InsufficientAccuracy :
|
||||
lineTwo = "Err: LowAccuracy";
|
||||
break;
|
||||
|
||||
case Autopilot::State::NoRoute :
|
||||
lineTwo = "Err: No route";
|
||||
break;
|
||||
|
||||
case Autopilot::State::NavigationStarted :
|
||||
lineTwo = "Nav started";
|
||||
break;
|
||||
|
||||
case Autopilot::State::GetToStartPoint :
|
||||
lineTwo = "Drive to start";
|
||||
break;
|
||||
|
||||
case Autopilot::State::SelfDrivingAvailable :
|
||||
lineTwo = "Autopilot ready";
|
||||
break;
|
||||
|
||||
case Autopilot::State::SelfDriving :
|
||||
lineTwo = "Autopilot active";
|
||||
break;
|
||||
|
||||
case Autopilot::State::SelfDrivingRotate :
|
||||
lineTwo = "Rotating";
|
||||
break;
|
||||
|
||||
case Autopilot::State::TargetReached :
|
||||
lineTwo = "Target reached";
|
||||
break;
|
||||
|
||||
default:
|
||||
lineTwo = "UNKOWN - ";
|
||||
lineTwo.concat(static_cast<int>(this->autopilot->getState()));
|
||||
|
||||
break;
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
case 1:
|
||||
if (!navigationStarted) {
|
||||
lineOne = "Navigation is";
|
||||
lineTwo = "not started";
|
||||
break;
|
||||
}
|
||||
lineOne = "Distance: ";
|
||||
lineOne.concat(distanceString);
|
||||
lineTwo = "Turn: ";
|
||||
lineTwo.concat(correction.correction);
|
||||
break;
|
||||
|
||||
case 2:
|
||||
lineOne = "Target waypoint";
|
||||
lineTwo = "";
|
||||
lineTwo.concat(this->autopilot->getRouteInfo().currentPoint);
|
||||
lineTwo.concat(" from ");
|
||||
lineTwo.concat(this->autopilot->getRouteInfo().totalPoints);
|
||||
break;
|
||||
|
||||
case 3: {
|
||||
NTRIPClientStates status = this->driveManager->getNavigation()->getNTRIPClient()->getClientState();
|
||||
uint8_t carrSoln = gpsData->flags.bits.carrSoln;
|
||||
lineOne = "GNSS: ";
|
||||
|
||||
if (status == NTRIPClientStates::pushData)
|
||||
if (carrSoln == 0)
|
||||
lineOne.concat("None");
|
||||
else if (carrSoln == 1)
|
||||
lineOne.concat("Floating");
|
||||
else if (carrSoln == 2)
|
||||
lineOne.concat("Fixed");
|
||||
else
|
||||
lineOne.concat("UNKNOWN");
|
||||
else if (status == NTRIPClientStates::notAvailable)
|
||||
lineOne.concat("No WiFi");
|
||||
else
|
||||
lineOne.concat("Offline");
|
||||
|
||||
lineTwo = "hAcc: ";
|
||||
if (gpsData->fixType)
|
||||
lineTwo.concat(gpsData->hAcc);
|
||||
else
|
||||
lineTwo.concat("0");
|
||||
break;
|
||||
}
|
||||
|
||||
case 4:
|
||||
if (this->driveManager->getNavigation()->getCalcAzimuthState() == Navigation::CalcAzimuthState::Good
|
||||
||this->driveManager->getNavigation()->getCalcAzimuthState() == Navigation::CalcAzimuthState::Super)
|
||||
{
|
||||
lineOne = "Calc Azi: ";
|
||||
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;
|
||||
}
|
||||
} else {
|
||||
lineOne = "Real Azi: ";
|
||||
lineTwo = "";
|
||||
lineOne.concat(this->driveManager->getNavigation()->getAzimuth());
|
||||
}
|
||||
break;
|
||||
|
||||
case 5:
|
||||
lineOne = "Loop mode is";
|
||||
if (this->autopilot->getLoopMode())
|
||||
lineTwo = "enabled";
|
||||
else
|
||||
lineTwo = "disabled";
|
||||
break;
|
||||
|
||||
case 6:
|
||||
lineOne = "Freeze target is";
|
||||
if (this->targetFreezed)
|
||||
lineTwo = "activated";
|
||||
else
|
||||
lineTwo = "deactivated";
|
||||
break;
|
||||
|
||||
case 7:
|
||||
lineOne = "MinDisToPoint:";
|
||||
lineTwo = "<- ";
|
||||
lineTwo.concat(this->minDistance);
|
||||
lineTwo.concat(" ->");
|
||||
break;
|
||||
|
||||
default:
|
||||
this->printDefault();
|
||||
return;
|
||||
}
|
||||
|
||||
this->print(lineOne, lineTwo);
|
||||
}
|
||||
|
||||
void MenuAutopilot::runCommand() const {
|
||||
switch (this->getCurrentPage()) {
|
||||
case 5:
|
||||
this->autopilot->switchLoopMode();
|
||||
break;
|
||||
|
||||
case 6:
|
||||
if (this->targetFreezed) {
|
||||
this->targetFreezed = false;
|
||||
this->driveManager->getNavigation()->freezeTargetPoint(false);
|
||||
} else {
|
||||
this->targetFreezed = true;
|
||||
this->driveManager->getNavigation()->freezeTargetPoint();
|
||||
}
|
||||
break;
|
||||
|
||||
case 7:
|
||||
this->minDistance = this->driveManager->getNavigation()->increaseMinDistanceToReachPoint();
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void MenuAutopilot::runCommandNo() const {
|
||||
switch (this->getCurrentPage()) {
|
||||
case 7:
|
||||
this->minDistance = this->driveManager->getNavigation()->decreaseMinDistanceToReachPoint();
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void MenuAutopilot::update() {
|
||||
if (!this->autopilot->shouldUpdate())
|
||||
return;
|
||||
|
||||
this->routeInfo = this->autopilot->getRouteInfo();
|
||||
this->printMenu();
|
||||
}
|
||||
|
||||
void MenuAutopilot::init() {
|
||||
this->setCountPages(8);
|
||||
this->driveManager->changeModus(Modi::Autopilot);
|
||||
this->autopilot = (Autopilot*) this->driveManager->getDriveModiPtr();
|
||||
this->routeInfo = this->autopilot->getRouteInfo();
|
||||
this->driveManager->getNavigation()->increaseMinDistanceToReachPoint();
|
||||
this->minDistance = this->driveManager->getNavigation()->decreaseMinDistanceToReachPoint();
|
||||
}
|
||||
@@ -1,59 +0,0 @@
|
||||
/**
|
||||
* @file menuAutopilot.h
|
||||
* @author Alexander Klein (alex@kleiax.de)
|
||||
* @brief Contains a class to print informations about the Autopilot
|
||||
* @version 0.1
|
||||
* @date 2022-02-03
|
||||
*
|
||||
* @copyright Copyright (c) 2022
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef MENU_AUTOPILOT_DRIVE_H
|
||||
#define MENU_AUTOPILOT_DRIVE_H
|
||||
|
||||
#include "SpecialMenus/driveModi/menuDriveMode.h"
|
||||
|
||||
/**
|
||||
* @brief A class to print informations about the Autopilot
|
||||
*
|
||||
*/
|
||||
class MenuAutopilot : public MenuDriveMode {
|
||||
public:
|
||||
/**
|
||||
* @brief Construct a new Menu Autopilot object
|
||||
*
|
||||
* @param driveManager for MenuDriveMode
|
||||
*/
|
||||
MenuAutopilot(DriveManager* driveManager) : MenuDriveMode(driveManager) {}
|
||||
|
||||
/**
|
||||
* @brief Prints the Information to display and console
|
||||
*
|
||||
* The informations are only printed to the display if it
|
||||
* set.
|
||||
*
|
||||
* Changes the DriveModi to Autopilot if it is the first time called
|
||||
* and save the Autopilot object.
|
||||
*/
|
||||
void printPage() const override;
|
||||
|
||||
|
||||
/**
|
||||
* @brief can be called to update shown data
|
||||
*/
|
||||
void update() override;
|
||||
|
||||
private:
|
||||
void init() override;
|
||||
void runCommand() const override;
|
||||
void runCommandNo() const override;
|
||||
|
||||
bool mutable targetFreezed = false;
|
||||
double mutable minDistance;
|
||||
|
||||
Autopilot* autopilot;
|
||||
RouteInfo routeInfo;
|
||||
};
|
||||
|
||||
#endif // MENU_AUTOPILOT_DRIVE_H
|
||||
@@ -1,138 +0,0 @@
|
||||
/**
|
||||
* @file menuCaptureRoute.cpp
|
||||
* @author Alexander Klein (alex@kleiax.de)
|
||||
* @brief Contains an implementation of the class MenuCaptureRoute
|
||||
* @version 0.1
|
||||
* @date 2022-01-31
|
||||
*
|
||||
* @copyright Copyright (c) 2022
|
||||
*
|
||||
*/
|
||||
#include "menuCaptureRoute.h"
|
||||
|
||||
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()) {
|
||||
case 0:
|
||||
lineOne = "Capture Route";
|
||||
lineTwo = "You can drive";
|
||||
break;
|
||||
|
||||
case 1:
|
||||
lineOne = "Saved waypoints";
|
||||
lineTwo.concat(routeInfo.totalPoints);
|
||||
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 4: {
|
||||
NTRIPClientStates status = this->driveManager->getNavigation()->getNTRIPClient()->getClientState();
|
||||
lineOne = "NTRIP Client is";
|
||||
|
||||
if (status == NTRIPClientStates::pushData)
|
||||
lineTwo = "enabled";
|
||||
else if (status == NTRIPClientStates::notAvailable)
|
||||
lineTwo = "not available";
|
||||
else
|
||||
lineTwo = "disabled";
|
||||
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;
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
this->print(lineOne, lineTwo);
|
||||
}
|
||||
|
||||
void MenuCaptureRoute::update() {
|
||||
if (!this->captureRoute->shouldUpdate())
|
||||
return;
|
||||
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(8);
|
||||
this->updateDelay = 500;
|
||||
this->driveManager->changeModus(Modi::CaptureRoute);
|
||||
this->captureRoute = (CaptureRoute*) this->driveManager->getDriveModiPtr();
|
||||
|
||||
}
|
||||
@@ -1,53 +0,0 @@
|
||||
/**
|
||||
* @file menuCaptureRoute.h
|
||||
* @author Alexander Klein (alex@kleiax.de)
|
||||
* @brief Contains a class to print informations about CaptureRoute
|
||||
* @version 0.1
|
||||
* @date 2022-01-31
|
||||
*
|
||||
* @copyright Copyright (c) 2022
|
||||
*
|
||||
*/
|
||||
#ifndef MENU_CAPTURE_ROUTE_H
|
||||
#define MENU_MANUAL_DRIVE_H
|
||||
|
||||
#include "SpecialMenus/driveModi/menuDriveMode.h"
|
||||
|
||||
/**
|
||||
* @brief A class to print informations about the Autopilot
|
||||
*
|
||||
*/
|
||||
class MenuCaptureRoute : public MenuDriveMode {
|
||||
public:
|
||||
/**
|
||||
* @brief Construct a new Menu Capture Route object
|
||||
*
|
||||
* @param driveManager
|
||||
*/
|
||||
MenuCaptureRoute(DriveManager* driveManager) : MenuDriveMode(driveManager) {}
|
||||
|
||||
/**
|
||||
* @brief Prints the Information to display and console
|
||||
*
|
||||
* The informations are only printed to the display if it
|
||||
* set.
|
||||
*
|
||||
* Changes the DriveModi to CaptureRoute if it is the first time called
|
||||
* and save the CaptureRoute object.
|
||||
*/
|
||||
void printPage() const override;
|
||||
|
||||
/**
|
||||
* @brief can be called to update shown data
|
||||
*/
|
||||
void update() override;
|
||||
|
||||
void runCommand() const override;
|
||||
|
||||
private:
|
||||
void init() override;
|
||||
|
||||
CaptureRoute* captureRoute;
|
||||
};
|
||||
|
||||
#endif // MENU_MANUAL_DRIVE_H
|
||||
@@ -1,250 +0,0 @@
|
||||
/**
|
||||
* @file autopilot.cpp
|
||||
* @author Alexander Klein (alex@kleiax.de)
|
||||
* @brief Contains the implementation of the class Autopilot
|
||||
* @version 0.1
|
||||
* @date 2022-02-02
|
||||
*
|
||||
* @copyright Copyright (c) 2022
|
||||
*
|
||||
*/
|
||||
|
||||
#include "driveModi/Modi/Autopilot/autopilot.h"
|
||||
#include "autopilot.h"
|
||||
|
||||
DirectionChangeSignal::DirectionChangeSignal(Navigation* navigation) {
|
||||
this->navigation = navigation;
|
||||
this->action();
|
||||
}
|
||||
|
||||
DirectionChangeSignal::~DirectionChangeSignal() {
|
||||
navigation->disableCalcAzimuth();
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
void Autopilot::run() {
|
||||
this->routeInfo = this->navigation->getRouteInfo();
|
||||
|
||||
switch (this->state) {
|
||||
case State::InsufficientAccuracy:
|
||||
this->askNavigationForOrder();
|
||||
return;
|
||||
|
||||
case State::NoRoute:
|
||||
return;
|
||||
|
||||
case State::None:
|
||||
return;
|
||||
|
||||
case State::NavigationStarted:
|
||||
this->askNavigationForOrder();
|
||||
this->checkButtonInput();
|
||||
break;
|
||||
|
||||
case State::GetToStartPoint:
|
||||
ManualControl::run();
|
||||
this->askNavigationForOrder();
|
||||
if (this->routeInfo.currentPoint >= 2)
|
||||
this->state = State::SelfDrivingAvailable;
|
||||
break;
|
||||
|
||||
case State::SelfDrivingAvailable:
|
||||
ManualControl::run();
|
||||
this->askNavigationForOrder();
|
||||
this->checkButtonInput();
|
||||
break;
|
||||
|
||||
case State::SelfDriving:
|
||||
this->askNavigationForOrder();
|
||||
this->checkButtonInput();
|
||||
this->selfDriving();
|
||||
break;
|
||||
|
||||
case SelfDrivingRotate:
|
||||
this->checkButtonInput();
|
||||
this->rotate();
|
||||
break;
|
||||
|
||||
case State::TargetReached:
|
||||
if (this->loopMode)
|
||||
this->restartLoop();
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void Autopilot::restart() {
|
||||
this->init();
|
||||
}
|
||||
|
||||
bool Autopilot::shouldUpdate() {
|
||||
if (this->updateDisplay) {
|
||||
this->updateDisplay = false;
|
||||
return true;
|
||||
}
|
||||
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;
|
||||
else
|
||||
this->state = State::NoRoute;
|
||||
this->routeInfo = this->navigation->getRouteInfo();
|
||||
this->navigation->getNTRIPClient()->setAutoReconnect(true);
|
||||
|
||||
this->lastOrderStatus = this->navigation->getCourseCorrection(this->courseCorrection, true);
|
||||
|
||||
this->courseCorrection.correction = 0;
|
||||
this->courseCorrection.distance = 0;
|
||||
this->updateDisplay = true;
|
||||
|
||||
this->maxRotationSpeed = 7;
|
||||
this->maxForwardSpeed = 1.5;
|
||||
}
|
||||
|
||||
void Autopilot::drive() {
|
||||
this->moveControl->setRotationSpeed(0);
|
||||
if (this->courseCorrection.distance >= this->minRemainingDistance)
|
||||
this->moveControl->setSpeed(this->maxForwardSpeed);
|
||||
else
|
||||
this,moveControl->setSpeed(0);
|
||||
}
|
||||
|
||||
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->maxRotationSpeed);
|
||||
else
|
||||
this->moveControl->setRotationSpeed(this->maxRotationSpeed);
|
||||
}
|
||||
}
|
||||
|
||||
void Autopilot::rotate() {
|
||||
if (abs(this->navigation->getAzimuth() - this->rotationAimAzimuth) < this->maxCourseDeviationBeforeAct) {
|
||||
this->endRotate();
|
||||
return;
|
||||
}
|
||||
|
||||
if ((abs(this->navigation->getAzimuth() - this->rotationAimAzimuth) < this->maxCourseDeviationBeforeAct * 3)
|
||||
&&
|
||||
((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)
|
||||
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() {
|
||||
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:
|
||||
if (this->state == State::InsufficientAccuracy)
|
||||
break;
|
||||
this->lastState = this->state;
|
||||
this->state = State::InsufficientAccuracy;
|
||||
this->moveControl->setSpeed(0);
|
||||
this->moveControl->setRotationSpeed(0);
|
||||
break;
|
||||
|
||||
case Navigation::Status::Unchanged:
|
||||
if (this->state == State::InsufficientAccuracy)
|
||||
this->state = this->lastState;
|
||||
break;
|
||||
|
||||
case Navigation::Status::Updated:
|
||||
if (this->state == State::InsufficientAccuracy)
|
||||
this->state = this->lastState;
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void Autopilot::selfDriving() {
|
||||
if (this->state != State::SelfDriving)
|
||||
return;
|
||||
|
||||
if (abs(this->courseCorrection.correction) >= this->maxCourseDeviationBeforeAct)
|
||||
this->beginRotate();
|
||||
else
|
||||
this->drive();
|
||||
}
|
||||
|
||||
void Autopilot::restartLoop() {
|
||||
this->navigation->startNavigation();
|
||||
this->state = State::SelfDriving;
|
||||
this->routeInfo = this->navigation->getRouteInfo();
|
||||
this->lastOrderStatus = this->navigation->getCourseCorrection(this->courseCorrection, true);
|
||||
this->updateDisplay = true;
|
||||
}
|
||||
@@ -1,134 +0,0 @@
|
||||
/**
|
||||
* @file autopilot.h
|
||||
* @author Alexander Klein (alex@kleiax.de)
|
||||
* @brief Contains a class which use the navigate class to drive automaticaly
|
||||
* @version 0.1
|
||||
* @date 2022-02-02
|
||||
*
|
||||
* @copyright Copyright (c) 2022
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef AUTOPILOT_H
|
||||
#define AUTOPILOT_H
|
||||
|
||||
|
||||
#include "navigation.h"
|
||||
#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
|
||||
*
|
||||
* This class get the information from the navigate class. When an
|
||||
* object of this class is constructed the rover can be driven manually.
|
||||
* When the Rover is near to the first position of the Route, you can
|
||||
* switch to automatic drive.
|
||||
*
|
||||
*/
|
||||
class Autopilot : public ManualControl {
|
||||
public:
|
||||
enum State {
|
||||
InsufficientAccuracy = -2,
|
||||
NoRoute = -1,
|
||||
None = 0,
|
||||
NavigationStarted,
|
||||
GetToStartPoint,
|
||||
SelfDrivingAvailable,
|
||||
SelfDriving,
|
||||
SelfDrivingRotate,
|
||||
TargetReached
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Construct a new Autopilot object
|
||||
*
|
||||
* @param moveControl for ManualControl
|
||||
* @param navigation for route instructions
|
||||
*/
|
||||
Autopilot(MoveControl* moveControl, const ControlPadInput *input, Navigation* navigation);
|
||||
|
||||
/**
|
||||
* @brief Destroy the Autopilot object
|
||||
*
|
||||
* Disconnect the NTRIP-Client
|
||||
*/
|
||||
~Autopilot();
|
||||
|
||||
void restart();
|
||||
|
||||
/**
|
||||
* @brief Get the Route Info object
|
||||
*
|
||||
* @return RouteInfo
|
||||
*/
|
||||
RouteInfo getRouteInfo() const { return this->routeInfo; }
|
||||
|
||||
/**
|
||||
* @brief Get the Course Correction object
|
||||
*
|
||||
* @return CourseCorrection
|
||||
*/
|
||||
CourseCorrection getCourseCorrection() const { return this->courseCorrection; }
|
||||
|
||||
/**
|
||||
* @brief Get the State object
|
||||
*
|
||||
* @return State
|
||||
*/
|
||||
State getState() const { return this->state; }
|
||||
|
||||
/**
|
||||
* @brief Tells if there are new informations to display
|
||||
*
|
||||
* @return true
|
||||
* @return false
|
||||
*/
|
||||
bool shouldUpdate();
|
||||
void testRotate(int16_t degree);
|
||||
void endRotate();
|
||||
void switchLoopMode() { this->loopMode = !this->loopMode; }
|
||||
bool getLoopMode() const { return this->loopMode; }
|
||||
|
||||
private:
|
||||
void init();
|
||||
void drive();
|
||||
void beginRotate();
|
||||
void rotate();
|
||||
void run() override;
|
||||
void checkButtonInput();
|
||||
void askNavigationForOrder();
|
||||
void selfDriving();
|
||||
void restartLoop();
|
||||
|
||||
Navigation* navigation;
|
||||
CourseCorrection courseCorrection;
|
||||
RouteInfo routeInfo;
|
||||
State state = State::None;
|
||||
State lastState = State::None;
|
||||
Navigation::Status lastOrderStatus;
|
||||
DirectionChangeSignal* directionChangeSignal;
|
||||
|
||||
bool updateDisplay = false;
|
||||
bool loopMode = false;
|
||||
|
||||
uint8_t maxCourseDeviationBeforeAct = 5;
|
||||
uint16_t autopilotChangeDelayMillis = 500;
|
||||
uint32_t lastAutopilotChangeMillis = 0;
|
||||
|
||||
int16_t rotationAimAzimuth;
|
||||
|
||||
double minRemainingDistance = 0.25;
|
||||
};
|
||||
|
||||
#endif // AUTOPILOT_H
|
||||
@@ -1,50 +0,0 @@
|
||||
/**
|
||||
* @file captureRoute.cpp
|
||||
* @author Alexander Klein (alex@kleiax.de)
|
||||
* @brief Contains the implementation of the class CaptureRoute
|
||||
* @version 0.1
|
||||
* @date 2022-02-15
|
||||
*
|
||||
* @copyright Copyright (c) 2022
|
||||
*
|
||||
*/
|
||||
|
||||
#include "driveModi/Modi/CaptureRoute/captureRoute.h"
|
||||
|
||||
CaptureRoute::CaptureRoute(MoveControl* moveControl, const ControlPadInput *input, Navigation* navigation)
|
||||
: 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);
|
||||
}
|
||||
|
||||
void CaptureRoute::run() {
|
||||
ManualControl::run();
|
||||
if (ControlPadButton::isControlPadButtonPressed(this->input, ControlPadButton::PadButton::Action)) {
|
||||
this->status = this->navigation->addCurrentPosToRoute();
|
||||
if (this->status == Navigation::Status::Updated) {
|
||||
this->lastSavedPoint = this->navigation->getCurrentPosition();
|
||||
this->routeInfo = navigation->getRouteInfo();
|
||||
this->updateDisplay = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
double CaptureRoute::getDistanceToLastPoint() const {
|
||||
if (!this->lastSavedPoint.isInit())
|
||||
return 0;
|
||||
return this->lastSavedPoint.distanceTo(this->navigation->getCurrentPosition());
|
||||
}
|
||||
|
||||
bool CaptureRoute::shouldUpdate() {
|
||||
if (this->updateDisplay) {
|
||||
this->updateDisplay = false;
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
@@ -1,85 +0,0 @@
|
||||
/**
|
||||
* @file captureRoute.h
|
||||
* @author Alexander Klein (alex@kleiax.de)
|
||||
* @brief Contains a class to capture a driven route
|
||||
* @version 0.1
|
||||
* @date 2022-02-15
|
||||
*
|
||||
* @copyright Copyright (c) 2022
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef CAPTURE_ROUTE_H
|
||||
#define CAPTURE_ROUTE_H
|
||||
|
||||
#include <iostream>
|
||||
|
||||
#include "driveModi/Modi/ManualControl/manualControl.h"
|
||||
#include "navigation.h"
|
||||
|
||||
/**
|
||||
* @brief A class to capture a driven class
|
||||
*
|
||||
* This class inherits ManualControl so you can drive
|
||||
* normally as in ManualControl. If GPS signal is valid
|
||||
* you can add a Point everytime you want.
|
||||
*
|
||||
*/
|
||||
class CaptureRoute : public ManualControl {
|
||||
public:
|
||||
/**
|
||||
* @brief Construct a new Capture Route object
|
||||
*
|
||||
* @param moveControl for ManualControl
|
||||
* @param navigation to add Points
|
||||
*/
|
||||
CaptureRoute(MoveControl* moveControl, const ControlPadInput *input, Navigation* navigation);
|
||||
|
||||
/**
|
||||
* @brief Destroy the Capture Route object
|
||||
*
|
||||
* Disconnect the NTRIP-Client
|
||||
*/
|
||||
~CaptureRoute();
|
||||
|
||||
// /**
|
||||
// * @brief Get the Navigation object
|
||||
// *
|
||||
// * @return Navigation*
|
||||
// */
|
||||
// Navigation* getNavigation() const { return this->navigation; }
|
||||
|
||||
/**
|
||||
* @brief Get the Route Info object
|
||||
*
|
||||
* @return RouteInfo
|
||||
*/
|
||||
RouteInfo getRouteInfo() const { return this->routeInfo; }
|
||||
Navigation::Status getLastStatus() const { return this->status; }
|
||||
|
||||
/**
|
||||
* @brief Get the distance to the last saved oint
|
||||
*
|
||||
* @return double in meters
|
||||
*/
|
||||
double getDistanceToLastPoint() const;
|
||||
|
||||
/**
|
||||
* @brief Tells if there are new informations to display
|
||||
*
|
||||
* @return true
|
||||
* @return false
|
||||
*/
|
||||
bool shouldUpdate();
|
||||
private:
|
||||
void run() override;
|
||||
|
||||
Navigation* navigation;
|
||||
RouteInfo routeInfo;
|
||||
Point lastSavedPoint;
|
||||
Navigation::Status status = Navigation::Status::Complete;
|
||||
|
||||
bool updateDisplay = false;
|
||||
};
|
||||
|
||||
#endif // CAPTURE_ROUTE_H
|
||||
Reference in New Issue
Block a user