delete everthing for bachelore
This commit is contained in:
@@ -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