Code:
if(currentPWM >= goalPWM + sensitivity)
return currentPWM - sensitivity;
if(currentPWM <= goalPWM - sensitivity)
return currentPWM + sensitivity;
Since everything is an unsigned char, then this code is a problem.
If goalPWM is 254 and sensitivity is 10 and CurrentPWM is 127, the result of (goalPWM + sensitivity) is 8 and so CurrentPWM is greater then 8 and you return 117 not 137 like you hope.