ready to test capture Route and Autopiloz

2022-02-03 19:50:48 +01:00
parent 5ca21b09b2
commit bc37abcb66
20 changed files with 226 additions and 39 deletions
@@ -11,10 +11,61 @@

 #include "driveModi/Modi/Autopilot/autopilot.h"

-Autopilot::Autopilot() {
+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);
 }