104 lines
2.9 KiB
C++
104 lines
2.9 KiB
C++
/**
|
|
* @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"
|
|
|
|
Autopilot::Autopilot(MoveControl* moveControl, Navigation* navigation)
|
|
: ManualControl(moveControl) {
|
|
this->navigation = navigation;
|
|
this->navigationStarted = this->navigation->startNavigation();
|
|
this->navigation->getRouteInfo();
|
|
this->courseCorrection.correction = 0;
|
|
this->courseCorrection.distance = 0;
|
|
this->updateDisplay = true;
|
|
}
|
|
|
|
void Autopilot::loop() {
|
|
if (!this->selfDriving && this->navigationStarted)
|
|
ManualControl::loop();
|
|
|
|
if (millis() - this->last_millis < delay)
|
|
return;
|
|
|
|
this->runAutopilot();
|
|
this->last_millis = millis();
|
|
}
|
|
|
|
void Autopilot::runAutopilot() {
|
|
if (!this->navigationStarted || this->navigationEnded)
|
|
return;
|
|
|
|
this->courseCorrection = this->navigation->getCourseCorrection();
|
|
this->routeInfo = this->navigation->getRouteInfo();
|
|
this->updateDisplay = true;
|
|
|
|
// Check if start Point is near to current Location
|
|
if (this->routeInfo.currentPoint >= 2)
|
|
this->selfDrivingAvailable = true;
|
|
|
|
if (!this->selfDriving && this->selfDrivingAvailable)
|
|
this->setSelfDriving(Ps3.data.button.triangle);
|
|
|
|
if (this->routeInfo.currentPoint == this->routeInfo.totalPoints) {
|
|
this->navigationEnded = true;
|
|
this->navigationStarted = false;
|
|
this->selfDrivingAvailable = false;
|
|
this->setSelfDriving(false);
|
|
}
|
|
|
|
if (selfDriving) {
|
|
this->setSpeedInRelToDistance(this->courseCorrection.distance);
|
|
this->setRotInRelToDistance(this->courseCorrection.correction);
|
|
}
|
|
}
|
|
|
|
bool Autopilot::shouldUpdate() {
|
|
if (this->updateDisplay) {
|
|
this->updateDisplay = false;
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
void Autopilot::setSelfDriving(bool val) {
|
|
if (!this->navigationStarted)
|
|
return;
|
|
|
|
this->updateDisplay = true;
|
|
|
|
this->selfDriving = val;
|
|
this->moveControl->setSpeed(0);
|
|
this->moveControl->setRotationspeed(0);
|
|
}
|
|
|
|
void Autopilot::setSpeedInRelToDistance(double distance) {
|
|
if (distance >= MIN_DISTANCE_TO_NEXT_POINT)
|
|
this->moveControl->setSpeed(AUTOPILOT_MAX_SPEED);
|
|
else if (distance < MIN_DISTANCE_TO_NEXT_POINT)
|
|
this->moveControl->setSpeed(AUTOPILOT_SPEED_2);
|
|
else {
|
|
this->moveControl->setSpeed(0);
|
|
std::cout << "Error in Autopilot::setSpeedInRelToDistance" << std::endl;
|
|
}
|
|
}
|
|
|
|
void Autopilot::setRotInRelToDistance(int16_t course) {
|
|
int8_t steps = course / COURSE_CORRECTION_FACTOR;
|
|
|
|
if (steps == 0 && abs(course) >= MIN_COURSE_CORRECTION_TO_ACT)
|
|
steps = 1;
|
|
|
|
if (course == 0)
|
|
this->moveControl->setRotationspeed(0);
|
|
else
|
|
this->moveControl->setRotationspeed(steps);
|
|
}
|