diff --git a/AutonomousMode/ControlModule/control_module.cpp b/AutonomousMode/ControlModule/control_module.cpp index b6666b0..1f235b3 100644 --- a/AutonomousMode/ControlModule/control_module.cpp +++ b/AutonomousMode/ControlModule/control_module.cpp @@ -1,16 +1,16 @@ #include "control_module.h" -ControlModule::ControlModule(): ControlModule(1.0, 1.0, 1.0) +ControlModule::ControlModule(): ControlModule(0.0, 1.0, 1.0) { } -ControlModule::ControlModule(float forwardSpeed, float rotateSpeed, float moveSideSpeed) +ControlModule::ControlModule(float maxSpeed, float rotateSpeed, float moveSideSpeed) { motors[0] = 0.0; motors[1] = 0.0; motors[2] = 0.0; motors[3] = 0.0; - this->forwardSpeed = forwardSpeed; + this->maxSpeed = maxSpeed; this->rotateSpeed = rotateSpeed; this->moveSideSpeed = moveSideSpeed; } @@ -19,9 +19,24 @@ ControlModule::~ControlModule() { } -void ControlModule::moveForward(){ - for(int i = 0; i < 4; i++){ - motors[i] += forwardSpeed; +void ControlModule::adjustSpeed(){ + double factor = 0.0; + if(maxSpeed > 1.0) + { + factor = 1.0; + } + else if(maxSpeed < -1.0) + { + factor = -1.0; + } + else + { + factor = maxSpeed; + } + + for(int i = 0; i < 4; i++) + { + motors[i] *= maxSpeed; } }; @@ -60,10 +75,10 @@ void ControlModule::unit(){ void ControlModule::calcSpeeds(int imageColumsMiddle, int contourColumsMiddle, int angle){ std::unique_lock lock(mtx); - moveForward(); - moveSide(imageColumsMiddle, contourColumsMiddle); + //moveSide(imageColumsMiddle, contourColumsMiddle); rotate(angle); unit(); + adjustSpeed(); } std::vector ControlModule::readMotors() diff --git a/AutonomousMode/ControlModule/control_module.h b/AutonomousMode/ControlModule/control_module.h index fc7d5b6..4022ff5 100644 --- a/AutonomousMode/ControlModule/control_module.h +++ b/AutonomousMode/ControlModule/control_module.h @@ -8,14 +8,14 @@ class ControlModule private: mutable std::mutex mtx; float motors[4]; //LeftFront; RightFront; LeftBack; RightBack - float forwardSpeed; + float maxSpeed; float rotateSpeed; float moveSideSpeed; public: ControlModule(/* args */); - ControlModule(float forwardSpeed, float rotateSpeed, float moveSideSpeed); + ControlModule(float maxSpeed, float rotateSpeed, float moveSideSpeed); ~ControlModule(); - void moveForward(); + void adjustSpeed(); void moveSide(int imageColumsMiddle, int contourColumsMiddle); void rotate(int angle); void unit(); //Brings the max Value to 1.0 diff --git a/AutonomousMode/autonomous_mode_main.cpp b/AutonomousMode/autonomous_mode_main.cpp index a3f4414..1b72dfa 100644 --- a/AutonomousMode/autonomous_mode_main.cpp +++ b/AutonomousMode/autonomous_mode_main.cpp @@ -10,10 +10,11 @@ int main(void) const int videoHeight = 720; const int videoWidth = 1280; const int gaussKernelSize = 11; + const double maxSpeed = 0.5; std::mutex mutex; - LFR lfr(videoHeight, videoWidth, thresholdBinary, gaussKernelSize, [&](std::exception const &ex) + LFR lfr(videoHeight, videoWidth, thresholdBinary, gaussKernelSize, maxSpeed, [&](std::exception const &ex) { std::unique_lock lock(mutex); std::cerr<<"camera exception:"< lock(mutex); std::cerr<<"camera exception:"<