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

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;

AutonomousMode/Processing/processing.cpp

@@ -82,58 +82,68 @@ void Processing::calcAngles(FrameData &data, int imageColums, int imageRows, boo
     
     if(data.contours.size() > 0)
     { 
-    int angle = 0;
-    int index;    
-            if(turnLeft){
-                // get the most left/right contour
-                int leftmostEdge = imageColums;
-                
-                for (int i = 0; i < data.leftEdges.size(); i++)
-                {
-                    int edge = data.leftEdges[i].x;
-                                            
-                    if (edge <= leftmostEdge)
-                    {
-                        leftmostEdge = edge;
-                        index = i;
-                    }
-                }
-            }else
+        int index;
+        // get the most left/right contour
+        
+        int leftmostEdge = imageColums;
+        int rightmostEdge = 0;
+
+        for (int i = 0; i < data.leftEdges.size(); i++)
+        {
+            int edge = data.leftEdges[i].x;
+                                    
+            if (edge <= leftmostEdge && turnLeft)
             {
-                int rightmostEdge = 0;
-                
-                for (int i = 0; i < data.leftEdges.size(); i++)
-                {
-                    int edge = data.leftEdges[i].x;
-                    
-                    if (edge >= rightmostEdge)
-                    {
-                        rightmostEdge = edge;
-                        index = i;
-                    }
-                }
-            }    
-            // Find the Top-Left and Buttom-Left Point of the Contour
-            
-            std::vector<cv::Point> leftMostContour = data.contours[index];
-            int xleftButtom = imageColums; 
-            int xleftTop = imageColums;
-            int yButtom = data.boundingBoxes[index].y;
-            int yTop = data.boundingBoxes[index].height;
+                leftmostEdge = edge;
+                index = i;
+            }
+            else if(edge >= rightmostEdge && !turnLeft)
+            {
+                rightmostEdge = edge;
+                index = i;
+            }
+        }
+  
+        // Find the Top-Left and Bottom-Left Point of the Contour
             
-            for (int i = 0; i < leftMostContour.size(); i++){
-                if(leftMostContour[i].y == 0 && leftMostContour[i].x < xleftButtom)
-                {
-                    xleftButtom = leftMostContour[i].x;
-                }else if(leftMostContour[i].y > yTop -4 && leftMostContour[i].x < xleftTop){
-                    xleftTop = leftMostContour[i].x;
-                }
+        std::vector<cv::Point> leftMostContour = data.contours[index];
+
+        /*
+        Not going to dive into that part to fix it.
+        Trying to use centroid instead.
+        int xleftBottom = imageColums; 
+        int xleftTop = imageColums;
+        int yBottom = data.boundingBoxes[index].y;
+        int yTop = data.boundingBoxes[index].height;
+        
+        for (int i = 0; i < leftMostContour.size(); i++)
+        {
+            if(leftMostContour[i].y == 0 && leftMostContour[i].x < xleftBottom)
+            {
+                xleftBottom = leftMostContour[i].x;
+            }
+            else if(leftMostContour[i].y > yTop -4 && leftMostContour[i].x < xleftTop)
+            {
+                xleftTop = leftMostContour[i].x;
             }
+        }
 
         // calculate angle
-        int deltaX = abs(xleftButtom - xleftTop);
+        int deltaX = abs(xleftBottom - xleftTop);
         int deltaY = yTop;                
         angle = Calcs::calcAngle(deltaX, deltaY);
+        */
+
+        //Get the contours center using moments:
+        cv::Moments moments=cv::moments(leftMostContour);
+        LFRPoint contourCenter(moments.m10/moments.m00, moments.m01/moments.m00);
+        LFRPoint imageCenter(double(imageColums)/2.0, double(imageRows)/2.0);
+        LFRPoint focusPoint(imageCenter.x, imageCenter.y+1000.0);
+
+        LFRVector a(imageCenter-focusPoint);
+        LFRVector b(contourCenter-focusPoint);
+
+        double angle = a.angle(b);
 
         //Write to Data
         data.angle = angle;

AutonomousMode/lfr.cpp

@@ -139,12 +139,13 @@ cv::Mat LFR::provideOutput(Mat originalImage, Mat processedImage, const FrameDat
     {
         //Draw the Arrow for the check of the angle
         int length = 100;
-        Point P1 = frameData.middlePoints[frameData.index];
-        Point P2;
 
-        P2.x =  (int)round(P1.x + length * cos(frameData.angle * CV_PI / 180.0));
-        P2.y =  (int)round(P1.y + length * sin(frameData.angle * CV_PI / 180.0));
-        cv::arrowedLine(originalImage, P1, P2, Scalar(0,0,255), 2, 8);
+        Point contourCenter = frameData.middlePoints[frameData.index];
+        Point imageCenter = Point(originalImage.size().width/2.0, originalImage.size().height/2.0);
+        Point focalPoint = Point(imageCenter.x, imageCenter.y+1000.0);
+
+        cv::arrowedLine(originalImage, focalPoint, contourCenter, Scalar(0,0,255), 2, 8, 0, 0.03);
+        cv::arrowedLine(originalImage, focalPoint, imageCenter, Scalar(0,0,255), 2, 8, 0, 0.03);
     }
     return originalImage;
 }