Incorporated encoder readings and teleop control