Creating my Own Controller and Quad Code, Need Help!

Hello everyone. I have been trying to create my own controller and quad copter and i hit a roadblock. The problem is that when i try to fly the quadcopter, it feels like its going to take off as your increase the throttle but once it gets to a point where its actually going to start taking off, it just goes berserk. it never takes off and it just starts going haywire. If i hold it with my hand from the buttom and try to do some basic tests, it seems to work. if you tilt towards a motor, that motor increases and power and if you tilt quickly, the motor reacts with faster speed.

Im not sure if im missing a step or something. i looked into a million equations and tried quite a lot of them, its not working.

here is what i have right now, the Code takes input from the controller and changes throttle speed. The Angles comes from the chip MPU 6050 using Jeff’s library. as far is i know, the Library filters the Acc and Gyro and uses both to give you the Angles but im not sure.

No code for movement yet since i cant even take off vertically. Im Using arduino nano for the quad and uno for the controller. Right now, the leveling code is inside the Quad itself.


the MPU chip:

The ESC :



float Porpotional_gain=3;
float Integral_gain=1.3;
float Derivative_gain=0.2;

PidTime=millis(); // we Read the Current time.
CurrentReading_Yaw=ypr[0] * 180/M_PI;; // we read current Angles.
CurrentReading_Roll=ypr[2] * 180/M_PI;
CurrentReading_Pitch=ypr[1]* 180/M_PI ;

Desired_Roll=Initial_Roll;      //the initial Angle of the MPU when its flat on the ground




Derivative_Roll=Derivative_gain*(( Error_Roll - Prev_Error_Roll)/TimeError);
Derivative_Pitch=Derivative_gain*(( Error_Pitch - Prev_Error_Pitch)/TimeError);


Motor_4_Speed=((MotorPower*MotorPower_Scaler)+MinMicroSeconds_Motor) +Input_Roll;

Prev_Error_Pitch=Error_Pitch; // update values that we need to carry for the next loop call.

Please, i just want to know if im missing something or is this just an "keep adjusting PID " type of problem. The MinMicroSeconds is that smallest value the motor can take which is 1000 milis.
MotorPower is Potentiometer reading from the controller and its scaled by 3.

there is very little that happens outside of this loop in terms of balancing the Quad. What else does this need to get stable flight?

Can the gyro have feedback from the motors? (Mechanical) Try mounting it on the battery, the biggest mass, the motors will have less impact.