Improvements to focus code

can measure ifu + rc focus simultaneously
more stable focus measurements
corrected off-by-one bug in rc focus