diff --git a/AutonomousMode/ControlModule/control_module.cpp b/AutonomousMode/ControlModule/control_module.cpp index 00aed54..28dd4db 100644 --- a/AutonomousMode/ControlModule/control_module.cpp +++ b/AutonomousMode/ControlModule/control_module.cpp @@ -1,9 +1,29 @@ #include "control_module.h" -ControlModule::ControlModule(/* args */) +ControlModule::ControlModule(){ +} + +ControlModule::ControlModule(float forwardSpeed, float rotateSpeed, float moveSideSpeed) { + this->forwardSpeed = forwardSpeed; + this->rotateSpeed = rotateSpeed; + this->moveSideSpeed = moveSideSpeed; } ControlModule::~ControlModule() { +} + +void ControlModule::moveForward(){ + for(int i = 0; motors; i++){ + + } +}; + +void ControlModule::moveSide(){ + +} + +void ControlModule::rotate(){ + } \ No newline at end of file diff --git a/AutonomousMode/ControlModule/control_module.h b/AutonomousMode/ControlModule/control_module.h index 96c01a3..26fd8f2 100644 --- a/AutonomousMode/ControlModule/control_module.h +++ b/AutonomousMode/ControlModule/control_module.h @@ -3,9 +3,15 @@ class ControlModule { private: - /* data */ + float motors[4]; //LeftFront; RightFront; LeftBack; RightBack + float forwardSpeed; + float rotateSpeed; + float moveSideSpeed; public: ControlModule(/* args */); - + ControlModule(float forwardSpeed, float rotateSpeed, float moveSideSpeed); ~ControlModule(); + void moveForward(); + void rotate(); + void moveSide(); }; \ No newline at end of file diff --git a/AutonomousMode/Input/input.cpp b/AutonomousMode/Input/input.cpp index c4af429..fee41a1 100644 --- a/AutonomousMode/Input/input.cpp +++ b/AutonomousMode/Input/input.cpp @@ -1,8 +1,8 @@ #include "input.h" // TODO: Wenn ihr in die Zeile den Pfad zum Testvideo statt der 0 packt, benmutzt er das Testvideo. -//Input::Input(int videoHeight, int videoWidth) : cap("C:\\Users\\User\\Desktop\\Studium\\02_Master_MSY\\2. Semester Winter 22 23\\Projekt\\Line-Following-Robot\\AutonomousMode\\Test_data\\video1.h264"), videoHeight(videoHeight), videoWidth(videoWidth) -Input::Input(int videoHeight, int videoWidth) : cap(0), videoHeight(videoHeight), videoWidth(videoWidth) +Input::Input(int videoHeight, int videoWidth) : cap("C:\\Line-Following-Robot\\AutonomousMode\\Test_data\\video1.h264"), videoHeight(videoHeight), videoWidth(videoWidth) +//Input::Input(int videoHeight, int videoWidth) : cap(0), videoHeight(videoHeight), videoWidth(videoWidth) { this->cap.set(CAP_PROP_FRAME_HEIGHT, videoHeight); this->cap.set(CAP_PROP_FRAME_WIDTH, videoWidth); diff --git a/AutonomousMode/Processing/processing.cpp b/AutonomousMode/Processing/processing.cpp index 181f677..96157b4 100644 --- a/AutonomousMode/Processing/processing.cpp +++ b/AutonomousMode/Processing/processing.cpp @@ -1,5 +1,8 @@ #include "processing.h" + + + Processing::Processing(/* args */) { } @@ -47,6 +50,7 @@ void Processing::filterReflections(FrameData& frameData) indicesToDelete.push_back(i); continue; } + } //reverse the vector with the indices so the order isn't messed up when deleting: @@ -82,6 +86,66 @@ bool Processing::checkIntersection(const std::vector>& lines, return true; } +int Processing::calcAngles(FrameData &data, int imageColums, int imageRows, bool turnLeft) +{ + int angle = 0; + if(data.contours.size() > 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 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 leftMostContour = data.contours[index]; + int xleftButtom = imageColums; + int xleftTop = imageColums; + int yButtom = 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 < xleftButtom) + { + xleftButtom = 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 deltaY = yTop; + angle = VectorOfLines::calcAngle(deltaX, deltaY); + } + return angle; +} + void Processing::processImage(Mat& inputPicture, int thresholdBinary, int gaussKernelSize) { @@ -126,7 +190,7 @@ FrameData Processing::calculateLineSegments(Mat& inputPicture, const cv::Rect& r auto iterator = data.contours.begin(); while(iterator != data.contours.end()) { - if (contourArea(*iterator) < 3500) + if (contourArea(*iterator) < 7000) { iterator = data.contours.erase(iterator); } @@ -137,8 +201,6 @@ FrameData Processing::calculateLineSegments(Mat& inputPicture, const cv::Rect& r else { //Check for intersection with lines: - - Rect boundingBox = boundingRect(*iterator); boundingBox.x += roi.x; boundingBox.y += roi.y; diff --git a/AutonomousMode/Processing/processing.h b/AutonomousMode/Processing/processing.h index 522cb9f..520ce2f 100644 --- a/AutonomousMode/Processing/processing.h +++ b/AutonomousMode/Processing/processing.h @@ -3,6 +3,7 @@ #include #include + using namespace cv; using namespace std; @@ -23,4 +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); }; \ No newline at end of file diff --git a/AutonomousMode/Utils/utils.cpp b/AutonomousMode/Utils/utils.cpp index f6e0eb8..3bd315a 100644 --- a/AutonomousMode/Utils/utils.cpp +++ b/AutonomousMode/Utils/utils.cpp @@ -1,5 +1,7 @@ #include "utils.h" +#define PI 3.14159265359 + LFRPoint::LFRPoint(/* args */) : x(0.0), y(0.0) { } @@ -99,4 +101,25 @@ vector VectorOfLines::findMiddleLine(vector &lines){ } return middleLines; +} + +int VectorOfLines::calcAngle(int deltaX, int deltaY){ + + int refAngle = 0; + + if(deltaX > 10E-12){ + refAngle = atan(deltaY/deltaX) * 180/PI; + + // convert from img coordinates to regbot coordinates + refAngle = -(refAngle); + + if (refAngle > 90) + refAngle = refAngle - 180; + + cout << refAngle << "\n"; + }else{ + refAngle = -90; + } + return refAngle; + } \ No newline at end of file diff --git a/AutonomousMode/Utils/utils.h b/AutonomousMode/Utils/utils.h index 46de2a9..78834eb 100644 --- a/AutonomousMode/Utils/utils.h +++ b/AutonomousMode/Utils/utils.h @@ -59,6 +59,7 @@ class VectorOfLines{ 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); }; @@ -69,6 +70,7 @@ public: std::vector boundingBoxes; std::vector leftEdges; std::vector middlePoints; + std::vector angles; FrameData(): contours(), boundingBoxes(), leftEdges(), middlePoints() {} }; \ No newline at end of file diff --git a/AutonomousMode/lfr.cpp b/AutonomousMode/lfr.cpp index a2f4239..beb7309 100644 --- a/AutonomousMode/lfr.cpp +++ b/AutonomousMode/lfr.cpp @@ -28,8 +28,8 @@ void LFR::loop() { Mat originalImage = input.readWebcam(); - Point roiOrigin(0, int(originalImage.rows*(7.5/12.0))); - Rect roi(roiOrigin.x, roiOrigin.y, originalImage.cols, originalImage.rows/12); + Point roiOrigin(0, int(originalImage.rows*(3.25/6.0))); + Rect roi(roiOrigin.x, roiOrigin.y, originalImage.cols, originalImage.rows/6); //Mat processedImage = originalImage(roi); Mat processedImage = originalImage; @@ -37,7 +37,8 @@ void LFR::loop() //processedImage = processedImage(roi); FrameData data = processing.calculateLineSegments(processedImage, roi); processing.filterReflections(data); - this->provideOutput(originalImage, processedImage, data, roi); + int angle = processing.calcAngles(data, originalImage.cols, originalImage.rows, false); + this->provideOutput(originalImage, processedImage, data, roi, angle); } if(this->videoFlag) {destroyWindow("Display window");} input.freeWebcam(); @@ -56,7 +57,7 @@ void LFR::endLoop() return; } -void LFR::provideOutput(Mat originalImage, Mat processedImage, const FrameData& frameData, const Rect& roi) +void LFR::provideOutput(Mat originalImage, Mat processedImage, const FrameData& frameData, const Rect& roi, const int angle) { for(int i = 0; i < frameData.contours.size(); i++) { @@ -67,6 +68,15 @@ 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)); + + int length = 100; + Point P1 = frameData.middlePoints[i]; + 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)); + 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 22140fb..3765120 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); + void provideOutput(Mat originalImage, Mat processedImage, const FrameData& frameData, const Rect& roi, const int angle); public: