Getting ready to refactor teleop
Reorganized utilities