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

Merge branch 'master' of https://git.efi.th-nuernberg.de/gitea/yasarba71520/Line-Following-Robot

Browse Source
This commit is contained in:
Baran Yasar 2023-02-01 20:10:17 +01:00
commit 6a55bda519
3 changed files with 41 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

54
AutonomousMode/ControlModule/control_module.cpp
View File

@ -48,24 +48,20 @@ void ControlModule::moveSide(int imageColumsMiddle, int contourColumsMiddle){
motors[3] += speed;
}
void ControlModule::rotate(double angle){
double minAngularSpeed = -1.0;
double maxAngularSpeed = 1.0;
double speedRange = maxAngularSpeed - minAngularSpeed;
void ControlModule::drive(double ratio)
{
motors[0] += ratio;
motors[1] += ratio;
motors[2] += ratio;
motors[3] += ratio;
}
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;
motors[3] -= speed;
void ControlModule::rotate(double ratio)
{
motors[0] += ratio;
motors[1] -= ratio;
motors[2] += ratio;
motors[3] -= ratio;
}
void ControlModule::unit(){
@ -87,8 +83,9 @@ void ControlModule::unit(){
void ControlModule::calcSpeeds(int imageColumsMiddle, int contourColumsMiddle, double angle){
std::unique_lock<std::mutex> lock(mtx);
//moveSide(imageColumsMiddle, contourColumsMiddle);
rotate(angle);
double rotationRatio = ratio(angle);
rotate(rotationRatio);
drive(1.0-rotationRatio);
unit();
adjustSpeed();
}
@ -97,4 +94,21 @@ std::vector<double> ControlModule::readMotors()
{
std::unique_lock<std::mutex> lock(mtx);
return std::vector<double> {motors[0], motors[1], motors[2], motors[3]};
}
}
double ControlModule::ratio(double angle)
{
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;
return abs(speed);
}

4
AutonomousMode/ControlModule/control_module.h
View File

@ -17,8 +17,10 @@ public:
~ControlModule();
void adjustSpeed();
void moveSide(int imageColumsMiddle, int contourColumsMiddle);
void rotate(double angle);
void rotate(double ratio);
void drive(double ratio);
void unit(); //Brings the max Value to 1.0
double ratio(double angle);
void calcSpeeds(int imageColumsMiddle, int contourColumsMiddle, double angle); //Funktion to be called
std::vector<double> readMotors();

4
AutonomousMode/Processing/processing.cpp
View File

@ -144,6 +144,10 @@ void Processing::calcAngles(FrameData &data, int imageColums, int imageRows, boo
LFRVector b(contourCenter-focusPoint);
double angle = a.angle(b);
if (b.x < 0.0)
{
angle *= -1.0;
}
//Write to Data
data.angle = angle;