Caching values and taking mean for crude smoothing

Helps, but position still wobbles an annoying amount. May
need to incorporate the gyroscope data next...