yasarba71520/Line-Following-Robot
2
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Erster Entwurft für Controlling

Browse Source
This commit is contained in:
Baran Yasar 2022-12-14 16:14:00 +01:00
parent d21882f036
commit aad6de3687
8 changed files with 67 additions and 102 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
AutonomousMode/ControlModule/control_module.cpp
View File

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

7
AutonomousMode/ControlModule/control_module.h
View File

@ -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
};

25
AutonomousMode/Processing/processing.cpp
View File

@ -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<std::vector<Point>>& 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;
}

2
AutonomousMode/Processing/processing.h
View File

@ -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);
};

61
AutonomousMode/Utils/utils.cpp
View File

@ -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;
}

15
AutonomousMode/Utils/utils.h
View File

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

21
AutonomousMode/lfr.cpp
View File

@ -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)
{

2
AutonomousMode/lfr.h
View File

@ -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: