Redirect telegram to uart

lfr_state_machine.cpp

@@ -1,6 +1,78 @@
 #include "lfr_state_machine.h"
 
-LFR_StateMachine::LFR_StateMachine(LFR_IState& startState):    
+// 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);
@@ -19,11 +91,11 @@ LFR_StateMachine::LFR_StateMachine(LFR_IState& startState):
     socket.addListener([&](LFR_Socket::LFR_Telegram telegram)
     {
         std::unique_lock<std::mutex> lock(mutex);
-        std::cout << telegram;
+        parseString(std::string(telegram));
     }, &mutex);
     socket.startLoop();
     
-    currentState = &startState;
+    currentState = &State::Idle::getInstance();
     currentState->enter(this);
 
     //Start the permanent loop
@@ -37,4 +109,19 @@ LFR_StateMachine::LFR_StateMachine(LFR_IState& startState):
         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

@@ -1,14 +1,16 @@
 #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;
-    LFR_StateMachine() = delete;
 
     const int thresholdBinary = 140;
     const int videoHeight = 720;
@@ -20,15 +22,14 @@ class LFR_StateMachine
     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(LFR_IState& startState);
+    LFR_StateMachine();
     inline LFR_IState* getCurrentState() const {return currentState;}
-    void setState(LFR_IState& newState)
-    {
-        currentState->exit(this);
-        currentState = &newState;
-        currentState->enter(this);
-    }
+    void setState(LFR_IState& newState);
 };

lfr_states.cpp

@@ -18,12 +18,12 @@ LFR_IState& State::Idle::getInstance()
 
 void State::Autonomous::enter(LFR_StateMachine* stateMachine)
 {
-
+    std::cout << "enter autonomous mode" << std::endl;
 }
 
 void State::Autonomous::exit(LFR_StateMachine* stateMachine)
 {
-    stateMachine->setState(State::Idle::getInstance());
+    std::cout << "exit autonomous mode" << std::endl;
 }
 
 LFR_IState& State::Autonomous::getInstance()
@@ -34,12 +34,13 @@ LFR_IState& State::Autonomous::getInstance()
 
 void State::Manual::enter(LFR_StateMachine* stateMachine)
 {
+    std::cout << "enter manual mode" << std::endl;
 
 }
 
 void State::Manual::exit(LFR_StateMachine* stateMachine)
 {
-    stateMachine->setState(State::Idle::getInstance());
+    std::cout << "exit manual mode" << std::endl;
 }
 
 LFR_IState& State::Manual::getInstance()

main.cpp

@@ -9,6 +9,6 @@ int main(void)
     std::cout << "started central" << std::endl;
     // Init State Machine;
     std::cout << "create State Machine" << std::endl;
-    LFR_StateMachine stateMachine(State::Idle::getInstance());
+    LFR_StateMachine stateMachine;
     return 0;
 }