From 59ce30a82805c77e9595dbd1e29f9ad6c3e18a1f Mon Sep 17 00:00:00 2001
From: Tim Zeuner <zeunerti74726@th-nuernberg.de>
Date: Sun, 29 Jan 2023 22:42:33 +0100
Subject: [PATCH] Start Autonomous via socket; redirect signals to uart

---
 lfr_state_machine.cpp | 44 +++++++++++++++++++++++++++++++++++++++++++
 lfr_state_machine.h   |  2 ++
 lfr_states.cpp        |  2 ++
 3 files changed, 48 insertions(+)

diff --git a/lfr_state_machine.cpp b/lfr_state_machine.cpp
index 1036549..63a3cbd 100644
--- a/lfr_state_machine.cpp
+++ b/lfr_state_machine.cpp
@@ -124,4 +124,48 @@ void LFR_StateMachine::setState(LFR_IState& newState)
         std::cout << "no switch" << std::endl;
     }
 
+}
+
+void LFR_StateMachine::enterAutonomous()
+{
+    //cv::Mat img;
+    autonomousMode.addListener([&](LFR_Result result)
+    {
+        std::unique_lock<std::mutex> lock(mutex);
+        if (!result.rawImage.empty())
+        {
+            /*
+            img = result.rawImage;
+            //Resize to minimize latency using raspi over ssh/adapt to 3.5in screen
+            cv::resize(result.rawImage, img, cv::Size(480, 320));
+
+            //Calculate frame rate
+            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());
+            double delta = static_cast<double>(deltaMs) / 1000.0;
+            double frameRate = 1.0 / static_cast<double>(delta);*/
+            double frameRate = -1.0;
+
+            if (result.validLane)
+            {
+                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;
+                uartCommunicator.sendTelegram(result.motorSignals[0], result.motorSignals[1], result.motorSignals[2], result.motorSignals[3]);
+            }
+            else
+            {
+                std::cout << "No lane found." << std::endl;;
+            }
+            //last = now;
+        }
+    }, &mutex);
+
+    autonomousMode.startLoop();
+}
+
+void LFR_StateMachine::exitAutonomous()
+{
+    autonomousMode.removeListener(&mutex);
+    autonomousMode.endLoop();
 }
\ No newline at end of file
diff --git a/lfr_state_machine.h b/lfr_state_machine.h
index fc5ea4f..6274492 100644
--- a/lfr_state_machine.h
+++ b/lfr_state_machine.h
@@ -29,6 +29,8 @@ class LFR_StateMachine
     void parseString(string s);    
 
 public:
+    void enterAutonomous();
+    void exitAutonomous();
     LFR_StateMachine();
     inline LFR_IState* getCurrentState() const {return currentState;}
     void setState(LFR_IState& newState);
diff --git a/lfr_states.cpp b/lfr_states.cpp
index 24164be..d0d7471 100644
--- a/lfr_states.cpp
+++ b/lfr_states.cpp
@@ -19,11 +19,13 @@ LFR_IState& State::Idle::getInstance()
 void State::Autonomous::enter(LFR_StateMachine* stateMachine)
 {
     std::cout << "enter autonomous mode" << std::endl;
+    stateMachine->enterAutonomous();
 }
 
 void State::Autonomous::exit(LFR_StateMachine* stateMachine)
 {
     std::cout << "exit autonomous mode" << std::endl;
+    stateMachine->exitAutonomous();
 }
 
 LFR_IState& State::Autonomous::getInstance()