added sensor testing and updated teleop to use encoder