🏍️ Adds motionservice with data sync

2024-05-02 22:28:47 +02:00
parent ae1cb70710
commit 9dee0e1bb1
8 changed files with 131 additions and 14 deletions
@@ -0,0 +1,103 @@
+#ifndef MotionService_h
+#define MotionService_h
+
+#include <EventSocket.h>
+#include <TaskManager.h>
+
+#define DEFAULT_STATE false
+#define LIGHT_SETTINGS_ENDPOINT_PATH "/api/input"
+#define ANGLES_EVENT "angles"
+#define INPUT_EVENT "input"
+#define MODE_EVENT "mode"
+
+#define MOTION_INTERVAL 100
+
+enum class MOTION_STATE
+{
+    IDLE,
+    REST,
+    STAND,
+    WALK
+};
+
+class MotionService
+{
+  public:
+    MotionService(PsychicHttpServer *server, EventSocket *socket, SecurityManager *securityManager, TaskManager *taskManager)
+        : _server(server), _socket(socket), _securityManager(securityManager), _taskManager(taskManager)
+    {
+    }
+
+    void begin()
+    {
+        _socket->registerEvent(INPUT_EVENT);
+        _socket->registerEvent(ANGLES_EVENT);
+        _socket->registerEvent(MODE_EVENT);
+
+        _socket->onEvent(INPUT_EVENT, [&](JsonObject &root, int originId) { handleInput(root, originId); });
+
+        _socket->onEvent(MODE_EVENT, [&](JsonObject &root, int originId) { handleMode(root, originId); });
+
+        _socket->onEvent(ANGLES_EVENT, [&](JsonObject &root, int originId) { anglesEvent(root, originId); });
+        _socket->onSubscribe(ANGLES_EVENT,
+                             std::bind(&MotionService::syncState, this, std::placeholders::_1, std::placeholders::_2));
+    }
+
+    void syncState(const String &originId, bool sync = false)
+    {
+        DynamicJsonDocument jsonDocument{200};
+        char output[200];
+        JsonObject root = jsonDocument.to<JsonObject>();
+        root["angles"] = angles;
+        serializeJson(root, output);
+        ESP_LOGV("MotionState", "Syncing state: %s", output);
+        _socket->emit(ANGLES_EVENT, output, originId.c_str());
+    }
+
+    void anglesEvent(JsonObject &root, int originId)
+    {
+        JsonArray array = root["data"].as<JsonArray>();
+        for (int i = 0; i < 12; i++)
+        {
+            angles[i] = array[i];
+        }
+        char output[100];
+        serializeJson(array, output);
+        sprintf(output, "[%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d]", angles[0], angles[1], angles[2], angles[3], angles[4],
+                angles[5], angles[6], angles[7], angles[8], angles[9], angles[10], angles[11]);
+        _socket->emit(ANGLES_EVENT, output, String(originId).c_str());
+    }
+
+    void handleInput(JsonObject &root, int originId)
+    {
+        String jsonString;
+        JsonArray array = root["data"].as<JsonArray>();
+        serializeJson(array, jsonString);
+        ESP_LOGI("MotionService", "%s", jsonString.c_str());
+        for (int i = 0; i < 7; i++)
+        {
+            input[i] = array[i];
+        }
+        ESP_LOGI("MotionService", "Input: %d %d %d %d %d %d %d", input[0], input[1], input[2], input[3], input[4], input[5], input[6]);
+    }
+
+    void handleMode(JsonObject &root, int originId)
+    {
+        ESP_LOGV("MotionService", "Mode %d", root["data"].as<int>());
+        motionState = (MOTION_STATE)root["data"].as<int>();
+        char output[2];
+        sprintf(output, "%d", motionState);
+        _socket->emit(MODE_EVENT, output, String(originId).c_str());
+    }
+
+  private:
+    PsychicHttpServer *_server;
+    EventSocket *_socket;
+    SecurityManager *_securityManager;
+    TaskManager *_taskManager;
+    int8_t input[7] = {0, 0, 0, 0, 0, 0, 0};
+    int16_t angles[12] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
+    MOTION_STATE motionState = MOTION_STATE::IDLE;
+};
+
+#endif