Erster Entwurft für Controlling

AutonomousMode/Utils/utils.cpp

@@ -48,75 +48,20 @@ LFRLine::~LFRLine()
 {
 }
 
-VectorOfLines::VectorOfLines()
-{
-}
 
-VectorOfLines::~VectorOfLines()
-{
-}
 
-double VectorOfLines::calcGradient(Point p0, Point p1)
-{
-    double gradient = (p1.y - p0.y)/(p1.x - p0.x + 1e-10);
-    return p1.x > p0.x ? gradient : - gradient;
-}
-    
-float VectorOfLines::calcZeroPoint(cv::Point x, float m)
-{
-    return 0.0;
-}
-
-double VectorOfLines::calcDistance(Point p0, Point p1)
-{
-    return sqrt(pow(p1.y - p0.y, 2) + pow(p1.x - p0.x, 2));
-}
-
-vector<Vec4i> VectorOfLines::findMiddleLine(vector<Vec4i> &lines){
-    Point point11;
-    Point point12;
-    Point point21;
-    Point point22;
-    vector<Vec4i> middleLines;
-    for( size_t i = 0; i < (lines.size() - 1); i++ )
-    {
-        point11 = Point(lines[i][0], lines[i][1]);
-        point12 = Point( lines[i][2], lines[i][3]);
-        double gradient1 = VectorOfLines::calcGradient(point11, point12);
-
-        //Compare every Line with the other
-        for( size_t j = 0; j < (lines.size()); j++ )
-        {
-            if(j != i)
-            {
-                point21 = Point(lines[j][0], lines[j][1]);
-                point22 = Point(lines[j][2], lines[j][3]);
-                double gradient2 = VectorOfLines::calcGradient(point21, point22);
-                if(norm(gradient1 - gradient2) < 0.15)
-                {
-                    middleLines.push_back(Vec4i((point11.x+point21.x)/2, (point11.y+point21.y)/2, (point12.x+point22.x)/2, (point12.y+point22.y)/2));   
-                }
-            }
-        }
-    
-    }
-    return middleLines;
-}
-
-int VectorOfLines::calcAngle(int deltaX, int deltaY){
+int Calcs::calcAngle(int deltaX, int deltaY){
 
         int refAngle = 0;
 
         if(deltaX > 10E-12){
-        refAngle = atan(deltaY/deltaX) * 180/PI;
+        refAngle = (int)((atan(deltaY/deltaX) * 180.0/PI) + 0.5 - (refAngle<0));  //Here 0.5 (or -0.5) is added to round a float number to int right
 
         // convert from img coordinates to regbot coordinates
         refAngle = -(refAngle);
 
         if (refAngle > 90)
-            refAngle = refAngle - 180;
-
-        cout << refAngle << "\n";
+            refAngle = refAngle - 180; 
         }else{
             refAngle = -90;
         }

AutonomousMode/Utils/utils.h

@@ -47,20 +47,10 @@ public:
     ~LFRLine();
 };
 
-class VectorOfLines{
+class Calcs{
     private:
     public:
-    Point startPoint;
-    float gradient;
-    float zeroPoint;
-    VectorOfLines();
-    ~VectorOfLines();
-    static double calcGradient(Point x, Point y);
-    float calcZeroPoint(cv::Point x, float m);
-    static double calcDistance(Point p0, Point p1);
-    vector<Vec4i> findMiddleLine(vector<Vec4i> &lines);
     static int calcAngle(int deltaX, int deltaY);
-
 };
 
 class FrameData
@@ -70,7 +60,8 @@ public:
     std::vector<cv::Rect> boundingBoxes;
     std::vector<cv::Point> leftEdges;
     std::vector<cv::Point> middlePoints;
-    std::vector<double> angles;
+    int angle; //Angle of the contour the robot has to follow to
+    int index; //Index of the contour the robot has to follow to
 
     FrameData(): contours(), boundingBoxes(), leftEdges(), middlePoints() {}
 };