From 0f5c2e3f4251715dbae5d7e2581c8dd32793b046 Mon Sep 17 00:00:00 2001
From: TimZnr <zeunerti74726@th-nuernberg.de>
Date: Tue, 31 Jan 2023 22:03:25 +0100
Subject: [PATCH] P-Controller Concept

---
 AutonomousMode/ControlModule/control_module.cpp | 14 +++++++++++++-
 1 file changed, 13 insertions(+), 1 deletion(-)

diff --git a/AutonomousMode/ControlModule/control_module.cpp b/AutonomousMode/ControlModule/control_module.cpp
index a80bea4..1fd8120 100644
--- a/AutonomousMode/ControlModule/control_module.cpp
+++ b/AutonomousMode/ControlModule/control_module.cpp
@@ -49,7 +49,19 @@ void ControlModule::moveSide(int imageColumsMiddle, int contourColumsMiddle){
 }
 
 void ControlModule::rotate(double angle){
-    double speed = rotateSpeed * (angle + 90.0)/90.0;
+    double minAngularSpeed = -1.0;
+    double maxAngularSpeed = 1.0;
+    double speedRange = maxAngularSpeed - minAngularSpeed;
+
+    double minAngle = -90.0;
+    double maxAngle = 90.0;
+    double angularRange = maxAngle - minAngle;
+
+    double progress = angle - minAngle;
+    double progressPercent = progress/angularRange;
+
+    double speed = minAngularSpeed + progressPercent*speedRange;
+
     motors[0] += speed;
     motors[1] -= speed;
     motors[2] += speed;