From aad6de3687aa3fc3d00b557fbf5756dd2e825662 Mon Sep 17 00:00:00 2001 From: yasarba71520 Date: Wed, 14 Dec 2022 16:14:00 +0100 Subject: [PATCH] =?UTF-8?q?Erster=20Entwurft=20f=C3=BCr=20Controlling?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../ControlModule/control_module.cpp | 36 +++++++++-- AutonomousMode/ControlModule/control_module.h | 7 ++- AutonomousMode/Processing/processing.cpp | 25 +++----- AutonomousMode/Processing/processing.h | 2 +- AutonomousMode/Utils/utils.cpp | 61 +------------------ AutonomousMode/Utils/utils.h | 15 +---- AutonomousMode/lfr.cpp | 21 ++++--- AutonomousMode/lfr.h | 2 +- 8 files changed, 67 insertions(+), 102 deletions(-) diff --git a/AutonomousMode/ControlModule/control_module.cpp b/AutonomousMode/ControlModule/control_module.cpp index 28dd4db..2d2ab87 100644 --- a/AutonomousMode/ControlModule/control_module.cpp +++ b/AutonomousMode/ControlModule/control_module.cpp @@ -15,15 +15,41 @@ ControlModule::~ControlModule() } 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(){ +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::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(); } \ No newline at end of file diff --git a/AutonomousMode/ControlModule/control_module.h b/AutonomousMode/ControlModule/control_module.h index 26fd8f2..ab99c88 100644 --- a/AutonomousMode/ControlModule/control_module.h +++ b/AutonomousMode/ControlModule/control_module.h @@ -12,6 +12,9 @@ public: 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 }; \ No newline at end of file diff --git a/AutonomousMode/Processing/processing.cpp b/AutonomousMode/Processing/processing.cpp index 96157b4..b93581b 100644 --- a/AutonomousMode/Processing/processing.cpp +++ b/AutonomousMode/Processing/processing.cpp @@ -11,15 +11,6 @@ Processing::~Processing() { } -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. @@ -86,12 +77,13 @@ bool Processing::checkIntersection(const std::vector>& lines, 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; @@ -141,9 +133,12 @@ int Processing::calcAngles(FrameData &data, int imageColums, int imageRows, bool // 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; } diff --git a/AutonomousMode/Processing/processing.h b/AutonomousMode/Processing/processing.h index 520ce2f..43698a1 100644 --- a/AutonomousMode/Processing/processing.h +++ b/AutonomousMode/Processing/processing.h @@ -24,5 +24,5 @@ public: 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); }; \ No newline at end of file diff --git a/AutonomousMode/Utils/utils.cpp b/AutonomousMode/Utils/utils.cpp index 3bd315a..f23524c 100644 --- a/AutonomousMode/Utils/utils.cpp +++ b/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 VectorOfLines::findMiddleLine(vector &lines){ - Point point11; - Point point12; - Point point21; - Point point22; - vector 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; } diff --git a/AutonomousMode/Utils/utils.h b/AutonomousMode/Utils/utils.h index 78834eb..e0e0809 100644 --- a/AutonomousMode/Utils/utils.h +++ b/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 findMiddleLine(vector &lines); static int calcAngle(int deltaX, int deltaY); - }; class FrameData @@ -70,7 +60,8 @@ public: std::vector boundingBoxes; std::vector leftEdges; std::vector middlePoints; - std::vector 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() {} }; \ No newline at end of file diff --git a/AutonomousMode/lfr.cpp b/AutonomousMode/lfr.cpp index beb7309..4343ae2 100644 --- a/AutonomousMode/lfr.cpp +++ b/AutonomousMode/lfr.cpp @@ -1,5 +1,7 @@ #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() @@ -37,8 +39,8 @@ void LFR::loop() //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(); @@ -57,7 +59,7 @@ void LFR::endLoop() 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++) { @@ -68,15 +70,18 @@ void LFR::provideOutput(Mat originalImage, Mat processedImage, const FrameData& 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) { diff --git a/AutonomousMode/lfr.h b/AutonomousMode/lfr.h index 3765120..22140fb 100644 --- a/AutonomousMode/lfr.h +++ b/AutonomousMode/lfr.h @@ -28,7 +28,7 @@ class LFR 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: