From 7175b68abec1c87651a5c4200e37237cca189fb4 Mon Sep 17 00:00:00 2001
From: TimZnr <zeunerti74726@th-nuernberg.de>
Date: Wed, 4 Jan 2023 21:50:59 +0100
Subject: [PATCH] motor signals in the loop

---
 AutonomousMode/autonomous_mode_main.cpp | 4 +++-
 AutonomousMode/lfr.cpp                  | 3 +++
 AutonomousMode/lfr.h                    | 1 +
 3 files changed, 7 insertions(+), 1 deletion(-)

diff --git a/AutonomousMode/autonomous_mode_main.cpp b/AutonomousMode/autonomous_mode_main.cpp
index ca229bc..7976423 100644
--- a/AutonomousMode/autonomous_mode_main.cpp
+++ b/AutonomousMode/autonomous_mode_main.cpp
@@ -40,7 +40,9 @@ int main(void)
             double delta = static_cast<double>(deltaMs) / 1000.0;
             double frameRate = 1.0 / static_cast<double>(delta);
 
-            std::cout << "Frame rate: " << frameRate << std::endl;
+            std::cout << "Frame rate: " << frameRate << " angle: " << result.data.angle
+                << " motor 1: " << result.motorSignals[0] << " motor 2: " << result.motorSignals[1]
+                << " motor 3: " << result.motorSignals[2] << " motor 4: " << result.motorSignals[3] <<std::endl;
             last = now;
         }
     }, &mutex);
diff --git a/AutonomousMode/lfr.cpp b/AutonomousMode/lfr.cpp
index d445464..992d901 100644
--- a/AutonomousMode/lfr.cpp
+++ b/AutonomousMode/lfr.cpp
@@ -67,12 +67,15 @@ void LFR::createThread()
                     FrameData data = processing.calculateLineSegments(processedImage, roi);
                     processing.filterReflections(data);
                     processing.calcAngles(data, originalImage.cols, originalImage.rows, left);
+                    
+                    controlModule.calcSpeeds(1,1, data.angle);
 
                     processedImage = provideOutput(originalImage, processedImage, data, roi);
 
                     result.rawImage = originalImage;
                     result.processedImage = processedImage;
                     result.data = data;
+                    result.motorSignals = controlModule.readMotors();
                 }
                 catch(std::exception const &ex)
                 {
diff --git a/AutonomousMode/lfr.h b/AutonomousMode/lfr.h
index b421d16..34729f4 100644
--- a/AutonomousMode/lfr.h
+++ b/AutonomousMode/lfr.h
@@ -21,6 +21,7 @@ struct LFR_Result
     cv::Mat rawImage;
     cv::Mat processedImage;
     FrameData data;
+    std::vector<float> motorSignals;
 };
 
 class LFR