diff --git a/Balancebot.ino b/Balancebot.ino
index 8330389..6e1960b 100644
--- a/Balancebot.ino
+++ b/Balancebot.ino
@@ -29,12 +29,7 @@ int dT = 0;
 float dT_s = 0.0;
 
 
-//Motor variables
-const int PWM_CYCLE = 12000;
-const byte PWM_RES = 12;
-
-
-//Encoders variables
+//Encoder variables
 long int m1Raw, m1RawLast;
 long int m2Raw, m2RawLast;
 volatile bool M1_A_state, M1_B_state;
@@ -45,6 +40,66 @@ volatile bool M2_A_state, M2_B_state;
 const char* _ps3Address = "18:5e:0f:92:00:6c";
 
 
+//MotorControl Constants
+const int PWM_CYCLE = 12000;
+const byte PWM_RES = 12;
+const int MOTOR_SATURATION = round(pow(2, PWM_RES));
+const float BASE_WIDTH = 0.1837;
+const float WHEEL_DIAMETER = 0.0677;
+const float PULSES_PER_TURN = 1320.0;
+const float BALANCE_POINT = 0.05;
+const float SPEED_REF = 0.00;
+const float TURN_SPEED_REF = 0.00;
+const float DEADBAND_M1_POS = 90.0;
+const float DEADBAND_M1_NEG = 90.0;
+const float DEADBAND_M2_POS = 90.0;
+const float DEADBAND_M2_NEG = 90.0;
+
+
+//MotorControl Tuning
+const float gainScale = 1;
+const float K_SC = 18.5 * gainScale;  //Speed controller gain
+const float K_TC = 90.0 * gainScale;  //Turn controller gain
+const float K_OL = 13.0 * gainScale;  //Outer loop balance controller gain
+const float K_IL = 72.0 * gainScale;  //Inner loop balance controller gain
+const float I_IL = 80.0 * gainScale;  //Inner loop balance controller Igain
+const float filter_gain = 16.0;       //Motor speed LPF gain
+
+
+//MotorControl Help variables
+int M1_Speed_CMD, M2_Speed_CMD;
+float rem_speed_ref, rem_turn_speed_ref;
+float SC_cont_out;
+float TC_cont_out;
+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;
+
+
+//MotorControl Matrices
+mtx_type motor_ang_vel[2][1];
+mtx_type vel_Matrix[2][1];
+mtx_type inv_Kin[2][2];
+
+
+//IMU CONSTANTS
+const float alpha = 0.95;
+const int acc_overflow_value = 65535;
+const int gyro_overflow_value = 4558;  // 4096+512-50=4558 ?
+
+
+//IMU VARIABLES
+int ax, ay, az;
+int cx, cy, cz;
+int gx, gy, gz;
+float gt;
+float acc_pitch;
+float pitch_rate;
+float pitch = 0;
+float pitch_prev = 0;
+
+
 //UDP variables
 uint8_t data[30 * 4];
 
diff --git a/IMU.ino b/IMU.ino
index 83ff38b..55c8455 100644
--- a/IMU.ino
+++ b/IMU.ino
@@ -1,20 +1,3 @@
-//CONSTANTS
-const float alpha = 0.95;
-const int acc_overflow_value = 65535;
-const int gyro_overflow_value = 4558;  // 4096+512-50=4558 ?
-
-
-//IMU VARIABLES
-int ax, ay, az;
-int cx, cy, cz;
-int gx, gy, gz;
-float gt;
-float acc_pitch;
-float pitch_rate;
-float pitch = 0;
-float pitch_prev = 0;
-
-
 void readIMU() {
   //Acceletometer
   ax = convertInt(IMU.accelerometer_x(IMU.readFromAccelerometer()), acc_overflow_value);
diff --git a/UDP.ino b/UDP.ino
index a387256..7c97938 100644
--- a/UDP.ino
+++ b/UDP.ino
@@ -30,7 +30,6 @@ void ConnectToWiFi() {
 }
 
 void PackUdpData() {
-
   int i = 0;
   data[i] = watchdog++;
   data[i += 1] = balancingOn << 1;
diff --git a/motorControl.ino b/motorControl.ino
index 1d61c0f..02970fa 100644
--- a/motorControl.ino
+++ b/motorControl.ino
@@ -1,43 +1,3 @@
-//Constants
-const int MOTOR_SATURATION = round(pow(2, PWM_RES));
-const float BASE_WIDTH = 0.1837;
-const float WHEEL_DIAMETER = 0.0677;
-const float PULSES_PER_TURN = 1320.0;
-const float BALANCE_POINT = 0.05;
-const float SPEED_REF = 0.00;
-const float TURN_SPEED_REF = 0.00;
-const float DEADBAND_M1_POS = 90.0;
-const float DEADBAND_M1_NEG = 90.0;
-const float DEADBAND_M2_POS = 90.0;
-const float DEADBAND_M2_NEG = 90.0;
-
-
-//Tuning
-const float gainScale = 1;
-const float K_SC = 18.5 * gainScale;  //Speed controller gain
-const float K_TC = 90.0 * gainScale;  //Turn controller gain
-const float K_OL = 13.0 * gainScale;  //Outer loop balance controller gain
-const float K_IL = 72.0 * gainScale;  //Inner loop balance controller gain
-const float I_IL = 80.0 * gainScale;  //Inner loop balance controller Igain
-const float filter_gain = 16.0;       //Motor speed LPF gain
-
-//Help variables
-int M1_Speed_CMD, M2_Speed_CMD;
-float rem_speed_ref, rem_turn_speed_ref;
-float SC_cont_out;
-float TC_cont_out;
-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;
-
-//Matrices
-mtx_type motor_ang_vel[2][1];
-mtx_type vel_Matrix[2][1];
-mtx_type inv_Kin[2][2];
-
-
 void initMotors() {
   // Inverse Kinematic matrix of differential drive robot
   inv_Kin[0][0] = WHEEL_DIAMETER / 4;