From 8ed91c4b11b53a430e7334a2738eeb29918951d9 Mon Sep 17 00:00:00 2001
From: Alexander Klein <alex@kleiax.de>
Date: Mon, 7 Aug 2023 13:05:42 +0200
Subject: [PATCH] small fixes

---
 doc/Notes and TODOs/TODO allgemein.txt             |  4 ++++
 lib/MotorControl/motorControl.h                    |  2 +-
 lib/Navigation/navigation.cpp                      |  1 +
 .../driveModi/Autopilot/menuAutopilot.cpp          |  2 +-
 src/driveModi/Modi/Autopilot/autopilot.cpp         | 14 ++++++--------
 src/driveModi/Modi/Autopilot/autopilot.h           |  2 +-
 src/moveControl.cpp                                |  4 ++--
 7 files changed, 16 insertions(+), 13 deletions(-)

diff --git a/doc/Notes and TODOs/TODO allgemein.txt b/doc/Notes and TODOs/TODO allgemein.txt
index 03df2c4..d987cd2 100644
--- a/doc/Notes and TODOs/TODO allgemein.txt	
+++ b/doc/Notes and TODOs/TODO allgemein.txt	
@@ -1,3 +1,7 @@
     -> Program underfloorLighting
     -> Add an beeper
     -> Program the beeper
+
+    Speedometer buffer ergibt kaum Sinn
+    Abweichung max 25 cm bevor zu ungenau damit nie mehr als 50 cm gesamt.
+    
diff --git a/lib/MotorControl/motorControl.h b/lib/MotorControl/motorControl.h
index 9632e99..ed070aa 100644
--- a/lib/MotorControl/motorControl.h
+++ b/lib/MotorControl/motorControl.h
@@ -21,7 +21,7 @@
 #define PWMRES 8
 #define POWERSTEPS 2       //  A total of 20 levels ( 100 / SPEED_STEPS ) * RUN_MOTOR_CONTROL_DELAY = 500ms
 #define PWMMIN 55
-#define PWMMAX 90         // Max 98% of 2^PWM_RES
+#define PWMMAX 94         // Max 98% of 2^PWM_RES
 
 /**
  * @brief A class which use PWM to control the power of DC Motor
diff --git a/lib/Navigation/navigation.cpp b/lib/Navigation/navigation.cpp
index cdae8df..23a2bc9 100644
--- a/lib/Navigation/navigation.cpp
+++ b/lib/Navigation/navigation.cpp
@@ -134,6 +134,7 @@ Navigation::Status Navigation::getCourseCorrection(CourseCorrection& correction,
     if (this->currentPosition.distanceTo(this->lastPointCalcCorrection) < (this->minDistanceToReachPoint / 2.0)
         && !forceUpdate) {
         correction.correction = this->calculateCourseCorrection(this->lastPointCalcCorrection);
+        correction.distance = this->lastPointCalcCorrection.distanceTo(this->targetPoint);
         return Status::Unchanged;
     }
 
diff --git a/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp b/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp
index 1c6bb46..94d3828 100644
--- a/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp
+++ b/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp
@@ -43,7 +43,7 @@ void MenuAutopilot::printPage() const {
             lineTwo = "";
             switch (this->autopilot->getState()) {
                 case Autopilot::State::InsufficientAccuarcy :
-                    lineTwo = "Err: No NTRIP";
+                    lineTwo = "Err: LowAccuracy";
                     break;
 
                 case Autopilot::State::NoRoute :
diff --git a/src/driveModi/Modi/Autopilot/autopilot.cpp b/src/driveModi/Modi/Autopilot/autopilot.cpp
index 8fbc417..fe5ddbd 100644
--- a/src/driveModi/Modi/Autopilot/autopilot.cpp
+++ b/src/driveModi/Modi/Autopilot/autopilot.cpp
@@ -119,10 +119,10 @@ void Autopilot::drive() {
 void Autopilot::rotate() {
     this->moveControl->setSpeed(0);
     if (this->courseCorrection.correction > 0)
-        this->moveControl->setRotationSpeed(this->rotationSpeed);
+        this->moveControl->setRotationSpeed(-this->rotationSpeed);
     else
 
-    this->moveControl->setRotationSpeed(-this->rotationSpeed);
+    this->moveControl->setRotationSpeed(this->rotationSpeed);
 }
 
 void Autopilot::checkButtonInput() {
@@ -172,10 +172,8 @@ void Autopilot::askNavigationForOrder() {
 }
 
 void Autopilot::selfDriving() {
-    if (this->lastOrderStatus == Navigation::Status::Updated) {
-        if (abs(this->courseCorrection.correction) >= this->maxCourseDeviationBerforeAct)
-            this->rotate();
-        else
-            this->drive();
-    }
+    if (abs(this->courseCorrection.correction) >= this->maxCourseDeviationBerforeAct)
+        this->rotate();
+    else
+        this->drive();
 }
diff --git a/src/driveModi/Modi/Autopilot/autopilot.h b/src/driveModi/Modi/Autopilot/autopilot.h
index 4cd124b..a64c87b 100644
--- a/src/driveModi/Modi/Autopilot/autopilot.h
+++ b/src/driveModi/Modi/Autopilot/autopilot.h
@@ -132,7 +132,7 @@ class Autopilot : public ManualControl {
         uint32_t lastAutopilotChangeMillis = 0;
 
         double drivingSpeed = 1;
-        double rotationSpeed = 3;
+        double rotationSpeed = 4.5;
         double minRemainingDistance = 0.25;
 };
 
diff --git a/src/moveControl.cpp b/src/moveControl.cpp
index 48090c6..a8bc80d 100644
--- a/src/moveControl.cpp
+++ b/src/moveControl.cpp
@@ -187,9 +187,9 @@ void MoveControl::calcTargetWheelSpeed() {
                \ 1 -b / \ T /   \ Xr /  */
 
     // (1 / r) * 1
-    const static double A = 15.82278481;
+    constexpr double A = 15.82278481;
     // (1 / r) * b
-    const static double B = 2.096518987;
+    constexpr double B = 2.096518987;
 
     this->wheelspeed_right_target = (A * this->x_speed + B * this->rotation_speed) * (WHEEL_DIAMETER / 2);
     this->wheelspeed_left_target  = (A * this->x_speed + (-B) * this->rotation_speed) * (WHEEL_DIAMETER / 2);