Too big too handle

AutonomousMode/ControlModule/control_module.cpp

@@ -36,7 +36,7 @@ void ControlModule::adjustSpeed(){
 
     for(int i = 0; i < 4; i++)
     {
-        motors[i] *= maxSpeed;
+        motors[i] *= factor;
     }
 };
 
@@ -85,7 +85,7 @@ void ControlModule::calcSpeeds(int imageColumsMiddle, int contourColumsMiddle, d
     std::unique_lock<std::mutex> lock(mtx);
     double rotationRatio = ratio(angle);
     rotate(rotationRatio);
-    drive(1.0-rotationRatio);
+    drive(1.0-std::fabs(rotationRatio));
     unit();
     adjustSpeed();
 }
@@ -102,13 +102,21 @@ double ControlModule::ratio(double angle)
     double maxAngularSpeed = 1.0;
     double speedRange = maxAngularSpeed - minAngularSpeed;
 
-    double minAngle = -90.0;
-    double maxAngle = 90.0;
+    double minAngle = -20.0;
+    double maxAngle = 20.0;
     double angularRange = maxAngle - minAngle;
 
+    if(angle < minAngle)
+    {
+        angle = minAngle;
+    }
+    if(angle > maxAngle)
+    {
+        angle = maxAngle;
+    }
     double progress = angle - minAngle;
     double progressPercent = progress/angularRange;
 
     double speed = minAngularSpeed + progressPercent*speedRange;
-    return abs(speed);
+    return speed;
 }

AutonomousMode/ControlModule/control_module.h

@@ -2,6 +2,7 @@
 #include <mutex>
 #include <vector>
 #include <iostream>
+#include <cmath>
 
 class ControlModule
 {

Communication/uart_communication.cpp

@@ -16,7 +16,7 @@ void LFR_UART::openFile(const char *fileName) {
 
 int LFR_UART::writeDataToFile(int8_t *buff, uint32_t bufferLength) {
     //std::cout << "Sending uart: " << std::bitset<8>(buff[0]) << std::endl;
-    std::cout << "Sending uart: " << std::bitset<8>(buff[0]) << ", " << std::bitset<8>(buff[1]) << ", " << std::bitset<8>(buff[2]) << ", " << std::bitset<8>(buff[3]) << " csum:  "  << std::bitset<8>(buff[4]) << std::endl;
+    std::cout << "Sending Uart: " << std::bitset<8>(buff[0]) << ", " << std::bitset<8>(buff[1]) << ", " << std::bitset<8>(buff[2]) << ", " << std::bitset<8>(buff[3]) << " csum:  "  << std::bitset<8>(buff[4]) << std::endl;
     return write(this->fileDescriptor, buff, bufferLength);
 }
 
@@ -66,7 +66,7 @@ double LFR_UART::byteToDouble(int8_t in){
 void LFR_UART::sendTelegram(double wheel1, double wheel2, double wheel3, double wheel4){
     std::chrono::milliseconds now = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch());
     unsigned int deltaMs = static_cast<unsigned int>((now-last).count());
-    if (deltaMs < 250)
+    if (deltaMs < 50 && (std::fabs(wheel1)+std::fabs(wheel2)+std::fabs(wheel3)+std::fabs(wheel4)) > 0.0005)
     {
         std::cout << "Too fast" << std::endl;
         return;

Communication/uart_communication.h

@@ -12,6 +12,7 @@
 #include <exception>
 #include <iostream>
 #include <chrono>
+#include <cmath>
 
 #include <bitset>
 

lfr_state_machine.cpp

@@ -104,16 +104,26 @@ LFR_StateMachine::LFR_StateMachine():
     currentState = &State::Idle::getInstance();
     currentState->enter(this);
 
-    //Start the permanent loop
-    char input;
-    std::cout << "press q to quit" << std::endl;
-    std::cin >> input;
-    std::cout << "binned" << std::endl;
-    while (input != 'q')
+    cv::VideoWriter writer = cv::VideoWriter("video_200.avi", cv::VideoWriter::fourcc('M','J','P','G'), 4.0, cv::Size(videoWidth, videoHeight), true);
+    
+    auto t_start = std::chrono::high_resolution_clock::now();
+    auto t_end = std::chrono::high_resolution_clock::now();
+    double dur = std::chrono::duration<double, std::milli>(t_end-t_start).count();
+    
+    while(dur < 60000)
     {
-        std::cin >> input;
-        std::cout << "binned" << std::endl;
+        t_end = std::chrono::high_resolution_clock::now();
+        dur = std::chrono::duration<double, std::milli>(t_end-t_start).count();
+        {
+            this_thread::sleep_for(std::chrono::milliseconds(50));
+            std::unique_lock<std::mutex> lock(imgMutex);
+            if(!this->image.empty())
+            {
+                writer.write(this->image);
             }
+        }
+    }
+    writer.release();
     std::cout << "Exiting central" << std::endl;
 }
 
@@ -146,6 +156,10 @@ void LFR_StateMachine::enterAutonomous()
             double delta = static_cast<double>(deltaMs) / 1000.0;
             double frameRate = 1.0 / static_cast<double>(delta);*/
             double frameRate = -1.0;
+            {
+                std::unique_lock<std::mutex> lock(imgMutex);
+                image = result.rawImage;
+            }
 
             if (result.validLane)
             {

lfr_state_machine.h

@@ -16,7 +16,7 @@ class LFR_StateMachine
     const int videoHeight = 720;
     const int videoWidth = 1280;
     const int gaussKernelSize = 11;
-    const double maxSpeed = 0.5;
+    const double maxSpeed = 0.20;
 
     std::mutex mutex;
 
@@ -24,6 +24,9 @@ class LFR_StateMachine
     LFR_UART uartCommunicator;
     LFR_Socket socket;
 
+    std::mutex imgMutex;
+    Mat image;
+
     vector<string> split (string s, string delimiter) const;
     void sanitize (string& s) const;
     bool checkStringValidity (const std::vector<std::string>& s) const;
@@ -36,3 +39,5 @@ public:
     inline LFR_IState* getCurrentState() const {return currentState;}
     void setState(LFR_IState& newState);
 };
+
+