Added code to:
- read encoder winch
- display it on the smartdashboard
- use it to gain schedule the PID when there's slack in the arm.

Also added gain scheduling method to PID classes