From 6da9bfff542362d6a9ee13efef0b301c49d52165 Mon Sep 17 00:00:00 2001
From: Tim Zeuner <zeunerti74726@th-nuernberg.de>
Date: Fri, 3 Mar 2023 18:46:31 +0100
Subject: [PATCH] Bugs found during tests

---
 .../ControlModule/control_module.cpp           | 18 +++++++++++++-----
 1 file changed, 13 insertions(+), 5 deletions(-)

diff --git a/AutonomousMode/ControlModule/control_module.cpp b/AutonomousMode/ControlModule/control_module.cpp
index d40c90d..8798153 100644
--- a/AutonomousMode/ControlModule/control_module.cpp
+++ b/AutonomousMode/ControlModule/control_module.cpp
@@ -36,7 +36,7 @@ void ControlModule::adjustSpeed(){
 
     for(int i = 0; i < 4; i++)
     {
-        motors[i] *= maxSpeed;
+        motors[i] *= factor;
     }
 };
 
@@ -85,7 +85,7 @@ void ControlModule::calcSpeeds(int imageColumsMiddle, int contourColumsMiddle, d
     std::unique_lock<std::mutex> lock(mtx);
     double rotationRatio = ratio(angle);
     rotate(rotationRatio);
-    drive(1.0-rotationRatio);
+    drive(1.0-std::fabs(rotationRatio));
     unit();
     adjustSpeed();
 }
@@ -102,13 +102,21 @@ double ControlModule::ratio(double angle)
     double maxAngularSpeed = 1.0;
     double speedRange = maxAngularSpeed - minAngularSpeed;
 
-    double minAngle = -90.0;
-    double maxAngle = 90.0;
+    double minAngle = -20.0;
+    double maxAngle = 20.0;
     double angularRange = maxAngle - minAngle;
 
+    if(angle < minAngle)
+    {
+        angle = minAngle;
+    }
+    if(angle > maxAngle)
+    {
+        angle = maxAngle;
+    }
     double progress = angle - minAngle;
     double progressPercent = progress/angularRange;
 
     double speed = minAngularSpeed + progressPercent*speedRange;
-    return std::fabs(speed);
+    return speed;
 }
\ No newline at end of file