Balancebot.ino

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

UDP.ino

@@ -1,36 +0,0 @@
-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 = false;
+bool balancingOn = true;
 
 //Matrices
 mtx_type motor_ang_vel[2][1];