Untested closed-loop code; open-loop should still work ok