From 5f0b6f53660d704df88b99b1b9172824dbf25fc1 Mon Sep 17 00:00:00 2001
From: Xoaquin Castrelo <xoaquin.cb@gmail.com>
Date: Sat, 13 Nov 2021 21:34:09 +0000
Subject: [PATCH] Added state controller.
---
emb/state-controller.cpp | 54 ++++++++++++++++++++++++++++++++++++++++
emb/state-controller.hpp | 12 +++++++++
2 files changed, 66 insertions(+)
create mode 100644 emb/state-controller.cpp
create mode 100644 emb/state-controller.hpp
diff --git a/emb/state-controller.cpp b/emb/state-controller.cpp
new file mode 100644
index 0000000..08c3d7d
--- /dev/null
+++ b/emb/state-controller.cpp
@@ -0,0 +1,54 @@
+#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;
+ }
+}
+
+
diff --git a/emb/state-controller.hpp b/emb/state-controller.hpp
new file mode 100644
index 0000000..409b6af
--- /dev/null
+++ b/emb/state-controller.hpp
@@ -0,0 +1,12 @@
+#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 */
--
GitLab