Merge branch 'master' of https://git.efi.th-nuernberg.de/gitea/yasarba71520/Line-Following-Robot

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;
-
-    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;
+void ControlModule::drive(double ratio)
+{
+    motors[0] += ratio;
+    motors[1] += ratio;
+    motors[2] += ratio;
+    motors[3] += ratio;
+}
 
-    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]};
-} 
+}
+
+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);
+}

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();

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;