lfr_state_machine.cpp

@@ -25,27 +25,25 @@ void LFR_StateMachine::sanitize (string& s) const
 
 bool LFR_StateMachine::checkStringValidity (const std::vector<std::string>& s) const
 {
+    if(s.size() != 5)
+    {
+        return false;
+    }
+
     try
     {
-        int i = stoi(s[0]);
-        if(i == 0 && s.size() == 5)
+        for(int i = 0; i < 4; i ++)
         {
-            for(int i = 0; i < 4; i ++)
+            if(stod(s[i]) > 1.0 || stod(s[i]) < -1.0)
             {
-                if(stod(s[i]) > 1.0 || stod(s[i]) < -1.0)
-                {
-                    return false;
-                }
+                return false;
             }
         }
-        else if(i == 1)
-        {
-            return true;
-        }
-        else
+        int i = stoi(s[4]);
+        if(i != 0 && i != 1)
         {
             return false;
-        }       
+        }
     }
     catch(const std::exception& e) {return false;}
     return true;
@@ -61,12 +59,12 @@ void LFR_StateMachine::parseString(string s)
         return;
     }
     double wheels[4] = {0.0, 0.0, 0.0, 0.0};
-    int mode = std::stoi(splitStr[0]);
+    int mode = std::stoi(splitStr[4]);
+    for(int i = 0; i < 3; i++)
+    {
+        wheels[i] = std::stod(splitStr[i]);
+    }
     if(mode == 0) {
-        for(int i = 1; i < 4; i++)
-        {
-            wheels[i] = std::stod(splitStr[i]);
-        }
         setState(State::Manual::getInstance());
         uartCommunicator.sendTelegram(wheels[0], wheels[1], wheels[2], wheels[3]);
     }
@@ -75,7 +73,7 @@ void LFR_StateMachine::parseString(string s)
 }
 
 LFR_StateMachine::LFR_StateMachine():    
-    autonomousMode(videoHeight, videoWidth, thresholdBinary, gaussKernelSize, maxSpeed, [&](std::exception const &ex)
+    autonomousMode(videoHeight, videoWidth, thresholdBinary, gaussKernelSize, maxSpeed [&](std::exception const &ex)
     {
         std::unique_lock<std::mutex> lock(mutex);
         std::cerr<<"camera exception:"<<ex.what()<<std::endl;