Figure out how to have consistent loop times

[Piphi5]

It would be beneficial for our controls loops to be able to assume that the code runs at a constant period (e.g 20 ms)