Log Uart communication

CMakeLists.txt

@@ -10,7 +10,7 @@ add_subdirectory(AutonomousMode)
 add_subdirectory(Communication)
 add_subdirectory(Socket)
 
-add_executable(lfr_central main.cpp)
+add_executable(lfr_central main.cpp lfr_state_machine.cpp lfr_states.cpp)
 
 target_include_directories(lfr_central PRIVATE ${lfr_image_processing_SOURCE_DIRS})
 target_include_directories(lfr_central PRIVATE ${lfr_uart_SOURCE_DIRS})

Communication/uart_communication.cpp

@@ -14,6 +14,7 @@ void LFR_UART::openFile(const char *fileName) {
 }
 
 int LFR_UART::writeDataToFile(uint8_t *buff, uint32_t bufferLength) {
+    std::cout << "Sending uart telegram" << std::endl;
     return write(this->fileDescriptor, buff, bufferLength);
 }
 

Communication/uart_communication.h

@@ -10,6 +10,7 @@
 #include <termios.h>
 #include <iostream>
 #include <exception>
+#include <iostream>
 
 #include <bitset>
 

Communication/uart_communication_main.cpp

@@ -1,5 +1,12 @@
 #include "uart_communication.h"
 
+/*
+* Usage: telnet [ip] [port]
+* eg.: telnet 192.158.4.1 8080
+* To quit: ctrl + +
+*           q
+*/
+
 int main(void) {
     printf("Starting the loopback application...\r\n");
     

lfr_state_interface.h

@@ -0,0 +1,11 @@
+#pragma once
+
+class LFR_StateMachine;
+
+class LFR_IState
+{
+ public:
+    virtual void enter(LFR_StateMachine* stateMachine) = 0;
+    virtual void exit(LFR_StateMachine* stateMachine) = 0;
+    virtual ~LFR_IState() {}
+};

lfr_state_machine.cpp

@@ -0,0 +1,127 @@
+#include "lfr_state_machine.h"
+
+// for string delimiter
+vector<string> LFR_StateMachine::split (string s, string delimiter) const {
+    size_t pos_start = 0, pos_end, delim_len = delimiter.length();
+    string token;
+    vector<string> res;
+
+    while ((pos_end = s.find (delimiter, pos_start)) != string::npos) {
+        token = s.substr (pos_start, pos_end - pos_start);
+        pos_start = pos_end + delim_len;
+        res.push_back (token);
+    }
+
+    res.push_back (s.substr (pos_start));
+    return res;
+}
+
+void LFR_StateMachine::sanitize (string& s) const
+{
+        char r = '\r', n = '\n';
+        s.erase(std::remove(s.begin(), s.end(), n), s.end());
+        s.erase(std::remove(s.begin(), s.end(), r), s.end());
+}
+
+bool LFR_StateMachine::checkStringValidity (const std::vector<std::string>& s) const
+{
+    if(s.size() != 5)
+    {
+        return false;
+    }
+
+    try
+    {
+        for(int i = 0; i < 4; i ++)
+        {
+            if(stod(s[i]) > 1.0 || stod(s[i]) < -1.0)
+            {
+                return false;
+            }
+        }
+        int i = stoi(s[4]);
+        if(i != 0 && i != 1)
+        {
+            return false;
+        }
+    }
+    catch(const std::exception& e) {return false;}
+    return true;
+}
+
+void LFR_StateMachine::parseString(string s)
+{
+    sanitize(s);
+    std::vector<std::string> splitStr = split(s, ";");
+    if(!checkStringValidity(splitStr))
+    {
+        std::cout<<"Invalid String" << std::endl;
+        return;
+    }
+    double wheels[4] = {0.0, 0.0, 0.0, 0.0};
+    int mode = std::stoi(splitStr[4]);
+    for(int i = 0; i < 3; i++)
+    {
+        wheels[i] = std::stod(splitStr[i]);
+    }
+    if(mode == 0) {
+        setState(State::Manual::getInstance());
+        uartCommunicator.sendTelegram(wheels[0], wheels[1], wheels[2], wheels[3]);
+    }
+    else if (mode == 1) {setState(State::Autonomous::getInstance());}
+    return;
+}
+
+LFR_StateMachine::LFR_StateMachine():    
+    autonomousMode(videoHeight, videoWidth, thresholdBinary, gaussKernelSize, [&](std::exception const &ex)
+    {
+        std::unique_lock<std::mutex> lock(mutex);
+        std::cerr<<"camera exception:"<<ex.what()<<std::endl;
+        return false;
+    }),
+    uartCommunicator(),
+    socket([&](std::exception const &ex)
+    {
+        std::unique_lock<std::mutex> lock(mutex);
+        std::cerr<<"socket exception:"<<ex.what()<<std::endl;
+        return false;
+    })
+{
+    // Connect String parser to socket
+    socket.addListener([&](LFR_Socket::LFR_Telegram telegram)
+    {
+        std::unique_lock<std::mutex> lock(mutex);
+        parseString(std::string(telegram));
+    }, &mutex);
+    socket.startLoop();
+    
+    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')
+    {
+        std::cin >> input;
+        std::cout << "binned" << std::endl;
+    }
+    std::cout << "Exiting central" << std::endl;
+}
+
+void LFR_StateMachine::setState(LFR_IState& newState)
+{
+    if (&newState != currentState)
+    {
+        currentState->exit(this);
+        currentState = &newState;
+        currentState->enter(this);
+    }
+    else
+    {
+        std::cout << "no switch" << std::endl;
+    }
+
+}

lfr_state_machine.h

@@ -0,0 +1,35 @@
+#pragma once
+#include <iostream>
+#include <algorithm>
+
+#include <lfr.h>
+#include <lfr_socket.h>
+#include <uart_communication.h>
+#include "lfr_state_interface.h"
+#include "lfr_states.h"
+
+class LFR_StateMachine
+{
+    LFR_IState* currentState;
+
+    const int thresholdBinary = 140;
+    const int videoHeight = 720;
+    const int videoWidth = 1280;
+    const int gaussKernelSize = 11;
+
+    std::mutex mutex;
+
+    LFR autonomousMode;
+    LFR_UART uartCommunicator;
+    LFR_Socket socket;
+
+    vector<string> split (string s, string delimiter) const;
+    void sanitize (string& s) const;
+    bool checkStringValidity (const std::vector<std::string>& s) const;
+    void parseString(string s);    
+
+public:
+    LFR_StateMachine();
+    inline LFR_IState* getCurrentState() const {return currentState;}
+    void setState(LFR_IState& newState);
+};

lfr_states.cpp

@@ -0,0 +1,50 @@
+#include "lfr_states.h"
+
+void State::Idle::enter(LFR_StateMachine* stateMachine)
+{
+
+}
+
+void State::Idle::exit(LFR_StateMachine* stateMachine)
+{
+    
+}
+
+LFR_IState& State::Idle::getInstance()
+{
+    static State::Idle singleton;
+    return singleton;
+}
+
+void State::Autonomous::enter(LFR_StateMachine* stateMachine)
+{
+    std::cout << "enter autonomous mode" << std::endl;
+}
+
+void State::Autonomous::exit(LFR_StateMachine* stateMachine)
+{
+    std::cout << "exit autonomous mode" << std::endl;
+}
+
+LFR_IState& State::Autonomous::getInstance()
+{
+    static State::Autonomous singleton;
+    return singleton;
+}
+
+void State::Manual::enter(LFR_StateMachine* stateMachine)
+{
+    std::cout << "enter manual mode" << std::endl;
+
+}
+
+void State::Manual::exit(LFR_StateMachine* stateMachine)
+{
+    std::cout << "exit manual mode" << std::endl;
+}
+
+LFR_IState& State::Manual::getInstance()
+{
+    static State::Manual singleton;
+    return singleton;
+}

lfr_states.h

@@ -0,0 +1,44 @@
+#pragma once
+#include "lfr_state_interface.h"
+#include "lfr_state_machine.h"
+
+namespace State
+{
+    class Idle: public LFR_IState
+    {
+        Idle() {}
+        Idle(const Idle& other) {};
+        Idle& operator= (const Idle& other);
+
+    public:
+        void enter(LFR_StateMachine*);
+        void exit(LFR_StateMachine*);
+        static LFR_IState& getInstance();
+    };
+
+
+    class Autonomous: public LFR_IState
+    {
+        Autonomous() {}
+        Autonomous(const Autonomous& other) {};
+        Autonomous& operator= (const Autonomous& other);
+
+    public:
+        void enter(LFR_StateMachine*);
+        void exit(LFR_StateMachine*);
+        static LFR_IState& getInstance();
+    };
+
+
+    class Manual: public LFR_IState
+    {
+        Manual() {}
+        Manual(const Manual& other) {};
+        Manual& operator= (const Manual& other);
+
+    public:
+        void enter(LFR_StateMachine*);
+        void exit(LFR_StateMachine*);
+        static LFR_IState& getInstance();
+    };
+}

main.cpp

@@ -1,37 +1,14 @@
 #include <iostream>
-#include <lfr.h>
-#include <lfr_socket.h>
-#include <uart_communication.h>
+#include "lfr_state_machine.h"
+#include "lfr_states.h"
+
+
 
 int main(void)
 {
-    std::cout << "hello central" << std::endl;
-
-    const int thresholdBinary = 140;
-    const int videoHeight = 720;
-    const int videoWidth = 1280;
-    const int gaussKernelSize = 11;
-
-    std::mutex mutex;
-
-    std::cout << "create lfr" << std::endl;
-    LFR lfr(videoHeight, videoWidth, thresholdBinary, gaussKernelSize, [&](std::exception const &ex)
-    {
-        std::unique_lock<std::mutex> lock(mutex);
-        std::cerr<<"camera exception:"<<ex.what()<<std::endl;
-        return false;
-    });
-
-    std::cout << "create uart" << std::endl;
-    LFR_UART uartCommunicator;
-
-    std::cout << "create socket" << std::endl;
-    LFR_Socket socket([&](std::exception const &ex)
-    {
-        std::unique_lock<std::mutex> lock(mutex);
-        std::cerr<<"socket exception:"<<ex.what()<<std::endl;
-        return false;
-    });
-
+    std::cout << "started central" << std::endl;
+    // Init State Machine;
+    std::cout << "create State Machine" << std::endl;
+    LFR_StateMachine stateMachine;
     return 0;
 }