motor signals in the loop

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);

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

AutonomousMode/lfr.h

@@ -21,6 +21,7 @@ struct LFR_Result
     cv::Mat rawImage;
     cv::Mat processedImage;
     FrameData data;
+    std::vector<float> motorSignals;
 };
 
 class LFR