delete magic numbers in autopilot

include/networkConfig.h

@@ -14,7 +14,7 @@
 
 // General config
 #define MQTT_TIME_RECONNECT 2500
-#define ESP32BROKER
+#define HW1
 
 
 //Network config ESP32

src/driveModi/Modi/Autopilot/autopilot.cpp

@@ -80,20 +80,20 @@ void Autopilot::setSelfDriving(bool val) {
 }
 
 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
+    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) {
-    // TODO: Delte Magic Numbers
-    int8_t steps = course / 30;
+    int8_t steps = course / COURSE_CORRECTION_FACTOR;
 
-    if (steps == 0 && abs(course) >= 5)
+    if (steps == 0 && abs(course) >= MIN_COURSE_CORRECTION_TO_ACT)
         steps = 1;
 
     if (course == 0)

src/driveModi/Modi/Autopilot/autopilot.h

@@ -15,6 +15,7 @@
 
 #include "navigation.h"
 #include "moveControl.h"
+#include "driveModi/Modi/Autopilot/autopilotConfig.h"
 #include "driveModi/Modi/ManualControl/manualControl.h"
 
 /**

src/driveModi/Modi/Autopilot/autopilotConfig.h

@@ -0,0 +1,54 @@
+/**
+ * @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 2
+
+
+/**
+ * @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 30
+
+#endif // AUTOPILOT_CONFIG_H