71 lines
1.8 KiB
C++
71 lines
1.8 KiB
C++
/**
|
|
* @file autopilot.cpp
|
|
* @author Alexander Klein (alex@kleiax.de)
|
|
* @brief
|
|
* @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->courseCorrection.correction = 0;
|
|
this->courseCorrection.distance = 0;
|
|
}
|
|
|
|
void Autopilot::loop() {
|
|
if (!this->selfDriving)
|
|
ManualControl::loop();
|
|
|
|
if (millis() - this->last_millis < delay) {
|
|
return;
|
|
}
|
|
this->runAutopilot();
|
|
this->last_millis = millis();
|
|
}
|
|
|
|
void Autopilot::runAutopilot() {
|
|
this->courseCorrection = this->navigation->getCourseCorrection();
|
|
if (selfDriving) {
|
|
this->setSpeedInRelToDistance(this->courseCorrection.distance);
|
|
this->setRotInRelToDistance(this->courseCorrection.correction);
|
|
}
|
|
}
|
|
|
|
void Autopilot::setSelfDriving(bool val) {
|
|
if (!this->navigationStarted)
|
|
return;
|
|
|
|
this->selfDriving = val;
|
|
this->moveControl->setSpeed(0);
|
|
this->moveControl->setRotationspeed(0);
|
|
}
|
|
|
|
void Autopilot::setSpeedInRelToDistance(double distance) {
|
|
// TODO: Delte Magic Numbers
|
|
if (distance > 2)
|
|
this->moveControl->setSpeed(1.5);
|
|
else if (distance < 0.5)
|
|
this->moveControl->setSpeed(1.5);
|
|
else
|
|
this->moveControl->setSpeed(0);
|
|
}
|
|
|
|
void Autopilot::setRotInRelToDistance(int16_t course) {
|
|
// TODO: Delte Magic Numbers
|
|
int8_t steps = course / 30;
|
|
|
|
if (steps == 0 && abs(course) >= 5)
|
|
steps = 1;
|
|
|
|
if (course == 0)
|
|
this->moveControl->setRotationspeed(0);
|
|
else
|
|
this->moveControl->setRotationspeed(steps);
|
|
} |