Robin2:
One possibility is that a 16MHz Arduino is not fast enough for what you want.How often do you want the time critical function to repeat?
I think 16MHz should be fast enough, I just need to do it in an intelligent way.
Roughly speaking, the total loop for the 4 functions wants to be around 50mS. From the code I posted initially I'll have some interrupts to add in and then a servo write part, not sure how much or if it all that will take it over the 50mS target. I will compile everything, see how long it takes to run then that will be the loop time as the faster you sample the more accurate the data becomes.