Update teleop.py

Almost final version