Added Teleop and PID Functions

teleop driving and shifting, PID reading/writing