From 6692a2fcdaa869153e808a378b0a214199d05500 Mon Sep 17 00:00:00 2001
From: TimZnr <zeunerti74726@th-nuernberg.de>
Date: Wed, 1 Feb 2023 17:26:35 +0100
Subject: [PATCH 1/2] combine rotation and translation

---
 .../ControlModule/control_module.cpp          | 54 ++++++++++++-------
 AutonomousMode/ControlModule/control_module.h |  4 +-
 2 files changed, 37 insertions(+), 21 deletions(-)

diff --git a/AutonomousMode/ControlModule/control_module.cpp b/AutonomousMode/ControlModule/control_module.cpp
index 1fd8120..e510a4a 100644
--- a/AutonomousMode/ControlModule/control_module.cpp
+++ b/AutonomousMode/ControlModule/control_module.cpp
@@ -48,24 +48,20 @@ void ControlModule::moveSide(int imageColumsMiddle, int contourColumsMiddle){
     motors[3] += speed;
 }
 
-void ControlModule::rotate(double angle){
-    double minAngularSpeed = -1.0;
-    double maxAngularSpeed = 1.0;
-    double speedRange = maxAngularSpeed - minAngularSpeed;
+void ControlModule::drive(double ratio)
+{
+    motors[0] += ratio;
+    motors[1] += ratio;
+    motors[2] += ratio;
+    motors[3] += ratio;
+}
 
-    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;
-    motors[3] -= speed;
+void ControlModule::rotate(double ratio)
+{
+    motors[0] += ratio;
+    motors[1] -= ratio;
+    motors[2] += ratio;
+    motors[3] -= ratio;
 }
 
 void ControlModule::unit(){
@@ -87,8 +83,9 @@ void ControlModule::unit(){
 
 void ControlModule::calcSpeeds(int imageColumsMiddle, int contourColumsMiddle, double angle){
     std::unique_lock<std::mutex> lock(mtx);
-    //moveSide(imageColumsMiddle, contourColumsMiddle);
-    rotate(angle);
+    double rotationRatio = ratio(angle);
+    rotate(rotationRatio);
+    drive(1.0-rotationRatio);
     unit();
     adjustSpeed();
 }
@@ -97,4 +94,21 @@ std::vector<double> ControlModule::readMotors()
 {
     std::unique_lock<std::mutex> lock(mtx);
     return std::vector<double> {motors[0], motors[1], motors[2], motors[3]};
-} 
\ No newline at end of file
+}
+
+double ControlModule::ratio(double angle)
+{
+    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;
+    return abs(speed);
+}
\ No newline at end of file
diff --git a/AutonomousMode/ControlModule/control_module.h b/AutonomousMode/ControlModule/control_module.h
index 33aee68..07a0cf6 100644
--- a/AutonomousMode/ControlModule/control_module.h
+++ b/AutonomousMode/ControlModule/control_module.h
@@ -17,8 +17,10 @@ public:
     ~ControlModule();
     void adjustSpeed();
     void moveSide(int imageColumsMiddle, int contourColumsMiddle);
-    void rotate(double angle);
+    void rotate(double ratio);
+    void drive(double ratio);
     void unit();            //Brings the max Value to 1.0
+    double ratio(double angle);
 
     void calcSpeeds(int imageColumsMiddle, int contourColumsMiddle, double angle); //Funktion to be called
     std::vector<double> readMotors();

From 7814f648730ade098d6a9cf6e136ce38423fd46e Mon Sep 17 00:00:00 2001
From: TimZnr <zeunerti74726@th-nuernberg.de>
Date: Wed, 1 Feb 2023 17:35:52 +0100
Subject: [PATCH 2/2] Allow neg. angle

---
 AutonomousMode/Processing/processing.cpp | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/AutonomousMode/Processing/processing.cpp b/AutonomousMode/Processing/processing.cpp
index 630bd72..d041ead 100644
--- a/AutonomousMode/Processing/processing.cpp
+++ b/AutonomousMode/Processing/processing.cpp
@@ -144,6 +144,10 @@ void Processing::calcAngles(FrameData &data, int imageColums, int imageRows, boo
         LFRVector b(contourCenter-focusPoint);
 
         double angle = a.angle(b);
+        if (b.x < 0.0)
+        {
+            angle *= -1.0;
+        }
 
         //Write to Data
         data.angle = angle;