Added smoothing on servo output

Couple of changes;

1) Correct the spelling of "counter"
2) Added proportional for servo output to smooth movements and remove the jitter.