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)
+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);
 }

src/driveModi/Modi/Autopilot/autopilot.h

@@ -15,7 +15,6 @@
 
 #include "navigation.h"
 #include "moveControl.h"
-#include "driveModi/Modi/Autopilot/autopilotConfig.h"
 #include "driveModi/Modi/ManualControl/manualControl.h"
 
 /**
@@ -110,8 +109,8 @@ class Autopilot : public ManualControl {
     private:
         void init();
         void checkNtrip();
-        void setSpeedInRelToDistance(double distance);
-        void setRotInRelToDistance(int16_t course);
+        void drive();
+        void rotate();
 
         Navigation* navigation;
         CourseCorrection courseCorrection;
@@ -124,6 +123,7 @@ class Autopilot : public ManualControl {
         uint8_t loopDelayMillis = 40;
         uint8_t ntripReconnectAttemps = 0;
         uint8_t ntripReconnectMaxAttemps = 10;
+        uint8_t maxCourseDeviationBerforeAct = 5;
         uint16_t displayUpdateDelayMillis = 1000;
         uint16_t reconnectNtripDelayMillis = 1000;
         uint16_t autopilotChangeDelayMillis = 500;
@@ -131,6 +131,10 @@ class Autopilot : public ManualControl {
         uint32_t loopLastMillis = 0;
         uint32_t reconnectNtripLastMillis = 0;
         uint32_t lastAutopilotChangeMillis = 0;
+
+        double drivingSpeed = 1;
+        double rotationSpeed = 7;
+        double minRemainingDistance = 0.25;
 };
 
 #endif // AUTOPILOT_H

src/driveModi/Modi/Autopilot/autopilotConfig.h

@@ -1,54 +0,0 @@
-/**
- * @file autopilotConfig.h
- * @author Alexander Klein (alex@kleiax.de)
- * @brief Some defines to configure the behaviour of the autopilot
- * @version 0.1
- * @date 2022-03-02
- * 
- * @copyright Copyright (c) 2022
- * 
- */
-
-#ifndef AUTOPILOT_CONFIG_H
-#define AUTOPILOT_CONFIG_H
-
-/**
- * @brief 
- * 
- */
-#define AUTOPILOT_MAX_SPEED 0.5
-
-/**
- * @brief Max speed in the near of target point
- */
-#define AUTOPILOT_SPEED_2 0.25
-
-/**
- * @brief Distance in meters
- * 
- * If this distance to the next point is reached the autopilot will drive
- * now with the speed set by AUTOPILOT_SPEED_2
- */
-#define MIN_DISTANCE_FOR_SPEED_2 4
-
-/**
- * @brief Needed distance to the next point to choose the next point as target.
- */
-#define MIN_DISTANCE_TO_NEXT_POINT 0.5
-
-
-/**
- * @brief Minimal course correction
- * If the needed correction in degree is lower than this value the rover will
- * be drive straight on.
- */
-#define MIN_COURSE_CORRECTION_TO_ACT 5
-
-/**
- * @brief Course correction factor
- * This value is used by the setRotInRelToDistance function and is used to calculate
- * the value for setRotationSpeed.
- */
-#define COURSE_CORRECTION_FACTOR 60
-
-#endif // AUTOPILOT_CONFIG_H