refactor autopilot

src/driveModi/Modi/Autopilot/autopilot.cpp

@@ -72,8 +72,10 @@ void Autopilot::runAutopilot() {
     }
 
     if (this->state == State::SelfDriving) {
-        this->setSpeedInRelToDistance(this->courseCorrection.distance);
-        this->setRotInRelToDistance(this->courseCorrection.correction);
+        if (abs(this->courseCorrection.correction) >= this->maxCourseDeviationBerforeAct)
+            this->rotate();
+        else
+            this->drive();
     }   
 }
 
@@ -127,25 +129,18 @@ void Autopilot::checkNtrip() {
     }
 }
 
-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);
+void Autopilot::drive() {
+    this->moveControl->setRotationSpeed(0);
+    if (this->courseCorrection.distance >= this->minRemainingDistance) 
+        this->moveControl->setSpeed(this->drivingSpeed);
     else
-        this->moveControl->setRotationSpeed(steps);
+        this,moveControl->setSpeed(0);
+}
+
+void Autopilot::rotate() {
+    this->moveControl->setSpeed(0);
+    if (this->courseCorrection.correction > 0)
+        this->moveControl->setRotationSpeed(this->rotationSpeed);
+    else
+        this->moveControl->setRotationSpeed(-this->rotationSpeed);
 }