Added position saving to teleop.

Saves 10 times each second, but only 5 are effective.