From 8150850bb6bef7570b329a36f00185425c1dda1b Mon Sep 17 00:00:00 2001
From: TimZnr <zeunerti74726@th-nuernberg.de>
Date: Wed, 4 Jan 2023 21:47:54 +0100
Subject: [PATCH] float mistakes

---
 .../ControlModule/control_module.cpp          | 34 ++++++++++++++-----
 AutonomousMode/ControlModule/control_module.h |  5 +++
 2 files changed, 31 insertions(+), 8 deletions(-)

diff --git a/AutonomousMode/ControlModule/control_module.cpp b/AutonomousMode/ControlModule/control_module.cpp
index 2d2ab87..b6666b0 100644
--- a/AutonomousMode/ControlModule/control_module.cpp
+++ b/AutonomousMode/ControlModule/control_module.cpp
@@ -1,10 +1,15 @@
 #include "control_module.h"
 
-ControlModule::ControlModule(){    
+ControlModule::ControlModule(): ControlModule(1.0, 1.0, 1.0)
+{    
 }
 
 ControlModule::ControlModule(float forwardSpeed, float rotateSpeed, float moveSideSpeed)
 {
+    motors[0] = 0.0;
+    motors[1] = 0.0;
+    motors[2] = 0.0;
+    motors[3] = 0.0;
     this->forwardSpeed = forwardSpeed;
     this->rotateSpeed = rotateSpeed;
     this->moveSideSpeed = moveSideSpeed;
@@ -15,13 +20,13 @@ ControlModule::~ControlModule()
 }
 
 void ControlModule::moveForward(){
-    for(int i = 0; i <= sizeof(motors)/sizeof(int); i++){
+    for(int i = 0; i < 4; i++){
         motors[i] += forwardSpeed;
     }
 };
 
 void ControlModule::moveSide(int imageColumsMiddle, int contourColumsMiddle){
-    float speed = moveSideSpeed * (contourColumsMiddle - imageColumsMiddle)/imageColumsMiddle;
+    float speed = moveSideSpeed * static_cast<float>(contourColumsMiddle - imageColumsMiddle)/static_cast<float>(imageColumsMiddle);
     motors[0] += speed;
     motors[1] -= speed;
     motors[2] -= speed;
@@ -29,7 +34,7 @@ void ControlModule::moveSide(int imageColumsMiddle, int contourColumsMiddle){
 }
 
 void ControlModule::rotate(int angle){
-    float speed = rotateSpeed * (angle + 90)/90;
+    float speed = rotateSpeed * (static_cast<float>(angle) + 90.0f)/90.0f;
     motors[0] += speed;
     motors[1] -= speed;
     motors[2] += speed;
@@ -37,19 +42,32 @@ void ControlModule::rotate(int angle){
 }
 
 void ControlModule::unit(){
-    float max = 10E-12;
+    float max = 1.0E-6;
     for(int i = 0; i <= sizeof(motors)/sizeof(int); i++){
         if(motors[i] > max)
         max = motors[i];
     }
-    for(int i = 0; i <= sizeof(motors)/sizeof(int); i++){
-        motors[i] /= max;
+
+    //Avoid dividing by zero
+    if (max > 0.001)
+    {
+        for(int i = 0; i < 4; i++){
+            motors[i] /= max;
+        }
     }
+
 }
 
 void ControlModule::calcSpeeds(int imageColumsMiddle, int contourColumsMiddle, int angle){
+    std::unique_lock<std::mutex> lock(mtx);
     moveForward();
     moveSide(imageColumsMiddle, contourColumsMiddle);
     rotate(angle);
     unit();
-}
\ No newline at end of file
+}
+
+std::vector<float> ControlModule::readMotors()
+{
+    std::unique_lock<std::mutex> lock(mtx);
+    return std::vector<float> {motors[0], motors[1], motors[2], motors[3]};
+} 
\ No newline at end of file
diff --git a/AutonomousMode/ControlModule/control_module.h b/AutonomousMode/ControlModule/control_module.h
index ab99c88..fc7d5b6 100644
--- a/AutonomousMode/ControlModule/control_module.h
+++ b/AutonomousMode/ControlModule/control_module.h
@@ -1,8 +1,12 @@
 #pragma once
+#include <mutex>
+#include <vector>
+#include <iostream>
 
 class ControlModule
 {
 private:
+    mutable std::mutex mtx;
     float motors[4]; //LeftFront; RightFront; LeftBack; RightBack
     float forwardSpeed;
     float rotateSpeed;
@@ -17,4 +21,5 @@ public:
     void unit();            //Brings the max Value to 1.0
 
     void calcSpeeds(int imageColumsMiddle, int contourColumsMiddle, int angle); //Funktion to be called
+    std::vector<float> readMotors();
 };
\ No newline at end of file