From 57c4c3236e7f05d93aa81a87de82be86df98faf6 Mon Sep 17 00:00:00 2001
From: GHOSCHT <31184695+GHOSCHT@users.noreply.github.com>
Date: Sun, 10 Apr 2022 17:20:40 +0200
Subject: [PATCH] Fix 'Too many messages queued'

---
 Firmware/src/main.cpp | 48 ++++++++++++++++++++++++++++++-------------
 1 file changed, 34 insertions(+), 14 deletions(-)

diff --git a/Firmware/src/main.cpp b/Firmware/src/main.cpp
index a790a26..57d7053 100644
--- a/Firmware/src/main.cpp
+++ b/Firmware/src/main.cpp
@@ -9,7 +9,7 @@
 #include <ESPAsyncWebServer.h>
 #include <Credentials/Credentials.h>
 
-const int STEPS = 5;
+int STEPS = 5;
 
 BluetoothSerial bt;
 AsyncWebServer server(80);
@@ -23,37 +23,57 @@ LightController *light;
 void TaskBlink(void *pvParameters);
 void TaskAnalogRead(void *pvParameters);
 
-void setup()
+void websocketTask(void *parameter)
 {
+    while (true)
+    {
+        websocket->sendMessage(light->getBjtState(), light->getBjtCount());
+        websocket->clearBuffer();
 
-    int bjtCount = 4;
-    const int bjtPin[bjtCount] = {SIG1A, SIG1B, SIG2A, SIG2B};
-    computer = new SerialCommunicator(Serial, 9600, 5, 50);
-    phone = new BluetoothCommunicator(bt, 5, 50);
-    light = new LightController(bjtPin, bjtCount);
+        vTaskDelay(90 / portTICK_PERIOD_MS);
+    }
+    vTaskDelete(NULL);
+}
 
+void registerTasks()
+{
+    xTaskCreate(websocketTask, "websocketTask", 10000, NULL, 1, NULL);
+}
+
+void connectWifi()
+{
     WiFi.begin(WIFI_SSID, WIFI_PW);
-    Serial.println("Connecting to WiFi");
+    Serial.print("Connecting to WiFi");
     while (WiFi.status() != WL_CONNECTED)
     {
         Serial.print(".");
         delay(1000);
     }
+    Serial.println("");
     Serial.println(WiFi.localIP());
+}
 
+void setup()
+{
+    int bjtCount = 4;
+    const int bjtPin[bjtCount] = {SIG1A, SIG1B, SIG2A, SIG2B};
+    computer = new SerialCommunicator(Serial, 9600, 5, 50);
+    phone = new BluetoothCommunicator(bt, 5, 50);
+    light = new LightController(bjtPin, bjtCount);
     websocket = new WebsocketCommunicator(ws, server, 50);
+
+    connectWifi();
+    registerTasks();
 }
 
 void loop()
 {
     light->updateState(computer->receiveMessage(), STEPS);
     light->updateState(phone->receiveMessage(), STEPS);
-    websocket->sendMessage(light->getBjtState(), light->getBjtCount());
-    websocket->clearBuffer();
-    // computer->sendMessage(light->getBjtState(), light->getBjtCount());
-    // computer->clearBuffer();
-    // phone->sendMessage(light->getBjtState(), light->getBjtCount());
-    // phone->clearBuffer();
+    computer->sendMessage(light->getBjtState(), light->getBjtCount());
+    computer->clearBuffer();
+    phone->sendMessage(light->getBjtState(), light->getBjtCount());
+    phone->clearBuffer();
 
     ws.cleanupClients();
     server.begin();