Erster Entwurft für Controlling

AutonomousMode/ControlModule/control_module.cpp

 }
 
 void ControlModule::moveForward(){
-    for(int i = 0; motors; i++){
-
+    for(int i = 0; i <= sizeof(motors)/sizeof(int); i++){
+        motors[i] += forwardSpeed;
     }
 };
 
-void ControlModule::moveSide(){
+void ControlModule::moveSide(int imageColumsMiddle, int contourColumsMiddle){
+    float speed = moveSideSpeed * (contourColumsMiddle - imageColumsMiddle)/imageColumsMiddle;
+    motors[0] += speed;
+    motors[1] -= speed;
+    motors[2] -= speed;
+    motors[3] += speed;
+}
 
+void ControlModule::rotate(int angle){
+    float speed = rotateSpeed * (angle + 90)/90;
+    motors[0] += speed;
+    motors[1] -= speed;
+    motors[2] += speed;
+    motors[3] -= speed;
 }
 
-void ControlModule::rotate(){
+void ControlModule::unit(){
+    float max = 10E-12;
+    for(int i = 0; i <= sizeof(motors)/sizeof(int); i++){
+        if(motors[i] > max)
+        max = motors[i];
+    }
+    for(int i = 0; i <= sizeof(motors)/sizeof(int); i++){
+        motors[i] /= max;
+    }
+}
 
+void ControlModule::calcSpeeds(int imageColumsMiddle, int contourColumsMiddle, int angle){
+    moveForward();
+    moveSide(imageColumsMiddle, contourColumsMiddle);
+    rotate(angle);
+    unit();
 }

AutonomousMode/ControlModule/control_module.h

     ControlModule(float forwardSpeed, float rotateSpeed, float moveSideSpeed);
     ~ControlModule();
     void moveForward();
-    void rotate();
-    void moveSide();
+    void moveSide(int imageColumsMiddle, int contourColumsMiddle);
+    void rotate(int angle);
+    void unit();            //Brings the max Value to 1.0
+
+    void calcSpeeds(int imageColumsMiddle, int contourColumsMiddle, int angle); //Funktion to be called
 };

AutonomousMode/Processing/processing.cpp

 {
 }
 
-static double angle( Point pt1, Point pt2, Point pt0 )
-{
-    double dx1 = pt1.x - pt0.x;
-    double dy1 = pt1.y - pt0.y;
-    double dx2 = pt2.x - pt0.x;
-    double dy2 = pt2.y - pt0.y;
-    return (dx1*dx2 + dy1*dy2)/sqrt((dx1*dx1 + dy1*dy1)*(dx2*dx2 + dy2*dy2) + 1e-10);
-}
-
 void Processing::filterReflections(FrameData& frameData)
 {
     //Try to filter reflections from the frame data.
     return true;
 }
 
-int Processing::calcAngles(FrameData &data, int imageColums, int imageRows, bool turnLeft)
+void Processing::calcAngles(FrameData &data, int imageColums, int imageRows, bool turnLeft)
 {
-    int angle = 0;
+    
     if(data.contours.size() > 0)
-    {
-            int index;
+    { 
+    int angle = 0;
+    int index;    
             if(turnLeft){
                 // get the most left/right contour
                 int leftmostEdge = imageColums;
         // calculate angle
         int deltaX = abs(xleftButtom - xleftTop);
         int deltaY = yTop;                
-        angle = VectorOfLines::calcAngle(deltaX, deltaY);
+        angle = Calcs::calcAngle(deltaX, deltaY);
+
+        //Write to Data
+        data.angle = angle;
+        data.index = index;
     }
-    return angle;
 }
 
 

AutonomousMode/Processing/processing.h

     void processImage(Mat& inputPicture, int thresholdBinary, int gaussKernelSize);
     void filterReflections(FrameData& frameData);
     FrameData calculateLineSegments(Mat& inputPicture, const cv::Rect& roi);
-    int calcAngles(FrameData &data, int imageColums, int imageRows, bool turnLeft);
+    void calcAngles(FrameData &data, int imageColums, int imageRows, bool turnLeft);
 };

AutonomousMode/Utils/utils.cpp

 {
 }
 
-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

     ~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
     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() {}
 };

AutonomousMode/lfr.cpp

 #include "lfr.h"
 
+#define right false
+#define left true
 
 LFR::LFR(int videoHeight, int videoWidth, int thresholdBinary, int gaussKernelSize)
     : iAmLooping(false), input(videoHeight, videoWidth), processing(), controlModule(), interpreter(), intersectionHandler()
         //processedImage = processedImage(roi);
         FrameData data = processing.calculateLineSegments(processedImage, roi);
         processing.filterReflections(data);
-        int angle = processing.calcAngles(data, originalImage.cols, originalImage.rows, false);
-        this->provideOutput(originalImage, processedImage, data, roi, angle);
+        processing.calcAngles(data, originalImage.cols, originalImage.rows, left);
+        this->provideOutput(originalImage, processedImage, data, roi);
     }
     if(this->videoFlag) {destroyWindow("Display window");}
     input.freeWebcam();
     return;
 }
 
-void LFR::provideOutput(Mat originalImage, Mat processedImage, const FrameData& frameData, const Rect& roi, const int angle)
+void LFR::provideOutput(Mat originalImage, Mat processedImage, const FrameData& frameData, const Rect& roi)
 {
     for(int i = 0; i < frameData.contours.size(); i++)
     {
         rectangle(originalImage, center, Scalar(0,0,255));
         Rect leftRect(Point(frameData.leftEdges[i].x-2, frameData.leftEdges[i].y-2), Point(frameData.leftEdges[i].x+2, frameData.leftEdges[i].y+2));
         rectangle(originalImage, leftRect, Scalar(0,0,255));
-        
+    }
+
+    if(frameData.contours.size() > 0)
+    {
+        //Draw the Arrow for the check of the angle
         int length = 100;
-        Point P1 = frameData.middlePoints[i];
+        Point P1 = frameData.middlePoints[frameData.index];
         Point P2;
 
-        P2.x =  (int)round(P1.x + length * cos(angle * CV_PI / 180.0));
-        P2.y =  (int)round(P1.y + length * sin(angle * CV_PI / 180.0));
+        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);
-
     }
     if(this->videoFlag)
     {

AutonomousMode/lfr.h

     int thresholdBinary;
     int gaussKernelSize;
 
-    void provideOutput(Mat originalImage, Mat processedImage, const FrameData& frameData, const Rect& roi, const int angle);
+    void provideOutput(Mat originalImage, Mat processedImage, const FrameData& frameData, const Rect& roi);
 
 public: