From be7b35f668edfd1a470ba8a27e533303011607af Mon Sep 17 00:00:00 2001 From: TimZnr Date: Tue, 31 Jan 2023 22:01:25 +0100 Subject: [PATCH 1/3] New angle calculation concept --- AutonomousMode/Processing/processing.cpp | 102 +++++++++++++---------- AutonomousMode/lfr.cpp | 11 +-- 2 files changed, 62 insertions(+), 51 deletions(-) diff --git a/AutonomousMode/Processing/processing.cpp b/AutonomousMode/Processing/processing.cpp index b93581b..630bd72 100644 --- a/AutonomousMode/Processing/processing.cpp +++ b/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 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; - } + leftmostEdge = edge; + index = i; } + else if(edge >= rightmostEdge && !turnLeft) + { + rightmostEdge = edge; + index = i; + } + } + + // Find the Top-Left and Bottom-Left Point of the Contour + + std::vector 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; diff --git a/AutonomousMode/lfr.cpp b/AutonomousMode/lfr.cpp index 2ca6457..5387485 100644 --- a/AutonomousMode/lfr.cpp +++ b/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); + cv::arrowedLine(originalImage, focalPoint, imageCenter, Scalar(0,0,255), 2, 8); } return originalImage; } From 0f5c2e3f4251715dbae5d7e2581c8dd32793b046 Mon Sep 17 00:00:00 2001 From: TimZnr Date: Tue, 31 Jan 2023 22:03:25 +0100 Subject: [PATCH 2/3] P-Controller Concept --- AutonomousMode/ControlModule/control_module.cpp | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/AutonomousMode/ControlModule/control_module.cpp b/AutonomousMode/ControlModule/control_module.cpp index a80bea4..1fd8120 100644 --- a/AutonomousMode/ControlModule/control_module.cpp +++ b/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; From f6d346a3c12aaae79c637f893dcf141583aa8684 Mon Sep 17 00:00:00 2001 From: TimZnr Date: Tue, 31 Jan 2023 22:06:00 +0100 Subject: [PATCH 3/3] Deeskalation bei den Pfeilspitzen --- AutonomousMode/lfr.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/AutonomousMode/lfr.cpp b/AutonomousMode/lfr.cpp index 5387485..ce48be0 100644 --- a/AutonomousMode/lfr.cpp +++ b/AutonomousMode/lfr.cpp @@ -144,8 +144,8 @@ cv::Mat LFR::provideOutput(Mat originalImage, Mat processedImage, const FrameDat 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); - cv::arrowedLine(originalImage, focalPoint, imageCenter, Scalar(0,0,255), 2, 8); + 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; }