First UDP test

Copied code from UDP example
Added calls in setup and loop
Added Wifi config

Balancebot.ino

@@ -85,6 +85,9 @@ void setup() {
 
   //Initialize PS3 controller connection
   Ps3.begin(_ps3Address);
+
+  //Init UDP
+  UdpInit();
 }
 
 void loop() {
@@ -106,13 +109,12 @@ void loop() {
   // Plot
   plot();
 
+  //Udp
+  UdpLoop();
 
   //Save time for next cycle
   tLast = tNow;
 
-
   //Delay
   delay(5);
-
-  //Test
 }

UDP.ino

@@ -0,0 +1,52 @@
+#include "WiFi.h"
+#include "AsyncUDP.h"
+
+const char * ssid = "CaveBot";
+const char * password = "Eating0-Untaxed0-Pod6-Jokester8";
+
+AsyncUDP udp;
+
+void UdpInit()
+{
+    //Serial.begin(115200);
+    WiFi.mode(WIFI_STA);
+    WiFi.begin(ssid, password);
+    if (WiFi.waitForConnectResult() != WL_CONNECTED) {
+        Serial.println("WiFi Failed");
+        while(1) {
+            delay(1000);
+        }
+    }
+    if(udp.listenMulticast(IPAddress(239,1,2,3), 1234)) {
+        Serial.print("UDP Listening on IP: ");
+        Serial.println(WiFi.localIP());
+        udp.onPacket([](AsyncUDPPacket packet) {
+            Serial.print("UDP Packet Type: ");
+            Serial.print(packet.isBroadcast()?"Broadcast":packet.isMulticast()?"Multicast":"Unicast");
+            Serial.print(", From: ");
+            Serial.print(packet.remoteIP());
+            Serial.print(":");
+            Serial.print(packet.remotePort());
+            Serial.print(", To: ");
+            Serial.print(packet.localIP());
+            Serial.print(":");
+            Serial.print(packet.localPort());
+            Serial.print(", Length: ");
+            Serial.print(packet.length());
+            Serial.print(", Data: ");
+            Serial.write(packet.data(), packet.length());
+            Serial.println();
+            //reply to the client
+            packet.printf("Got %u bytes of data", packet.length());
+        });
+        //Send multicast
+        udp.print("Hello!");
+    }
+}
+
+void UdpLoop()
+{
+    delay(1000);
+    //Send multicast
+    udp.print("Anyone here?");
+}

motorControl.ino

@@ -30,7 +30,7 @@ float OL_cont_out;
 float ref_IL, act_IL, error_IL, IL_cont_out, iError_IL, IL_anti_windup;
 float speedCmd1, speedCmd2;
 
-bool balancingOn = true;
+bool balancingOn = false;
 
 //Matrices
 mtx_type motor_ang_vel[2][1];