diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c
index 219a66d..0939bce 100644
--- a/UAV-ControlSystem/src/Flight/pid.c
+++ b/UAV-ControlSystem/src/Flight/pid.c
@@ -22,7 +22,7 @@
 
 
 #define PTERM_SCALE 0.032029f										/*P-term used as a scale value to the PID controller*/
-#define ITERM_SCALE 0.244381f										/*I-term used as a scale value to the PID controller*/
+#define ITERM_SCALE 0.0012f										/*I-term used as a scale value to the PID controller*/
 #define DTERM_SCALE 0.000529f										/*D-term used as a scale value to the PID controller*/
 
 #define RADIO_RANGE 500												/*Radio range input*/
@@ -31,7 +31,7 @@
 #define GYRO_RANGE 720												/*Determines the maximum rotational limit (limits the rc input)*/
 #define COMPASS_RANGE 180											/*Determines the maximum compass limit (limits the rc input)*/
 
-#define PID_MAX_I 256/2												/*Constrains ITerm*/
+#define PID_MAX_I 256											/*Constrains ITerm*/
 #define PID_MAX_D 512												/*Constrains DTerm*/
 
 /*Struct that belongs to a certain PID controller*/
diff --git a/UAV-ControlSystem/src/main.c b/UAV-ControlSystem/src/main.c
index c4af970..2c9dfaf 100644
--- a/UAV-ControlSystem/src/main.c
+++ b/UAV-ControlSystem/src/main.c
@@ -47,6 +47,10 @@ void init_system()
 	//Configure the clock
 	system_clock_config();
 
+	//init motors to run with oneshot 125, small delay
+	HAL_Delay(1000);
+	pwmEnableAllMotors(Oneshot125);
+
 	//Initializes all the pids that are used in the system. This part will also init the gyro and accelerometer.
 	pidInit();
 
@@ -97,9 +101,7 @@ void init_system()
 
 #endif
 
-	//init motors to run with oneshot 125, small delay
-	HAL_Delay(1000);
-	pwmEnableAllMotors(Oneshot125);
+
 
 }