Introduce maxSpeed

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(){
+void ControlModule::adjustSpeed(){
-    for(int i = 0; i < 4; i++){
+    double factor = 0.0;
-        motors[i] += forwardSpeed;
+    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<std::mutex> lock(mtx);
-    moveForward();
+    //moveSide(imageColumsMiddle, contourColumsMiddle);
-    moveSide(imageColumsMiddle, contourColumsMiddle);
     rotate(angle);
     unit();
+    adjustSpeed();
 }
 
 std::vector<float> ControlModule::readMotors()

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

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<std::mutex> lock(mutex);
         std::cerr<<"camera exception:"<<ex.what()<<std::endl;

AutonomousMode/lfr.cpp

@@ -3,11 +3,11 @@
 #define right false
 #define left true
 
-LFR::LFR(int videoHeight, int videoWidth, int thresholdBinary, int gaussKernelSize, ExceptionCallback cb):
+LFR::LFR(int videoHeight, int videoWidth, int thresholdBinary, int gaussKernelSize, double maxSpeed, ExceptionCallback cb):
     stop(false),
     input(videoHeight, videoWidth),
     processing(),
-    controlModule(),
+    controlModule(maxSpeed, 1.0, 1.0),
     interpreter(),
     intersectionHandler(),
     cb(cb)

AutonomousMode/lfr.h

@@ -57,7 +57,7 @@ private:
 
 public:
     LFR() = delete;
-    LFR(int videoHeight, int videoWidth, int thresholdBinary, int gaussKernelSize, ExceptionCallback cb);
+    LFR(int videoHeight, int videoWidth, int thresholdBinary, int gaussKernelSize, double maxSpeed, ExceptionCallback cb);
     ~LFR();
 
     void startLoop();

lfr_state_machine.cpp

@@ -73,7 +73,7 @@ void LFR_StateMachine::parseString(string s)
 }
 
 LFR_StateMachine::LFR_StateMachine():    
-    autonomousMode(videoHeight, videoWidth, thresholdBinary, gaussKernelSize, [&](std::exception const &ex)
+    autonomousMode(videoHeight, videoWidth, thresholdBinary, gaussKernelSize, maxSpeed [&](std::exception const &ex)
     {
         std::unique_lock<std::mutex> lock(mutex);
         std::cerr<<"camera exception:"<<ex.what()<<std::endl;

lfr_state_machine.h

@@ -16,6 +16,7 @@ class LFR_StateMachine
     const int videoHeight = 720;
     const int videoWidth = 1280;
     const int gaussKernelSize = 11;
+    const double maxSpeed = 0.5;
 
     std::mutex mutex;