Added state controller.

emb/state-controller.cpp

+#include "servos.hpp"
+#include "command.h"
+#include "time.hpp"
+
+#include <avr/interrupt.h>
+
+enum State
+{
+    STATE_DELAY, STATE_READY
+};
+
+static State state = STATE_READY;
+static uint32_t delayEnd = 0;
+
+void parseCommand(command *comm)
+{
+    switch (comm->comm_ch)
+    {
+        case 's':
+            setServoAngle(comm->arg[0] - 1, comm->arg[1] * 60);
+            updateServos();
+            break;
+        case 'd':
+            delayEnd = getCurrentMillis() + comm->arg[0];
+            state = STATE_DELAY;
+            break;
+    }
+}
+
+void updateState()
+{
+    switch (state)
+    {
+        case STATE_READY:
+        {
+            cli();
+            command *tmp = get_command();
+            command comm = *tmp;
+            sei();
+            
+            if (tmp != nullptr)
+                parseCommand(&comm);
+            
+            break;
+        }
+        case STATE_DELAY:
+            if (getCurrentMillis() > delayEnd)
+                state = STATE_READY;
+            
+            break;
+    }
+}
+
+

emb/state-controller.hpp

+#ifndef STATE_CONTROLLER_H
+#define STATE_CONTROLLER_H
+
+#include "command.h"
+
+/**
+ * Updates command the state.
+ * Should be called periodically.
+ */
+void updateState();
+
+#endif /* STATE_CONTROLLER_H */