Updated Controller loop. TODO move to command design pattern, add overlap checks