Updated Teleop

Added the lock position detection code into the teleop.