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

Bugs found during tests

Browse Source
This commit is contained in:
Tim Zeuner 2023-03-03 18:46:31 +01:00
parent c40b69ebe2
commit 6da9bfff54
1 changed files with 13 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
AutonomousMode/ControlModule/control_module.cpp
View File

@ -36,7 +36,7 @@ void ControlModule::adjustSpeed(){
for(int i = 0; i < 4; i++) for(int i = 0; i < 4; i++)
{ {
motors[i] *= maxSpeed; motors[i] *= factor;
} }
}; };
@ -85,7 +85,7 @@ void ControlModule::calcSpeeds(int imageColumsMiddle, int contourColumsMiddle, d
std::unique_lock<std::mutex> lock(mtx); std::unique_lock<std::mutex> lock(mtx);
double rotationRatio = ratio(angle); double rotationRatio = ratio(angle);
rotate(rotationRatio); rotate(rotationRatio);
drive(1.0-rotationRatio); drive(1.0-std::fabs(rotationRatio));
unit(); unit();
adjustSpeed(); adjustSpeed();
} }
@ -102,13 +102,21 @@ double ControlModule::ratio(double angle)
double maxAngularSpeed = 1.0; double maxAngularSpeed = 1.0;
double speedRange = maxAngularSpeed - minAngularSpeed; double speedRange = maxAngularSpeed - minAngularSpeed;
double minAngle = -90.0; double minAngle = -20.0;
double maxAngle = 90.0; double maxAngle = 20.0;
double angularRange = maxAngle - minAngle; double angularRange = maxAngle - minAngle;
if(angle < minAngle)
{
angle = minAngle;
}
if(angle > maxAngle)
{
angle = maxAngle;
}
double progress = angle - minAngle; double progress = angle - minAngle;
double progressPercent = progress/angularRange; double progressPercent = progress/angularRange;
double speed = minAngularSpeed + progressPercent*speedRange; double speed = minAngularSpeed + progressPercent*speedRange;
return std::fabs(speed); return speed;
} }