Watchdog in the first Byte

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,36 @@
+const char* ssid = "CaveBot";
+const char* password = "&nHM%D2!$]Qg[VUv";
+
+#include "WiFi.h"
+#include "AsyncUDP.h"
+
+const IPAddress multicastIP = IPAddress(239, 1, 2, 3);
+int port = 1234;
+
+byte watchdog = 0;
+
+AsyncUDP udp;
+
+void UdpInit() {
+  //Serial.begin(115200);
+  ConnectToWiFi();
+  //udp.connect(multicastIP, port);
+}
+
+void UdpLoop() {
+  byte data[8] = { 1, 2, 3, 4, 5, 6, 7, 8 };
+  data[0] = watchdog++;
+  udp.writeTo(data, sizeof(data), multicastIP, port);
+}
+
+
+void ConnectToWiFi() {
+  WiFi.mode(WIFI_STA);
+  WiFi.begin(ssid, password);
+  if (WiFi.waitForConnectResult() != WL_CONNECTED) {
+    Serial.println("WiFi Failed");
+    while (1) {
+      delay(1000);
+    }
+  }
+}

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];