Well you will have to tune your PID gains (Kp, Ki, Kd) according to your system. More over be careful about the integral term as it can cause actuator saturation (i.e. maximum PWM). The integral error is usually neglected unless the difference between the desired position and the actual position becomes very small. The reason is that if you keep on adding the integral error then it will become so large that it will cause maximum PWM duty cycle and the other terms will become ineffective. One way to deal with this problem in software is
if(error<5)
{
Ierror = Ierror + error
}
else
{
Ierror=0;
}
c =((KP*error) + (KD*derror) + (KI*Ierror)); //PID equations
Remember to keep the value of Ki very small compared to Kp and Kd. e.g. if Kp=5 and Kd=1.6, then Ki=0.005;
Hopefully this will solve your problems.