Updated Teleop and Movement to work

Works with the new servo setup