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(){
-    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<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