hi, I have not read in deep your code, but, a servo needs to be "refreshed" with a pulse in [0.5-2ms] every 20 ms (aprox.).
If you wait for the receiving of one new information string from the sensor for longer or shorter than this, your servo will not work properly.
Take a look on this.
Bye.