diff --git a/motorControl.ino b/motorControl.ino
index 9220e02..ac66cee 100644
--- a/motorControl.ino
+++ b/motorControl.ino
@@ -65,19 +65,19 @@ void motors() {
       IMU.init();
   }
 
-  if (balancingOn) {
 
     //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);
 
-    //Calculate robot linear and angular velocity
-    Matrix.Multiply((mtx_type*)inv_Kin, (mtx_type*)motor_ang_vel, 2, 2, 1, (mtx_type*)vel_Matrix);
+  if (balancingOn) {
 
-    //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);
 
     // Speed Controller
     SC_cont_out = PController(rem_speed_ref, vel_Matrix[0][0], K_SC);