diff --git a/Balancebot.ino b/Balancebot.ino
index fae01b6..1f08c1b 100644
--- a/Balancebot.ino
+++ b/Balancebot.ino
@@ -2,7 +2,6 @@
 #include <GY_85.h>
 #include <Wire.h>
 #include <MatrixMath.h>
-#include <Math.h>
 #include <Ps3Controller.h>
 
 //Declare library objects
@@ -40,13 +39,17 @@ long int m2Raw, m2RawLast;
 volatile bool M1_A_state, M1_B_state;
 volatile bool M2_A_state, M2_B_state;
 
+
 //PS3 Controller variables
 const char* _ps3Address = "18:5e:0f:92:00:6c";
 
+
 //UDP variables
 uint8_t data[30 * 4];
 
+
 void setup() {
+
   //Initialize serial
   Serial.begin(9600);
   delay(10);
@@ -55,12 +58,8 @@ void setup() {
   Wire.begin(IMU_I2C_SDA, IMU_I2C_SCL);
   delay(10);
 
-
   //Initialize IMU
-  Serial.println("Before IMU init");
   IMU.init();
-  Serial.println("After IMU init");
-
   delay(10);
 
   //Initialize encoder interrupts
@@ -94,17 +93,15 @@ void setup() {
 }
 
 void loop() {
-  // Serial.println("Loop");
+
   //Update time variables
   tNow = micros();
   dT = tNow - tLast;        //[Cycle time in microseconds]
   dT_s = dT * pow(10, -6);  //[Cycle time in seconds]
 
-
   //Get sensor data
   readIMU();
 
-
   //Control motors
   motors();
 
diff --git a/UDP.ino b/UDP.ino
index 19e6b33..39141e8 100644
--- a/UDP.ino
+++ b/UDP.ino
@@ -11,16 +11,13 @@ byte watchdog = 0;
 AsyncUDP udp;
 
 void UdpInit() {
-  //Serial.begin(115200);
   ConnectToWiFi();
-  //udp.connect(multicastIP, port);
 }
 
 void UdpLoop() {
   udp.writeTo(data, sizeof(data), multicastIP, port);
 }
 
-
 void ConnectToWiFi() {
   WiFi.mode(WIFI_STA);
   WiFi.begin(ssid, password);
diff --git a/interruptEncoders.ino b/interruptEncoders.ino
index 12aa1a7..b9a02d6 100644
--- a/interruptEncoders.ino
+++ b/interruptEncoders.ino
@@ -93,7 +93,6 @@ void IRAM_ATTR m2_B_changed() {
   }
 }
 
-
 void initEncoderInterrupt() {
   pinMode(M1_ENC_A, INPUT_PULLUP);
   attachInterrupt(digitalPinToInterrupt(M1_ENC_A), m1_A_changed, CHANGE);
@@ -106,4 +105,4 @@ void initEncoderInterrupt() {
 
   pinMode(M2_ENC_B, INPUT_PULLUP);
   attachInterrupt(digitalPinToInterrupt(M2_ENC_B), m2_B_changed, CHANGE);
-}
+}
\ No newline at end of file
diff --git a/motorControl.ino b/motorControl.ino
index 736d6f3..1d61c0f 100644
--- a/motorControl.ino
+++ b/motorControl.ino
@@ -65,16 +65,16 @@ void motors() {
     IMU.init();
   }
 
+  //Calculate wheel angular velocity
+  motor_ang_vel[0][0] = encoderReaderAngVel(m1Raw, m1RawLast, motor_ang_vel[0][0], PULSES_PER_TURN, WHEEL_DIAMETER, dT_s, filter_gain);
+  motor_ang_vel[1][0] = encoderReaderAngVel(m2Raw, m2RawLast, motor_ang_vel[1][0], PULSES_PER_TURN, WHEEL_DIAMETER, dT_s, filter_gain);
 
-    //Calculate wheel angular velocity
-    motor_ang_vel[0][0] = encoderReaderAngVel(m1Raw, m1RawLast, motor_ang_vel[0][0], PULSES_PER_TURN, WHEEL_DIAMETER, dT_s, filter_gain);
-    motor_ang_vel[1][0] = encoderReaderAngVel(m2Raw, m2RawLast, motor_ang_vel[1][0], PULSES_PER_TURN, WHEEL_DIAMETER, dT_s, filter_gain);
   //Calculate robot linear and angular velocity
   Matrix.Multiply((mtx_type*)inv_Kin, (mtx_type*)motor_ang_vel, 2, 2, 1, (mtx_type*)vel_Matrix);
 
   //Get Control Commands
-  rem_turn_speed_ref = floatMap(Ps3.data.analog.stick.ly, -128.0, 127.0, -3.75, 3.75);
   rem_speed_ref = floatMap(Ps3.data.analog.stick.ry, -128.0, 127.0, -0.35, 0.35);
+  rem_turn_speed_ref = floatMap(Ps3.data.analog.stick.lx, -128.0, 127.0, -3.75, 3.75);
 
   if (balancingOn) {
 
@@ -157,6 +157,7 @@ float motorControl(byte motorID, int speedCMD_, int saturation, float dbPos_, fl
   byte ch2 = motorID * 2;
   byte ch1 = ch2 - 1;
   float windup = 0;
+
   //Deadband
   if (speedCMD_ > 0 && speedCMD_ < dbPos_) {
     speedCMD_ = dbPos_;
@@ -171,13 +172,10 @@ float motorControl(byte motorID, int speedCMD_, int saturation, float dbPos_, fl
   } else if (speedCMD_ < -saturation) {
     windup = -saturation - speedCMD_;
     speedCMD_ = -saturation;
-  }
-
-  else {
+  } else {
     speedCMD_ = speedCMD_;
   }
 
-
   //Apply speed command to PWM output
   if (speedCMD_ > 0) {
     ledcWrite(ch1, 0);