Baran Yasar 1 year ago
parent
commit
6a55bda519

+ 34
- 20
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;

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;
void ControlModule::drive(double ratio)
{
motors[0] += ratio;
motors[1] += ratio;
motors[2] += ratio;
motors[3] += ratio;
}

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

+ 3
- 1
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
- 0
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;

Loading…
Cancel
Save