diff --git a/CommandParser.cpp b/CommandParser.cpp
new file mode 100755
index 0000000..dc599c0
--- /dev/null
+++ b/CommandParser.cpp
@@ -0,0 +1,25 @@
+#include "CommandParser.hpp"
+#include "UartHandler.hpp"
+#include "StateMachine.hpp"
+
+CommandParser &CommandParser::getInstance() noexcept
+{
+    static CommandParser instance;
+    return instance;
+}
+
+CommandParser::Command CommandParser::parsePacket(const uint8_t *packet) noexcept
+{
+    Command command;
+    command.instruction = (Command::TYPE)packet[0];
+    // Fetch args by big endian byte ordering
+    command.args[0] = ((uint16_t)packet[1] << 8) | packet[2];
+    command.args[1] = ((uint16_t)packet[3] << 8) | packet[4];
+
+    // Validate instruction
+    if ((uint8_t)command.instruction > 0x02) {
+        StateMachine::getInstance().raiseError("Received unknown instruction");
+    }
+
+    return command;
+}
diff --git a/CommandParser.hpp b/CommandParser.hpp
new file mode 100755
index 0000000..2c4c777
--- /dev/null
+++ b/CommandParser.hpp
@@ -0,0 +1,30 @@
+#pragma once
+#include <Arduino.h>
+
+// This class takes max-16-character packet and parses commands from it
+// Singleton-class
+class CommandParser
+{
+    // Each command is 5 bytes long (assuming no padding)
+    struct Command {
+        enum class TYPE : uint8_t {
+            MOVE_ABSOLUTE   = 0x00,
+            MOVE_RELATIVE   = 0x01,
+            RESET_POSITIONS = 0x02,
+        } instruction;
+        uint16_t args[2];
+    };
+
+    public:
+    
+    static CommandParser& getInstance() noexcept;
+
+    // Expecting command to be a packet of max size UartHandler::PACKET_SIZE
+    Command parsePacket(const uint8_t* packet) noexcept;
+
+    private:
+        CommandParser() noexcept;
+        CommandParser(const CommandParser&) = delete;
+        CommandParser(CommandParser&&) = delete;
+        ~CommandParser();
+};
diff --git a/MainLoop.hpp b/MainLoop.hpp
index 2272846..512709c 100755
--- a/MainLoop.hpp
+++ b/MainLoop.hpp
@@ -5,13 +5,15 @@
 class MainLoop
 { 
     public:
-        MainLoop(const MainLoop&) = delete;
-        MainLoop(MainLoop&&) = delete;
-        static MainLoop& getInstance() noexcept;
         void setup() noexcept;
         void update() noexcept;
 
+        static MainLoop& getInstance() noexcept;
+
     private:
+        MainLoop(const MainLoop&) = delete;
+        MainLoop(MainLoop&&) = delete;
+
         MainLoop() noexcept;
         ~MainLoop() noexcept;
 };
diff --git a/StateMachine.hpp b/StateMachine.hpp
index 091b0bd..646f97e 100755
--- a/StateMachine.hpp
+++ b/StateMachine.hpp
@@ -4,11 +4,6 @@
 // Singleton-instance
 class StateMachine
 {
-    public:
-    StateMachine(const StateMachine&) = delete;
-    StateMachine(StateMachine&&) = delete;
-    static StateMachine& getInstance() noexcept;
-
      enum class State {
         INITIALIZING,
         IDLING,
@@ -16,6 +11,11 @@ class StateMachine
         ERROR
     };
 
+    public:
+    StateMachine(const StateMachine&) = delete;
+    StateMachine(StateMachine&&) = delete;
+    static StateMachine& getInstance() noexcept;
+
     State getState() const noexcept { return state; };
     String getLastError() const noexcept { return lastError; };
     // May fail if a state transition from state a to b is not valid
diff --git a/UartHandler.cpp b/UartHandler.cpp
index cdf1080..cd0a478 100755
--- a/UartHandler.cpp
+++ b/UartHandler.cpp
@@ -66,14 +66,14 @@ void UartHandler::poll()
         }
         
         // Is the last byte read 0x0A? Then push the packet
-        if (inBufPos > 0 && inBuf[inBufPos - 1] == 0x0A) {
+        if (inBufPos > 0 && inBuf[inBufPos - 1] == TERMINATOR_BYTE) {
             pushPacket();
             inBufPos = 0;
         }
 
         // Is the last byte NOT 0x0A and we are on the last possibly byte?
         // Then something is wrong and we are declaring error.
-        if (inBufPos == PACKET_SIZE && inBuf[inBufPos - 1] != 0x0A) {
+        if (inBufPos == PACKET_SIZE && inBuf[inBufPos - 1] != TERMINATOR_BYTE) {
             StateMachine::getInstance().raiseError("UART packet did not contain the 0x0A endbyte! May it be too long?");
         }
     }
diff --git a/UartHandler.hpp b/UartHandler.hpp
index db1ca15..679978b 100755
--- a/UartHandler.hpp
+++ b/UartHandler.hpp
@@ -1,9 +1,6 @@
 #pragma once
 #include <Arduino.h>
 
-constexpr uint8_t PACKET_SIZE = 16;
-constexpr uint8_t PACKET_RINGBUF_SIZE = 16;
-
 // Will parse serial messages using the serial interface supplied by the dev board
 // Expects packets of at max 16 bytes.
 // Terminating byte must be 0x0A.
@@ -30,6 +27,10 @@ class UartHandler
 
         bool isOpen() const noexcept;
 
+        static constexpr uint8_t TERMINATOR_BYTE = 0x0A;
+        static constexpr uint8_t PACKET_SIZE = 16;
+        static constexpr uint8_t PACKET_RINGBUF_SIZE = 16;
+
     private:
         // Will move the current contents of inBuf into packetRingBuf. Does NOT reset inBufPos!
         void pushPacket();