I agree walter, if going to the trouble, why not be correct.

Ken, heres something I was thinking,
Set up a 5msec interupt timer.
on each int, increment a counter
first count (@5msec) ping sonar 1
second count (@10msec) update servos
third count (@15msec) ping sonar 2
fourth count (@20msec) reset counter, write things (from looking at your code), adjust pulse times for next servo update.

so your int handler will do:
reload timer, inc counter, clear jump flags, retrun

main will do something like this:
if counter =1 and sonar1flag=0 then ping sonar 1
if counter =2 and update flag=0 then updateservos
if counter = 3 and sonar2flag=0 then ping sonar 2
if counter =4 and mathflag=0 then math stuff

in each subroutine first thing to do is set the flag, then do the routine.